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_scored_poses.erase(all_scored_poses.begin() + drop_indices[i - 1]);
-            all_pairs.erase(all_pairs.begin() + drop_indices[i]);
+            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
+    return newK;
-    icam.K = newK;
-    icam.DC = cv::Mat::zeros(5, 1, CV_64F);
 }
 
 // =================================================================================================
@@ -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(

swig/spt.i

@@ -49,3 +49,21 @@ namespace std {
 // Parse the header file to generate wrappers
 %include "../spt/camera.hpp"
 %include "../spt/interface.hpp"
+
+// Add additional Python code to the module
+%pythoncode %{
+def convert_cameras(cameras):
+    """Convert cameras from Python to C++."""
+    c_cameras = []
+    for cam in cameras:
+        camera = Camera()
+        camera.name = cam["name"]
+        camera.K = cam["K"]
+        camera.DC = cam["DC"]
+        camera.R = cam["R"]
+        camera.T = cam["T"]
+        camera.width = cam["width"]
+        camera.height = cam["height"]
+        c_cameras.append(camera)
+    return c_cameras
+%}

tests/test_interface.py

@@ -26,7 +26,7 @@ def main():
     print("")
 
     # Load input data
-    roomparams = [[0, 0, 1], [4, 4, 2]]
+    roomparams = [[0, 0, 1.0], [4.8, 6.0, 2.0]]
     joint_names = [
         "nose",
         "eye_left",
@@ -57,17 +57,7 @@ def main():
         pdata = json.load(file)
     cams = cdata["cameras"]
     poses_2d = pdata["2D"]
-    cameras = []
+    cameras = spt.convert_cameras(cams)
-    for cam in cams:
-        camera = spt.Camera()
-        camera.name = cam["name"]
-        camera.K = cam["K"]
-        camera.DC = cam["DC"]
-        camera.R = cam["R"]
-        camera.T = cam["T"]
-        camera.width = cam["width"]
-        camera.height = cam["height"]
-        cameras.append(camera)
 
     # Run triangulation
     triangulator = spt.Triangulator(min_score=0.95)
@@ -78,6 +68,27 @@ def main():
     print("3D time:", time.time() - stime)
     print(np.array(poses_3d))
 
+    # Load input data
+    roomparams = [[0, -0.5, 1.2], [5.6, 6.4, 2.4]]
+    cpath = "/SimplePoseTriangulation/data/p1/sample.json"
+    ppath = "/SimplePoseTriangulation/tests/poses_p1.json"
+    with open(cpath, "r") as file:
+        cdata = json.load(file)
+    with open(ppath, "r") as file:
+        pdata = json.load(file)
+    cams = cdata["cameras"]
+    poses_2d = pdata["2D"]
+    cameras = spt.convert_cameras(cams)
+
+    # Run triangulation
+    triangulator.reset()
+    stime = time.time()
+    poses_3d = triangulator.triangulate_poses(
+        poses_2d, cameras, roomparams, joint_names
+    )
+    print("3D time:", time.time() - stime)
+    print(np.array(poses_3d))
+
 
 # ==================================================================================================