From 76c39fcafd0ca7e2c1e7674f0cc05f4a8ff4949a Mon Sep 17 00:00:00 2001 From: davidpagnon Date: Tue, 2 Jan 2024 20:15:43 +0100 Subject: [PATCH] should work on cameras with distortions; still needs to be fully tested --- Pose2Sim/Demo/S00_Demo_Session/Config.toml | 1 + .../S00_Calibration/Calib_qualisys.toml | 39 +++++++++++++ .../S00_P00_Participant/Config.toml | 1 + .../S00_P00_T00_StaticTrial/Config.toml | 1 + .../S00_P00_T01_BalancingTrial/Config.toml | 1 + Pose2Sim/Demo/S00_Demo_Session/test.py | 4 +- Pose2Sim/Demo/S01_Empty_Session/Config.toml | 1 + .../S01_P00_Participant0/Config.toml | 1 + .../S01_P00_T00_StaticTrial/Config.toml | 1 + .../S01_P00_T01_MotionTrial1/Config.toml | 1 + .../S01_P00_T02_MotionTrial2/Config.toml | 1 + .../S01_P01_Participant1/Config.toml | 1 + .../S01_P01_T00_StaticTrial/Config.toml | 1 + .../S01_P01_T01_MotionTrial1/Config.toml | 1 + .../S01_P01_T02_MotionTrial2/Config.toml | 1 + Pose2Sim/Utilities/reproj_from_trc_calib.py | 45 +++++++++++++- Pose2Sim/common.py | 48 +++++++++++++-- Pose2Sim/personAssociation.py | 9 +-- Pose2Sim/triangulation.py | 58 ++++++++++--------- setup.cfg | 1 + 20 files changed, 177 insertions(+), 40 deletions(-) create mode 100644 Pose2Sim/Demo/S00_Demo_Session/S00_Calibration/Calib_qualisys.toml diff --git a/Pose2Sim/Demo/S00_Demo_Session/Config.toml b/Pose2Sim/Demo/S00_Demo_Session/Config.toml index 1bdca5f..c14518f 100644 --- a/Pose2Sim/Demo/S00_Demo_Session/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S00_Demo_Session/S00_Calibration/Calib_qualisys.toml b/Pose2Sim/Demo/S00_Demo_Session/S00_Calibration/Calib_qualisys.toml new file mode 100644 index 0000000..f291f73 --- /dev/null +++ b/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 diff --git a/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/Config.toml b/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/Config.toml index b00e47f..45778b1 100644 --- a/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T00_StaticTrial/Config.toml b/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T00_StaticTrial/Config.toml index b00e47f..45778b1 100644 --- a/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T00_StaticTrial/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T01_BalancingTrial/Config.toml b/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T01_BalancingTrial/Config.toml index 01a1e2b..9fd9ab5 100644 --- a/Pose2Sim/Demo/S00_Demo_Session/S00_P00_Participant/S00_P00_T01_BalancingTrial/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S00_Demo_Session/test.py b/Pose2Sim/Demo/S00_Demo_Session/test.py index c3dec73..ba634ed 100644 --- a/Pose2Sim/Demo/S00_Demo_Session/test.py +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/Config.toml index dec2604..b96ff1c 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/Config.toml index 1765461..a66a3ce 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T00_StaticTrial/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T00_StaticTrial/Config.toml index 1765461..a66a3ce 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T00_StaticTrial/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T01_MotionTrial1/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T01_MotionTrial1/Config.toml index 666591e..69256b2 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T01_MotionTrial1/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T02_MotionTrial2/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T02_MotionTrial2/Config.toml index 7dcf944..feda0ac 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P00_Participant0/S01_P00_T02_MotionTrial2/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/Config.toml index 8cad338..d89ad43 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T00_StaticTrial/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T00_StaticTrial/Config.toml index 8cad338..d89ad43 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T00_StaticTrial/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T01_MotionTrial1/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T01_MotionTrial1/Config.toml index 1b3200f..fef43f4 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T01_MotionTrial1/Config.toml +++ b/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 diff --git a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T02_MotionTrial2/Config.toml b/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T02_MotionTrial2/Config.toml index 1b3200f..fef43f4 100644 --- a/Pose2Sim/Demo/S01_Empty_Session/S01_P01_Participant1/S01_P01_T02_MotionTrial2/Config.toml +++ b/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 diff --git a/Pose2Sim/Utilities/reproj_from_trc_calib.py b/Pose2Sim/Utilities/reproj_from_trc_calib.py index 64be15a..69fb178 100644 --- a/Pose2Sim/Utilities/reproj_from_trc_calib.py +++ b/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) diff --git a/Pose2Sim/common.py b/Pose2Sim/common.py index 219820e..64f9471 100644 --- a/Pose2Sim/common.py +++ b/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 ]]) diff --git a/Pose2Sim/personAssociation.py b/Pose2Sim/personAssociation.py index 98425ba..fcfa93f 100644 --- a/Pose2Sim/personAssociation.py +++ b/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, \ - euclidean_distance, natural_sort +from Pose2Sim.common import retrieve_calib_params, computeP, weighted_triangulation, \ + 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,8 +255,8 @@ 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 model = eval(pose_model) diff --git a/Pose2Sim/triangulation.py b/Pose2Sim/triangulation.py index 59d51f4..3c8238d 100644 --- a/Pose2Sim/triangulation.py +++ b/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, \ - euclidean_distance, natural_sort +from Pose2Sim.common import retrieve_calib_params, computeP, weighted_triangulation, \ + 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]) - for id_swapped in range(len(id_cams_swapped))] - for id_off in range(len(id_cams_off))]) + if undistort_points: + 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_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,8 +495,9 @@ 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 model = eval(pose_model) @@ -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 - # 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 - - # # 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 + # undistort points + if undistort_points: + 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], dist[i], None, optim_K[i]) for i in range(n_cams)] + x_files = np.array([[u[i][0][0] for i in range(len(u))] for u in undistorted_points]) + 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 # 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) diff --git a/setup.cfg b/setup.cfg index 3720b1f..dfaaee9 100644 --- a/setup.cfg +++ b/setup.cfg @@ -46,6 +46,7 @@ install_requires = statsmodels filterpy ipython + c3d packages = find_namespace: [options.package_data]