Faster camera matrix undistortion.

rpt/triangulator.cpp

@@ -85,13 +85,23 @@
 CameraInternal::CameraInternal(const Camera &cam)
 {
     this->cam = cam;
-
-    this->invK = invert3x3(cam.K);
     this->invR = transpose3x3(cam.R);
 
     // Camera center:
     // C = -(Rᵀ * t) = -(Rᵀ * (R * (T * -1))) = -(Rᵀ * (R * -T)) = -(Rᵀ * -R * T) = -(-T) = T
     this->center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]};
+
+    // Undistort camera matrix
+    // As with the undistortion, the own implementation avoids some overhead compared to OpenCV
+    if (cam.type == "fisheye")
+    {
+        newK = calc_optimal_camera_matrix_fisheye(1.0, {cam.width, cam.height});
+    }
+    else
+    {
+        newK = calc_optimal_camera_matrix_pinhole(1.0, {cam.width, cam.height});
+    }
+    this->invK = invert3x3(newK);
 }
 
 // =================================================================================================
@@ -366,13 +376,19 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_p
     }
 
     // Define key points
-    const size_t N = 9;
+    // Calculate only the contour points of the image, and use less points,
+    // the edges and centers should be enough if the camera has no strange distortions
+    const size_t N = 3;
     std::vector<std ::array<float, 2>> pts;
-    pts.reserve(N * N);
+    pts.reserve(4 * (N - 1));
     for (size_t y = 0; y < N; ++y)
     {
         for (size_t x = 0; x < N; ++x)
         {
+            if (x != 0 && x != N - 1 && y != 0 && y != N - 1)
+            {
+                continue;
+            }
             pts.push_back({x * (w - 1) / (N - 1), y * (h - 1) / (N - 1)});
         }
     }
@@ -406,22 +422,34 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_p
     {
         for (size_t x = 0; x < N; ++x)
         {
+            if (x != 0 && x != N - 1 && y != 0 && y != N - 1)
+            {
+                continue;
+            }
+
             auto &pt = pts[k];
             k += 1;
 
-            oX0 = std::min(oX0, pt[0]);
-            oX1 = std::max(oX1, pt[0]);
-            oY0 = std::min(oY0, pt[1]);
-            oY1 = std::max(oY1, pt[1]);
-
             if (x == 0)
+            {
+                oX0 = std::min(oX0, pt[0]);
                 iX0 = std::max(iX0, pt[0]);
+            }
             if (x == N - 1)
+            {
+                oX1 = std::max(oX1, pt[0]);
                 iX1 = std::min(iX1, pt[0]);
+            }
             if (y == 0)
+            {
+                oY0 = std::min(oY0, pt[1]);
                 iY0 = std::max(iY0, pt[1]);
+            }
             if (y == N - 1)
+            {
+                oY1 = std::max(oY1, pt[1]);
                 iY1 = std::min(iY1, pt[1]);
+            }
         }
     }
     float inner_width = iX1 - iX0;
@@ -916,29 +944,14 @@ void TriangulatorInternal::print_stats()
 void TriangulatorInternal::undistort_poses(
     std::vector<std::vector<std::array<float, 3>>> &poses_2d, CameraInternal &icam)
 {
-    int width = icam.cam.width;
-    int height = icam.cam.height;
-
-    // Undistort camera matrix
-    // As with the undistortion, the own implementation avoids some overhead compared to OpenCV
-    std::array<std::array<float, 3>, 3> newK;
-    if (icam.cam.type == "fisheye")
-    {
-        newK = icam.calc_optimal_camera_matrix_fisheye(1.0, {width, height});
-    }
-    else
-    {
-        newK = icam.calc_optimal_camera_matrix_pinhole(1.0, {width, height});
-    }
-
     float ifx_old = 1.0 / icam.cam.K[0][0];
     float ify_old = 1.0 / icam.cam.K[1][1];
     float cx_old = icam.cam.K[0][2];
     float cy_old = icam.cam.K[1][2];
-    float fx_new = newK[0][0];
-    float fy_new = newK[1][1];
-    float cx_new = newK[0][2];
-    float cy_new = newK[1][2];
+    float fx_new = icam.newK[0][0];
+    float fy_new = icam.newK[1][1];
+    float cx_new = icam.newK[0][2];
+    float cy_new = icam.newK[1][2];
 
     // Undistort all the points
     size_t num_persons = poses_2d.size();
@@ -971,6 +984,8 @@ void TriangulatorInternal::undistort_poses(
     }
 
     // Mask out points that are far outside the image (points slightly outside are still valid)
+    int width = icam.cam.width;
+    int height = icam.cam.height;
     float mask_offset = (width + height) / 20.0;
     for (size_t i = 0; i < num_persons; ++i)
     {
@@ -985,18 +1000,6 @@ void TriangulatorInternal::undistort_poses(
             }
         }
     }
-
-    // Update the camera intrinsics
-    icam.cam.K = newK;
-    icam.invK = CameraInternal::invert3x3(newK);
-    if (icam.cam.type == "fisheye")
-    {
-        icam.cam.DC = {0.0, 0.0, 0.0, 0.0};
-    }
-    else
-    {
-        icam.cam.DC = {0.0, 0.0, 0.0, 0.0, 0.0};
-    }
 }
 
 // =================================================================================================
@@ -1017,7 +1020,7 @@ TriangulatorInternal::project_poses(
     all_dists.resize(num_persons);
 
     // Get camera parameters
-    const std::array<std::array<float, 3>, 3> &K = icam.cam.K;
+    const std::array<std::array<float, 3>, 3> &K = icam.newK;
     const std::array<std::array<float, 3>, 3> &R = icam.cam.R;
     const std::array<std::array<float, 1>, 3> &T = icam.cam.T;
 

rpt/triangulator.hpp

@@ -15,9 +15,10 @@ public:
 
     Camera cam;
 
-    std::array<std::array<float, 3>, 3> invK;
     std::array<std::array<float, 3>, 3> invR;
     std::array<float, 3> center;
+    std::array<std::array<float, 3>, 3> newK;
+    std::array<std::array<float, 3>, 3> invK;
 
     static std::array<std::array<float, 3>, 3> transpose3x3(
         const std::array<std::array<float, 3>, 3> &M);