Fixing some errors.

spt/triangulator.cpp

@@ -251,10 +251,10 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
     }
     if (!drop_indices.empty())
     {
-        for (size_t i = drop_indices.size() - 1; i >= 0; --i)
+        for (size_t i = drop_indices.size(); i > 0; --i)
         {
-            all_scored_poses.erase(all_scored_poses.begin() + drop_indices[i]);
-            all_pairs.erase(all_pairs.begin() + drop_indices[i]);
+            all_scored_poses.erase(all_scored_poses.begin() + drop_indices[i - 1]);
+            all_pairs.erase(all_pairs.begin() + drop_indices[i - 1]);
         }
     }
 
@@ -328,7 +328,7 @@ void TriangulatorInternal::reset()
 
 // =================================================================================================
 
-void TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam)
+cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam)
 {
     int width = icam.cam.width;
     int height = icam.cam.height;
@@ -340,9 +340,7 @@ void TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &ica
     // Undistort points
     cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
 
-    // Update the camera parameters
-    icam.K = newK;
-    icam.DC = cv::Mat::zeros(5, 1, CV_64F);
+    return newK;
 }
 
 // =================================================================================================
@@ -363,7 +361,13 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
         }
 
         // Undistort the points
-        undistort_points(points, icam);
+        cv::Mat newK = undistort_points(points, icam);
+        if (p == poses.size() - 1)
+        {
+            // Update the camera matrix as well
+            icam.K = newK;
+            icam.DC = cv::Mat::zeros(5, 1, CV_64F);
+        }
 
         // Update the original poses with the undistorted points
         for (int j = 0; j < num_joints; ++j)

spt/triangulator.hpp

@@ -60,7 +60,7 @@ private:
 
     std::vector<cv::Mat> last_poses_3d;
 
-    void undistort_points(cv::Mat &points, CameraInternal &icam);
+    cv::Mat undistort_points(cv::Mat &points, CameraInternal &icam);
     void undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam);
 
     std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(