Run skelda tests with new implementation.

scripts/test_triangulate.py

@@ -1,6 +1,7 @@
 import copy
 import json
 import os
+import sys
 import time
 from typing import List
 
@@ -9,10 +10,12 @@ import matplotlib
 import numpy as np
 
 import draw_utils
-import triangulate_poses
 import utils_2d_pose
 from skelda import utils_pose
 
+sys.path.append("/SimplePoseTriangulation/swig/")
+import spt
+
 # ==================================================================================================
 
 filepath = os.path.dirname(os.path.realpath(__file__)) + "/"
@@ -437,7 +440,7 @@ def main():
         poses_2d = utils_2d_pose.get_2d_pose(kpt_model, images_2d)
         poses_2d = update_keypoints(poses_2d, joint_names_2d)
         print("2D time:", time.time() - stime)
-        # print(np.array(poses_2d).round(3).tolist())
+        # print([np.array(p).round(6).tolist() for p in poses_2d])
 
         fig1 = draw_utils.show_poses2d(
             poses_2d, np.array(images_2d), joint_names_2d, "2D detections"
@@ -454,11 +457,19 @@ def main():
             poses3D = np.zeros([1, len(joint_names_3d), 4])
             poses2D = np.zeros([len(images_2d), 1, len(joint_names_3d), 3])
         else:
+            cameras = spt.convert_cameras(camparams)
+            roomp = [roomparams["room_center"], roomparams["room_size"]]
+            triangulator = spt.Triangulator(min_score=0.95)
+
             stime = time.time()
-            poses3D = triangulate_poses.get_3d_pose(
-                poses_2d, camparams, roomparams, joint_names_2d
+            poses_3d = triangulator.triangulate_poses(
+                poses_2d, cameras, roomp, joint_names_2d
             )
+            poses3D = np.array(poses_3d)
+            if len(poses3D) == 0:
+                poses3D = np.zeros([1, len(joint_names_3d), 4])
             print("3D time:", time.time() - stime)
+
             poses2D = []
             for cam in camparams:
                 poses_2d, _ = utils_pose.project_poses(poses3D, cam)