From 608f89d6b61c9b12aee42a1ec6c53bbf66e7dcdb Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 27 Feb 2025 13:06:08 +0100 Subject: [PATCH] Replaced undistortion with std::vectors as well. --- media/RESULTS.md | 236 +++++++++++++++++++++---------------------- rpt/triangulator.cpp | 137 +++++++++++++++++++------ 2 files changed, 224 insertions(+), 149 deletions(-) diff --git a/media/RESULTS.md b/media/RESULTS.md index b506bf1..09d8759 100644 --- a/media/RESULTS.md +++ b/media/RESULTS.md @@ -294,26 +294,26 @@ Results of the model in various experiments on different datasets. \ ```json { - "img_loading": 0.0424014, - "demosaicing": 0.000721194, - "avg_time_2d": 0.0148998, - "avg_time_3d": 0.000186437, - "fps": 63.2615 + "img_loading": 0.0418024, + "demosaicing": 0.000712412, + "avg_time_2d": 0.014805, + "avg_time_3d": 0.000291986, + "fps": 63.2536 } { "triangulator_calls": 301, - "init_time": 7.22995e-06, - "undistort_time": 2.88306e-05, - "project_time": 3.50066e-06, - "match_time": 1.16445e-05, - "pairs_time": 6.11458e-06, - "pair_scoring_time": 4.69781e-05, - "grouping_time": 7.95871e-06, - "full_time": 4.9667e-05, - "merge_time": 1.32834e-05, - "post_time": 9.24932e-06, - "convert_time": 2.06937e-07, - "total_time": 0.000184941 + "init_time": 6.44811e-06, + "undistort_time": 4.05236e-05, + "project_time": 3.04309e-06, + "match_time": 1.07992e-05, + "pairs_time": 6.09893e-06, + "pair_scoring_time": 6.0547e-05, + "grouping_time": 7.94037e-06, + "full_time": 5.80405e-05, + "merge_time": 1.34738e-05, + "post_time": 9.23313e-06, + "convert_time": 1.58432e-07, + "total_time": 0.000216593 } { "person_nums": { @@ -332,25 +332,25 @@ Results of the model in various experiments on different datasets. \ }, "mpjpe": { "count": 477, - "mean": 0.047957, + "mean": 0.047978, "median": 0.042546, - "std": 0.014906, - "sem": 0.000683, + "std": 0.014958, + "sem": 0.000686, "min": 0.03014, "max": 0.12344, "recall-0.025": 0.0, "recall-0.05": 0.698113, - "recall-0.1": 0.985325, + "recall-0.1": 0.987421, "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.387635, - "ap-0.1": 0.728518, - "ap-0.15": 0.744056, - "ap-0.25": 0.744056, - "ap-0.5": 0.744056 + "ap-0.05": 0.387605, + "ap-0.1": 0.730843, + "ap-0.15": 0.744044, + "ap-0.25": 0.744044, + "ap-0.5": 0.744044 }, "head": { "count": 477, @@ -402,13 +402,13 @@ Results of the model in various experiments on different datasets. \ }, "elbow_left": { "count": 477, - "mean": 0.040748, + "mean": 0.040782, "median": 0.032111, - "std": 0.02924, + "std": 0.029225, "sem": 0.00134, - "min": 0.003362, + "min": 0.003363, "max": 0.326353, - "recall-0.025": 0.314465, + "recall-0.025": 0.312369, "recall-0.05": 0.756813, "recall-0.1": 0.953878, "recall-0.15": 0.997904, @@ -450,25 +450,25 @@ Results of the model in various experiments on different datasets. \ }, "wrist_right": { "count": 477, - "mean": 0.059129, - "median": 0.054282, - "std": 0.033512, - "sem": 0.001536, + "mean": 0.05937, + "median": 0.054488, + "std": 0.034178, + "sem": 0.001567, "min": 0.009831, "max": 0.371597, "recall-0.025": 0.109015, "recall-0.05": 0.419287, "recall-0.1": 0.899371, - "recall-0.15": 0.981132, + "recall-0.15": 0.979036, "recall-0.25": 0.997904, "recall-0.5": 1.0, "num_labels": 477 }, "hip_left": { "count": 477, - "mean": 0.048032, + "mean": 0.048082, "median": 0.042309, - "std": 0.026348, + "std": 0.02636, "sem": 0.001208, "min": 0.006447, "max": 0.14256, @@ -498,15 +498,15 @@ Results of the model in various experiments on different datasets. \ }, "knee_left": { "count": 477, - "mean": 0.04045, + "mean": 0.040396, "median": 0.037751, - "std": 0.024566, - "sem": 0.001126, + "std": 0.02446, + "sem": 0.001121, "min": 0.004904, "max": 0.190671, "recall-0.025": 0.259958, "recall-0.05": 0.748428, - "recall-0.1": 0.972746, + "recall-0.1": 0.974843, "recall-0.15": 0.989518, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -564,7 +564,7 @@ Results of the model in various experiments on different datasets. \ "num_labels": 6201, "recall-0.025": 0.20997, "recall-0.05": 0.61571, - "recall-0.1": 0.94243, + "recall-0.1": 0.94275, "recall-0.15": 0.98645, "recall-0.25": 0.99839, "recall-0.5": 1.0 @@ -5967,26 +5967,26 @@ Results of the model in various experiments on different datasets. \ ```json { - "img_loading": 0.27609, - "demosaicing": 0.0112097, - "avg_time_2d": 0.0240132, - "avg_time_3d": 0.000671032, - "fps": 27.8599 + "img_loading": 0.282821, + "demosaicing": 0.011311, + "avg_time_2d": 0.0240066, + "avg_time_3d": 0.00055045, + "fps": 27.8799 } { "triangulator_calls": 121, - "init_time": 2.1143e-05, - "undistort_time": 3.8166e-05, - "project_time": 3.43309e-05, - "match_time": 2.66535e-05, - "pairs_time": 5.949e-05, - "pair_scoring_time": 0.000217684, - "grouping_time": 2.1364e-05, - "full_time": 0.000180708, - "merge_time": 2.17519e-05, - "post_time": 1.14353e-05, - "convert_time": 5.88083e-07, - "total_time": 0.000633601 + "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 } { "person_nums": { @@ -6005,21 +6005,21 @@ Results of the model in various experiments on different datasets. \ }, "mpjpe": { "count": 363, - "mean": 0.025729, - "median": 0.0249, - "std": 0.007288, - "sem": 0.000383, + "mean": 0.0257, + "median": 0.024739, + "std": 0.007243, + "sem": 0.000381, "min": 0.011333, "max": 0.051735, - "recall-0.025": 0.506887, - "recall-0.05": 0.99449, + "recall-0.025": 0.515152, + "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.272651, - "ap-0.05": 0.993013, + "ap-0.025": 0.277608, + "ap-0.05": 0.99638, "ap-0.1": 1.0, "ap-0.15": 1.0, "ap-0.25": 1.0, @@ -6027,15 +6027,15 @@ Results of the model in various experiments on different datasets. \ }, "head": { "count": 363, - "mean": 0.028076, - "median": 0.022721, - "std": 0.017771, - "sem": 0.000934, + "mean": 0.027713, + "median": 0.022633, + "std": 0.017317, + "sem": 0.00091, "min": 0.00109, - "max": 0.112785, - "recall-0.025": 0.550964, - "recall-0.05": 0.884298, - "recall-0.1": 0.997245, + "max": 0.087763, + "recall-0.025": 0.553719, + "recall-0.05": 0.887052, + "recall-0.1": 1.0, "recall-0.15": 1.0, "recall-0.25": 1.0, "recall-0.5": 1.0, @@ -6043,14 +6043,14 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_left": { "count": 363, - "mean": 0.027266, - "median": 0.02188, - "std": 0.021199, - "sem": 0.001114, + "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.892562, + "recall-0.05": 0.895317, "recall-0.1": 0.975207, "recall-0.15": 0.997245, "recall-0.25": 1.0, @@ -6059,10 +6059,10 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_right": { "count": 363, - "mean": 0.023379, + "mean": 0.023389, "median": 0.021151, - "std": 0.012785, - "sem": 0.000672, + "std": 0.012799, + "sem": 0.000673, "min": 0.003682, "max": 0.101851, "recall-0.025": 0.61157, @@ -6077,8 +6077,8 @@ Results of the model in various experiments on different datasets. \ "count": 363, "mean": 0.022276, "median": 0.019385, - "std": 0.014907, - "sem": 0.000784, + "std": 0.014902, + "sem": 0.000783, "min": 0.001441, "max": 0.194618, "recall-0.025": 0.694215, @@ -6091,9 +6091,9 @@ Results of the model in various experiments on different datasets. \ }, "elbow_right": { "count": 363, - "mean": 0.018547, - "median": 0.016646, - "std": 0.010171, + "mean": 0.018528, + "median": 0.016603, + "std": 0.010173, "sem": 0.000535, "min": 0.001046, "max": 0.083441, @@ -6107,13 +6107,13 @@ Results of the model in various experiments on different datasets. \ }, "wrist_left": { "count": 363, - "mean": 0.023528, + "mean": 0.023532, "median": 0.018873, - "std": 0.018383, + "std": 0.018388, "sem": 0.000966, "min": 0.00279, "max": 0.199397, - "recall-0.025": 0.68595, + "recall-0.025": 0.683196, "recall-0.05": 0.931129, "recall-0.1": 0.991736, "recall-0.15": 0.997245, @@ -6123,9 +6123,9 @@ Results of the model in various experiments on different datasets. \ }, "wrist_right": { "count": 363, - "mean": 0.01958, + "mean": 0.019579, "median": 0.017651, - "std": 0.011203, + "std": 0.011201, "sem": 0.000589, "min": 0.002333, "max": 0.076342, @@ -6139,13 +6139,13 @@ Results of the model in various experiments on different datasets. \ }, "hip_left": { "count": 363, - "mean": 0.031085, - "median": 0.026292, - "std": 0.016989, + "mean": 0.031156, + "median": 0.026379, + "std": 0.016985, "sem": 0.000893, - "min": 0.006012, + "min": 0.006013, "max": 0.117111, - "recall-0.025": 0.446281, + "recall-0.025": 0.443526, "recall-0.05": 0.859504, "recall-0.1": 0.997245, "recall-0.15": 1.0, @@ -6155,14 +6155,14 @@ Results of the model in various experiments on different datasets. \ }, "hip_right": { "count": 363, - "mean": 0.031145, - "median": 0.028754, - "std": 0.016728, - "sem": 0.000879, + "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.900826, + "recall-0.05": 0.903581, "recall-0.1": 0.99449, "recall-0.15": 1.0, "recall-0.25": 1.0, @@ -6171,9 +6171,9 @@ Results of the model in various experiments on different datasets. \ }, "knee_left": { "count": 363, - "mean": 0.028289, + "mean": 0.028282, "median": 0.020833, - "std": 0.023122, + "std": 0.023126, "sem": 0.001215, "min": 0.001686, "max": 0.127237, @@ -6203,14 +6203,14 @@ Results of the model in various experiments on different datasets. \ }, "ankle_left": { "count": 363, - "mean": 0.028177, - "median": 0.021538, - "std": 0.022712, - "sem": 0.001194, + "mean": 0.028125, + "median": 0.021539, + "std": 0.02265, + "sem": 0.00119, "min": 0.002656, "max": 0.178927, - "recall-0.025": 0.575758, - "recall-0.05": 0.895317, + "recall-0.025": 0.578512, + "recall-0.05": 0.900826, "recall-0.1": 0.980716, "recall-0.15": 0.99449, "recall-0.25": 1.0, @@ -6219,10 +6219,10 @@ Results of the model in various experiments on different datasets. \ }, "ankle_right": { "count": 363, - "mean": 0.029428, - "median": 0.022095, - "std": 0.027113, - "sem": 0.001425, + "mean": 0.029496, + "median": 0.022264, + "std": 0.027073, + "sem": 0.001423, "min": 0.002482, "max": 0.263543, "recall-0.025": 0.584022, @@ -6235,9 +6235,9 @@ Results of the model in various experiments on different datasets. \ }, "joint_recalls": { "num_labels": 4719, - "recall-0.025": 0.60606, - "recall-0.05": 0.91566, - "recall-0.1": 0.98962, + "recall-0.025": 0.60585, + "recall-0.05": 0.9163, + "recall-0.1": 0.99004, "recall-0.15": 0.99746, "recall-0.25": 0.99958, "recall-0.5": 1.0 diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index cd6bca7..0f3e6c7 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -210,7 +210,6 @@ std::vector>> TriangulatorInternal::triangulate stime = std::chrono::steady_clock::now(); // Undistort 2D poses - #pragma omp parallel for for (size_t i = 0; i < cameras.size(); ++i) { undistort_poses(i_poses_2d[i], internal_cameras[i]); @@ -604,6 +603,84 @@ 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) { @@ -624,44 +701,42 @@ void TriangulatorInternal::undistort_poses( icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); } - // Convert vectors to single mat - size_t num_persons = poses_2d.size(); - size_t num_joints = poses_2d[0].size(); - std::vector dims = {(int)(num_persons * num_joints), 2}; - cv::Mat points_mat = cv::Mat(dims, CV_32F); - float *mat_ptr = points_mat.ptr(0); - for (size_t i = 0; i < num_persons; ++i) - { - for (size_t j = 0; j < num_joints; ++j) - { - for (size_t k = 0; k < 2; ++k) - { - mat_ptr[i * num_joints * 2 + j * 2 + k] = poses_2d[i][j][k]; - } - } - } + 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); // Undistort all the points - points_mat = points_mat.reshape(2, static_cast(points_mat.rows)); - if (icam.cam.type == "fisheye") - { - cv::fisheye::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK); - } - else - { - cv::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK); - } - points_mat = points_mat.reshape(1, static_cast(points_mat.rows)); - - // Overwrite the old coordinates with the undistorted ones + size_t num_persons = poses_2d.size(); + size_t num_joints = poses_2d[0].size(); for (size_t i = 0; i < num_persons; ++i) { 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; + poses_2d[i][j][1] = (poses_2d[i][j][1] - cy_old) * ify_old; + + // Undistort + // 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") { - poses_2d[i][j][k] = mat_ptr[i * num_joints * 2 + j * 2 + k]; + undistort_point_fisheye(poses_2d[i][j], icam.cam.DC); } + else + { + undistort_point_pinhole(poses_2d[i][j], icam.cam.DC); + } + + // Map the undistorted normalized point to the new image coordinates + 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; } }