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

Replaced undistortion with std::vectors as well.

Browse Source
This commit is contained in:
Daniel
2025-02-27 13:06:08 +01:00
parent 63e00e9b13
commit 608f89d6b6
2 changed files with 224 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

236
media/RESULTS.md
View File

@ -294,26 +294,26 @@ Results of the model in various experiments on different datasets. \
```json ```json
{ {
"img_loading": 0.0424014, "img_loading": 0.0418024,
"demosaicing": 0.000721194, "demosaicing": 0.000712412,
"avg_time_2d": 0.0148998, "avg_time_2d": 0.014805,
"avg_time_3d": 0.000186437, "avg_time_3d": 0.000291986,
"fps": 63.2615 "fps": 63.2536
} }
{ {
"triangulator_calls": 301, "triangulator_calls": 301,
"init_time": 7.22995e-06, "init_time": 6.44811e-06,
"undistort_time": 2.88306e-05, "undistort_time": 4.05236e-05,
"project_time": 3.50066e-06, "project_time": 3.04309e-06,
"match_time": 1.16445e-05, "match_time": 1.07992e-05,
"pairs_time": 6.11458e-06, "pairs_time": 6.09893e-06,
"pair_scoring_time": 4.69781e-05, "pair_scoring_time": 6.0547e-05,
"grouping_time": 7.95871e-06, "grouping_time": 7.94037e-06,
"full_time": 4.9667e-05, "full_time": 5.80405e-05,
"merge_time": 1.32834e-05, "merge_time": 1.34738e-05,
"post_time": 9.24932e-06, "post_time": 9.23313e-06,
"convert_time": 2.06937e-07, "convert_time": 1.58432e-07,
"total_time": 0.000184941 "total_time": 0.000216593
} }
{ {
"person_nums": { "person_nums": {
@ -332,25 +332,25 @@ Results of the model in various experiments on different datasets. \
}, },
"mpjpe": { "mpjpe": {
"count": 477, "count": 477,
"mean": 0.047957, "mean": 0.047978,
"median": 0.042546, "median": 0.042546,
"std": 0.014906, "std": 0.014958,
"sem": 0.000683, "sem": 0.000686,
"min": 0.03014, "min": 0.03014,
"max": 0.12344, "max": 0.12344,
"recall-0.025": 0.0, "recall-0.025": 0.0,
"recall-0.05": 0.698113, "recall-0.05": 0.698113,
"recall-0.1": 0.985325, "recall-0.1": 0.987421,
"recall-0.15": 1.0, "recall-0.15": 1.0,
"recall-0.25": 1.0, "recall-0.25": 1.0,
"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.387635, "ap-0.05": 0.387605,
"ap-0.1": 0.728518, "ap-0.1": 0.730843,
"ap-0.15": 0.744056, "ap-0.15": 0.744044,
"ap-0.25": 0.744056, "ap-0.25": 0.744044,
"ap-0.5": 0.744056 "ap-0.5": 0.744044
}, },
"head": { "head": {
"count": 477, "count": 477,
@ -402,13 +402,13 @@ Results of the model in various experiments on different datasets. \
}, },
"elbow_left": { "elbow_left": {
"count": 477, "count": 477,
"mean": 0.040748, "mean": 0.040782,
"median": 0.032111, "median": 0.032111,
"std": 0.02924, "std": 0.029225,
"sem": 0.00134, "sem": 0.00134,
"min": 0.003362, "min": 0.003363,
"max": 0.326353, "max": 0.326353,
"recall-0.025": 0.314465, "recall-0.025": 0.312369,
"recall-0.05": 0.756813, "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,
@ -450,25 +450,25 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_right": { "wrist_right": {
"count": 477, "count": 477,
"mean": 0.059129, "mean": 0.05937,
"median": 0.054282, "median": 0.054488,
"std": 0.033512, "std": 0.034178,
"sem": 0.001536, "sem": 0.001567,
"min": 0.009831, "min": 0.009831,
"max": 0.371597, "max": 0.371597,
"recall-0.025": 0.109015, "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.979036,
"recall-0.25": 0.997904, "recall-0.25": 0.997904,
"recall-0.5": 1.0, "recall-0.5": 1.0,
"num_labels": 477 "num_labels": 477
}, },
"hip_left": { "hip_left": {
"count": 477, "count": 477,
"mean": 0.048032, "mean": 0.048082,
"median": 0.042309, "median": 0.042309,
"std": 0.026348, "std": 0.02636,
"sem": 0.001208, "sem": 0.001208,
"min": 0.006447, "min": 0.006447,
"max": 0.14256, "max": 0.14256,
@ -498,15 +498,15 @@ Results of the model in various experiments on different datasets. \
}, },
"knee_left": { "knee_left": {
"count": 477, "count": 477,
"mean": 0.04045, "mean": 0.040396,
"median": 0.037751, "median": 0.037751,
"std": 0.024566, "std": 0.02446,
"sem": 0.001126, "sem": 0.001121,
"min": 0.004904, "min": 0.004904,
"max": 0.190671, "max": 0.190671,
"recall-0.025": 0.259958, "recall-0.025": 0.259958,
"recall-0.05": 0.748428, "recall-0.05": 0.748428,
"recall-0.1": 0.972746, "recall-0.1": 0.974843,
"recall-0.15": 0.989518, "recall-0.15": 0.989518,
"recall-0.25": 1.0, "recall-0.25": 1.0,
"recall-0.5": 1.0, "recall-0.5": 1.0,
@ -564,7 +564,7 @@ Results of the model in various experiments on different datasets. \
"num_labels": 6201, "num_labels": 6201,
"recall-0.025": 0.20997, "recall-0.025": 0.20997,
"recall-0.05": 0.61571, "recall-0.05": 0.61571,
"recall-0.1": 0.94243, "recall-0.1": 0.94275,
"recall-0.15": 0.98645, "recall-0.15": 0.98645,
"recall-0.25": 0.99839, "recall-0.25": 0.99839,
"recall-0.5": 1.0 "recall-0.5": 1.0
@ -5967,26 +5967,26 @@ Results of the model in various experiments on different datasets. \
```json ```json
{ {
"img_loading": 0.27609, "img_loading": 0.282821,
"demosaicing": 0.0112097, "demosaicing": 0.011311,
"avg_time_2d": 0.0240132, "avg_time_2d": 0.0240066,
"avg_time_3d": 0.000671032, "avg_time_3d": 0.00055045,
"fps": 27.8599 "fps": 27.8799
} }
{ {
"triangulator_calls": 121, "triangulator_calls": 121,
"init_time": 2.1143e-05, "init_time": 1.3244e-05,
"undistort_time": 3.8166e-05, "undistort_time": 3.09483e-05,
"project_time": 3.43309e-05, "project_time": 5.23027e-06,
"match_time": 2.66535e-05, "match_time": 1.68509e-05,
"pairs_time": 5.949e-05, "pairs_time": 1.37968e-05,
"pair_scoring_time": 0.000217684, "pair_scoring_time": 0.000160537,
"grouping_time": 2.1364e-05, "grouping_time": 1.73903e-05,
"full_time": 0.000180708, "full_time": 0.000199044,
"merge_time": 2.17519e-05, "merge_time": 3.75236e-05,
"post_time": 1.14353e-05, "post_time": 1.03264e-05,
"convert_time": 5.88083e-07, "convert_time": 1.91669e-07,
"total_time": 0.000633601 "total_time": 0.000505528
} }
{ {
"person_nums": { "person_nums": {
@ -6005,21 +6005,21 @@ Results of the model in various experiments on different datasets. \
}, },
"mpjpe": { "mpjpe": {
"count": 363, "count": 363,
"mean": 0.025729, "mean": 0.0257,
"median": 0.0249, "median": 0.024739,
"std": 0.007288, "std": 0.007243,
"sem": 0.000383, "sem": 0.000381,
"min": 0.011333, "min": 0.011333,
"max": 0.051735, "max": 0.051735,
"recall-0.025": 0.506887, "recall-0.025": 0.515152,
"recall-0.05": 0.99449, "recall-0.05": 0.997245,
"recall-0.1": 1.0, "recall-0.1": 1.0,
"recall-0.15": 1.0, "recall-0.15": 1.0,
"recall-0.25": 1.0, "recall-0.25": 1.0,
"recall-0.5": 1.0, "recall-0.5": 1.0,
"num_labels": 363, "num_labels": 363,
"ap-0.025": 0.272651, "ap-0.025": 0.277608,
"ap-0.05": 0.993013, "ap-0.05": 0.99638,
"ap-0.1": 1.0, "ap-0.1": 1.0,
"ap-0.15": 1.0, "ap-0.15": 1.0,
"ap-0.25": 1.0, "ap-0.25": 1.0,
@ -6027,15 +6027,15 @@ Results of the model in various experiments on different datasets. \
}, },
"head": { "head": {
"count": 363, "count": 363,
"mean": 0.028076, "mean": 0.027713,
"median": 0.022721, "median": 0.022633,
"std": 0.017771, "std": 0.017317,
"sem": 0.000934, "sem": 0.00091,
"min": 0.00109, "min": 0.00109,
"max": 0.112785, "max": 0.087763,
"recall-0.025": 0.550964, "recall-0.025": 0.553719,
"recall-0.05": 0.884298, "recall-0.05": 0.887052,
"recall-0.1": 0.997245, "recall-0.1": 1.0,
"recall-0.15": 1.0, "recall-0.15": 1.0,
"recall-0.25": 1.0, "recall-0.25": 1.0,
"recall-0.5": 1.0, "recall-0.5": 1.0,
@ -6043,14 +6043,14 @@ Results of the model in various experiments on different datasets. \
}, },
"shoulder_left": { "shoulder_left": {
"count": 363, "count": 363,
"mean": 0.027266, "mean": 0.027215,
"median": 0.02188, "median": 0.021616,
"std": 0.021199, "std": 0.021167,
"sem": 0.001114, "sem": 0.001113,
"min": 0.002899, "min": 0.002899,
"max": 0.151257, "max": 0.151257,
"recall-0.025": 0.584022, "recall-0.025": 0.584022,
"recall-0.05": 0.892562, "recall-0.05": 0.895317,
"recall-0.1": 0.975207, "recall-0.1": 0.975207,
"recall-0.15": 0.997245, "recall-0.15": 0.997245,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@ -6059,10 +6059,10 @@ Results of the model in various experiments on different datasets. \
}, },
"shoulder_right": { "shoulder_right": {
"count": 363, "count": 363,
"mean": 0.023379, "mean": 0.023389,
"median": 0.021151, "median": 0.021151,
"std": 0.012785, "std": 0.012799,
"sem": 0.000672, "sem": 0.000673,
"min": 0.003682, "min": 0.003682,
"max": 0.101851, "max": 0.101851,
"recall-0.025": 0.61157, "recall-0.025": 0.61157,
@ -6077,8 +6077,8 @@ Results of the model in various experiments on different datasets. \
"count": 363, "count": 363,
"mean": 0.022276, "mean": 0.022276,
"median": 0.019385, "median": 0.019385,
"std": 0.014907, "std": 0.014902,
"sem": 0.000784, "sem": 0.000783,
"min": 0.001441, "min": 0.001441,
"max": 0.194618, "max": 0.194618,
"recall-0.025": 0.694215, "recall-0.025": 0.694215,
@ -6091,9 +6091,9 @@ Results of the model in various experiments on different datasets. \
}, },
"elbow_right": { "elbow_right": {
"count": 363, "count": 363,
"mean": 0.018547, "mean": 0.018528,
"median": 0.016646, "median": 0.016603,
"std": 0.010171, "std": 0.010173,
"sem": 0.000535, "sem": 0.000535,
"min": 0.001046, "min": 0.001046,
"max": 0.083441, "max": 0.083441,
@ -6107,13 +6107,13 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_left": { "wrist_left": {
"count": 363, "count": 363,
"mean": 0.023528, "mean": 0.023532,
"median": 0.018873, "median": 0.018873,
"std": 0.018383, "std": 0.018388,
"sem": 0.000966, "sem": 0.000966,
"min": 0.00279, "min": 0.00279,
"max": 0.199397, "max": 0.199397,
"recall-0.025": 0.68595, "recall-0.025": 0.683196,
"recall-0.05": 0.931129, "recall-0.05": 0.931129,
"recall-0.1": 0.991736, "recall-0.1": 0.991736,
"recall-0.15": 0.997245, "recall-0.15": 0.997245,
@ -6123,9 +6123,9 @@ Results of the model in various experiments on different datasets. \
}, },
"wrist_right": { "wrist_right": {
"count": 363, "count": 363,
"mean": 0.01958, "mean": 0.019579,
"median": 0.017651, "median": 0.017651,
"std": 0.011203, "std": 0.011201,
"sem": 0.000589, "sem": 0.000589,
"min": 0.002333, "min": 0.002333,
"max": 0.076342, "max": 0.076342,
@ -6139,13 +6139,13 @@ Results of the model in various experiments on different datasets. \
}, },
"hip_left": { "hip_left": {
"count": 363, "count": 363,
"mean": 0.031085, "mean": 0.031156,
"median": 0.026292, "median": 0.026379,
"std": 0.016989, "std": 0.016985,
"sem": 0.000893, "sem": 0.000893,
"min": 0.006012, "min": 0.006013,
"max": 0.117111, "max": 0.117111,
"recall-0.025": 0.446281, "recall-0.025": 0.443526,
"recall-0.05": 0.859504, "recall-0.05": 0.859504,
"recall-0.1": 0.997245, "recall-0.1": 0.997245,
"recall-0.15": 1.0, "recall-0.15": 1.0,
@ -6155,14 +6155,14 @@ Results of the model in various experiments on different datasets. \
}, },
"hip_right": { "hip_right": {
"count": 363, "count": 363,
"mean": 0.031145, "mean": 0.03111,
"median": 0.028754, "median": 0.028792,
"std": 0.016728, "std": 0.01668,
"sem": 0.000879, "sem": 0.000877,
"min": 0.003451, "min": 0.003451,
"max": 0.138183, "max": 0.138183,
"recall-0.025": 0.38292, "recall-0.025": 0.38292,
"recall-0.05": 0.900826, "recall-0.05": 0.903581,
"recall-0.1": 0.99449, "recall-0.1": 0.99449,
"recall-0.15": 1.0, "recall-0.15": 1.0,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@ -6171,9 +6171,9 @@ Results of the model in various experiments on different datasets. \
}, },
"knee_left": { "knee_left": {
"count": 363, "count": 363,
"mean": 0.028289, "mean": 0.028282,
"median": 0.020833, "median": 0.020833,
"std": 0.023122, "std": 0.023126,
"sem": 0.001215, "sem": 0.001215,
"min": 0.001686, "min": 0.001686,
"max": 0.127237, "max": 0.127237,
@ -6203,14 +6203,14 @@ Results of the model in various experiments on different datasets. \
}, },
"ankle_left": { "ankle_left": {
"count": 363, "count": 363,
"mean": 0.028177, "mean": 0.028125,
"median": 0.021538, "median": 0.021539,
"std": 0.022712, "std": 0.02265,
"sem": 0.001194, "sem": 0.00119,
"min": 0.002656, "min": 0.002656,
"max": 0.178927, "max": 0.178927,
"recall-0.025": 0.575758, "recall-0.025": 0.578512,
"recall-0.05": 0.895317, "recall-0.05": 0.900826,
"recall-0.1": 0.980716, "recall-0.1": 0.980716,
"recall-0.15": 0.99449, "recall-0.15": 0.99449,
"recall-0.25": 1.0, "recall-0.25": 1.0,
@ -6219,10 +6219,10 @@ Results of the model in various experiments on different datasets. \
}, },
"ankle_right": { "ankle_right": {
"count": 363, "count": 363,
"mean": 0.029428, "mean": 0.029496,
"median": 0.022095, "median": 0.022264,
"std": 0.027113, "std": 0.027073,
"sem": 0.001425, "sem": 0.001423,
"min": 0.002482, "min": 0.002482,
"max": 0.263543, "max": 0.263543,
"recall-0.025": 0.584022, "recall-0.025": 0.584022,
@ -6235,9 +6235,9 @@ Results of the model in various experiments on different datasets. \
}, },
"joint_recalls": { "joint_recalls": {
"num_labels": 4719, "num_labels": 4719,
"recall-0.025": 0.60606, "recall-0.025": 0.60585,
"recall-0.05": 0.91566, "recall-0.05": 0.9163,
"recall-0.1": 0.98962, "recall-0.1": 0.99004,
"recall-0.15": 0.99746, "recall-0.15": 0.99746,
"recall-0.25": 0.99958, "recall-0.25": 0.99958,
"recall-0.5": 1.0 "recall-0.5": 1.0

125
rpt/triangulator.cpp
View File

@ -210,7 +210,6 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
stime = std::chrono::steady_clock::now(); stime = std::chrono::steady_clock::now();
// Undistort 2D poses // Undistort 2D poses
#pragma omp parallel for
for (size_t i = 0; i < cameras.size(); ++i) for (size_t i = 0; i < cameras.size(); ++i)
{ {
undistort_poses(i_poses_2d[i], internal_cameras[i]); undistort_poses(i_poses_2d[i], internal_cameras[i]);
@ -604,6 +603,84 @@ void TriangulatorInternal::print_stats()
// ================================================================================================= // =================================================================================================
void undistort_point_pinhole(std::array<float, 3> &p, const std::vector<float> &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<float, 3> &p, const std::vector<float> &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( void TriangulatorInternal::undistort_poses(
std::vector<std::vector<std::array<float, 3>>> &poses_2d, CameraInternal &icam) std::vector<std::vector<std::array<float, 3>>> &poses_2d, CameraInternal &icam)
{ {
@ -624,44 +701,42 @@ void TriangulatorInternal::undistort_poses(
icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height));
} }
// Convert vectors to single mat 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<float>(0, 0);
float fy_new = newK.at<float>(1, 1);
float cx_new = newK.at<float>(0, 2);
float cy_new = newK.at<float>(1, 2);
// Undistort all the points
size_t num_persons = poses_2d.size(); size_t num_persons = poses_2d.size();
size_t num_joints = poses_2d[0].size(); size_t num_joints = poses_2d[0].size();
std::vector<int> dims = {(int)(num_persons * num_joints), 2};
cv::Mat points_mat = cv::Mat(dims, CV_32F);
float *mat_ptr = points_mat.ptr<float>(0);
for (size_t i = 0; i < num_persons; ++i) for (size_t i = 0; i < num_persons; ++i)
{ {
for (size_t j = 0; j < num_joints; ++j) for (size_t j = 0; j < num_joints; ++j)
{ {
for (size_t k = 0; k < 2; ++k) // Normalize the point using the original camera matrix
{ poses_2d[i][j][0] = (poses_2d[i][j][0] - cx_old) * ifx_old;
mat_ptr[i * num_joints * 2 + j * 2 + k] = poses_2d[i][j][k]; poses_2d[i][j][1] = (poses_2d[i][j][1] - cy_old) * ify_old;
}
}
}
// Undistort all the points // Undistort
points_mat = points_mat.reshape(2, static_cast<int>(points_mat.rows)); // Using own implementation is faster than using OpenCV, because it avoids the
// overhead of creating cv::Mat objects and further unnecessary calculations for
// additional distortion parameters and identity rotations in this usecase.
if (icam.cam.type == "fisheye") if (icam.cam.type == "fisheye")
{ {
cv::fisheye::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK); undistort_point_fisheye(poses_2d[i][j], icam.cam.DC);
} }
else else
{ {
cv::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK); undistort_point_pinhole(poses_2d[i][j], icam.cam.DC);
} }
points_mat = points_mat.reshape(1, static_cast<int>(points_mat.rows));
// Overwrite the old coordinates with the undistorted ones // Map the undistorted normalized point to the new image coordinates
for (size_t i = 0; i < num_persons; ++i) poses_2d[i][j][0] = (poses_2d[i][j][0] * fx_new) + cx_new;
{ poses_2d[i][j][1] = (poses_2d[i][j][1] * fy_new) + cy_new;
for (size_t j = 0; j < num_joints; ++j)
{
for (size_t k = 0; k < 2; ++k)
{
poses_2d[i][j][k] = mat_ptr[i * num_joints * 2 + j * 2 + k];
}
} }
} }