diff --git a/media/RESULTS.md b/media/RESULTS.md index b2062a5..a2e5331 100644 --- a/media/RESULTS.md +++ b/media/RESULTS.md @@ -294,72 +294,72 @@ Results of the model in various experiments on different datasets. \ ```json { - "img_loading": 0.180589, - "demosaicing": 0.000695076, - "avg_time_2d": 0.0152607, - "avg_time_3d": 0.000150192, - "fps": 62.0888 + "img_loading": 0.0424103, + "demosaicing": 0.000724716, + "avg_time_2d": 0.01494, + "avg_time_3d": 0.000128772, + "fps": 63.3173 } { "triangulator_calls": 301, - "init_time": 3.53967e-06, - "undistort_time": 3.48582e-05, - "project_time": 2.18348e-06, - "match_time": 8.45481e-06, - "pairs_time": 4.53164e-06, - "pair_scoring_time": 3.10183e-05, - "grouping_time": 4.6499e-06, - "full_time": 3.33672e-05, - "merge_time": 1.02807e-05, - "post_time": 7.00402e-06, - "convert_time": 1.11306e-07, - "total_time": 0.000140236 + "init_time": 1.60891e-06, + "undistort_time": 2.57178e-05, + "project_time": 2.22848e-06, + "match_time": 8.41567e-06, + "pairs_time": 4.53139e-06, + "pair_scoring_time": 2.67118e-05, + "grouping_time": 4.63213e-06, + "full_time": 2.72313e-05, + "merge_time": 1.03292e-05, + "post_time": 7.36791e-06, + "convert_time": 1.27439e-07, + "total_time": 0.00011914 } { "person_nums": { "total_frames": 301, "total_labels": 477, - "total_preds": 829, + "total_preds": 827, "considered_empty": 0, "valid_preds": 477, - "invalid_preds": 352, + "invalid_preds": 350, "missing": 0, - "invalid_fraction": 0.42461, - "precision": 0.57539, + "invalid_fraction": 0.42322, + "precision": 0.57678, "recall": 1.0, - "f1": 0.73047, - "non_empty": 829 + "f1": 0.7316, + "non_empty": 827 }, "mpjpe": { "count": 477, - "mean": 0.047984, - "median": 0.042648, - "std": 0.014812, - "sem": 0.000679, + "mean": 0.047983, + "median": 0.042569, + "std": 0.01486, + "sem": 0.000681, "min": 0.03012, - "max": 0.116312, + "max": 0.116311, "recall-0.025": 0.0, "recall-0.05": 0.70021, - "recall-0.1": 0.985325, + "recall-0.1": 0.983229, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, "num_labels": 477, "ap-0.025": 0.0, - "ap-0.05": 0.39114, - "ap-0.1": 0.735434, - "ap-0.15": 0.751482, - "ap-0.25": 0.751482, - "ap-0.5": 0.751482 + "ap-0.05": 0.389102, + "ap-0.1": 0.729848, + "ap-0.15": 0.747198, + "ap-0.25": 0.747198, + "ap-0.5": 0.747198 }, "head": { "count": 477, - "mean": 0.054212, - "median": 0.050157, - "std": 0.024854, + "mean": 0.054217, + "median": 0.050158, + "std": 0.024847, "sem": 0.001139, - "min": 0.005599, - "max": 0.180565, + "min": 0.005598, + "max": 0.180564, "recall-0.025": 0.081761, "recall-0.05": 0.496855, "recall-0.1": 0.937107, @@ -370,11 +370,11 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_left": { "count": 477, - "mean": 0.042435, - "median": 0.03702, - "std": 0.02058, + "mean": 0.042429, + "median": 0.037021, + "std": 0.020584, "sem": 0.000943, - "min": 0.00431, + "min": 0.004311, "max": 0.136587, "recall-0.025": 0.161426, "recall-0.05": 0.727463, @@ -386,14 +386,14 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_right": { "count": 477, - "mean": 0.049634, - "median": 0.045795, - "std": 0.023121, + "mean": 0.049623, + "median": 0.045796, + "std": 0.02312, "sem": 0.00106, - "min": 0.00535, - "max": 0.14745, + "min": 0.005349, + "max": 0.147448, "recall-0.025": 0.100629, - "recall-0.05": 0.559748, + "recall-0.05": 0.561845, "recall-0.1": 0.955975, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -402,12 +402,12 @@ Results of the model in various experiments on different datasets. \ }, "elbow_left": { "count": 477, - "mean": 0.040763, + "mean": 0.04078, "median": 0.032063, - "std": 0.029259, - "sem": 0.001341, + "std": 0.029273, + "sem": 0.001342, "min": 0.003449, - "max": 0.326227, + "max": 0.326226, "recall-0.025": 0.316562, "recall-0.05": 0.756813, "recall-0.1": 0.953878, @@ -418,13 +418,13 @@ Results of the model in various experiments on different datasets. \ }, "elbow_right": { "count": 477, - "mean": 0.053368, - "median": 0.045043, - "std": 0.040851, - "sem": 0.001872, - "min": 0.003529, - "max": 0.244051, - "recall-0.025": 0.255765, + "mean": 0.053274, + "median": 0.044357, + "std": 0.040895, + "sem": 0.001874, + "min": 0.003528, + "max": 0.244052, + "recall-0.025": 0.257862, "recall-0.05": 0.561845, "recall-0.1": 0.901468, "recall-0.15": 0.958071, @@ -434,9 +434,9 @@ Results of the model in various experiments on different datasets. \ }, "wrist_left": { "count": 477, - "mean": 0.060002, + "mean": 0.059994, "median": 0.053953, - "std": 0.03861, + "std": 0.038609, "sem": 0.00177, "min": 0.002051, "max": 0.322481, @@ -450,12 +450,12 @@ Results of the model in various experiments on different datasets. \ }, "wrist_right": { "count": 477, - "mean": 0.059207, + "mean": 0.059177, "median": 0.054405, - "std": 0.033578, - "sem": 0.001539, + "std": 0.033566, + "sem": 0.001538, "min": 0.009618, - "max": 0.371667, + "max": 0.371666, "recall-0.025": 0.115304, "recall-0.05": 0.415094, "recall-0.1": 0.899371, @@ -466,15 +466,15 @@ Results of the model in various experiments on different datasets. \ }, "hip_left": { "count": 477, - "mean": 0.047948, - "median": 0.042251, - "std": 0.026295, - "sem": 0.001205, + "mean": 0.048042, + "median": 0.042252, + "std": 0.026486, + "sem": 0.001214, "min": 0.006475, - "max": 0.145903, - "recall-0.025": 0.188679, + "max": 0.145904, + "recall-0.025": 0.190776, "recall-0.05": 0.618449, - "recall-0.1": 0.953878, + "recall-0.1": 0.951782, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -482,15 +482,15 @@ Results of the model in various experiments on different datasets. \ }, "hip_right": { "count": 477, - "mean": 0.058483, + "mean": 0.058447, "median": 0.05753, - "std": 0.023762, - "sem": 0.001089, + "std": 0.0237, + "sem": 0.001086, "min": 0.005137, - "max": 0.132318, + "max": 0.132317, "recall-0.025": 0.098532, "recall-0.05": 0.39413, - "recall-0.1": 0.943396, + "recall-0.1": 0.945493, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -498,15 +498,15 @@ Results of the model in various experiments on different datasets. \ }, "knee_left": { "count": 477, - "mean": 0.040438, - "median": 0.03808, - "std": 0.024403, - "sem": 0.001118, - "min": 0.004928, - "max": 0.190069, + "mean": 0.040484, + "median": 0.038079, + "std": 0.024499, + "sem": 0.001123, + "min": 0.004927, + "max": 0.190068, "recall-0.025": 0.257862, "recall-0.05": 0.748428, - "recall-0.1": 0.974843, + "recall-0.1": 0.972746, "recall-0.15": 0.989518, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -514,12 +514,12 @@ Results of the model in various experiments on different datasets. \ }, "knee_right": { "count": 477, - "mean": 0.040168, - "median": 0.03623, - "std": 0.023114, + "mean": 0.040167, + "median": 0.036232, + "std": 0.023115, "sem": 0.001059, "min": 0.00733, - "max": 0.184933, + "max": 0.184932, "recall-0.025": 0.310273, "recall-0.05": 0.708595, "recall-0.1": 0.976939, @@ -530,15 +530,15 @@ Results of the model in various experiments on different datasets. \ }, "ankle_left": { "count": 477, - "mean": 0.036353, + "mean": 0.036403, "median": 0.028172, - "std": 0.030783, - "sem": 0.001411, - "min": 0.004787, - "max": 0.223747, + "std": 0.03066, + "sem": 0.001405, + "min": 0.004789, + "max": 0.223748, "recall-0.025": 0.433962, "recall-0.05": 0.81761, - "recall-0.1": 0.945493, + "recall-0.1": 0.947589, "recall-0.15": 0.983229, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -546,14 +546,14 @@ Results of the model in various experiments on different datasets. \ }, "ankle_right": { "count": 477, - "mean": 0.040777, - "median": 0.030897, - "std": 0.037254, + "mean": 0.040745, + "median": 0.030898, + "std": 0.03726, "sem": 0.001708, "min": 0.003323, - "max": 0.27012, - "recall-0.025": 0.303983, - "recall-0.05": 0.802935, + "max": 0.270118, + "recall-0.025": 0.301887, + "recall-0.05": 0.805031, "recall-0.1": 0.930818, "recall-0.15": 0.968553, "recall-0.25": 0.997904, @@ -562,8 +562,8 @@ Results of the model in various experiments on different datasets. \ }, "joint_recalls": { "num_labels": 6201, - "recall-0.025": 0.21093, - "recall-0.05": 0.6149, + "recall-0.025": 0.21158, + "recall-0.05": 0.61538, "recall-0.1": 0.94275, "recall-0.15": 0.98645, "recall-0.25": 0.99871, @@ -5967,26 +5967,26 @@ Results of the model in various experiments on different datasets. \ ```json { - "img_loading": 0.282821, - "demosaicing": 0.011311, - "avg_time_2d": 0.0240066, - "avg_time_3d": 0.00055045, - "fps": 27.8799 + "img_loading": 0.287326, + "demosaicing": 0.0112221, + "avg_time_2d": 0.0240407, + "avg_time_3d": 0.00023535, + "fps": 28.1705 } { "triangulator_calls": 121, - "init_time": 1.3244e-05, - "undistort_time": 3.09483e-05, - "project_time": 5.23027e-06, - "match_time": 1.68509e-05, - "pairs_time": 1.37968e-05, - "pair_scoring_time": 0.000160537, - "grouping_time": 1.73903e-05, - "full_time": 0.000199044, - "merge_time": 3.75236e-05, - "post_time": 1.03264e-05, - "convert_time": 1.91669e-07, - "total_time": 0.000505528 + "init_time": 2.2796e-06, + "undistort_time": 2.07935e-05, + "project_time": 3.8538e-06, + "match_time": 1.31121e-05, + "pairs_time": 9.31819e-06, + "pair_scoring_time": 6.0523e-05, + "grouping_time": 1.10819e-05, + "full_time": 7.05671e-05, + "merge_time": 2.99701e-05, + "post_time": 7.38921e-06, + "convert_time": 1.13331e-07, + "total_time": 0.000229241 } { "person_nums": { @@ -6005,21 +6005,21 @@ Results of the model in various experiments on different datasets. \ }, "mpjpe": { "count": 363, - "mean": 0.0257, - "median": 0.024739, - "std": 0.007243, + "mean": 0.025723, + "median": 0.024899, + "std": 0.00724, "sem": 0.000381, - "min": 0.011333, - "max": 0.051735, - "recall-0.025": 0.515152, + "min": 0.011774, + "max": 0.052562, + "recall-0.025": 0.504132, "recall-0.05": 0.997245, "recall-0.1": 1.0, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, "num_labels": 363, - "ap-0.025": 0.277608, - "ap-0.05": 0.99638, + "ap-0.025": 0.274693, + "ap-0.05": 0.996479, "ap-0.1": 1.0, "ap-0.15": 1.0, "ap-0.25": 1.0, @@ -6027,14 +6027,14 @@ Results of the model in various experiments on different datasets. \ }, "head": { "count": 363, - "mean": 0.027713, - "median": 0.022633, - "std": 0.017317, - "sem": 0.00091, - "min": 0.00109, - "max": 0.087763, - "recall-0.025": 0.553719, - "recall-0.05": 0.887052, + "mean": 0.02732, + "median": 0.022736, + "std": 0.016828, + "sem": 0.000884, + "min": 0.00115, + "max": 0.085467, + "recall-0.025": 0.573003, + "recall-0.05": 0.895317, "recall-0.1": 1.0, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6043,30 +6043,30 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_left": { "count": 363, - "mean": 0.027215, - "median": 0.021616, - "std": 0.021167, - "sem": 0.001113, - "min": 0.002899, - "max": 0.151257, - "recall-0.025": 0.584022, - "recall-0.05": 0.895317, - "recall-0.1": 0.975207, - "recall-0.15": 0.997245, + "mean": 0.027043, + "median": 0.021957, + "std": 0.020841, + "sem": 0.001095, + "min": 0.002793, + "max": 0.149897, + "recall-0.025": 0.589532, + "recall-0.05": 0.887052, + "recall-0.1": 0.986226, + "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, "num_labels": 363 }, "shoulder_right": { "count": 363, - "mean": 0.023389, - "median": 0.021151, - "std": 0.012799, + "mean": 0.022955, + "median": 0.020511, + "std": 0.012805, "sem": 0.000673, - "min": 0.003682, - "max": 0.101851, - "recall-0.025": 0.61157, - "recall-0.05": 0.955923, + "min": 0.003064, + "max": 0.101875, + "recall-0.025": 0.628099, + "recall-0.05": 0.961433, "recall-0.1": 0.997245, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6075,13 +6075,13 @@ Results of the model in various experiments on different datasets. \ }, "elbow_left": { "count": 363, - "mean": 0.022276, - "median": 0.019385, - "std": 0.014902, - "sem": 0.000783, - "min": 0.001441, - "max": 0.194618, - "recall-0.025": 0.694215, + "mean": 0.02252, + "median": 0.019538, + "std": 0.015672, + "sem": 0.000824, + "min": 0.001254, + "max": 0.207405, + "recall-0.025": 0.680441, "recall-0.05": 0.961433, "recall-0.1": 0.997245, "recall-0.15": 0.997245, @@ -6091,14 +6091,14 @@ Results of the model in various experiments on different datasets. \ }, "elbow_right": { "count": 363, - "mean": 0.018528, - "median": 0.016603, - "std": 0.010173, - "sem": 0.000535, - "min": 0.001046, - "max": 0.083441, - "recall-0.025": 0.801653, - "recall-0.05": 0.988981, + "mean": 0.018549, + "median": 0.016702, + "std": 0.010286, + "sem": 0.000541, + "min": 0.002522, + "max": 0.082821, + "recall-0.025": 0.807163, + "recall-0.05": 0.991736, "recall-0.1": 1.0, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6107,14 +6107,14 @@ Results of the model in various experiments on different datasets. \ }, "wrist_left": { "count": 363, - "mean": 0.023532, - "median": 0.018873, - "std": 0.018388, - "sem": 0.000966, - "min": 0.00279, - "max": 0.199397, - "recall-0.025": 0.683196, - "recall-0.05": 0.931129, + "mean": 0.023641, + "median": 0.018944, + "std": 0.018362, + "sem": 0.000965, + "min": 0.002721, + "max": 0.199952, + "recall-0.025": 0.688705, + "recall-0.05": 0.928375, "recall-0.1": 0.991736, "recall-0.15": 0.997245, "recall-0.25": 1.0, @@ -6123,14 +6123,14 @@ Results of the model in various experiments on different datasets. \ }, "wrist_right": { "count": 363, - "mean": 0.019579, - "median": 0.017651, - "std": 0.011201, - "sem": 0.000589, - "min": 0.002333, - "max": 0.076342, + "mean": 0.019745, + "median": 0.017758, + "std": 0.011172, + "sem": 0.000587, + "min": 0.001634, + "max": 0.076393, "recall-0.025": 0.760331, - "recall-0.05": 0.977961, + "recall-0.05": 0.975207, "recall-0.1": 1.0, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6139,14 +6139,14 @@ Results of the model in various experiments on different datasets. \ }, "hip_left": { "count": 363, - "mean": 0.031156, - "median": 0.026379, - "std": 0.016985, - "sem": 0.000893, - "min": 0.006013, - "max": 0.117111, - "recall-0.025": 0.443526, - "recall-0.05": 0.859504, + "mean": 0.03119, + "median": 0.026563, + "std": 0.017008, + "sem": 0.000894, + "min": 0.005051, + "max": 0.116967, + "recall-0.025": 0.438017, + "recall-0.05": 0.848485, "recall-0.1": 0.997245, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6155,15 +6155,15 @@ Results of the model in various experiments on different datasets. \ }, "hip_right": { "count": 363, - "mean": 0.03111, - "median": 0.028792, - "std": 0.01668, - "sem": 0.000877, - "min": 0.003451, - "max": 0.138183, - "recall-0.025": 0.38292, - "recall-0.05": 0.903581, - "recall-0.1": 0.99449, + "mean": 0.03118, + "median": 0.028448, + "std": 0.01705, + "sem": 0.000896, + "min": 0.003029, + "max": 0.138279, + "recall-0.025": 0.371901, + "recall-0.05": 0.900826, + "recall-0.1": 0.991736, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -6171,15 +6171,15 @@ Results of the model in various experiments on different datasets. \ }, "knee_left": { "count": 363, - "mean": 0.028282, - "median": 0.020833, - "std": 0.023126, - "sem": 0.001215, - "min": 0.001686, - "max": 0.127237, - "recall-0.025": 0.625344, - "recall-0.05": 0.859504, - "recall-0.1": 0.983471, + "mean": 0.028395, + "median": 0.020711, + "std": 0.02313, + "sem": 0.001216, + "min": 0.001921, + "max": 0.12193, + "recall-0.025": 0.633609, + "recall-0.05": 0.856749, + "recall-0.1": 0.977961, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -6187,14 +6187,14 @@ Results of the model in various experiments on different datasets. \ }, "knee_right": { "count": 363, - "mean": 0.023698, - "median": 0.020666, - "std": 0.013376, - "sem": 0.000703, - "min": 0.001969, - "max": 0.066531, - "recall-0.025": 0.597796, - "recall-0.05": 0.944904, + "mean": 0.023775, + "median": 0.020622, + "std": 0.013654, + "sem": 0.000718, + "min": 0.002869, + "max": 0.067651, + "recall-0.025": 0.592287, + "recall-0.05": 0.939394, "recall-0.1": 1.0, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6203,14 +6203,14 @@ Results of the model in various experiments on different datasets. \ }, "ankle_left": { "count": 363, - "mean": 0.028125, - "median": 0.021539, - "std": 0.02265, - "sem": 0.00119, - "min": 0.002656, - "max": 0.178927, - "recall-0.025": 0.578512, - "recall-0.05": 0.900826, + "mean": 0.028379, + "median": 0.022211, + "std": 0.022536, + "sem": 0.001184, + "min": 0.002738, + "max": 0.179868, + "recall-0.025": 0.570248, + "recall-0.05": 0.895317, "recall-0.1": 0.980716, "recall-0.15": 0.99449, "recall-0.25": 1.0, @@ -6219,13 +6219,13 @@ Results of the model in various experiments on different datasets. \ }, "ankle_right": { "count": 363, - "mean": 0.029496, - "median": 0.022264, - "std": 0.027073, - "sem": 0.001423, - "min": 0.002482, - "max": 0.263543, - "recall-0.025": 0.584022, + "mean": 0.029711, + "median": 0.022134, + "std": 0.026933, + "sem": 0.001416, + "min": 0.003345, + "max": 0.265506, + "recall-0.025": 0.573003, "recall-0.05": 0.865014, "recall-0.1": 0.969697, "recall-0.15": 0.99449, @@ -6235,10 +6235,10 @@ Results of the model in various experiments on different datasets. \ }, "joint_recalls": { "num_labels": 4719, - "recall-0.025": 0.60585, - "recall-0.05": 0.9163, - "recall-0.1": 0.99004, - "recall-0.15": 0.99746, + "recall-0.025": 0.60776, + "recall-0.05": 0.91524, + "recall-0.1": 0.99046, + "recall-0.15": 0.99788, "recall-0.25": 0.99958, "recall-0.5": 1.0 } diff --git a/rpt/camera.cpp b/rpt/camera.cpp index 55d7450..211f4a1 100644 --- a/rpt/camera.cpp +++ b/rpt/camera.cpp @@ -56,7 +56,6 @@ std::string Camera::to_string() const out << "'R': " << print_matrix(R) << ", "; out << "'T': " << print_matrix(T) << ", "; - out << "'P': " << print_matrix(P) << ", "; out << "'width': " << width << ", "; out << "'height': " << height << ", "; diff --git a/rpt/camera.hpp b/rpt/camera.hpp index 03412f0..cbe12b5 100644 --- a/rpt/camera.hpp +++ b/rpt/camera.hpp @@ -14,7 +14,6 @@ struct Camera std::vector DC; std::array, 3> R; std::array, 3> T; - std::array, 4> P; int width; int height; std::string type; diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index ea2f7cd..185698a 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -123,22 +123,372 @@ CameraInternal::CameraInternal(const Camera &cam) { this->cam = cam; - // Convert camera matrix to cv::Mat for OpenCV - K = cv::Mat(3, 3, CV_32FC1, const_cast(&cam.K[0][0])).clone(); - DC = cv::Mat(cam.DC.size(), 1, CV_32FC1, const_cast(cam.DC.data())).clone(); - R = cv::Mat(3, 3, CV_32FC1, const_cast(&cam.R[0][0])).clone(); - T = cv::Mat(3, 1, CV_32FC1, const_cast(&cam.T[0][0])).clone(); + this->invK = invert3x3(cam.K); + this->invR = transpose3x3(cam.R); + + // Camera center: + // C = -(Rᵀ * t) = -(Rᵀ * (R * (T * -1))) = -(Rᵀ * (R * -T)) = -(Rᵀ * -R * T) = -(-T) = T + this->center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]}; } // ================================================================================================= -void CameraInternal::update_projection_matrix() +std::array, 3> CameraInternal::transpose3x3( + const std::array, 3> &M) { - // Calculate opencv-style projection matrix - cv::Mat Tr, RT; - Tr = R * (T * -1); - cv::hconcat(R, Tr, RT); - P = K * RT; + return {{{M[0][0], M[1][0], M[2][0]}, + {M[0][1], M[1][1], M[2][1]}, + {M[0][2], M[1][2], M[2][2]}}}; +} + +// ================================================================================================= + +std::array, 3> CameraInternal::invert3x3( + const std::array, 3> &M) +{ + // Compute the inverse using the adjugate method + // See: https://scicomp.stackexchange.com/a/29206 + + std::array, 3> adj = { + {{ + M[1][1] * M[2][2] - M[1][2] * M[2][1], + M[0][2] * M[2][1] - M[0][1] * M[2][2], + M[0][1] * M[1][2] - M[0][2] * M[1][1], + }, + { + M[1][2] * M[2][0] - M[1][0] * M[2][2], + M[0][0] * M[2][2] - M[0][2] * M[2][0], + M[0][2] * M[1][0] - M[0][0] * M[1][2], + }, + { + M[1][0] * M[2][1] - M[1][1] * M[2][0], + M[0][1] * M[2][0] - M[0][0] * M[2][1], + M[0][0] * M[1][1] - M[0][1] * M[1][0], + }}}; + + float det = M[0][0] * adj[0][0] + M[0][1] * adj[1][0] + M[0][2] * adj[2][0]; + if (std::fabs(det) < 1e-6f) + { + return {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}}; + } + + float idet = 1.0f / det; + std::array, 3> inv = { + {{ + adj[0][0] * idet, + adj[0][1] * idet, + adj[0][2] * idet, + }, + { + adj[1][0] * idet, + adj[1][1] * idet, + adj[1][2] * idet, + }, + { + adj[2][0] * idet, + adj[2][1] * idet, + adj[2][2] * idet, + }}}; + + return inv; +} + +// ================================================================================================= + +void CameraInternal::undistort_point_pinhole(std::array &p, const std::vector &k) +{ + // Following: cv::cvUndistortPointsInternal + // Uses only the distortion coefficients: [k1, k2, p1, p2, k3] + // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/undistort.dispatch.cpp#L432 + + float x0 = p[0]; + float y0 = p[1]; + float x = x0; + float y = y0; + + // Iteratively refine the estimate for the undistorted point. + int max_iterations = 5; + for (int iter = 0; iter < max_iterations; ++iter) + { + float r2 = x * x + y * y; + double icdist = 1.0 / (1 + ((k[4] * r2 + k[1]) * r2 + k[0]) * r2); + if (icdist < 0) + { + x = x0; + y = y0; + break; + } + + float deltaX = 2 * k[2] * x * y + k[3] * (r2 + 2 * x * x); + float deltaY = k[2] * (r2 + 2 * y * y) + 2 * k[3] * x * y; + x = (x0 - deltaX) * icdist; + y = (y0 - deltaY) * icdist; + } + + p[0] = x; + p[1] = y; +} + +// ================================================================================================= + +void CameraInternal::undistort_point_fisheye(std::array &p, const std::vector &k) +{ + // Following: cv::fisheye::undistortPoints + // Uses only the distortion coefficients: [k1, k2, k3, k4] + // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/fisheye.cpp#L429 + + float theta_d = std::sqrt(p[0] * p[0] + p[1] * p[1]); + float pi_half = std::numbers::pi * 0.5; + theta_d = std::min(std::max(-pi_half, theta_d), pi_half); + + if (theta_d < 1e-6) + { + return; + } + + float scale = 0.0; + float theta = theta_d; + + int max_iterations = 5; + for (int iter = 0; iter < max_iterations; ++iter) + { + float theta2 = theta * theta; + float theta4 = theta2 * theta2; + float theta6 = theta4 * theta2; + float theta8 = theta4 * theta4; + + float k0_theta2 = k[0] * theta2; + float k1_theta4 = k[1] * theta4; + float k2_theta6 = k[2] * theta6; + float k3_theta8 = k[3] * theta8; + + float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / + (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8); + theta = theta - theta_fix; + + if (std::fabs(theta_fix) < 1e-6) + { + break; + } + } + + scale = std::tan(theta) / theta_d; + p[0] *= scale; + p[1] *= scale; +} + +// ================================================================================================= + +std::array, 3> CameraInternal::calc_optimal_camera_matrix_fisheye( + float balance, std::pair new_size) +{ + // Following: cv::fisheye::estimateNewCameraMatrixForUndistortRectify + // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/fisheye.cpp#L630 + + float fov_scale = 1.0; + float w = static_cast(cam.width); + float h = static_cast(cam.height); + balance = std::min(std::max(balance, 0.0f), 1.0f); + + // Define four key points at the middle of each edge + std::vector> pts = { + {w * 0.5f, 0.0}, + {w, h * 0.5f}, + {w * 0.5f, h}, + {0.0, h * 0.5f}}; + + // Extract intrinsic parameters + float fx = cam.K[0][0]; + float fy = cam.K[1][1]; + float cx = cam.K[0][2]; + float cy = cam.K[1][2]; + + // Undistort the edge points + for (auto &pt : pts) + { + std::array p_normed = {(pt[0] - cx) / fx, (pt[1] - cy) / fy, 1.0}; + undistort_point_fisheye(p_normed, cam.DC); + pt[0] = p_normed[0]; + pt[1] = p_normed[1]; + } + + // Compute center mass of the undistorted edge points + float sum_x = 0.0, sum_y = 0.0; + for (const auto &pt : pts) + { + sum_x += pt[0]; + sum_y += pt[1]; + } + float cn_x = sum_x / pts.size(); + float cn_y = sum_y / pts.size(); + + // Convert to identity ratio + float aspect_ratio = fx / fy; + cn_y *= aspect_ratio; + for (auto &pt : pts) + pt[1] *= aspect_ratio; + + // Find the bounding box of the undistorted points + float minx = std::numeric_limits::max(); + float miny = std::numeric_limits::max(); + float maxx = -std::numeric_limits::max(); + float maxy = -std::numeric_limits::max(); + for (const auto &pt : pts) + { + minx = std::min(minx, pt[0]); + maxx = std::max(maxx, pt[0]); + miny = std::min(miny, pt[1]); + maxy = std::max(maxy, pt[1]); + } + + // Calculate candidate focal lengths + float f1 = (w * 0.5) / (cn_x - minx); + float f2 = (w * 0.5) / (maxx - cn_x); + float f3 = (h * 0.5 * aspect_ratio) / (cn_y - miny); + float f4 = (h * 0.5 * aspect_ratio) / (maxy - cn_y); + float fmin = std::min({f1, f2, f3, f4}); + float fmax = std::max({f1, f2, f3, f4}); + + // Blend the candidate focal lengths + float f_val = balance * fmin + (1.0f - balance) * fmax; + if (fov_scale > 0.0f) + f_val /= fov_scale; + + // Compute new intrinsic parameters + float new_fx = f_val; + float new_fy = f_val; + float new_cx = -cn_x * f_val + (w * 0.5); + float new_cy = -cn_y * f_val + (h * aspect_ratio * 0.5); + + // Restore aspect ratio + new_fy /= aspect_ratio; + new_cy /= aspect_ratio; + + // Optionally scale parameters to new new image size + if (new_size.first > 0 && new_size.second > 0) + { + float rx = static_cast(new_size.first) / w; + float ry = static_cast(new_size.second) / h; + new_fx *= rx; + new_fy *= ry; + new_cx *= rx; + new_cy *= ry; + } + + std::array, 3> newK = { + {{new_fx, 0.0, new_cx}, + {0.0, new_fy, new_cy}, + {0.0, 0.0, 1.0}}}; + return newK; +} + +// ================================================================================================= + +std::array, 3> CameraInternal::calc_optimal_camera_matrix_pinhole( + float alpha, std::pair new_size) +{ + // Following: cv::getOptimalNewCameraMatrix + // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/calibration_base.cpp#L1565 + + bool center_principal_point = false; + bool use_pix_roi = false; + float w = static_cast(cam.width); + float h = static_cast(cam.height); + alpha = std::min(std::max(alpha, 0.0f), 1.0f); + + if (center_principal_point || use_pix_roi) + { + // Not implemented + exit(1); + } + + // Define key points + const size_t N = 9; + std::vector> pts; + pts.reserve(N * N); + for (size_t y = 0; y < N; ++y) + { + for (size_t x = 0; x < N; ++x) + { + pts.push_back({x * (w - 1) / (N - 1), y * (h - 1) / (N - 1)}); + } + } + + // Extract intrinsic parameters + float fx = cam.K[0][0]; + float fy = cam.K[1][1]; + float cx = cam.K[0][2]; + float cy = cam.K[1][2]; + + // Undistort the key points + for (auto &pt : pts) + { + std::array p_normed = {(pt[0] - cx) / fx, (pt[1] - cy) / fy, 1.0}; + undistort_point_pinhole(p_normed, cam.DC); + pt[0] = p_normed[0]; + pt[1] = p_normed[1]; + } + + // Get inscribed and circumscribed rectangles in normalized coordinates + float iX0 = -std::numeric_limits::max(); + float iX1 = std::numeric_limits::max(); + float iY0 = -std::numeric_limits::max(); + float iY1 = std::numeric_limits::max(); + float oX0 = std::numeric_limits::max(); + float oX1 = -std::numeric_limits::max(); + float oY0 = std::numeric_limits::max(); + float oY1 = -std::numeric_limits::max(); + size_t k = 0; + for (size_t y = 0; y < N; ++y) + { + for (size_t x = 0; x < N; ++x) + { + auto &pt = pts[k]; + k += 1; + + oX0 = std::min(oX0, pt[0]); + oX1 = std::max(oX1, pt[0]); + oY0 = std::min(oY0, pt[1]); + oY1 = std::max(oY1, pt[1]); + + if (x == 0) + iX0 = std::max(iX0, pt[0]); + if (x == N - 1) + iX1 = std::min(iX1, pt[0]); + if (y == 0) + iY0 = std::max(iY0, pt[1]); + if (y == N - 1) + iY1 = std::min(iY1, pt[1]); + } + } + float inner_width = iX1 - iX0; + float inner_height = iY1 - iY0; + float outer_width = oX1 - oX0; + float outer_height = oY1 - oY0; + + // Projection mapping inner rectangle to viewport + float fx0 = (new_size.first - 1) / inner_width; + float fy0 = (new_size.second - 1) / inner_height; + float cx0 = -fx0 * iX0; + float cy0 = -fy0 * iY0; + + // Projection mapping outer rectangle to viewport + float fx1 = (new_size.first - 1) / outer_width; + float fy1 = (new_size.second - 1) / outer_height; + float cx1 = -fx1 * oX0; + float cy1 = -fy1 * oY0; + + // Interpolate between the two optimal projections + float new_fx = fx0 * (1 - alpha) + fx1 * alpha; + float new_fy = fy0 * (1 - alpha) + fy1 * alpha; + float new_cx = cx0 * (1 - alpha) + cx1 * alpha; + float new_cy = cy0 * (1 - alpha) + cy1 * alpha; + + std::array, 3> newK = { + {{new_fx, 0.0, new_cx}, + {0.0, new_fy, new_cy}, + {0.0, 0.0, 1.0}}}; + return newK; } // ================================================================================================= @@ -213,7 +563,6 @@ std::vector>> TriangulatorInternal::triangulate for (size_t i = 0; i < cameras.size(); ++i) { undistort_poses(i_poses_2d[i], internal_cameras[i]); - internal_cameras[i].update_projection_matrix(); } elapsed = std::chrono::steady_clock::now() - stime; @@ -601,84 +950,6 @@ void TriangulatorInternal::print_stats() // ================================================================================================= -void undistort_point_pinhole(std::array &p, const std::vector &k) -{ - // Use distortion coefficients: [k1, k2, p1, p2, k3] - // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/undistort.dispatch.cpp#L432 - - float x0 = p[0]; - float y0 = p[1]; - float x = x0; - float y = y0; - - // Iteratively refine the estimate for the undistorted point. - int max_iterations = 5; - for (int iter = 0; iter < max_iterations; ++iter) - { - float r2 = x * x + y * y; - double icdist = 1.0 / (1 + ((k[4] * r2 + k[1]) * r2 + k[0]) * r2); - if (icdist < 0) - { - x = x0; - y = y0; - break; - } - - float deltaX = 2 * k[2] * x * y + k[3] * (r2 + 2 * x * x); - float deltaY = k[2] * (r2 + 2 * y * y) + 2 * k[3] * x * y; - x = (x0 - deltaX) * icdist; - y = (y0 - deltaY) * icdist; - } - - p[0] = x; - p[1] = y; -} - -void undistort_point_fisheye(std::array &p, const std::vector &k) -{ - // Use distortion coefficients: [k1, k2, k3, k4] - // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/fisheye.cpp#L429 - - float theta_d = std::sqrt(p[0] * p[0] + p[1] * p[1]); - float pi_half = std::numbers::pi * 0.5; - theta_d = std::min(std::max(-pi_half, theta_d), pi_half); - - if (theta_d < 1e-6) - { - return; - } - - float scale = 0.0; - float theta = theta_d; - - int max_iterations = 5; - for (int iter = 0; iter < max_iterations; ++iter) - { - float theta2 = theta * theta; - float theta4 = theta2 * theta2; - float theta6 = theta4 * theta2; - float theta8 = theta4 * theta4; - - float k0_theta2 = k[0] * theta2; - float k1_theta4 = k[1] * theta4; - float k2_theta6 = k[2] * theta6; - float k3_theta8 = k[3] * theta8; - - float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / - (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8); - theta = theta - theta_fix; - - if (std::fabs(theta_fix) < 1e-6) - { - break; - } - } - - scale = std::tan(theta) / theta_d; - p[0] *= scale; - p[1] *= scale; -} - void TriangulatorInternal::undistort_poses( std::vector>> &poses_2d, CameraInternal &icam) { @@ -686,27 +957,25 @@ void TriangulatorInternal::undistort_poses( int height = icam.cam.height; // Undistort camera matrix - cv::Mat newK; + // As with the undistortion, the own implementation avoids some overhead compared to OpenCV + std::array, 3> newK; if (icam.cam.type == "fisheye") { - cv::fisheye::estimateNewCameraMatrixForUndistortRectify( - icam.K, icam.DC, cv::Size(width, height), cv::Matx33d::eye(), - newK, 1.0, cv::Size(width, height), 1.0); + newK = icam.calc_optimal_camera_matrix_fisheye(1.0, {width, height}); } else { - newK = cv::getOptimalNewCameraMatrix( - icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); + newK = icam.calc_optimal_camera_matrix_pinhole(1.0, {width, height}); } float ifx_old = 1.0 / icam.cam.K[0][0]; float ify_old = 1.0 / icam.cam.K[1][1]; float cx_old = icam.cam.K[0][2]; float cy_old = icam.cam.K[1][2]; - float fx_new = newK.at(0, 0); - float fy_new = newK.at(1, 1); - float cx_new = newK.at(0, 2); - float cy_new = newK.at(1, 2); + float fx_new = newK[0][0]; + float fy_new = newK[1][1]; + float cx_new = newK[0][2]; + float cy_new = newK[1][2]; // Undistort all the points size_t num_persons = poses_2d.size(); @@ -725,11 +994,11 @@ void TriangulatorInternal::undistort_poses( // additional distortion parameters and identity rotations in this usecase. if (icam.cam.type == "fisheye") { - undistort_point_fisheye(poses_2d[i][j], icam.cam.DC); + CameraInternal::undistort_point_fisheye(poses_2d[i][j], icam.cam.DC); } else { - undistort_point_pinhole(poses_2d[i][j], icam.cam.DC); + CameraInternal::undistort_point_pinhole(poses_2d[i][j], icam.cam.DC); } // Map the undistorted normalized point to the new image coordinates @@ -754,23 +1023,15 @@ void TriangulatorInternal::undistort_poses( } } - // Update the camera matrix - icam.K = newK.clone(); - for (size_t i = 0; i < 3; ++i) - { - for (size_t j = 0; j < 3; ++j) - { - icam.cam.K[i][j] = newK.at(i, j); - } - } + // Update the camera intrinsics + icam.cam.K = newK; + icam.invK = CameraInternal::invert3x3(newK); if (icam.cam.type == "fisheye") { - icam.DC = cv::Mat::zeros(4, 1, CV_32F); icam.cam.DC = {0.0, 0.0, 0.0, 0.0}; } else { - icam.DC = cv::Mat::zeros(5, 1, CV_32F); icam.cam.DC = {0.0, 0.0, 0.0, 0.0, 0.0}; } } @@ -1008,62 +1269,6 @@ std::vector TriangulatorInternal::score_projection( // ================================================================================================= -/* Compute the inverse using the adjugate method */ -std::array, 3> invert3x3(const std::array, 3> &M) -{ - // See: https://scicomp.stackexchange.com/a/29206 - - std::array, 3> adj = { - {{ - M[1][1] * M[2][2] - M[1][2] * M[2][1], - M[0][2] * M[2][1] - M[0][1] * M[2][2], - M[0][1] * M[1][2] - M[0][2] * M[1][1], - }, - { - M[1][2] * M[2][0] - M[1][0] * M[2][2], - M[0][0] * M[2][2] - M[0][2] * M[2][0], - M[0][2] * M[1][0] - M[0][0] * M[1][2], - }, - { - M[1][0] * M[2][1] - M[1][1] * M[2][0], - M[0][1] * M[2][0] - M[0][0] * M[2][1], - M[0][0] * M[1][1] - M[0][1] * M[1][0], - }}}; - - float det = M[0][0] * adj[0][0] + M[0][1] * adj[1][0] + M[0][2] * adj[2][0]; - if (std::fabs(det) < 1e-6f) - { - return {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}}; - } - - float idet = 1.0f / det; - std::array, 3> inv = { - {{ - adj[0][0] * idet, - adj[0][1] * idet, - adj[0][2] * idet, - }, - { - adj[1][0] * idet, - adj[1][1] * idet, - adj[1][2] * idet, - }, - { - adj[2][0] * idet, - adj[2][1] * idet, - adj[2][2] * idet, - }}}; - - return inv; -} - -std::array, 3> transpose3x3(const std::array, 3> &M) -{ - return {{{M[0][0], M[1][0], M[2][0]}, - {M[0][1], M[1][1], M[2][1]}, - {M[0][2], M[1][2], M[2][2]}}}; -} - float dot(const std::array &a, const std::array &b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; @@ -1102,40 +1307,30 @@ std::array mat_mul_vec( return res; } -/* Compute camera center and corresponding ray direction */ -std::tuple, std::array> calc_center_and_ray( - const CameraInternal &icam, - const std::array &pt) +std::array calc_ray_dir(const CameraInternal &icam, const std::array &pt) { - // Compute Rᵀ and t - auto R_transpose = transpose3x3(icam.cam.R); - std::array t = {icam.cam.T[0][0], icam.cam.T[1][0], icam.cam.T[2][0]}; - t = mat_mul_vec(icam.cam.R, multiply(t, -1.0f)); + // Compute normalized ray direction from the point + std::array uv1 = {pt[0], pt[1], 1.0}; + auto d = mat_mul_vec(icam.invR, mat_mul_vec(icam.invK, uv1)); + auto ray_dir = normalize(d); - // Camera center: C = -Rᵀ * t - auto C = multiply(mat_mul_vec(R_transpose, t), -1.0f); - - // Compute ray direction: - std::array uv1 = {pt[0], pt[1], 1.0f}; - auto K_inv = invert3x3(icam.cam.K); - auto d = mat_mul_vec(R_transpose, mat_mul_vec(K_inv, uv1)); - auto rayDir = normalize(d); - - return std::make_tuple(C, rayDir); + return ray_dir; } -/* Triangulate two points by computing their two rays and the midpoint of their closest approach */ std::array triangulate_midpoint( const CameraInternal &icam1, const CameraInternal &icam2, const std::array &pt1, const std::array &pt2) { + // Triangulate two points by computing their two rays and the midpoint of their closest approach // See: https://en.wikipedia.org/wiki/Skew_lines#Nearest_points // Obtain the camera centers and ray directions for both views - auto [p1, d1] = calc_center_and_ray(icam1, pt1); - auto [p2, d2] = calc_center_and_ray(icam2, pt2); + std::array p1 = icam1.center; + std::array p2 = icam2.center; + std::array d1 = calc_ray_dir(icam1, pt1); + std::array d2 = calc_ray_dir(icam2, pt2); // Compute the perpendicular plane vectors std::array n = cross(d1, d2); diff --git a/rpt/triangulator.hpp b/rpt/triangulator.hpp index 5ce940d..aba7dc4 100644 --- a/rpt/triangulator.hpp +++ b/rpt/triangulator.hpp @@ -17,13 +17,24 @@ public: CameraInternal(const Camera &cam); Camera cam; - cv::Mat K; - cv::Mat DC; - cv::Mat R; - cv::Mat T; - cv::Mat P; - void update_projection_matrix(); + std::array, 3> invK; + std::array, 3> invR; + std::array center; + + static std::array, 3> transpose3x3( + const std::array, 3> &M); + + static std::array, 3> invert3x3( + const std::array, 3> &M); + + static void undistort_point_pinhole(std::array &p, const std::vector &k); + static void undistort_point_fisheye(std::array &p, const std::vector &k); + + std::array, 3> calc_optimal_camera_matrix_fisheye( + float balance, std::pair new_size); + std::array, 3> calc_optimal_camera_matrix_pinhole( + float alpha, std::pair new_size); }; // =================================================================================================