should work on cameras with distortions; still needs to be fully tested

Pose2Sim/Demo/S00_Demo_Session/Config.toml

@@ -128,6 +128,7 @@ interpolation = 'cubic' #linear, slinear, quadratic, cubic, or none
 interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S00_Demo_Session/S00_Calibration/Calib_qualisys.toml

@@ -0,0 +1,39 @@
+[cam_01]
+name = "cam_01"
+size = [ 1088.0, 1920.0]
+matrix = [ [ 1681.244873046875, 0.0, 532.97369384375], [ 0.0, 1681.075439453125, 948.137390140625], [ 0.0, 0.0, 1.0]]
+distortions = [ -0.000721609375, 0.002187234375, 9.5e-06, 1.078125e-05]
+rotation = [ 1.6882754799999993, 1.0483220499999997, -0.41955852000000016]
+translation = [ 0.3211048899999996, 0.9563320600000009, 2.8907130499999996]
+fisheye = false
+
+[cam_02]
+name = "cam_02"
+size = [ 1088.0, 1920.0]
+matrix = [ [ 1673.729614265625, 0.0, 534.494567875], [ 0.0, 1673.79724121875, 963.225891109375], [ 0.0, 0.0, 1.0]]
+distortions = [ -0.000747609375, 0.00213728125, 1.51875e-05, 4.546875e-06]
+rotation = [ 1.34975875, 1.5963809099999993, -1.1983285799999999]
+translation = [ -0.11152829000000017, 0.7766184800000001, 3.0675519599999994]
+fisheye = false
+
+[cam_03]
+name = "cam_03"
+size = [ 1088.0, 1920.0]
+matrix = [ [ 1681.598388671875, 0.0, 513.20837403125], [ 0.0, 1681.509887703125, 955.005126953125], [ 0.0, 0.0, 1.0]]
+distortions = [ -0.000729765625, 0.00215034375, -8.46875e-06, -8.078125e-06]
+rotation = [ 0.8109654899999995, -2.1972129299999996, 1.3760277799999996]
+translation = [ -0.7934803899999996, 0.32283594000000126, 4.353514870000001]
+fisheye = false
+
+[cam_04]
+name = "cam_04"
+size = [ 1088.0, 1920.0]
+matrix = [ [ 1675.234985359375, 0.0, 540.106201171875], [ 0.0, 1675.204223640625, 964.0302734375], [ 0.0, 0.0, 1.0]]
+distortions = [ -0.000744265625, 0.002104171875, 4.328125e-06, 3.109375e-06]
+rotation = [ 1.4045571699999995, -1.3887412699999993, 0.42535743000000026]
+translation = [ 0.5030217200000007, 0.04894934000000083, 4.406564460000002]
+fisheye = false
+
+[metadata]
+adjusted = false
+error = 0.0

Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T00_StaticTrial/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T01_BalancingTrial/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S00_Demo_Session/test.py

@@ -5,12 +5,14 @@ def test_workflow():
   
   from Pose2Sim import Pose2Sim
   
+  
   # Calibration
   config_dict = toml.load('Config.toml')
   config_dict.get("project").update({"project_dir":"."})
   Pose2Sim.calibration(config_dict)
   
-  # # Static trial
+  
+  # Static trial
   project_dir = os.path.join("S00_P00_Participant","S00_P00_T00_StaticTrial")
   config_dict.get("project").update({"project_dir":project_dir})
   config_dict['filtering']['display_figures'] = False

Pose2Sim/Demo/S01_Empty_Session/Config.toml

@@ -128,6 +128,7 @@ interpolation = 'cubic' #linear, slinear, quadratic, cubic, or none
 interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T00_StaticTrial/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T01_MotionTrial1/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T02_MotionTrial2/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T00_StaticTrial/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T01_MotionTrial1/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T02_MotionTrial2/Config.toml

@@ -129,6 +129,7 @@
 # interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
 # show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
 # handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
+# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
 # make_c3d = false # save triangulated data in c3d format in addition to trc
 
 

Pose2Sim/Utilities/reproj_from_trc_calib.py

