diff --git a/Pose2Sim/Demo/User/Config.toml b/Pose2Sim/Demo/User/Config.toml index c499fa4..574e4e0 100644 --- a/Pose2Sim/Demo/User/Config.toml +++ b/Pose2Sim/Demo/User/Config.toml @@ -15,7 +15,7 @@ pose_folder_name = 'pose' pose_json_folder_extension = 'json' pose_img_folder_extension = 'img' poseAssociated_folder_name = 'pose-associated' -pose3d_folder_name = 'triangulation' +pose3d_folder_name = 'pose-3d' opensim_folder_name = 'opensim' @@ -26,53 +26,53 @@ pose_model = 'BODY_25B' #CUSTOM, BODY_25B, BODY_25, BODY_135, BLAZEPOSE, HALPE_2 [calibration] -calibration_type = 'convert' # 'convert' or 'calculate' +calibration_type = 'calculate' # 'convert' or 'calculate' [calibration.convert] convert_from = 'qualisys' # 'qualisys', 'optitrack', or 'vicon' [calibration.convert.qualisys] binning_factor = 1 # Usually 1, except when filming in 540p where it usually is 2 - + - [calibration.calculate] # coming later: 'points' + [calibration.calculate] calculate_method = 'board' # 'board' or 'points' - [calibration.calculate.board.intrinsics] # camera properties, only needs to be done once - intrinsics_board_type = 'checkerboard' # 'checkerboard' ('charucoboard' not supported yet) + [calibration.calculate.board.intrinsics] # camera properties, only needs to be done once + intrinsics_board_type = 'checkerboard' # 'checkerboard' ('charucoboard' not supported yet) + overwrite_intrinsics = false # overwrite (or not) if they have already been calculated? + show_detection_intrinsics = true # true or false (lowercase) - overwrite_intrinsics = false # overwrite (or not) if they have already been calculated? - intrinsics_extension = 'avi' # any video or image extension - extract_every_N_sec = 1 # if video, extract frames every N seconds (can be <1 ) - - show_detection_intrinsics = true # true or false (lowercase). Close window to proceed to next detection - intrinsics_corners_nb = [6,9] - intrinsics_square_size = 80 # mm - intrinsics_marker_size = 60 # mm # only checked if charucoboard - intrinsics_aruco_dict = 'DICT_6X6_250' # only checked if charucoboard # see https://docs.opencv.org/3.4/dc/df7/dictionary_8hpp.html - + intrinsics_extension = 'jpg' # any video or image extension + extract_every_N_sec = 1 # if video, extract frames every N seconds (can be <1 ) - [calibration.calculate.board.extrinsics] # camera placement, needs to be done every time - extrinsics_board_type = 'checkerboard' # 'checkerboard', 'scene' ('charucoboard' not supported yet) - # 'board' should be large enough to be detected when laid on the floor. - # 'scene' involves manually clicking any point of know coordinates on scene. Usually more accurate if points are spread out - - calculate_extrinsic = true # true or false (lowercase) - extrinsics_extension = 'avi' # any video or image extension - - # if extrinsics_board_type = 'checkerboard' or 'charucoboard' - extrinsics_corners_nb = [6,9] # [H,W] rather than [w,h] - extrinsics_square_size = 80 # mm # [h,w] if square is actually a rectangle - extrinsics_marker_size = 60 # mm # only checked if 'charucoboard' (not supported yet) - extrinsics_aruco_dict = 'DICT_6X6_250' # only checked if 'charucoboard' # see https://docs.opencv.org/3.4/dc/df7/dictionary_8hpp.html - - # if extrinsics_board_type = 'scene' - # list of 3D coordinates to be manually labelled on images. Can also be 2 dimensional. - object_coords_3d = [[0,0,0],[10,0,0],[10,0,0],[10,10,0],[5,5,5]] - - - [calibration.calculate.points] - calibration_points = 'wand' # 'wand' or 'keypoints' - # Not supported yet. + intrinsics_corners_nb = [4,7] + intrinsics_square_size = 60 # mm + intrinsics_marker_size = 40 # mm # only checked if charucoboard + intrinsics_aruco_dict = 'DICT_6X6_250' # only checked if charucoboard # see https://docs.opencv.org/3.4/dc/df7/dictionary_8hpp.html + + [calibration.calculate.board.extrinsics] # camera placement, needs to be done every time + extrinsics_board_type = 'scene' # 'checkerboard', 'scene' ('charucoboard' not supported yet) + # 'board' should be large enough to be detected when laid on the floor. + # 'scene' involves manually clicking any point of know coordinates on scene. Usually more accurate if points are spread out + calculate_extrinsics = true # true or false (lowercase) + show_reprojection_error = true # true or false (lowercase) + + extrinsics_extension = 'png' # any video or image extension + + # if extrinsics_board_type = 'checkerboard' or 'charucoboard' + extrinsics_corners_nb = [4,7] # [H,W] rather than [w,h] + extrinsics_square_size = 60 # mm # [h,w] if square is actually a rectangle + extrinsics_marker_size = 40 # mm # only checked if 'charucoboard' (not supported yet) + extrinsics_aruco_dict = 'DICT_6X6_250' # only checked if 'charucoboard' # see https://docs.opencv.org/3.4/dc/df7/dictionary_8hpp.html + + # if extrinsics_board_type = 'scene' + # list of 3D coordinates to be manually labelled on images. Can also be a 2 dimensional plane. # in m + object_coords_3d = [[-2.0, 0.3, 0.0], [-2.0 , 0.0, 0.0], [-2.0, 0.0, 0.05], [-2.0, -0.3 , 0.0], [0.0, 0.3, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.05], [0.0, -0.3, 0.0]] # in meters -> Not in mm! <- + + + [calibration.calculate.points] + calibration_points = 'wand' # 'wand' or 'keypoints' + # Not supported yet. [personAssociation] @@ -91,15 +91,17 @@ show_interp_indices = false # true or false (lowercase). For each keypoint, retu [filtering] -type = 'butterworth' # butterworth, butterworth_on_speed, gaussian, LOESS, median +type = 'butterworth' # butterworth, kalman, gaussian, LOESS, median, butterworth_on_speed display_figures = true # true or false (lowercase) [filtering.butterworth] - type = 'low' order = 4 cut_off_frequency = 6 # Hz + [filtering.kalman] + # How much more do you trust triangulation results (measurements), than previous data (process assuming constant acceleration)? + trust_ratio = 100 # = measurement_trust/process_trust ~= process_noise/measurement_noise + smooth = true # should be true, unless you need real-time filtering [filtering.butterworth_on_speed] - type = 'low' order = 4 cut_off_frequency = 10 # Hz [filtering.gaussian] diff --git a/Pose2Sim/Demo/calibration/Calib.qca.txt b/Pose2Sim/Demo/calibration/Calib.qca.txt index 0f96abf..5ced260 100644 --- a/Pose2Sim/Demo/calibration/Calib.qca.txt +++ b/Pose2Sim/Demo/calibration/Calib.qca.txt @@ -2,7 +2,7 @@ - + @@ -10,7 +10,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -26,7 +26,7 @@ - + diff --git a/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extrandom-old.toml b/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extrandom-old.toml deleted file mode 100644 index 1501424..0000000 --- a/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extrandom-old.toml +++ /dev/null @@ -1,30 +0,0 @@ -[cam_1] -name = "cam_01" -size = [ 1920.0, 1080.0] -matrix = [ [ 1203.246243533484, 0.0, 959.5], [ 0.0, 1198.3711959207449, 539.5], [ 0.0, 0.0, 1.0]] -distortions = [ -0.044787602246347216, -0.5273833010005556, 0.009327766682582903, 0.00034371130233083687] -rotation = [ 0, 0, 0] -translation = [ 0, 0, 0] -fisheye = false - -[cam_2] -name = "cam_02" -size = [ 1920.0, 1080.0] -matrix = [ [ 1201.3535871331092, 0.0, 959.5], [ 0.0, 1296.9584035037935, 539.5], [ 0.0, 0.0, 1.0]] -distortions = [ 0.016997873879159856, 0.15076005731825853, -0.07849748162325841, 0.0031187917923049886] -rotation = [ 0, 0, 0] -translation = [ 0, 0, 0] -fisheye = false - -[cam_3] -name = "cam_03" -size = [ 1920.0, 1080.0] -matrix = [ [ 1375.7450722547337, 0.0, 959.5], [ 0.0, 1367.6433832166495, 539.5], [ 0.0, 0.0, 1.0]] -distortions = [ -0.008547555195961013, -0.1321001559843561, 0.002017158533123475, 0.0033830082027901435] -rotation = [ 0, 0, 0] -translation = [ 0, 0, 0] -fisheye = false - -[metadata] -adjusted = false -error = 0.0 diff --git a/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extrandom.toml b/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extrandom.toml deleted file mode 100644 index c9ac8ea..0000000 --- a/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extrandom.toml +++ /dev/null @@ -1,30 +0,0 @@ -[cam_1] -name = "cam_01" -size = [ 1920.0, 1080.0] -matrix = [ [ 1206.3039773711387, 0.0, 959.5], [ 0.0, 1201.3900241712472, 539.5], [ 0.0, 0.0, 1.0]] -distortions = [ -0.048195523248209984, -0.49433115008284967, 0.009146097668361275, 0.00023449575614186346] -rotation = [ 0, 0, 0] -translation = [ 0, 0, 0] -fisheye = false - -[cam_2] -name = "cam_02" -size = [ 1920.0, 1080.0] -matrix = [ [ 1201.3535871331092, 0.0, 959.5], [ 0.0, 1296.9584035037935, 539.5], [ 0.0, 0.0, 1.0]] -distortions = [ 0.016997873879159856, 0.15076005731825853, -0.07849748162325841, 0.0031187917923049886] -rotation = [ 0, 0, 0] -translation = [ 0, 0, 0] -fisheye = false - -[cam_3] -name = "cam_03" -size = [ 1920.0, 1080.0] -matrix = [ [ 1334.9930217175306, 0.0, 959.5], [ 0.0, 1527.484911404606, 539.5], [ 0.0, 0.0, 1.0]] -distortions = [ 0.03903616767043438, -0.21066010320288212, -0.024219751825038247, 0.004221082947531837] -rotation = [ 0, 0, 0] -translation = [ 0, 0, 0] -fisheye = false - -[metadata] -adjusted = false -error = 0.0 diff --git a/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extscene.toml b/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extscene.toml new file mode 100644 index 0000000..cef9ba9 --- /dev/null +++ b/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extscene.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.70179346], [1.04839803], [-0.40602443]] +translation = [ [0.31962238], [0.93995713], [2.97508412]] +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.35902244], [1.60141928], [-1.18177902]] +translation = [ [-0.12599186], [0.74502948], [3.17221429]] +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.81816165], [-2.22184308], [1.4608978]] +translation = [ [-0.76301979], [0.34503189], [4.43845847]] +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.4245729], [-1.39512301], [0.45721172]] +translation = [ [0.48659282], [0.02675454], [4.33496911]] +fisheye = false + +[metadata] +adjusted = false +error = 0.0 diff --git a/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extzeros.toml.old b/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extzeros.toml.old new file mode 100644 index 0000000..c48aa8d --- /dev/null +++ b/Pose2Sim/Demo/calibration/Calib_intcheckerboard_extzeros.toml.old @@ -0,0 +1,39 @@ +[cam_1] +name = "cam_01" +size = [ 1088.0, 1920.0] +matrix = [ [ 1687.6396622414502, 0.0, 543.5], [ 0.0, 1686.6625452424132, 959.5], [ 0.0, 0.0, 1.0]] +distortions = [ -0.05054882348909584, 0.1576374662929963, 0.002210439026715273, -0.002164250840100675] +rotation = [ 0, 0, 0] +translation = [ 0, 0, 0] +fisheye = false + +[cam_2] +name = "cam_02" +size = [ 1088.0, 1920.0] +matrix = [ [ 1687.859605173682, 0.0, 543.5], [ 0.0, 1685.4371774565182, 959.5], [ 0.0, 0.0, 1.0]] +distortions = [ -0.03518095956993663, 0.037170387967130306, 0.0017983248726869015, -0.0018747264134721386] +rotation = [ 0, 0, 0] +translation = [ 0, 0, 0] +fisheye = false + +[cam_3] +name = "cam_03" +size = [ 1088.0, 1920.0] +matrix = [ [ 1680.286039024573, 0.0, 543.5], [ 0.0, 1681.1761700950424, 959.5], [ 0.0, 0.0, 1.0]] +distortions = [ -0.030304789365317657, 0.04613237843981006, 0.004618104732402956, -0.001684974310757026] +rotation = [ 0, 0, 0] +translation = [ 0, 0, 0] +fisheye = false + +[cam_4] +name = "cam_04" +size = [ 1088.0, 1920.0] +matrix = [ [ 1689.483288223434, 0.0, 543.5], [ 0.0, 1688.5630322142706, 959.5], [ 0.0, 0.0, 1.0]] +distortions = [ -0.04369424846208241, 0.12212413289800295, 0.0017367557051414114, -0.0018985222378279064] +rotation = [ 0, 0, 0] +translation = [ 0, 0, 0] +fisheye = false + +[metadata] +adjusted = false +error = 0.0 diff --git a/Pose2Sim/Demo/calibration/Calib_qualisys.toml b/Pose2Sim/Demo/calibration/Calib_qualisys.toml index af40495..f291f73 100644 --- a/Pose2Sim/Demo/calibration/Calib_qualisys.toml +++ b/Pose2Sim/Demo/calibration/Calib_qualisys.toml @@ -1,5 +1,5 @@ -[cam_1] -name = "cam01" +[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] @@ -7,31 +7,31 @@ rotation = [ 1.6882754799999993, 1.0483220499999997, -0.41955852000000016] translation = [ 0.3211048899999996, 0.9563320600000009, 2.8907130499999996] fisheye = false -[cam_2] -name = "cam02" +[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.1115282900000002, 0.7766184800000001, 3.0675519599999994] +translation = [ -0.11152829000000017, 0.7766184800000001, 3.0675519599999994] fisheye = false -[cam_3] -name = "cam03" +[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.7934803899999995, 0.32283594000000126, 4.353514870000001] +translation = [ -0.7934803899999996, 0.32283594000000126, 4.353514870000001] fisheye = false -[cam_4] -name = "cam04" +[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.5030217200000006, 0.04894934000000085, 4.406564460000002] +translation = [ 0.5030217200000007, 0.04894934000000083, 4.406564460000002] fisheye = false [metadata] diff --git a/Pose2Sim/Demo/calibration/Calib_qualisys.toml.old b/Pose2Sim/Demo/calibration/Calib_qualisys.toml.old new file mode 100644 index 0000000..46f409b --- /dev/null +++ b/Pose2Sim/Demo/calibration/Calib_qualisys.toml.old @@ -0,0 +1,39 @@ +[cam_1] +name = "cam01" +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_2] +name = "cam02" +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_3] +name = "cam03" +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_4] +name = "cam04" +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/calibration/extrinsics/ext_cam1_vid/cam01_ext.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam01_ext.png new file mode 100644 index 0000000..308499a Binary files /dev/null and b/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam01_ext.png differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam1_00000.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam1_00000.png deleted file mode 100644 index dd264e3..0000000 Binary files a/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam1_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam2_00720.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam2_00720.png deleted file mode 100644 index 009c034..0000000 Binary files a/Pose2Sim/Demo/calibration/extrinsics/ext_cam1_vid/cam2_00720.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam2_vid/cam02_ext.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam2_vid/cam02_ext.png new file mode 100644 index 0000000..3330a58 Binary files /dev/null and b/Pose2Sim/Demo/calibration/extrinsics/ext_cam2_vid/cam02_ext.png differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam2_vid/cam2_00000.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam2_vid/cam2_00000.png deleted file mode 100644 index 487c909..0000000 Binary files a/Pose2Sim/Demo/calibration/extrinsics/ext_cam2_vid/cam2_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam03_ext.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam03_ext.png new file mode 100644 index 0000000..cc84014 Binary files /dev/null and b/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam03_ext.png differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam1_00000.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam1_00000.png deleted file mode 100644 index dd264e3..0000000 Binary files a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam1_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam2_00720.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam2_00720.png deleted file mode 100644 index 009c034..0000000 Binary files a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam2_00720.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam3_00000.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam3_00000.png deleted file mode 100644 index f79ed91..0000000 Binary files a/Pose2Sim/Demo/calibration/extrinsics/ext_cam3_vid/cam3_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam4_vid/.gitkeep b/Pose2Sim/Demo/calibration/extrinsics/ext_cam4_vid/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/Pose2Sim/Demo/calibration/extrinsics/ext_cam4_vid/cam04_ext.png b/Pose2Sim/Demo/calibration/extrinsics/ext_cam4_vid/cam04_ext.png new file mode 100644 index 0000000..0dfd93f Binary files /dev/null and b/Pose2Sim/Demo/calibration/extrinsics/ext_cam4_vid/cam04_ext.png differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00000.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00000.png deleted file mode 100644 index dd264e3..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00000_00000.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00000_00000.png deleted file mode 100644 index dd264e3..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00000_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00030.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00030.png deleted file mode 100644 index 16bc202..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00030.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00031.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00031.png deleted file mode 100644 index f7911c1..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00031.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00060.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00060.png deleted file mode 100644 index 69579fd..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00060.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00090.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00090.png deleted file mode 100644 index a7585ae..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00090.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00120.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00120.png deleted file mode 100644 index cb550ce..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00120.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00150.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00150.png deleted file mode 100644 index 5f9ad99..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00150.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00180.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00180.png deleted file mode 100644 index ff59926..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/cam1_00180.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_150.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_150.jpg new file mode 100644 index 0000000..7e486de Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_150.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_199.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_199.jpg new file mode 100644 index 0000000..ce2c9c3 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_199.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_204.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_204.jpg new file mode 100644 index 0000000..3653643 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_204.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_369.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_369.jpg new file mode 100644 index 0000000..ae2f773 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_369.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_386.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_386.jpg new file mode 100644 index 0000000..015646b Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_386.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_406.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_406.jpg new file mode 100644 index 0000000..7674345 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_406.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_449.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_449.jpg new file mode 100644 index 0000000..1629ddc Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_449.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_530.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_530.jpg new file mode 100644 index 0000000..f525d3e Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_530.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_536.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_536.jpg new file mode 100644 index 0000000..a34f46d Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid1_536.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_110.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_110.jpg new file mode 100644 index 0000000..2fdfcfd Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_110.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_139.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_139.jpg new file mode 100644 index 0000000..3f04828 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_139.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_146.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_146.jpg new file mode 100644 index 0000000..f9ace20 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_146.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_149.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_149.jpg new file mode 100644 index 0000000..3460afe Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_149.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_315.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_315.jpg new file mode 100644 index 0000000..c0f9477 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_315.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_369.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_369.jpg new file mode 100644 index 0000000..285f080 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_369.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_389.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_389.jpg new file mode 100644 index 0000000..5e85a5b Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_389.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_480.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_480.jpg new file mode 100644 index 0000000..ab0dbdc Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam1_vid/vid2_480.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00000.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00000.png deleted file mode 100644 index 487c909..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00240.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00240.png deleted file mode 100644 index f006ead..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00240.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00480.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00480.png deleted file mode 100644 index e9be3ec..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00480.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00720.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00720.png deleted file mode 100644 index 009c034..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00720.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00960.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00960.png deleted file mode 100644 index 34cde4c..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_00960.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01200.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01200.png deleted file mode 100644 index 21ec0b8..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01200.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01440.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01440.png deleted file mode 100644 index a7116fd..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01440.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01680.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01680.png deleted file mode 100644 index 56ea014..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/cam2_01680.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_001.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_001.jpg new file mode 100644 index 0000000..904e0f2 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_001.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_042.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_042.jpg new file mode 100644 index 0000000..955f2d6 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_042.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_053.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_053.jpg new file mode 100644 index 0000000..9e9cd5c Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_053.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_413.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_413.jpg new file mode 100644 index 0000000..597b333 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_413.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_437.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_437.jpg new file mode 100644 index 0000000..83c737f Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_437.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_440.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_440.jpg new file mode 100644 index 0000000..a937b86 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_440.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_455.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_455.jpg new file mode 100644 index 0000000..8f346a1 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_455.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_461.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_461.jpg new file mode 100644 index 0000000..3800c78 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_461.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_477.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_477.jpg new file mode 100644 index 0000000..441176d Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid1_477.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_175.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_175.jpg new file mode 100644 index 0000000..87c2ccf Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_175.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_182.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_182.jpg new file mode 100644 index 0000000..b8d9d81 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_182.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_195.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_195.jpg new file mode 100644 index 0000000..7466140 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_195.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_230.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_230.jpg new file mode 100644 index 0000000..8949324 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_230.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_243.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_243.jpg new file mode 100644 index 0000000..67e56c5 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_243.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_256.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_256.jpg new file mode 100644 index 0000000..f1dfa67 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam2_vid/vid2_256.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam1_00000.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam1_00000.png deleted file mode 100644 index dd264e3..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam1_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam2_00720.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam2_00720.png deleted file mode 100644 index 009c034..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam2_00720.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam3_00000.png b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam3_00000.png deleted file mode 100644 index f79ed91..0000000 Binary files a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/cam3_00000.png and /dev/null differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_231.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_231.jpg new file mode 100644 index 0000000..5fc1630 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_231.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_236.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_236.jpg new file mode 100644 index 0000000..b105f31 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_236.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_240.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_240.jpg new file mode 100644 index 0000000..a3d4e46 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_240.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_491.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_491.jpg new file mode 100644 index 0000000..2e99a4d Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_491.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_492.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_492.jpg new file mode 100644 index 0000000..d3ae211 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_492.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_507.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_507.jpg new file mode 100644 index 0000000..3a0a075 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid1_507.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_045.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_045.jpg new file mode 100644 index 0000000..14c1af3 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_045.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_049.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_049.jpg new file mode 100644 index 0000000..73d821b Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_049.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_211.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_211.jpg new file mode 100644 index 0000000..048c785 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_211.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_269.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_269.jpg new file mode 100644 index 0000000..da82833 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_269.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_271.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_271.jpg new file mode 100644 index 0000000..de3286a Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_271.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_275.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_275.jpg new file mode 100644 index 0000000..883152f Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_275.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_522.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_522.jpg new file mode 100644 index 0000000..a50ff6e Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_522.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_580.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_580.jpg new file mode 100644 index 0000000..bbf6de2 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_580.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_662.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_662.jpg new file mode 100644 index 0000000..5ba8b79 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam3_vid/vid2_662.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/.gitkeep b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_261.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_261.jpg new file mode 100644 index 0000000..7c95395 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_261.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_275.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_275.jpg new file mode 100644 index 0000000..cabaf8c Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_275.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_304.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_304.jpg new file mode 100644 index 0000000..ef559d2 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_304.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_350.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_350.jpg new file mode 100644 index 0000000..a8655e1 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_350.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_364.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_364.jpg new file mode 100644 index 0000000..228bcf2 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_364.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_516.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_516.jpg new file mode 100644 index 0000000..b7b401d Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid1_516.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_058.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_058.jpg new file mode 100644 index 0000000..0b87455 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_058.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_071.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_071.jpg new file mode 100644 index 0000000..73edabd Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_071.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_083.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_083.jpg new file mode 100644 index 0000000..24fa671 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_083.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_282.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_282.jpg new file mode 100644 index 0000000..741adee Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_282.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_285.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_285.jpg new file mode 100644 index 0000000..09e14f3 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_285.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_303.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_303.jpg new file mode 100644 index 0000000..065c081 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_303.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_477.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_477.jpg new file mode 100644 index 0000000..7042ea8 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_477.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_598.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_598.jpg new file mode 100644 index 0000000..b580ad2 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_598.jpg differ diff --git a/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_609.jpg b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_609.jpg new file mode 100644 index 0000000..4fda298 Binary files /dev/null and b/Pose2Sim/Demo/calibration/intrinsics/int_cam4_vid/vid2_609.jpg differ diff --git a/Pose2Sim/Pose2Sim.py b/Pose2Sim/Pose2Sim.py index eb77845..26d4003 100644 --- a/Pose2Sim/Pose2Sim.py +++ b/Pose2Sim/Pose2Sim.py @@ -28,10 +28,13 @@ # - conda install Pose2Sim Usage: + # First run Pose estimation and organize your directories (see Readme.md) from Pose2Sim import Pose2Sim - Pose2Sim.track2D() - Pose2Sim.triangulate3D() - Pose2Sim.filter3D() + Pose2Sim.calibration() + Pose2Sim.personAssociation() + Pose2Sim.triangulation() + Pose2Sim.filtering() + # Then run OpenSim (see Readme.md) ''' @@ -75,6 +78,7 @@ def base_params(config_dict): seq_name = os.path.basename(project_dir) frames = ["all frames" if frame_range == [] else f"frames {frame_range[0]} to {frame_range[1]}"][0] + if not os.path.exists('User'): os.mkdir('User') with open(os.path.join(project_dir, 'User', 'logs.txt'), 'a+') as log_f: pass logging.basicConfig(format='%(message)s', level=logging.INFO, handlers = [logging.handlers.TimedRotatingFileHandler(os.path.join(project_dir, 'User', 'logs.txt'), when='D', interval=7), logging.StreamHandler()]) diff --git a/Pose2Sim/__init__.py b/Pose2Sim/__init__.py index a8344b6..3ce7188 100644 --- a/Pose2Sim/__init__.py +++ b/Pose2Sim/__init__.py @@ -5,4 +5,15 @@ import sys __version__ = '0.4' -VERSION = __version__ \ No newline at end of file +VERSION = __version__ + + +## AUTHORSHIP INFORMATION +__author__ = "David Pagnon" +__copyright__ = "Copyright 2021, Pose2Sim" +__credits__ = ["David Pagnon"] +__license__ = "BSD 3-Clause License" +__version__ = '0.4' +__maintainer__ = "David Pagnon" +__email__ = "contact@david-pagnon.com" +__status__ = "Development" \ No newline at end of file diff --git a/Pose2Sim/calibration.py b/Pose2Sim/calibration.py index adf34e5..7746675 100644 --- a/Pose2Sim/calibration.py +++ b/Pose2Sim/calibration.py @@ -41,6 +41,9 @@ from lxml import etree import warnings import matplotlib.pyplot as plt from mpl_interactions import zoom_factory, panhandler +from PIL import Image +from contextlib import contextmanager,redirect_stderr,redirect_stdout +from os import devnull from Pose2Sim.common import RT_qca2cv, rotate_cam, quat2mat, euclidean_distance, natural_sort @@ -57,6 +60,19 @@ __status__ = "Development" ## FUNCTIONS +@contextmanager +def suppress_stdout_stderr(): + ''' + A context manager that redirects stdout and stderr to devnull + Supresses error message from a compiled C/Fortran sub-function + (namely FFMPEG, called by OpenCV). + ''' + + with open(devnull, 'w') as fnull: + with redirect_stderr(fnull) as err, redirect_stdout(fnull) as out: + yield (err, out) + + def calib_qca_fun(file_to_convert_path, binning_factor=1): ''' Convert a Qualisys .qca.txt calibration file @@ -78,6 +94,7 @@ def calib_qca_fun(file_to_convert_path, binning_factor=1): ''' + logging.info(f'Converting {file_to_convert_path} to .toml calibration file...') ret, C, S, D, K, R, T = read_qca(file_to_convert_path, binning_factor) RT = [RT_qca2cv(r,t) for r, t in zip(R, T)] @@ -222,6 +239,7 @@ def calib_vicon_fun(file_to_convert_path, binning_factor=1): ''' + logging.info(f'Converting {file_to_convert_path} to .toml calibration file...') ret, C, S, D, K, R, T = read_vicon(file_to_convert_path) RT = [RT_qca2cv(r,t) for r, t in zip(R, T)] @@ -323,32 +341,44 @@ def calib_board_fun(calib_dir, intrinsics_config_dict, extrinsics_config_dict): - T: extrinsic translation: list of arrays of floats ''' - ret, C, S, D, K, R, T = [], [], [], [], [], [], [] + overwrite_intrinsics = intrinsics_config_dict.get('overwrite_intrinsics') + calculate_extrinsics = extrinsics_config_dict.get('calculate_extrinsics') + # retrieve intrinsics if calib_file found and if overwrite_intrinsics=False - overwrite_intrinsics = extrinsics_config_dict.get('overwrite_intrinsics') try: calib_file = glob.glob(os.path.join(calib_dir, f'Calib*.toml'))[0] except: pass if not overwrite_intrinsics and 'calib_file' in locals(): - logging.info(f'Preexisting intrinsics file found: {calib_file}. Retrieving intrinsics.') + logging.info(f'\nPreexisting calibration file found: \'{calib_file}\'.') + logging.info(f'\nRetrieving intrinsic parameters from file. Set "overwrite_intrinsics" to true in Config.toml to overwrite.') calib_file = glob.glob(os.path.join(calib_dir, f'Calib*.toml'))[0] calib_data = toml.load(calib_file) - K, D = [], [] + + ret, C, S, D, K, R, T = [], [], [], [], [], [], [] for cam in calib_data: if cam != 'metadata': - K += calib_data[cam]['matrix'] - D += calib_data[cam]['distortions'] + ret += [0.0] + C += [calib_data[cam]['name']] + S += [calib_data[cam]['size']] + K += [np.array(calib_data[cam]['matrix'])] + D += [calib_data[cam]['distortions']] + R += [[0.0, 0.0, 0.0]] + T += [[0.0, 0.0, 0.0]] # calculate intrinsics otherwise else: - logging.info(f'Calculating intrinsics.') - K, D = calibrate_intrinsics(calib_dir, intrinsics_config_dict) + logging.info(f'\nCalculating intrinsic parameters...') + ret, C, S, D, K, R, T = calibrate_intrinsics(calib_dir, intrinsics_config_dict) # calculate extrinsics - calculate_extrinsic = extrinsics_config_dict.get('calculate_extrinsics') - if calculate_extrinsic: - R, T = calibrate_extrinsics(calib_dir, extrinsics_config_dict) + if calculate_extrinsics: + logging.info(f'\nCalculating extrinsic parameters...') + ret, C, S, D, K, R, T = calibrate_extrinsics(calib_dir, extrinsics_config_dict, C, S, K, D) + else: + logging.info(f'\nExtrinsic parameters won\'t be calculated. Set "calculate_extrinsics" to true in Config.toml to calculate them.') + + return ret, C, S, D, K, R, T def calibrate_intrinsics(calib_dir, intrinsics_config_dict): @@ -369,15 +399,15 @@ def calibrate_intrinsics(calib_dir, intrinsics_config_dict): try: intrinsics_cam_listdirs_names = next(os.walk(os.path.join(calib_dir, 'intrinsics')))[1] except StopIteration: + logging.exception(f'Error: No {os.path.join(calib_dir, "intrinsics")} folder found.') raise Exception(f'Error: No {os.path.join(calib_dir, "intrinsics")} folder found.') - intrinsics_board_type = intrinsics_config_dict.get('intrinsics_board_type') intrinsics_extension = intrinsics_config_dict.get('intrinsics_extension') extract_every_N_sec = intrinsics_config_dict.get('extract_every_N_sec') overwrite_extraction = False show_detection_intrinsics = intrinsics_config_dict.get('show_detection_intrinsics') intrinsics_corners_nb = intrinsics_config_dict.get('intrinsics_corners_nb') intrinsics_square_size = intrinsics_config_dict.get('intrinsics_square_size') - C, S, D, K, R, T = [], [], [], [], [], [] + ret, C, S, D, K, R, T = [], [], [], [], [], [], [] for i,cam in enumerate(intrinsics_cam_listdirs_names): # Prepare object points @@ -390,6 +420,7 @@ def calibrate_intrinsics(calib_dir, intrinsics_config_dict): logging.info(f'\nCamera {cam}:') img_vid_files = glob.glob(os.path.join(calib_dir, 'intrinsics', cam, f'*.{intrinsics_extension}')) if img_vid_files == []: + logging.exception(f'The folder {os.path.join(calib_dir, "intrinsics", cam)} does not exist or does not contain any files with extension .{intrinsics_extension}.') raise ValueError(f'The folder {os.path.join(calib_dir, "intrinsics", cam)} does not exist or does not contain any files with extension .{intrinsics_extension}.') img_vid_files = sorted(img_vid_files, key=lambda c: [int(n) for n in re.findall(r'\d+', c)]) #sorting paths with numbers # extract frames from video if video @@ -398,6 +429,7 @@ def calibrate_intrinsics(calib_dir, intrinsics_config_dict): cap = cv2.VideoCapture(img_vid_files[0]) cap.read() if cap.read()[0] == False: + logging.exception('No video in the folder or wrong extension.') raise ValueError('No video in the folder or wrong extension.') # extract frames from video extract_frames(img_vid_files[0], extract_every_N_sec, overwrite_extraction) @@ -418,29 +450,26 @@ def calibrate_intrinsics(calib_dir, intrinsics_config_dict): if isinstance(imgp_confirmed, np.ndarray): imgpoints.append(imgp_confirmed) objpoints.append(objp) - if len(imgpoints) <= 10: - logging.info(f'Corners were detected only on {len(imgpoints)} images for camera {cam}. Calibration of intrinsic parameters may not be accurate with less than 20 good images of the board.') + if len(imgpoints) < 10: + logging.info(f'Corners were detected only on {len(imgpoints)} images for camera {cam}. Calibration of intrinsic parameters may not be accurate with less than 10 good images of the board.') # calculate intrinsics img = cv2.imread(str(img_path)) - ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[1::-1], + ret_cam, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[1::-1], None, None, flags=(cv2.CALIB_FIX_K3 + cv2.CALIB_FIX_PRINCIPAL_POINT)) h, w = [np.float32(i) for i in img.shape[:-1]] + ret.append(ret_cam) C.append(f'cam_{str(i+1).zfill(2)}') S.append([w, h]) D.append(dist[0]) K.append(mtx) - R.append([0,0,0]) - T.append([0,0,0]) + R.append([0.0, 0.0, 0.0]) + T.append([0.0, 0.0, 0.0]) - # Write calibration file with calculated intrinsic parameters and random extrinsicc parameters - calib_file = os.path.join(calib_dir, f'Calib_int{intrinsics_board_type}_extrandom.toml') - toml_write(calib_file, C, S, D, K, R, T) - - return K, D + return ret, C, S, D, K, R, T -def calibrate_extrinsics(calib_dir, extrinsics_config_dict): +def calibrate_extrinsics(calib_dir, extrinsics_config_dict, C, S, K, D): ''' Calibrates extrinsic parameters from an image or the first frame of a video @@ -458,224 +487,78 @@ def calibrate_extrinsics(calib_dir, extrinsics_config_dict): try: extrinsics_cam_listdirs_names = next(os.walk(os.path.join(calib_dir, 'extrinsics')))[1] except StopIteration: + logging.exception(f'Error: No {os.path.join(calib_dir, "extrinsics")} folder found.') raise Exception(f'Error: No {os.path.join(calib_dir, "extrinsics")} folder found.') extrinsics_extension = extrinsics_config_dict.get('extrinsics_extension') extrinsics_board_type = extrinsics_config_dict.get('extrinsics_board_type') extrinsics_corners_nb = extrinsics_config_dict.get('extrinsics_corners_nb') - show_detection_extrinsics = True + extrinsics_square_size = extrinsics_config_dict.get('extrinsics_square_size') + object_coords_3d = np.array(extrinsics_config_dict.get('object_coords_3d'), np.float32) + show_reprojection_error = extrinsics_config_dict.get('show_reprojection_error') - for i,cam in enumerate(extrinsics_cam_listdirs_names): + ret, R, T = [], [], [] + for i, cam in enumerate(extrinsics_cam_listdirs_names): logging.info(f'\nCamera {cam}:') + # Read images or video img_vid_files = glob.glob(os.path.join(calib_dir, 'extrinsics', cam, f'*.{extrinsics_extension}')) if img_vid_files == []: + logging.exception(f'The folder {os.path.join(calib_dir, "extrinsics", cam)} does not exist or does not contain any files with extension .{extrinsics_extension}.') raise ValueError(f'The folder {os.path.join(calib_dir, "extrinsics", cam)} does not exist or does not contain any files with extension .{extrinsics_extension}.') img_vid_files = sorted(img_vid_files, key=lambda c: [int(n) for n in re.findall(r'\d+', c)]) #sorting paths with numbers + img = cv2.imread(img_vid_files[0]) + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) - + # Find corners or label by hand if extrinsics_board_type == 'checkerboard': - # find corners - imgp = findCorners(img_vid_files[0], extrinsics_corners_nb, objp=[], show_detection_intrinsics=show_detection_intrinsics=) - # CHANGE FINDCORNERS: 'O' for okay and next, 'D' for delete, 'C' for click - # DEFINE OBJECT POINTS - - # IF CORNERS NOT FOUND: RAISE ERROR: 'set "show_detection_extrinsics" to true to click corners by hand, or change extrinsic_board_type to "scene"' - - if isinstance(imgp, np.ndarray): - objpoints.append(objp) - imgpoints.append(imgp) - - - + imgp = findCorners(img_vid_files[0], extrinsics_corners_nb, objp=[], show=show_reprojection_error) + if imgp == []: + logging.exception('No corners found. Set "show_detection_extrinsics" to true to click corners by hand, or change extrinsic_board_type to "scene"') + raise ValueError('No corners found. Set "show_detection_extrinsics" to true to click corners by hand, or change extrinsic_board_type to "scene"') + objp = np.zeros((extrinsics_corners_nb[0]*extrinsics_corners_nb[1],3), np.float32) + objp[:,:2] = np.mgrid[0:extrinsics_corners_nb[0],0:extrinsics_corners_nb[1]].T.reshape(-1,2) + objp[:,:2] = objp[:,0:2]*extrinsics_square_size elif extrinsics_board_type == 'scene': - - - if len(objp) <= 10: - logging.info(f'Only {len(objp)} reference points for camera {cam}. Calibration of extrinsic parameters may not be accurate with less than 10 refeence points.') + imgp, objp = imgp_objp_visualizer_clicker(img, imgp=[], objp=object_coords_3d, img_path=img_vid_files[0]) + if imgp == []: + logging.exception('No points clicked (or fewer than 6). Press \'C\' when the image is displayed, and then click on the image points corresponding to the \'object_coords_3d\' you measured and wrote down in the Config.toml file.') + raise ValueError('No points clicked (or fewer than 6). Press \'C\' when the image is displayed, and then click on the image points corresponding to the \'object_coords_3d\' you measured and wrote down in the Config.toml file.') + if len(objp) < 10: + logging.info(f'Only {len(objp)} reference points for camera {cam}. Calibration of extrinsic parameters may not be accurate with less than 10 reference points, as spread out as possible.') + # Calculate extrinsics + mtx, dist = np.array(K[i]), np.array(D[i]) + _, r, t = cv2.solvePnP(objp, imgp, mtx, dist) - # try: - # intrinsics_cam_listdirs_names = next(os.walk(os.path.join(calib_dir, 'intrinsics')))[1] - # except StopIteration: - # raise Exception(f'Error: No {os.path.join(calib_dir, "intrinsics")} folder found.') - # intrinsics_board_type = intrinsics_config_dict.get('intrinsics_board_type') - # intrinsics_extension = intrinsics_config_dict.get('intrinsics_extension') - # extract_every_N_sec = intrinsics_config_dict.get('extract_every_N_sec') - # overwrite_extraction=False - # show_detection_intrinsics = intrinsics_config_dict.get('show_detection_intrinsics') - # intrinsics_corners_nb = intrinsics_config_dict.get('intrinsics_corners_nb') - # intrinsics_square_size = intrinsics_config_dict.get('intrinsics_square_size') - # C, S, D, K, R, T = [], [], [], [], [], [] + # Projection of object points to image plane + Kh_cam = np.block([mtx, np.zeros(3).reshape(3,1)]) + r_mat, _ = cv2.Rodrigues(r) + H_cam = np.block([[r_mat,t.reshape(3,1)], [np.zeros(3), 1 ]]) + P_cam = Kh_cam.dot(H_cam) + proj_obj = [ ( P_cam[0].dot(np.append(o, 1)) / P_cam[2].dot(np.append(o, 1)), P_cam[1].dot(np.append(o, 1)) / P_cam[2].dot(np.append(o, 1)) ) for o in objp] - # for i,cam in enumerate(intrinsics_cam_listdirs_names): - # # Prepare object points - # objp = np.zeros((intrinsics_corners_nb[0]*intrinsics_corners_nb[1],3), np.float32) - # objp[:,:2] = np.mgrid[0:intrinsics_corners_nb[0],0:intrinsics_corners_nb[1]].T.reshape(-1,2) - # objp[:,:2] = objp[:,0:2]*intrinsics_square_size - # objpoints = [] # 3d points in world space - # imgpoints = [] # 2d points in image plane + # Check calibration results + if show_reprojection_error: + img = cv2.imread(img_vid_files[0]) + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + for o in proj_obj: + cv2.circle(img, (int(o[0]), int(o[1])), 8, (0,0,255), -1) + for i in imgp: + cv2.drawMarker(img, (int(i[0][0]), int(i[0][1])), (0,255,0), cv2.MARKER_CROSS, 15, 2) + im_pil = Image.fromarray(img) + print(img_vid_files[0]) + im_pil.show(title = img_vid_files[0]) - # print(f'\nCamera {cam}:') - # img_vid_files = glob.glob(os.path.join(calib_dir, 'intrinsics', cam, f'*.{intrinsics_extension}')) - # if img_vid_files == []: - # raise ValueError(f'The folder {os.path.join(calib_dir, "intrinsics", cam)} does not exist or does not contain any files with extension .{intrinsics_extension}.') - # img_vid_files = sorted(img_vid_files, key=lambda c: [int(n) for n in re.findall(r'\d+', c)]) #sorting paths with numbers - # # extract frames from video if video - # try: - # # check if file is a video rather than an image - # cap = cv2.VideoCapture(img_vid_files[0]) - # cap.read() - # if cap.read()[0] == False: - # raise ValueError('No video in the folder or wrong extension.') - # ## extract frames from video - # extract_frames(img_vid_files[0], extract_every_N_sec, overwrite_extraction) - # img_vid_files = glob.glob(os.path.join(calib_dir, 'intrinsics', cam, f'*.png')) - # img_vid_files = sorted(img_vid_files, key=lambda c: [int(n) for n in re.findall(r'\d+', c)]) - # except: - # pass - - # # find corners - # for img_path in img_vid_files: - # imgp = findCorners(img_path, intrinsics_corners_nb, show_detection_intrinsics) - # if isinstance(imgp, np.ndarray): - # objpoints.append(objp) - # imgpoints.append(imgp) - # if len(imgpoints) <= 10: - # print(f'Corners were detected only on {len(imgpoints)} images for camera {cam}. Calibration of intrinsic parameters may not be accurate with less than 20 good images of the board.') - - # # calculate intrinsics - # img = cv2.imread(str(img_path)) - # ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[1::-1], - # None, None, flags=(cv2.CALIB_FIX_K3 + cv2.CALIB_FIX_PRINCIPAL_POINT)) - # h, w = [np.float32(i) for i in img.shape[:-1]] - # C.append(f'cam_{str(i+1).zfill(2)}') - # S.append([w, h]) - # D.append(dist[0]) - # K.append(mtx) - # R.append([0,0,0]) - # T.append([0,0,0]) - - # # Write calibration file with calculated intrinsic parameters and random extrinsicc parameters - # calib_file = os.path.join(calib_dir, f'Calib_int{intrinsics_board_type}_extrandom.toml') - # toml_write(calib_file, C, S, D, K, R, T) - - # return K, D - - - - - project_dir = config.get('project').get('project_dir') - if project_dir == '': project_dir = os.getcwd() - calib_folder_name = config.get('project').get('calib_folder_name') - calib_dir = os.path.join(project_dir, calib_folder_name) - cam_listdirs_names = next(os.walk(calib_dir))[1] - - corners_nb = config.get('calibration').get('checkerboard').get('corners_nb') - square_size = config.get('calibration').get('checkerboard').get('square_size') - square_size = [square_size, square_size] if isinstance(square_size, int)==True else square_size - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # stop refining after 30 iterations or if error less than 0.001px - - frame_for_origin = config.get('calibration').get('checkerboard').get('frame_for_origin') - show = config.get('calibration').get('checkerboard').get('show_corner_detection') - from_vid_or_img = config.get('calibration').get('checkerboard').get('from_vid_or_img') - vid_snapshot_every_N_frames = config.get('calibration').get('checkerboard').get('vid_snapshot_every_N_frames') - vid_extension = config.get('calibration').get('checkerboard').get('vid_extension') - img_extension = config.get('calibration').get('checkerboard').get('img_extension') - - ret, C, S, D, K, R, T = [], [], [], [], [], [], [] - for cam in cam_listdirs_names: - print(f'\nCamera {cam}:') - # Prepare object points - objp = np.zeros((corners_nb[0]*corners_nb[1],3), np.float32) - objp[:,:2] = np.mgrid[0:corners_nb[0],0:corners_nb[1]].T.reshape(-1,2) - objp[:,0] = objp[:,0]*square_size[0] - objp[:,1] = objp[:,1]*square_size[1] - objpoints = [] # 3d points in world space - imgpoints = [] # 2d points in image plane - - # Find corners in vid - # Try videocapture - if from_vid_or_img=='vid': - video = glob.glob(os.path.join(calib_dir, cam, '*.'+ vid_extension))[0] - cap = cv2.VideoCapture(video) - ret_vid, img = cap.read() - while ret_vid: - count = int(round(cap.get(1))) - ret_vid, img_vid = cap.read() - if count % vid_snapshot_every_N_frames == 0: - img = img_vid - imgp = findCorners(img, corners_nb, criteria, show) - if isinstance(imgp, np.ndarray): - objpoints.append(objp) - imgpoints.append(imgp) - cap.release() - - # Find corners in images - elif from_vid_or_img=='img': - images = glob.glob(os.path.join(calib_dir, cam, '*.'+ img_extension)) - images_sorted = sorted(images, key=lambda c: [int(n) for n in re.findall(r'\d+', c)]) #sorting paths with numbers - for image_f in images_sorted: - img = cv2.imread(image_f) - imgp = findCorners(img, corners_nb, criteria, show) - if isinstance(imgp, np.ndarray): - objpoints.append(objp) - imgpoints.append(imgp) - - # Calibration - r, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[1::-1], - None, None, flags=(cv2.CALIB_FIX_K3 + cv2.CALIB_FIX_PRINCIPAL_POINT)) - h, w = [np.float32(i) for i in img.shape[:-1]] - print(ret, repr(mtx), repr(dist)) - print(w,h) - - ret.append(r) - C.append(cam) - S.append([w, h]) - D.append(dist[0]) - K.append(mtx) - R.append(rvecs[frame_for_origin].squeeze()) - T.append(tvecs[frame_for_origin].squeeze()) - - # Object view to camera view - RT = [rotate_cam(r, t, ang_x=np.pi, ang_y=0, ang_z=0) for r, t in zip(R, T)] - R = [rt[0] for rt in RT] - T = [rt[1] for rt in RT] - R = [np.array(cv2.Rodrigues(r)[0]).flatten() for r in R] - T = np.array(T)/1000 + # Calculate reprojection error + imgp_to_objreproj_dist = [euclidean_distance(proj_obj[n], imgp[n]) for n in range(len(proj_obj))] + rms_px = np.sqrt(np.sum([d**2 for d in imgp_to_objreproj_dist])) + ret.append(rms_px) + R.append(r) + T.append(t) return ret, C, S, D, K, R, T -def extract_frames(video_path, extract_every_N_sec=1, overwrite_extraction=False): - ''' - Extract frames if don't exist yet or if overwrite==True - - INPUT: - - video_path: path to video whose frames need to be extracted - - extract_every_N_sec: extract one frame every N seconds (can be <1) - - overwrite_extraction: if True, overwrite even if frames have already been extracted - - OUTPUT: - - extracted frames in folder - ''' - - if not os.path.exists(os.path.splitext(video_path)[0] + '_00000.png') or overwrite_extraction: - cap = cv2.VideoCapture(str(video_path)) - if cap.isOpened(): - fps = round(cap.get(cv2.CAP_PROP_FPS)) - frame_nb = 0 - logging.info(f'Extracting frames...') - while cap.isOpened(): - ret, frame = cap.read() - if ret == True: - if frame_nb % (fps*extract_every_N_sec) == 0: - img_path = (os.path.splitext(video_path)[0] + '_' +str(frame_nb).zfill(5)+'.png') - cv2.imwrite(str(img_path), frame) - frame_nb+=1 - else: - break - - def findCorners(img_path, corner_nb, objp=[], show=True): ''' Find corners in the photo of a checkerboard. @@ -706,8 +589,9 @@ def findCorners(img_path, corner_nb, objp=[], show=True): try: img = cv2.imread(img_path) except: - with warnings.catch_warnings(): - warnings.simplefilter("ignore") + with suppress_stdout_stderr(): + # with warnings.catch_warnings(): + # warnings.simplefilter("ignore") cap = cv2.VideoCapture(img_path) ret, img = cap.read() gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) @@ -739,18 +623,13 @@ def findCorners(img_path, corner_nb, objp=[], show=True): # If corners are not found, dismiss or click points by hand else: - logging.info(f'{os.path.basename(img_path)}: Corners not found.') + logging.info(f'{os.path.basename(img_path)}: Corners not found. To label them by hand, set "show_detection_intrinsics" to true in the Config.toml file.') if show: # Visualizer and key press event handler imgp_objp_confirmed = imgp_objp_visualizer_clicker(img, imgp=[], objp=objp, img_path=img_path) else: imgp_objp_confirmed = [] - - # if len(imgp_objp_confirmed) == 1: - # imgp_confirmed = imgp_objp_confirmed - # objp_confirmed = objp - # else: - # imgp_confirmed, objp_confirmed = imgp_objp_confirmed + return imgp_objp_confirmed @@ -794,12 +673,16 @@ def imgp_objp_visualizer_clicker(img, imgp=[], objp=[], img_path=''): # If 'y', close all # If points have been clicked, imgp_confirmed is returned, else imgp # If objp is given, objp_confirmed is returned in addition - if 'scat' not in globals(): + if 'scat' not in globals() or 'imgp_confirmed' not in globals(): imgp_confirmed = imgp objp_confirmed = objp else: - imgp_confirmed = [imgp.astype('float32') for imgp in imgp_confirmed] + imgp_confirmed = np.array([imgp.astype('float32') for imgp in imgp_confirmed]) objp_confirmed = np.array(objp_confirmed) + # OpenCV needs at leas 4 correspondance points to calibrate + if len(imgp_confirmed) < 6: + objp_confirmed = [] + imgp_confirmed = [] # close all, del all global variables except imgp_confirmed and objp_confirmed plt.close('all') if len(objp) == 0: @@ -810,8 +693,7 @@ def imgp_objp_visualizer_clicker(img, imgp=[], objp=[], img_path=''): # If 'n', close all and return nothing plt.close('all') imgp_confirmed = [] - if len(objp) == 0: - objp_confirmed = [] + objp_confirmed = [] if event.key == 'c': # If 'c', allows retrieving imgp_confirmed by clicking them on the image @@ -829,8 +711,11 @@ def imgp_objp_visualizer_clicker(img, imgp=[], objp=[], img_path=''): ax_3d.scatter(xs,ys,zs, marker='.', color='k') ax_3d.text(xs,ys,zs, f'{str(i+1)}', size=10, zorder=1, color='k') set_axes_equal(ax_3d) + ax_3d.set_xlabel('X') + ax_3d.set_ylabel('Y') + ax_3d.set_zlabel('Z') if np.all(objp[:,2] == 0): - ax_3d.view_init(elev=90, azim=-90) + ax_3d.view_init(elev=-90, azim=0) fig_3d.show() if event.key == 'h': @@ -846,7 +731,7 @@ def imgp_objp_visualizer_clicker(img, imgp=[], objp=[], img_path=''): fig_3d.canvas.draw() elif count == len(objp)-1: # if all objp have been clicked or indicated as not visible, close all - imgp_confirmed = [imgp.astype('float32') for imgp in imgp_confirmed] + imgp_confirmed = np.array(imgp) plt.close('all') for var_to_delete in ['events', 'count', 'scat', 'fig_3d', 'ax_3d', 'objp_confirmed_notok']: if var_to_delete in globals(): @@ -895,8 +780,8 @@ def imgp_objp_visualizer_clicker(img, imgp=[], objp=[], img_path=''): fig_3d.canvas.draw() elif count == len(objp)-1: # retrieve objp_confirmed - objp_confirmed = [[objp[count]] if 'objp_confirmed' not in globals() else objp_confirmed+[objp[count]]][0] - imgp_confirmed = [imgp.astype('float32') for imgp in imgp_confirmed] + objp_confirmed = np.array([[objp[count].tolist()] if 'objp_confirmed' not in globals() else objp_confirmed+[objp[count]]][0]) + imgp_confirmed = np.array(imgp_confirmed) # close all, delete all plt.close('all') for var_to_delete in ['events', 'count', 'scat', 'scat_3d', 'fig_3d', 'ax_3d', 'objp_confirmed_notok']: @@ -913,44 +798,45 @@ def imgp_objp_visualizer_clicker(img, imgp=[], objp=[], img_path=''): # If last event was 'H' and objp given, remove last point from objp_confirmed_notok elif event.button == 3: # right click # If last event was left click: - if 'button' in dir(events[-1]): - if events[-1].button == 1: - # Remove lastpoint from image - new_xydata = scat.get_offsets()[:-1] - scat.set_offsets(new_xydata) - plt.draw() - # Remove last point from imgp_confirmed - imgp_confirmed = imgp_confirmed[:-1] + if 'events' in globals(): + if 'button' in dir(events[-1]): + if events[-1].button == 1: + # Remove lastpoint from image + new_xydata = scat.get_offsets()[:-1] + scat.set_offsets(new_xydata) + plt.draw() + # Remove last point from imgp_confirmed + imgp_confirmed = imgp_confirmed[:-1] + if len(objp) != 0: + if count >= 1: count -= 1 + # Remove last point from objp_confirmed + objp_confirmed = objp_confirmed[:-1] + # remove from plot + if len(ax_3d.collections) > len(objp): + ax_3d.collections[-1].remove() + fig_3d.canvas.draw() + + # If last event was 'h' key + elif events[-1].key == 'h': if len(objp) != 0: if count >= 1: count -= 1 - # Remove last point from objp_confirmed - objp_confirmed = objp_confirmed[:-1] - # remove from plot + # Remove last point from objp_confirmed_notok + objp_confirmed_notok = objp_confirmed_notok[:-1] + # remove from plot if len(ax_3d.collections) > len(objp): ax_3d.collections[-1].remove() - fig_3d.canvas.draw() - - # If last event was 'h' key - elif events[-1].key == 'h': - if len(objp) != 0: - if count >= 1: count -= 1 - # Remove last point from objp_confirmed_notok - objp_confirmed_notok = objp_confirmed_notok[:-1] - # remove from plot - if len(ax_3d.collections) > len(objp): - ax_3d.collections[-1].remove() - fig_3d.canvas.draw() + fig_3d.canvas.draw() def set_axes_equal(ax): - """ + ''' Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc. From https://stackoverflow.com/questions/13685386/how-to-set-the-equal-aspect-ratio-for-all-axes-x-y-z Input ax: a matplotlib axis, e.g., as output from plt.gca(). - """ + ''' x_limits = ax.get_xlim3d() y_limits = ax.get_ylim3d() @@ -1033,6 +919,37 @@ def calib_points_fun(config): pass +def extract_frames(video_path, extract_every_N_sec=1, overwrite_extraction=False): + ''' + Extract frames from video + if has not been done yet or if overwrite==True + + INPUT: + - video_path: path to video whose frames need to be extracted + - extract_every_N_sec: extract one frame every N seconds (can be <1) + - overwrite_extraction: if True, overwrite even if frames have already been extracted + + OUTPUT: + - extracted frames in folder + ''' + + if not os.path.exists(os.path.splitext(video_path)[0] + '_00000.png') or overwrite_extraction: + cap = cv2.VideoCapture(str(video_path)) + if cap.isOpened(): + fps = round(cap.get(cv2.CAP_PROP_FPS)) + frame_nb = 0 + logging.info(f'Extracting frames...') + while cap.isOpened(): + ret, frame = cap.read() + if ret == True: + if frame_nb % (fps*extract_every_N_sec) == 0: + img_path = (os.path.splitext(video_path)[0] + '_' +str(frame_nb).zfill(5)+'.png') + cv2.imwrite(str(img_path), frame) + frame_nb+=1 + else: + break + + def toml_write(calib_path, C, S, D, K, R, T): ''' Writes calibration parameters to a .toml file @@ -1052,7 +969,7 @@ def toml_write(calib_path, C, S, D, K, R, T): with open(os.path.join(calib_path), 'w+') as cal_f: for c in range(len(C)): - cam=f'[cam_{c+1}]\n' + cam=f'[cam_{(c+1):02d}]\n' name = f'name = "{C[c]}"\n' size = f'size = [ {S[c][0]}, {S[c][1]}]\n' mat = f'matrix = [ [ {K[c][0,0]}, 0.0, {K[c][0,2]}], [ 0.0, {K[c][1,1]}, {K[c][1,2]}], [ 0.0, 0.0, 1.0]]\n' @@ -1080,14 +997,15 @@ def recap_calibrate(ret, calib_path, calib_full_type): if cam != 'metadata': f_px = calib[cam]['matrix'][0][0] Dm = euclidean_distance(calib[cam]['translation'], [0,0,0]) + print(ret[c]) if calib_full_type=='convert_qualisys' or calib_full_type=='convert_vicon': - ret_m.append( np.around(ret[c], decimals=3) ) - ret_px.append( np.around(ret[c] / (Dm*1000) * f_px, decimals=3) ) + ret_m.append( np.around(ret[c]*1000, decimals=3) ) + ret_px.append( np.around(ret[c] / Dm * f_px, decimals=3) ) elif calib_full_type=='calculate_board': ret_px.append( np.around(ret[c], decimals=3) ) - ret_m.append( np.around(ret[c]*(Dm*1000) / f_px, decimals=3) ) + ret_m.append( np.around(ret[c]*Dm / f_px, decimals=3) ) - logging.info(f'\n--> Residual (RMS) calibration errors for each camera are respectively {ret_px} px, \nwhich corresponds to {ret_m} mm.\n') + logging.info(f'\n--> Residual (RMS) calibration errors for each camera are respectively {ret_px} px, \nwhich corresponds to {ret_m} m.\n') logging.info(f'Calibration file is stored at {calib_path}.') @@ -1118,11 +1036,10 @@ def calibrate_cams_all(config): convert_ext = '.qca.txt' binning_factor = config.get('calibration').get('convert').get('qualisys').get('binning_factor') elif convert_filetype=='optitrack': - print('See Readme.md to retrieve calibration values.') + logging.info('See Readme.md to retrieve Optitrack calibration values.') elif convert_filetype=='vicon': convert_ext = '.xcp' binning_factor = 1 - print('Not supported yet') file_to_convert_path = glob.glob(os.path.join(calib_dir, f'*{convert_ext}*'))[0] calib_output_path = os.path.join(calib_dir, f'Calib_{convert_filetype}.toml') @@ -1143,13 +1060,13 @@ def calibrate_cams_all(config): args_calib_fun = [calib_dir, intrinsics_config_dict, extrinsics_config_dict] elif calculate_method=='points': - print('Not supported yet') + logging.info('Calibration from wand or from keypoint detection not supported yet. See Readme.md if you want to contribute.') else: - print('Wrong calculate_method in Config.toml') + logging.info('Wrong calculate_method in Config.toml') else: - print('Wrong calibration_type in Config.toml') + logging.info('Wrong calibration_type in Config.toml') # Map calib function calib_mapping = { @@ -1166,6 +1083,6 @@ def calibrate_cams_all(config): # Write calibration file toml_write(calib_output_path, C, S, D, K, R, T) - + # Recap message - recap_calibrate(ret, calib_output_path, calib_full_type) + recap_calibrate(ret, calib_output_path, calib_full_type) \ No newline at end of file diff --git a/Pose2Sim/common.py b/Pose2Sim/common.py index fabbef1..219820e 100644 --- a/Pose2Sim/common.py +++ b/Pose2Sim/common.py @@ -36,6 +36,7 @@ __maintainer__ = "David Pagnon" __email__ = "contact@david-pagnon.com" __status__ = "Development" + ## FUNCTIONS def computeP(calib_file): ''' @@ -252,6 +253,7 @@ def natural_sort(list): return sorted(list, key=alphanum_key) + ## CLASSES class plotWindow(): ''' diff --git a/Pose2Sim/filtering.py b/Pose2Sim/filtering.py index 502e422..7d89fe5 100644 --- a/Pose2Sim/filtering.py +++ b/Pose2Sim/filtering.py @@ -33,6 +33,8 @@ import logging from scipy import signal from scipy.ndimage import gaussian_filter1d from statsmodels.nonparametric.smoothers_lowess import lowess +from filterpy.kalman import KalmanFilter, rts_smoother +from filterpy.common import Q_discrete_white_noise from Pose2Sim.common import plotWindow @@ -48,6 +50,128 @@ __status__ = "Development" ## FUNCTIONS +def kalman_filter(coords, frame_rate, measurement_noise, process_noise, nb_dimensions=3, nb_derivatives=3, smooth=True): + ''' + Filters coordinates with a Kalman filter or a Kalman smoother + + INPUTS: + - coords: array of shape (nframes, ndims) + - frame_rate: integer + - measurement_noise: integer + - process_noise: integer + - nb_dimensions: integer, number of dimensions (3 if 3D coordinates) + - nb_derivatives: integer, number of derivatives (3 if constant acceleration model) + - smooth: boolean. True if souble pass (recommended), False if single pass (if real-time) + + OUTPUTS: + - kpt_coords_filt: filtered coords + ''' + + # Variables + dim_x = nb_dimensions * nb_derivatives # 9 state variables + dt = 1/frame_rate + + # Filter definition + f = KalmanFilter(dim_x=dim_x, dim_z=nb_dimensions) + + # States: initial position, velocity, accel, in 3D + def derivate_array(arr, dt=1): + return np.diff(arr, axis=0)/dt + def repeat(func, arg_func, nb_reps): + for i in range(nb_reps): + arg_func = func(arg_func) + return arg_func + x_init = [] + for n_der in range(nb_derivatives): + x_init += [repeat(derivate_array, coords, n_der)[0]] # pose*3D, vel*3D, accel*3D + f.x = np.array(x_init).reshape(nb_dimensions,nb_derivatives).T.flatten() # pose, vel, accel *3D + + # State transition matrix + F_per_coord = np.zeros((int(dim_x/nb_dimensions), int(dim_x/nb_dimensions))) + for i in range(nb_derivatives): + for j in range(min(i+1, nb_derivatives)): + F_per_coord[j,i] = dt**(i-j) / np.math.factorial(i - j) + f.F = np.kron(np.eye(nb_dimensions),F_per_coord) + # F_per_coord= [[1, dt, dt**2/2], + # [ 0, 1, dt ], + # [ 0, 0, 1 ]]) + + # No control input + f.B = None + + # Measurement matrix (only positions) + H = np.zeros((nb_dimensions, dim_x)) + for i in range(min(nb_dimensions,dim_x)): + H[i, int(i*(dim_x/nb_dimensions))] = 1 + f.H = H + # H = [[1., 0., 0., 0., 0., 0., 0., 0., 0.], + # [0., 0., 0., 1., 0., 0., 0., 0., 0.], + # [0., 0., 0., 0., 0., 0., 1., 0., 0.]] + + # Covariance matrix + f.P *= measurement_noise + + # Measurement noise + f.R = np.diag([measurement_noise**2]*nb_dimensions) + + # Process noise + f.Q = Q_discrete_white_noise(nb_derivatives, dt=dt, var=process_noise**2, block_size=nb_dimensions) + + # Run filter: predict and update for each frame + mu, cov, _, _ = f.batch_filter(coords) # equivalent to below + # mu = [] + # for kpt_coord_frame in coords: + # f.predict() + # f.update(kpt_coord_frame) + # mu.append(f.x.copy()) + ind_of_position = [int(d*(dim_x/nb_dimensions)) for d in range(nb_dimensions)] + coords_filt = np.array(mu)[:,ind_of_position] + + # RTS smoother + if smooth == True: + mu2, P, C, _ = f.rts_smoother(mu, cov) + coords_filt = np.array(mu2)[:,ind_of_position] + + return coords_filt + + +def kalman_filter_1d(config, col): + ''' + 1D Kalman filter + Deals with nans + + INPUT: + - col: Pandas dataframe column + - trustratio: int, ratio process_noise/measurement_noise + - framerate: int + - smooth: boolean, True if double pass (recommended), False if single pass (if real-time) + + OUTPUT: + - col_filtered: Filtered pandas dataframe column + ''' + + trustratio = int(config.get('3d-filtering').get('kalman').get('trust_ratio')) + smooth = int(config.get('3d-filtering').get('kalman').get('smooth')) + framerate = config.get('project').get('frame_rate') + measurement_noise = 20 + process_noise = measurement_noise * trustratio + + # split into sequences of not nans + col_filtered = col.copy() + mask = np.isnan(col_filtered) | col_filtered.eq(0) + falsemask_indices = np.where(~mask)[0] + gaps = np.where(np.diff(falsemask_indices) > 1)[0] + 1 + idx_sequences = np.split(falsemask_indices, gaps) + if idx_sequences[0].size > 0: + idx_sequences_to_filter = [seq for seq in idx_sequences] + + # Filter each of the selected sequences + for seq_f in idx_sequences_to_filter: + col_filtered[seq_f] = kalman_filter(col_filtered[seq_f], framerate, measurement_noise, process_noise, nb_dimensions=1, nb_derivatives=3, smooth=smooth).flatten() + + return col_filtered + + def butterworth_filter_1d(config, col): ''' 1D Zero-phase Butterworth filter (dual pass) @@ -59,16 +183,16 @@ def butterworth_filter_1d(config, col): - cutoff: int - framerate: int - OUTPUT + OUTPUT: - col_filtered: Filtered pandas dataframe column ''' - type = config.get('3d-filtering').get('butterworth').get('type') + type = 'low' #config.get('3d-filtering').get('butterworth').get('type') order = int(config.get('3d-filtering').get('butterworth').get('order')) cutoff = int(config.get('3d-filtering').get('butterworth').get('cut_off_frequency')) framerate = config.get('project').get('frame_rate') - b, a = signal.butter(order/2, cutoff/(framerate/2), 'low', analog = False) + b, a = signal.butter(order/2, cutoff/(framerate/2), type, analog = False) padlen = 3 * max(len(a), len(b)) # split into sequences of not nans @@ -95,11 +219,11 @@ def butterworth_on_speed_filter_1d(config, col): - col: Pandas dataframe column - frame rate, order, cut-off frequency, type (from Config.toml) - OUTPUT + OUTPUT: - col_filtered: Filtered pandas dataframe column ''' - type = config.get('3d-filtering').get('butterworth_on_speed').get('type') + type = 'low' # config.get('3d-filtering').get('butterworth_on_speed').get('type') order = int(config.get('3d-filtering').get('butterworth_on_speed').get('order')) cutoff = int(config.get('3d-filtering').get('butterworth_on_speed').get('cut_off_frequency')) framerate = config.get('project').get('frame_rate') @@ -136,7 +260,7 @@ def gaussian_filter_1d(config, col): - col: Pandas dataframe column - gaussian_filter_sigma_kernel: kernel size from Config.toml - OUTPUT + OUTPUT: - col_filtered: Filtered pandas dataframe column ''' @@ -156,7 +280,7 @@ def loess_filter_1d(config, col): - loess_filter_nb_values: window used for smoothing from Config.toml frac = loess_filter_nb_values * frames_number - OUTPUT + OUTPUT: - col_filtered: Filtered pandas dataframe column ''' @@ -185,7 +309,7 @@ def median_filter_1d(config, col): - col: Pandas dataframe column - median_filter_kernel_size: kernel size from Config.toml - OUTPUT + OUTPUT: - col_filtered: Filtered pandas dataframe column ''' @@ -248,12 +372,13 @@ def filter1d(col, config, filter_type): - col: Pandas dataframe column - filter_type: filter type from Config.toml - OUTPUT + OUTPUT: - col_filtered: Filtered pandas dataframe column ''' # Choose filter filter_mapping = { + 'kalman': kalman_filter_1d, 'butterworth': butterworth_filter_1d, 'butterworth_on_speed': butterworth_on_speed_filter_1d, 'gaussian': gaussian_filter_1d, @@ -278,10 +403,13 @@ def recap_filter3d(config, trc_path): # Read Config filter_type = config.get('3d-filtering').get('type') - butterworth_filter_type = config.get('3d-filtering').get('butterworth').get('type') + kalman_filter_trustratio = int(config.get('3d-filtering').get('kalman').get('trust_ratio')) + kalman_filter_smooth = int(config.get('3d-filtering').get('kalman').get('smooth')) + kalman_filter_smooth_str = 'smoother' if kalman_filter_smooth else 'filter' + butterworth_filter_type = 'low' # config.get('3d-filtering').get('butterworth').get('type') butterworth_filter_order = int(config.get('3d-filtering').get('butterworth').get('order')) butterworth_filter_cutoff = int(config.get('3d-filtering').get('butterworth').get('cut_off_frequency')) - butter_speed_filter_type = config.get('3d-filtering').get('butterworth_on_speed').get('type') + butter_speed_filter_type = 'low' # config.get('3d-filtering').get('butterworth_on_speed').get('type') butter_speed_filter_order = int(config.get('3d-filtering').get('butterworth_on_speed').get('order')) butter_speed_filter_cutoff = int(config.get('3d-filtering').get('butterworth_on_speed').get('cut_off_frequency')) gaussian_filter_sigma_kernel = int(config.get('3d-filtering').get('gaussian').get('sigma_kernel')) @@ -290,6 +418,7 @@ def recap_filter3d(config, trc_path): # Recap filter_mapping_recap = { + 'kalman': f'--> Filter type: Kalman {kalman_filter_smooth_str}. Measurements trusted {kalman_filter_trustratio} times as much as previous data, assuming a constant acceleration process.', 'butterworth': f'--> Filter type: Butterworth {butterworth_filter_type}-pass. Order {butterworth_filter_order}, Cut-off frequency {butterworth_filter_cutoff} Hz.', 'butterworth_on_speed': f'--> Filter type: Butterworth on speed {butter_speed_filter_type}-pass. Order {butter_speed_filter_order}, Cut-off frequency {butter_speed_filter_cutoff} Hz.', 'gaussian': f'--> Filter type: Gaussian. Standard deviation kernel: {gaussian_filter_sigma_kernel}', @@ -316,10 +445,10 @@ def filter_all(config): # Read config project_dir = config.get('project').get('project_dir') if project_dir == '': project_dir = os.getcwd() - pose_tracked_folder_name = config.get('project').get('poseTracked_folder_name') + pose_tracked_folder_name = config.get('project').get('poseAssociated_folder_name') pose_folder_name = config.get('project').get('pose_folder_name') try: - pose_tracked_dir = os.path.join(project_dir, pose_folder_name) + pose_tracked_dir = os.path.join(project_dir, pose_tracked_folder_name) os.path.isdir(pose_tracked_dir) pose_dir = pose_tracked_dir except: @@ -329,8 +458,8 @@ def filter_all(config): seq_name = os.path.basename(project_dir) pose3d_folder_name = config.get('project').get('pose3d_folder_name') pose3d_dir = os.path.join(project_dir, pose3d_folder_name) - display_figures = config.get('3d-filtering').get('display_figures') - filter_type = config.get('3d-filtering').get('type') + display_figures = config.get('filtering').get('display_figures') + filter_type = config.get('filtering').get('type') # Frames range pose_listdirs_names = next(os.walk(pose_dir))[1] diff --git a/Pose2Sim/skeletons.py b/Pose2Sim/skeletons.py index 75668bd..dae0bcb 100644 --- a/Pose2Sim/skeletons.py +++ b/Pose2Sim/skeletons.py @@ -44,10 +44,6 @@ __email__ = "contact@david-pagnon.com" __status__ = "Development" -''' -SKELETONS -''' - '''CUSTOM SKELETON (e.g., from DeepLabCut detection)''' CUSTOM = Node("Root", id=0, children=[ Node("Child1", id=1), diff --git a/Pose2Sim/triangulation.py b/Pose2Sim/triangulation.py index fd1c4f1..0f62ccc 100644 --- a/Pose2Sim/triangulation.py +++ b/Pose2Sim/triangulation.py @@ -218,11 +218,14 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl logging.info(f'Mean reprojection error for {name} is {mean_error_keypoint_px} px (~ {mean_error_keypoint_m} m), reached with {mean_cam_excluded_keypoint} excluded cameras. ') if show_interp_indices: if interpolation_kind != 'none': - logging.info(f'Frames {list(interp_frames[idx])} were interpolated.') + if len(list(interp_frames[idx])) ==0: + logging.info(f' No frames needed to be interpolated.') + else: + logging.info(f' Frames {list(interp_frames[idx])} were interpolated.') if len(list(non_interp_frames[idx]))>0: - logging.info(f'Frames {list(non_interp_frames[idx])} could not be interpolated: consider adjusting thresholds.') + logging.info(f' Frames {list(non_interp_frames[idx])} could not be interpolated: consider adjusting thresholds.') else: - logging.info(f'No frames were interpolated because \'interpolation_kind\' was set to none. ') + logging.info(f' No frames were interpolated because \'interpolation_kind\' was set to none. ') mean_error_px = np.around(error['mean'].mean(), decimals=1) mean_error_mm = np.around(mean_error_px * Dm / fm *1000, decimals=1) @@ -488,8 +491,8 @@ def triangulate_all(config): zero_nan_frames_per_kpt = [zero_nan_frames[1][np.where(zero_nan_frames[0]==k)[0]] for k in range(keypoints_nb)] gaps = [np.where(np.diff(zero_nan_frames_per_kpt[k]) > 1)[0] + 1 for k in range(keypoints_nb)] sequences = [np.split(zero_nan_frames_per_kpt[k], gaps[k]) for k in range(keypoints_nb)] - interp_frames = [[f'{seq[0]}:{seq[-1]}' for seq in seq_kpt if len(seq)<=interp_gap_smaller_than and len(seq)>0] for seq_kpt in sequences] - non_interp_frames = [[f'{seq[0]}:{seq[-1]}' for seq in seq_kpt if len(seq)>interp_gap_smaller_than] for seq_kpt in sequences] + interp_frames = [[f'{seq[0]}:{seq[-1]+1}' for seq in seq_kpt if len(seq)<=interp_gap_smaller_than and len(seq)>0] for seq_kpt in sequences] + non_interp_frames = [[f'{seq[0]}:{seq[-1]+1}' for seq in seq_kpt if len(seq)>interp_gap_smaller_than] for seq_kpt in sequences] else: interp_frames = None diff --git a/README.md b/README.md index a7be8a8..649bc08 100644 --- a/README.md +++ b/README.md @@ -45,9 +45,9 @@ If you can only use a single camera and don't mind losing some accuracy, please 2. [Calculate from scratch](#calculate-from-scratch) 4. [Camera synchronization](#camera-synchronization) 5. [Tracking, Triangulating, Filtering](#tracking-triangulating-filtering) - 1. [Tracking the person of interest](#tracking-the-person-of-interest) + 1. [Associate persons across cameras](#associate-persons-across-cameras) 2. [Triangulating keypoints](#triangulating-keypoints) - 3. [Filtering 3D coordinates](#filtering-3D-coordinates) + 3. [Filtering 3D coordinates](#filtering-3d-coordinates) 6. [OpenSim kinematics](#opensim-kinematics) 3. [Utilities](#utilities) 4. [How to cite and how to contribute](#how-to-cite-and-how-to-contribute) @@ -234,7 +234,8 @@ N.B.: Markers are not needed in Pose2Sim and were used here for validation ## Camera calibration -> _**Convert a preexisting calibration file, or calculate calibration parameters from scratch.**_ +> _**Convert a preexisting calibration file, or calculate intrinsic and extrinsic parameters from scratch.**_ \ +> _**N.B.:**_ You can visualize your resulting camera calibration with my (experimental) [Maya-Mocap tool](https://github.com/davidpagnon/Maya-Mocap). ### Convert from Qualisys, Optitrack, or Vicon @@ -416,7 +417,8 @@ Output:\ ### Triangulating keypoints -> _**Triangulate your 2D coordinates in a robust way.**_ +> _**Triangulate your 2D coordinates in a robust way.**_ \ +> _**N.B.:**_ You can visualize your resulting 3D coordinates with my (experimental) [Maya-Mocap tool](https://github.com/davidpagnon/Maya-Mocap). Open an Anaconda prompt or a terminal, type `ipython`.\ By default, `triangulation()` will look for `Config.toml` in the `User` folder of your current directory. If you want to store it somewhere else (e.g. in your data directory), specify this path as an argument: `Pose2Sim.triangulation(r'path_to_config.toml')`. @@ -480,7 +482,8 @@ Output:\ ### Filtering 3D coordinates -> _**Filter your 3D coordinates.**_ +> _**Filter your 3D coordinates.**_\ +> _**N.B.:**_ You can visualize your resulting filtered 3D coordinates with my (experimental) [Maya-Mocap tool](https://github.com/davidpagnon/Maya-Mocap). Open an Anaconda prompt or a terminal, type `ipython`.\ By default, `filtering()` will look for `Config.toml` in the `User` folder of your current directory. If you want to store it somewhere else (e.g. in your data directory), specify this path as an argument: `Pose2Sim.filtering(r'path_to_config.toml')`. @@ -781,10 +784,11 @@ If you want to contribute to Pose2Sim, please follow [this guide](https://docs.g
> - [x] **Triangulation:** Triangulation weighted with confidence. -> - [x] **Triangulation:** Set likelihood threshold below which a camera should not be used, reprojection error threshold, and set minimum number of cameras allowed for triangulating a point. -> - [x] **Triangulation:** Show mean reprojection error in px and in mm for each point. +> - [x] **Triangulation:** Set a likelihood threshold below which a camera should not be used, a reprojection error threshold, and a minimum number of remaining cameras below which triangulation is skipped for this frame. +> - [x] **Triangulation:** Show mean reprojection error in px and in mm for each keypoint. +> - [x] **Triangulation:** Show how many cameras on average had to be excluded for each keypoint. > - [x] **Triangulation:** Evaluate which cameras were the least reliable. -> - [x] **Triangulation:** Show which frames had to be interpolated for each point. +> - [x] **Triangulation:** Show which frames had to be interpolated for each keypoint. > - [ ] **Triangulation:** [Undistort](https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga887960ea1bde84784e7f1710a922b93c) 2D points before triangulating (and [distort](https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301) them before computing reprojection error). > - [ ] **Triangulation:** Multiple person kinematics (output multiple .trc coordinates files). Triangulate all persons with reprojection error above threshold, and identify them by minimizing their displacement across frames. > - [ ] **Triangulation:** Offer the possibility of triangulating with Sparse Bundle Adjustment (SBA), Extended Kalman Filter (EKF), Full Trajectory Estimation (FTE) (see [AcinoSet](https://github.com/African-Robotics-Unit/AcinoSet)). @@ -820,4 +824,4 @@ If you want to contribute to Pose2Sim, please follow [this guide](https://docs.g > - [ ] **Catch errors** > - [ ] **Conda package and Docker image** -> - [ ] **Run from command line via click or typer** \ No newline at end of file +> - [ ] **Run from command line via click or typer** diff --git a/setup.cfg b/setup.cfg index a6f92d3..1c8ccd7 100644 --- a/setup.cfg +++ b/setup.cfg @@ -37,13 +37,14 @@ install_requires = lxml matplotlib mpl_interactions + Pillow PyQt5 tqdm anytree pandas<1.5 scipy statsmodels - pykalman + filterpy c3d ipython packages = find: