crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Replaced cv::mats with nested std::vectors to improve speed.

Browse Source
This commit is contained in:
Daniel
2025-02-11 13:53:31 +01:00
parent 651b806a3a
commit 24d951f31d
3 changed files with 832 additions and 803 deletions
Download Patch File Download Diff File Expand all files Collapse all files

320
media/RESULTS.md
View File

@ -294,47 +294,47 @@ Results of the model in various experiments on different datasets. \
```json ```json
{ {
"img_loading": 0.0420667, "img_loading": 0.0422137,
"demosaicing": 0.000741598, "demosaicing": 0.000738378,
"avg_time_2d": 0.0148536, "avg_time_2d": 0.0148837,
"avg_time_3d": 0.000350915, "avg_time_3d": 0.000220359,
"fps": 62.7112 "fps": 63.1215
} }
{ {
"triangulator_calls": 301, "triangulator_calls": 301,
"init_time": 9.29104e-06, "init_time": 7.04818e-06,
"undistort_time": 2.87243e-05, "undistort_time": 3.18955e-05,
"project_time": 3.63974e-05, "project_time": 3.92248e-06,
"match_time": 2.42599e-05, "match_time": 1.17322e-05,
"pairs_time": 3.40423e-05, "pairs_time": 6.12719e-06,
"pair_scoring_time": 7.63089e-05, "pair_scoring_time": 4.9701e-05,
"grouping_time": 1.0948e-05, "grouping_time": 8.03308e-06,
"full_time": 6.34768e-05, "full_time": 5.63869e-05,
"merge_time": 9.62578e-06, "merge_time": 1.32789e-05,
"post_time": 1.23629e-05, "post_time": 9.21993e-06,
"convert_time": 5.63644e-07, "convert_time": 2.10834e-07,
"total_time": 0.000306359 "total_time": 0.000197817
} }
{ {
"person_nums": { "person_nums": {
"total_frames": 301, "total_frames": 301,
"total_labels": 477, "total_labels": 477,
"total_preds": 835, "total_preds": 828,
"considered_empty": 0, "considered_empty": 0,
"valid_preds": 477, "valid_preds": 477,
"invalid_preds": 358, "invalid_preds": 351,
"missing": 0, "missing": 0,
"invalid_fraction": 0.42874, "invalid_fraction": 0.42391,
"precision": 0.57126, "precision": 0.57609,
"recall": 1.0, "recall": 1.0,
"f1": 0.72713, "f1": 0.73103,
"non_empty": 835 "non_empty": 828
}, },
"mpjpe": { "mpjpe": {
"count": 477, "count": 477,
"mean": 0.047948, "mean": 0.047957,
"median": 0.042546, "median": 0.042546,
"std": 0.014903, "std": 0.014906,
"sem": 0.000683, "sem": 0.000683,
"min": 0.03014, "min": 0.03014,
"max": 0.12344, "max": 0.12344,
@ -346,22 +346,22 @@ Results of the model in various experiments on different datasets. \
"recall-0.5": 1.0, "recall-0.5": 1.0,
"num_labels": 477, "num_labels": 477,
"ap-0.025": 0.0, "ap-0.025": 0.0,
"ap-0.05": 0.386198, "ap-0.05": 0.387635,
"ap-0.1": 0.725994, "ap-0.1": 0.728518,
"ap-0.15": 0.741431, "ap-0.15": 0.744056,
"ap-0.25": 0.741431, "ap-0.25": 0.744056,
"ap-0.5": 0.741431 "ap-0.5": 0.744056
}, },
"head": { "head": {
"count": 477, "count": 477,
"mean": 0.054142, "mean": 0.054194,
"median": 0.050182, "median": 0.050192,
"std": 0.024877, "std": 0.024884,
"sem": 0.00114, "sem": 0.001141,
"min": 0.005604, "min": 0.005604,
"max": 0.180414, "max": 0.180414,
"recall-0.025": 0.083857, "recall-0.025": 0.083857,
"recall-0.05": 0.498952, "recall-0.05": 0.496855,
"recall-0.1": 0.937107, "recall-0.1": 0.937107,
"recall-0.15": 0.995807, "recall-0.15": 0.995807,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@ -402,14 +402,14 @@ Results of the model in various experiments on different datasets. \
}, },
"elbow_left": { "elbow_left": {
"count": 477, "count": 477,
"mean": 0.040702, "mean": 0.040748,
"median": 0.032111, "median": 0.032111,
"std": 0.029146, "std": 0.02924,
"sem": 0.001336, "sem": 0.00134,
"min": 0.003362, "min": 0.003362,
"max": 0.326353, "max": 0.326353,
"recall-0.025": 0.312369, "recall-0.025": 0.314465,
"recall-0.05": 0.75891, "recall-0.05": 0.756813,
"recall-0.1": 0.953878, "recall-0.1": 0.953878,
"recall-0.15": 0.997904, "recall-0.15": 0.997904,
"recall-0.25": 0.997904, "recall-0.25": 0.997904,
@ -434,7 +434,7 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_left": { "wrist_left": {
"count": 477, "count": 477,
"mean": 0.060087, "mean": 0.060086,
"median": 0.053969, "median": 0.053969,
"std": 0.038695, "std": 0.038695,
"sem": 0.001774, "sem": 0.001774,
@ -450,13 +450,13 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_right": { "wrist_right": {
"count": 477, "count": 477,
"mean": 0.059116, "mean": 0.059129,
"median": 0.054488, "median": 0.054282,
"std": 0.033516, "std": 0.033512,
"sem": 0.001536, "sem": 0.001536,
"min": 0.009831, "min": 0.009831,
"max": 0.371597, "max": 0.371597,
"recall-0.025": 0.111111, "recall-0.025": 0.109015,
"recall-0.05": 0.419287, "recall-0.05": 0.419287,
"recall-0.1": 0.899371, "recall-0.1": 0.899371,
"recall-0.15": 0.981132, "recall-0.15": 0.981132,
@ -563,7 +563,7 @@ Results of the model in various experiments on different datasets. \
"joint_recalls": { "joint_recalls": {
"num_labels": 6201, "num_labels": 6201,
"recall-0.025": 0.20997, "recall-0.025": 0.20997,
"recall-0.05": 0.61635, "recall-0.05": 0.61571,
"recall-0.1": 0.94243, "recall-0.1": 0.94243,
"recall-0.15": 0.98645, "recall-0.15": 0.98645,
"recall-0.25": 0.99839, "recall-0.25": 0.99839,
@ -1091,26 +1091,26 @@ Results of the model in various experiments on different datasets. \
```json ```json
{ {
"img_loading": 0.0479787, "img_loading": 0.0460374,
"demosaicing": 0.00105853, "demosaicing": 0.00103523,
"avg_time_2d": 0.0168711, "avg_time_2d": 0.0168158,
"avg_time_3d": 0.000490739, "avg_time_3d": 0.00023421,
"fps": 54.2878 "fps": 55.2936
} }
{ {
"triangulator_calls": 420, "triangulator_calls": 420,
"init_time": 1.06278e-05, "init_time": 6.60958e-06,
"undistort_time": 2.61993e-05, "undistort_time": 3.08934e-05,
"project_time": 5.91157e-05, "project_time": 5.10071e-06,
"match_time": 7.7537e-05, "match_time": 1.76305e-05,
"pairs_time": 4.91138e-05, "pairs_time": 1.31107e-05,
"pair_scoring_time": 9.39502e-05, "pair_scoring_time": 5.49491e-05,
"grouping_time": 1.86132e-05, "grouping_time": 1.05505e-05,
"full_time": 8.07886e-05, "full_time": 4.52758e-05,
"merge_time": 9.90888e-06, "merge_time": 1.57141e-05,
"post_time": 1.35556e-05, "post_time": 1.02826e-05,
"convert_time": 7.28681e-07, "convert_time": 2.49755e-07,
"total_time": 0.000440487 "total_time": 0.000210623
} }
{ {
"person_nums": { "person_nums": {
@ -1129,35 +1129,35 @@ Results of the model in various experiments on different datasets. \
}, },
"mpjpe": { "mpjpe": {
"count": 1462, "count": 1462,
"mean": 0.032346, "mean": 0.032408,
"median": 0.029628, "median": 0.029662,
"std": 0.014512, "std": 0.014572,
"sem": 0.00038, "sem": 0.000381,
"min": 0.010671, "min": 0.010671,
"max": 0.136736, "max": 0.135869,
"recall-0.025": 0.339018, "recall-0.025": 0.337653,
"recall-0.05": 0.900409, "recall-0.05": 0.899045,
"recall-0.1": 0.99045, "recall-0.1": 0.99045,
"recall-0.15": 0.997271, "recall-0.15": 0.997271,
"recall-0.25": 0.997271, "recall-0.25": 0.997271,
"recall-0.5": 0.997271, "recall-0.5": 0.997271,
"num_labels": 1466, "num_labels": 1466,
"ap-0.025": 0.181063, "ap-0.025": 0.17872,
"ap-0.05": 0.865292, "ap-0.05": 0.863429,
"ap-0.1": 0.979068, "ap-0.1": 0.979024,
"ap-0.15": 0.989802, "ap-0.15": 0.989805,
"ap-0.25": 0.989802, "ap-0.25": 0.989805,
"ap-0.5": 0.989802 "ap-0.5": 0.989805
}, },
"nose": { "nose": {
"count": 1461, "count": 1461,
"mean": 0.01573, "mean": 0.015719,
"median": 0.011646, "median": 0.011701,
"std": 0.018004, "std": 0.017968,
"sem": 0.000471, "sem": 0.00047,
"min": 0.001311, "min": 0.001311,
"max": 0.276143, "max": 0.276143,
"recall-0.025": 0.899522, "recall-0.025": 0.898838,
"recall-0.05": 0.963773, "recall-0.05": 0.963773,
"recall-0.1": 0.992481, "recall-0.1": 0.992481,
"recall-0.15": 0.995899, "recall-0.15": 0.995899,
@ -1167,14 +1167,14 @@ Results of the model in various experiments on different datasets. \
}, },
"shoulder_left": { "shoulder_left": {
"count": 1462, "count": 1462,
"mean": 0.016823, "mean": 0.016838,
"median": 0.014758, "median": 0.014756,
"std": 0.010996, "std": 0.011066,
"sem": 0.000288, "sem": 0.00029,
"min": 0.000954, "min": 0.000954,
"max": 0.103637, "max": 0.103637,
"recall-0.025": 0.8397, "recall-0.025": 0.839018,
"recall-0.05": 0.982947, "recall-0.05": 0.982265,
"recall-0.1": 0.996589, "recall-0.1": 0.996589,
"recall-0.15": 0.997271, "recall-0.15": 0.997271,
"recall-0.25": 0.997271, "recall-0.25": 0.997271,
@ -1183,13 +1183,13 @@ Results of the model in various experiments on different datasets. \
}, },
"shoulder_right": { "shoulder_right": {
"count": 1461, "count": 1461,
"mean": 0.016777, "mean": 0.016814,
"median": 0.014699, "median": 0.014713,
"std": 0.011399, "std": 0.011424,
"sem": 0.000298, "sem": 0.000299,
"min": 0.001164, "min": 0.001164,
"max": 0.156188, "max": 0.156188,
"recall-0.025": 0.833447, "recall-0.025": 0.83413,
"recall-0.05": 0.983618, "recall-0.05": 0.983618,
"recall-0.1": 0.996587, "recall-0.1": 0.996587,
"recall-0.15": 0.996587, "recall-0.15": 0.996587,
@ -1199,30 +1199,30 @@ Results of the model in various experiments on different datasets. \
}, },
"elbow_left": { "elbow_left": {
"count": 1461, "count": 1461,
"mean": 0.022172, "mean": 0.022249,
"median": 0.016399, "median": 0.016379,
"std": 0.019077, "std": 0.019622,
"sem": 0.000499, "sem": 0.000514,
"min": 0.000543, "min": 0.000543,
"max": 0.210066, "max": 0.210065,
"recall-0.025": 0.735154, "recall-0.025": 0.736519,
"recall-0.05": 0.916724, "recall-0.05": 0.916041,
"recall-0.1": 0.990444, "recall-0.1": 0.989761,
"recall-0.15": 0.996587, "recall-0.15": 0.995904,
"recall-0.25": 0.99727, "recall-0.25": 0.99727,
"recall-0.5": 0.99727, "recall-0.5": 0.99727,
"num_labels": 1465 "num_labels": 1465
}, },
"elbow_right": { "elbow_right": {
"count": 1461, "count": 1461,
"mean": 0.021149, "mean": 0.021067,
"median": 0.015999, "median": 0.015999,
"std": 0.016746, "std": 0.016565,
"sem": 0.000438, "sem": 0.000434,
"min": 0.001472, "min": 0.001472,
"max": 0.162788, "max": 0.162788,
"recall-0.025": 0.780588, "recall-0.025": 0.780588,
"recall-0.05": 0.926863, "recall-0.05": 0.928913,
"recall-0.1": 0.995899, "recall-0.1": 0.995899,
"recall-0.15": 0.997949, "recall-0.15": 0.997949,
"recall-0.25": 0.998633, "recall-0.25": 0.998633,
@ -1231,14 +1231,14 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_left": { "wrist_left": {
"count": 1432, "count": 1432,
"mean": 0.035971, "mean": 0.036145,
"median": 0.016823, "median": 0.016837,
"std": 0.055313, "std": 0.055856,
"sem": 0.001462, "sem": 0.001477,
"min": 0.000898, "min": 0.000898,
"max": 0.450938, "max": 0.450938,
"recall-0.025": 0.67364, "recall-0.025": 0.672245,
"recall-0.05": 0.843794, "recall-0.05": 0.842399,
"recall-0.1": 0.904463, "recall-0.1": 0.904463,
"recall-0.15": 0.953975, "recall-0.15": 0.953975,
"recall-0.25": 0.974895, "recall-0.25": 0.974895,
@ -1247,29 +1247,29 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_right": { "wrist_right": {
"count": 1455, "count": 1455,
"mean": 0.026926, "mean": 0.026992,
"median": 0.016801, "median": 0.016745,
"std": 0.033427, "std": 0.033905,
"sem": 0.000877, "sem": 0.000889,
"min": 0.001361, "min": 0.001361,
"max": 0.280646, "max": 0.280646,
"recall-0.025": 0.690934, "recall-0.025": 0.692995,
"recall-0.05": 0.888736, "recall-0.05": 0.888049,
"recall-0.1": 0.964973, "recall-0.1": 0.964973,
"recall-0.15": 0.980769, "recall-0.15": 0.980082,
"recall-0.25": 0.997253, "recall-0.25": 0.996566,
"recall-0.5": 0.999313, "recall-0.5": 0.999313,
"num_labels": 1456 "num_labels": 1456
}, },
"hip_left": { "hip_left": {
"count": 1461, "count": 1461,
"mean": 0.034771, "mean": 0.034766,
"median": 0.031898, "median": 0.031916,
"std": 0.019216, "std": 0.019208,
"sem": 0.000503, "sem": 0.000503,
"min": 0.00101, "min": 0.00101,
"max": 0.181992, "max": 0.181992,
"recall-0.025": 0.32628, "recall-0.025": 0.324915,
"recall-0.05": 0.845051, "recall-0.05": 0.845051,
"recall-0.1": 0.989761, "recall-0.1": 0.989761,
"recall-0.15": 0.995904, "recall-0.15": 0.995904,
@ -1279,13 +1279,13 @@ Results of the model in various experiments on different datasets. \
}, },
"hip_right": { "hip_right": {
"count": 1462, "count": 1462,
"mean": 0.037383, "mean": 0.037408,
"median": 0.032592, "median": 0.032634,
"std": 0.024222, "std": 0.024199,
"sem": 0.000634, "sem": 0.000633,
"min": 0.002509, "min": 0.002509,
"max": 0.281736, "max": 0.281736,
"recall-0.025": 0.318554, "recall-0.025": 0.316508,
"recall-0.05": 0.802183, "recall-0.05": 0.802183,
"recall-0.1": 0.976126, "recall-0.1": 0.976126,
"recall-0.15": 0.993861, "recall-0.15": 0.993861,
@ -1295,15 +1295,15 @@ Results of the model in various experiments on different datasets. \
}, },
"knee_left": { "knee_left": {
"count": 1461, "count": 1461,
"mean": 0.038795, "mean": 0.038815,
"median": 0.032848, "median": 0.032849,
"std": 0.034462, "std": 0.034505,
"sem": 0.000902, "sem": 0.000903,
"min": 0.003309, "min": 0.003309,
"max": 0.473605, "max": 0.473605,
"recall-0.025": 0.287372, "recall-0.025": 0.287372,
"recall-0.05": 0.799317, "recall-0.05": 0.799317,
"recall-0.1": 0.978157, "recall-0.1": 0.976792,
"recall-0.15": 0.9843, "recall-0.15": 0.9843,
"recall-0.25": 0.990444, "recall-0.25": 0.990444,
"recall-0.5": 0.99727, "recall-0.5": 0.99727,
@ -1311,13 +1311,13 @@ Results of the model in various experiments on different datasets. \
}, },
"knee_right": { "knee_right": {
"count": 1455, "count": 1455,
"mean": 0.038488, "mean": 0.038477,
"median": 0.031543, "median": 0.031533,
"std": 0.026577, "std": 0.026575,
"sem": 0.000697, "sem": 0.000697,
"min": 0.003512, "min": 0.003512,
"max": 0.275123, "max": 0.275123,
"recall-0.025": 0.349554, "recall-0.025": 0.35024,
"recall-0.05": 0.749829, "recall-0.05": 0.749829,
"recall-0.1": 0.964359, "recall-0.1": 0.964359,
"recall-0.15": 0.993146, "recall-0.15": 0.993146,
@ -1327,50 +1327,50 @@ Results of the model in various experiments on different datasets. \
}, },
"ankle_left": { "ankle_left": {
"count": 1458, "count": 1458,
"mean": 0.056122, "mean": 0.056359,
"median": 0.034021, "median": 0.034166,
"std": 0.062141, "std": 0.062188,
"sem": 0.001628, "sem": 0.001629,
"min": 0.003035, "min": 0.003035,
"max": 0.432301, "max": 0.432301,
"recall-0.025": 0.347915, "recall-0.025": 0.347915,
"recall-0.05": 0.666439, "recall-0.05": 0.663021,
"recall-0.1": 0.855776, "recall-0.1": 0.853725,
"recall-0.15": 0.917977, "recall-0.15": 0.91661,
"recall-0.25": 0.971292, "recall-0.25": 0.971292,
"recall-0.5": 0.996582, "recall-0.5": 0.996582,
"num_labels": 1463 "num_labels": 1463
}, },
"ankle_right": { "ankle_right": {
"count": 1445, "count": 1445,
"mean": 0.053755, "mean": 0.054031,
"median": 0.030905, "median": 0.030928,
"std": 0.067212, "std": 0.067536,
"sem": 0.001769, "sem": 0.001777,
"min": 0.001698, "min": 0.001698,
"max": 0.489965, "max": 0.489965,
"recall-0.025": 0.378082, "recall-0.025": 0.376027,
"recall-0.05": 0.736301, "recall-0.05": 0.734247,
"recall-0.1": 0.856849, "recall-0.1": 0.856164,
"recall-0.15": 0.903425, "recall-0.15": 0.90274,
"recall-0.25": 0.962329, "recall-0.25": 0.961644,
"recall-0.5": 0.989726, "recall-0.5": 0.989726,
"num_labels": 1460 "num_labels": 1460
}, },
"joint_recalls": { "joint_recalls": {
"num_labels": 18990, "num_labels": 18990,
"recall-0.025": 0.57341, "recall-0.025": 0.57325,
"recall-0.05": 0.85408, "recall-0.05": 0.8535,
"recall-0.1": 0.9584, "recall-0.1": 0.95793,
"recall-0.15": 0.9772, "recall-0.15": 0.97694,
"recall-0.25": 0.99005, "recall-0.25": 0.98994,
"recall-0.5": 0.99674 "recall-0.5": 0.99674
} }
} }
{ {
"total_parts": 20444, "total_parts": 20444,
"correct_parts": 20204, "correct_parts": 20203,
"pcp": 0.988261 "pcp": 0.988212
} }
``` ```

1185
rpt/triangulator.cpp
View File

File diff suppressed because it is too large Load Diff

82
rpt/triangulator.hpp
View File

@ -70,64 +70,76 @@ private:
{"shoulder_left", "elbow_left"}, {"shoulder_left", "elbow_left"},
{"shoulder_right", "elbow_right"}, {"shoulder_right", "elbow_right"},
}; };
std::vector<cv::Mat> last_poses_3d; std::vector<std::vector<std::array<float, 4>>> last_poses_3d;
void undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam); void undistort_poses(
std::vector<std::vector<std::array<float, 3>>> &poses_2d, CameraInternal &icam);
std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses( std::tuple<std::vector<std::vector<std::array<float, 3>>>, std::vector<std::vector<float>>>
const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists); project_poses(
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
const CameraInternal &icam,
bool calc_dists);
float calc_pose_score( float calc_pose_score(
const cv::Mat &pose1, const std::vector<std::array<float, 3>> &pose1,
const cv::Mat &pose2, const std::vector<std::array<float, 3>> &pose2,
const cv::Mat &dist1, const std::vector<float> &dist1,
const CameraInternal &icam); const CameraInternal &icam);
cv::Mat score_projection( std::vector<float> score_projection(
const cv::Mat &pose1, const std::vector<std::array<float, 3>> &pose,
const cv::Mat &repro1, const std::vector<std::array<float, 3>> &repro,
const cv::Mat &dists1, const std::vector<float> &dists,
const cv::Mat &mask, const std::vector<bool> &mask,
float iscale); float iscale);
std::pair<cv::Mat, float> triangulate_and_score( std::pair<std::vector<std::array<float, 4>>, float> triangulate_and_score(
const cv::Mat &pose1, const std::vector<std::array<float, 3>> &pose1,
const cv::Mat &pose2, const std::vector<std::array<float, 3>> &pose2,
const CameraInternal &cam1, const CameraInternal &cam1,
const CameraInternal &cam2, const CameraInternal &cam2,
const std::array<std::array<float, 3>, 2> &roomparams, const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<std::array<size_t, 2>> &core_limbs_idx); const std::vector<std::array<size_t, 2>> &core_limbs_idx);
std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> calc_grouping( std::vector<std::tuple<
const std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs, std::array<float, 3>, std::vector<std::array<float, 4>>, std::vector<int>>>
const std::vector<std::pair<cv::Mat, float>> &all_scored_poses, calc_grouping(
const std::vector<std::pair<
std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
const std::vector<std::pair<std::vector<std::array<float, 4>>, float>> &all_scored_poses,
float min_score); float min_score);
cv::Mat merge_group(const std::vector<cv::Mat> &poses_3d, float min_score); std::vector<std::array<float, 4>> merge_group(
const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
float min_score);
void add_extra_joints(std::vector<cv::Mat> &poses, const std::vector<std::string> &joint_names); void add_extra_joints(
std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names);
void filter_poses( void filter_poses(
std::vector<cv::Mat> &poses, std::vector<std::vector<std::array<float, 4>>> &poses,
const std::array<std::array<float, 3>, 2> &roomparams, const std::array<std::array<float, 3>, 2> &roomparams,
const std::vector<size_t> &core_joint_idx, const std::vector<size_t> &core_joint_idx,
const std::vector<std::array<size_t, 2>> &core_limbs_idx); const std::vector<std::array<size_t, 2>> &core_limbs_idx);
void add_missing_joints( void add_missing_joints(
std::vector<cv::Mat> &poses, const std::vector<std::string> &joint_names); std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names);
// Statistics // Statistics
float num_calls = 0; double num_calls = 0;
float total_time = 0; double total_time = 0;
float init_time = 0; double init_time = 0;
float undistort_time = 0; double undistort_time = 0;
float project_time = 0; double project_time = 0;
float match_time = 0; double match_time = 0;
float pairs_time = 0; double pairs_time = 0;
float pair_scoring_time = 0; double pair_scoring_time = 0;
float grouping_time = 0; double grouping_time = 0;
float full_time = 0; double full_time = 0;
float merge_time = 0; double merge_time = 0;
float post_time = 0; double post_time = 0;
float convert_time = 0; double convert_time = 0;
}; };