@@ -92,12 +92,13 @@ BODY_25B = Node("CHip", id=None, children=[
 
 
 ## FUNCTIONS
-def computeP(calib_file):
+def computeP(calib_file, undistort=False):
     '''
     Compute projection matrices from toml calibration file.
     
     INPUT:
     - calib_file: calibration .toml file.
+    - undistort: boolean
     
     OUTPUT:
     - P: projection matrix as list of arrays
@@ -109,8 +110,14 @@ def computeP(calib_file):
     calib = toml.load(calib_file)
     for cam in list(calib.keys()):
         if cam != 'metadata':
+            S = np.array(calib[cam]['size'])
             K = np.array(calib[cam]['matrix'])
-            Kh = np.block([K, np.zeros(3).reshape(3,1)])
+            if undistort:
+                dist = np.array(calib[cam]['distortions'])
+                optim_K = cv2.getOptimalNewCameraMatrix(K, dist, [int(s) for s in S], 1, [int(s) for s in S])[0]
+                Kh = np.block([optim_K, np.zeros(3).reshape(3,1)])
+            else:
+                Kh = np.block([K, np.zeros(3).reshape(3,1)])
             R, _ = cv2.Rodrigues(np.array(calib[cam]['rotation']))
             T = np.array(calib[cam]['translation'])
             H = np.block([[R,T.reshape(3,1)], [np.zeros(3), 1 ]])
@@ -120,6 +127,38 @@ def computeP(calib_file):
     return P
     
     
+def retrieve_calib_params(calib_file):
+    '''
+    Compute projection matrices from toml calibration file.
+    
+    INPUT:
+    - calib_file: calibration .toml file.
+    
+    OUTPUT:
+    - S: (h,w) vectors as list of 2x1 arrays
+    - K: intrinsic matrices as list of 3x3 arrays
+    - dist: distortion vectors as list of 4x1 arrays
+    - optim_K: intrinsic matrices for undistorting points as list of 3x3 arrays
+    - R: rotation rodrigue vectors as list of 3x1 arrays
+    - T: translation vectors as list of 3x1 arrays
+    '''
+    
+    calib = toml.load(calib_file)
+
+    S, K, dist, optim_K, R, T = [], [], [], [], [], []
+    for c, cam in enumerate(calib.keys()):
+        if cam != 'metadata':
+            S.append(np.array(calib[cam]['size']))
+            K.append(np.array(calib[cam]['matrix']))
+            dist.append(np.array(calib[cam]['distortions']))
+            optim_K.append(cv2.getOptimalNewCameraMatrix(K[c], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0])
+            R.append(np.array(calib[cam]['rotation']))
+            T.append(np.array(calib[cam]['translation']))
+    calib_params = {'S': S, 'K': K, 'dist': dist, 'optim_K': optim_K, 'R': R, 'T': T}
+            
+    return calib_params
+
+
 def reprojection(P_all, Q):
     '''
     Reprojects 3D point on all cameras.
@@ -231,7 +270,7 @@ def reproj_from_trc_calib_func(*args):
     filename = os.path.splitext(os.path.basename(input_trc_file))[0]
     
     # Extract data from calibration file
-    P_all = computeP(input_calib_file)
+    P_all = computeP(input_calib_file, undistort_points=True)
 
     # Create camera folders
     reproj_dir = os.path.realpath(output_file_root)

Pose2Sim/common.py

@@ -38,25 +38,63 @@ __status__ = "Development"
 
 
 ## FUNCTIONS
-def computeP(calib_file):
+def retrieve_calib_params(calib_file):
     '''
     Compute projection matrices from toml calibration file.
     
     INPUT:
     - calib_file: calibration .toml file.
     
+    OUTPUT:
+    - S: (h,w) vectors as list of 2x1 arrays
+    - K: intrinsic matrices as list of 3x3 arrays
+    - dist: distortion vectors as list of 4x1 arrays
+    - optim_K: intrinsic matrices for undistorting points as list of 3x3 arrays
+    - R: rotation rodrigue vectors as list of 3x1 arrays
+    - T: translation vectors as list of 3x1 arrays
+    '''
+    
+    calib = toml.load(calib_file)
+
+    S, K, dist, optim_K, R, T = [], [], [], [], [], []
+    for c, cam in enumerate(calib.keys()):
+        if cam != 'metadata':
+            S.append(np.array(calib[cam]['size']))
+            K.append(np.array(calib[cam]['matrix']))
+            dist.append(np.array(calib[cam]['distortions']))
+            optim_K.append(cv2.getOptimalNewCameraMatrix(K[c], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0])
+            R.append(np.array(calib[cam]['rotation']))
+            T.append(np.array(calib[cam]['translation']))
+    calib_params = {'S': S, 'K': K, 'dist': dist, 'optim_K': optim_K, 'R': R, 'T': T}
+            
+    return calib_params
+
+
+def computeP(calib_file, undistort=False):
+    '''
+    Compute projection matrices from toml calibration file.
+    
+    INPUT:
+    - calib_file: calibration .toml file.
+    - undistort: boolean
+    
     OUTPUT:
     - P: projection matrix as list of arrays
     '''
     
-    K, R, T, Kh, H = [], [], [], [], []
-    P = []
-    
     calib = toml.load(calib_file)
+    
+    P = []
     for cam in list(calib.keys()):
         if cam != 'metadata':
+            S = np.array(calib[cam]['size'])
             K = np.array(calib[cam]['matrix'])
-            Kh = np.block([K, np.zeros(3).reshape(3,1)])
+            if undistort:
+                dist = np.array(calib[cam]['distortions'])
+                optim_K = cv2.getOptimalNewCameraMatrix(K, dist, [int(s) for s in S], 1, [int(s) for s in S])[0]
+                Kh = np.block([optim_K, np.zeros(3).reshape(3,1)])
+            else:
+                Kh = np.block([K, np.zeros(3).reshape(3,1)])
             R, _ = cv2.Rodrigues(np.array(calib[cam]['rotation']))
             T = np.array(calib[cam]['translation'])
             H = np.block([[R,T.reshape(3,1)], [np.zeros(3), 1 ]])

Pose2Sim/personAssociation.py

@@ -41,8 +41,8 @@ from anytree import RenderTree
 from anytree.importer import DictImporter
 import logging
 
-from Pose2Sim.common import computeP, weighted_triangulation, reprojection, \
+from Pose2Sim.common import retrieve_calib_params, computeP, weighted_triangulation, \
-    euclidean_distance, natural_sort
+    reprojection, euclidean_distance, natural_sort
 from Pose2Sim.skeletons import *
 
 
@@ -247,6 +247,7 @@ def track_2d_all(config):
     pose_model = config.get('pose').get('pose_model')
     tracked_keypoint = config.get('personAssociation').get('tracked_keypoint')
     frame_range = config.get('project').get('frame_range')
+    tracked_keypoint = config.get('triangulation').get('undistort_points')
     
     calib_dir = [os.path.join(session_dir, c) for c in os.listdir(session_dir) if ('Calib' or 'calib') in c][0]
     calib_file = glob.glob(os.path.join(calib_dir, '*.toml'))[0] # lastly created calibration file
@@ -254,7 +255,7 @@ def track_2d_all(config):
     poseTracked_dir = os.path.join(project_dir, 'pose-associated')
 
     # projection matrix from toml calibration file
-    P = computeP(calib_file)
+    P = computeP(calib_file, undistort=undistort_points)
         
     # selection of tracked keypoint id
     try: # from skeletons.py

Pose2Sim/triangulation.py

@@ -48,8 +48,8 @@ from anytree import RenderTree
 from anytree.importer import DictImporter
 import logging
 
-from Pose2Sim.common import computeP, weighted_triangulation, reprojection, \
+from Pose2Sim.common import retrieve_calib_params, computeP, weighted_triangulation, \
-    euclidean_distance, natural_sort
+    reprojection, euclidean_distance, natural_sort
 from Pose2Sim.skeletons import *
 
 
@@ -206,6 +206,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
     likelihood_threshold = config.get('triangulation').get('likelihood_threshold')
     show_interp_indices = config.get('triangulation').get('show_interp_indices')
     interpolation_kind = config.get('triangulation').get('interpolation')
+    handle_LR_swap = config.get('triangulation').get('handle_LR_swap')
     
     # Recap
     calib_cam1 = calib[list(calib.keys())[0]]
@@ -236,7 +237,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
     mean_cam_excluded = np.around(nb_cams_excluded['mean'].mean(), decimals=2)
 
     logging.info(f'\n--> Mean reprojection error for all points on all frames is {mean_error_px} px, which roughly corresponds to {mean_error_mm} mm. ')
-    logging.info(f'Cameras were excluded if likelihood was below {likelihood_threshold} and if the reprojection error was above {error_threshold_triangulation} px.')
+    logging.info(f'Cameras were excluded if likelihood was below {likelihood_threshold} and if the reprojection error was above {error_threshold_triangulation} px. Limb swapping was {"handled" if handle_LR_swap else "not handled"}.')
     logging.info(f'In average, {mean_cam_excluded} cameras had to be excluded to reach these thresholds.')
     cam_excluded_count = {i: v for i, v in zip(cam_names, cam_excluded_count.values())}
     str_cam_excluded_count = ''
@@ -252,7 +253,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
     logging.info(f'\n3D coordinates are stored at {trc_path}.')
 
 
-def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, projection_matrices):
+def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, projection_matrices, *calib_params):
     '''
     Triangulates 2D keypoint coordinates. If reprojection error is above threshold,
     tries swapping left and right sides. If still above, removes a camera until error
@@ -281,6 +282,7 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
     error_threshold_triangulation = config.get('triangulation').get('reproj_error_threshold_triangulation')
     min_cameras_for_triangulation = config.get('triangulation').get('min_cameras_for_triangulation')
     handle_LR_swap = config.get('triangulation').get('handle_LR_swap')
+    undistort_points = config.get('triangulation').get('undistort_points')
 
     # Initialize
     x_files, y_files, likelihood_files = coords_2D_kpt
@@ -319,7 +321,10 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
         Q_filt = [weighted_triangulation(projection_matrices_filt[i], x_files_filt[i], y_files_filt[i], likelihood_files_filt[i]) for i in range(len(id_cams_off))]
         
         # Reprojection
-        coords_2D_kpt_calc_filt = [reprojection(projection_matrices_filt[i], Q_filt[i])  for i in range(len(id_cams_off))]
+        if undistort_points:
+            coords_2D_kpt_calc_filt = [cv2.projectPoints(Q_filt[i], calib_params['R'][i], calib_params['T'][i], calib_params['K'][i], calib_params['dist'][i]) for i in range(len(id_cams_off))]
+        else:
+            coords_2D_kpt_calc_filt = [reprojection(projection_matrices_filt[i], Q_filt[i]) for i in range(len(id_cams_off))]
         coords_2D_kpt_calc_filt = np.array(coords_2D_kpt_calc_filt, dtype=object)
         x_calc_filt = coords_2D_kpt_calc_filt[:,0]
         y_calc_filt = coords_2D_kpt_calc_filt[:,1]
@@ -359,9 +364,14 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
                                                 for id_off in range(len(id_cams_off))] )
                 
                 # Reprojection
-                coords_2D_kpt_calc_off_swap = np.array([[reprojection(projection_matrices_filt[id_off], Q_filt_off_swap[id_off, id_swapped]) 
+                if undistort_points:
-                                                for id_swapped in range(len(id_cams_swapped))]
+                    coords_2D_kpt_calc_off_swap = np.array([[cv2.projectPoints(Q_filt[id_off, id_swapped], calib_params['R'][id_off], calib_params['T'][id_off], calib_params['K'][id_off], calib_params['dist'][id_off]) 
-                                                for id_off in range(len(id_cams_off))])
+                                                    for id_swapped in range(len(id_cams_swapped))]
+                                                    for id_off in range(len(id_cams_off))])
+                else:
+                    coords_2D_kpt_calc_off_swap = np.array([[reprojection(projection_matrices_filt[id_off], Q_filt_off_swap[id_off, id_swapped]) 
+                                                    for id_swapped in range(len(id_cams_swapped))]
+                                                    for id_off in range(len(id_cams_off))])
                 x_calc_off_swap = coords_2D_kpt_calc_off_swap[:,:,0]
                 y_calc_off_swap = coords_2D_kpt_calc_off_swap[:,:,1]
                 
@@ -477,6 +487,7 @@ def triangulate_all(config):
     interpolation_kind = config.get('triangulation').get('interpolation')
     interp_gap_smaller_than = config.get('triangulation').get('interp_if_gap_smaller_than')
     show_interp_indices = config.get('triangulation').get('show_interp_indices')
+    undistort_points = config.get('triangulation').get('undistort_points')
 
     calib_dir = [os.path.join(session_dir, c) for c in os.listdir(session_dir) if ('Calib' or 'calib') in c][0]
     calib_file = glob.glob(os.path.join(calib_dir, '*.toml'))[0] # lastly created calibration file
@@ -484,7 +495,8 @@ def triangulate_all(config):
     poseTracked_dir = os.path.join(project_dir, 'pose-associated')
     
     # Projection matrix from toml calibration file
-    P = computeP(calib_file)
+    P = computeP(calib_file, undistort=undistort_points)
+    calib_params = retrieve_calib_params(calib_file)
         
     # Retrieve keypoints from model
     try: # from skeletons.py
@@ -530,21 +542,13 @@ def triangulate_all(config):
         json_tracked_files_f = [json_tracked_files[c][f] for c in range(n_cams)]
         x_files, y_files, likelihood_files = extract_files_frame_f(json_tracked_files_f, keypoints_ids)
 
-        # # undistort points draft: start with
+        # undistort points
-        # points = [np.array(tuple(zip(x_files[i],y_files[i]))).reshape(-1, 1, 2) for i in range(n_cams)]
+        if undistort_points:
-        # # calculate optimal matrix optimal_mat cf https://stackoverflow.com/a/76635257/12196632
+            points = [np.array(tuple(zip(x_files[i],y_files[i]))).reshape(-1, 1, 2).astype('float32') for i in range(n_cams)]
-        # undistorted_points = [cv2.undistortPoints(points[i], K[i], distortions[i], None, optimal_mat[i]) for i in range(n_cams)]
+            undistorted_points = [cv2.undistortPoints(points[i], K[i], dist[i], None, optim_K[i]) for i in range(n_cams)]
-        # # then put back into original shape of x_files, y_files 
+            x_files =  np.array([[u[i][0][0] for i in range(len(u))] for u in undistorted_points])
-        # # Points are undistorted and better triangulated, however reprojection error is not accurate if points are not distorted again prior to reprojection
+            y_files =  np.array([[u[i][0][1] for i in range(len(u))] for u in undistorted_points])
-        # # This is good for slight distortion. For fishey camera, the model does not work anymore. See there for an example https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301
+            # This is good for slight distortion. For fishey camera, the model does not work anymore. See there for an example https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301
-        
-        # # undistort points draft: start with
-        # points = [np.array(tuple(zip(x_files[i],y_files[i]))).reshape(-1, 1, 2) for i in range(n_cams)]
-        # # calculate optimal matrix optimal_mat cf https://stackoverflow.com/a/76635257/12196632
-        # undistorted_points = [cv2.undistortPoints(points[i], K[i], distortions[i], None, optimal_mat[i]) for i in range(n_cams)]
-        # # then put back into original shape of x_files, y_files 
-        # # Points are undistorted and better triangulated, however reprojection error is not accurate if points are not distorted again prior to reprojection
-        # # This is good for slight distortion. For fishey camera, the model does not work anymore. See there for an example https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301
 
         # Replace likelihood by 0 if under likelihood_threshold
         with np.errstate(invalid='ignore'):
@@ -554,9 +558,9 @@ def triangulate_all(config):
         for keypoint_idx in keypoints_idx:
         # Triangulate cameras with min reprojection error
             coords_2D_kpt = np.array( (x_files[:, keypoint_idx], y_files[:, keypoint_idx], likelihood_files[:, keypoint_idx]) )
-            coords_2D_kpt_swapped = np.array(( x_files[:, keypoints_idx_swapped[keypoint_idx]], y_files[:, keypoints_idx_swapped[keypoint_idx]], likelihood_files[:, keypoints_idx_swapped[keypoint_idx]] ))# ADD coords_2D_kpt_swapped TO THE ARGUMENTS OF triangulation_from_best_cameras 
+            coords_2D_kpt_swapped = np.array(( x_files[:, keypoints_idx_swapped[keypoint_idx]], y_files[:, keypoints_idx_swapped[keypoint_idx]], likelihood_files[:, keypoints_idx_swapped[keypoint_idx]] ))
 
-            Q_kpt, error_kpt, nb_cams_excluded_kpt, id_excluded_cams_kpt = triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, P)
+            Q_kpt, error_kpt, nb_cams_excluded_kpt, id_excluded_cams_kpt = triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, P, calib_params) # P has been modified if undistort_points=True
 
             Q.append(Q_kpt)
             error.append(error_kpt)

setup.cfg

@@ -46,6 +46,7 @@ install_requires =
     statsmodels
     filterpy
     ipython
+    c3d
 packages = find_namespace:
 
 [options.package_data]