From 24d951f31d959ed4c1587df00df5f2d08cca1cef Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 11 Feb 2025 13:53:31 +0100 Subject: [PATCH] Replaced cv::mats with nested std::vectors to improve speed. --- media/RESULTS.md | 320 +++++------ rpt/triangulator.cpp | 1233 +++++++++++++++++++++--------------------- rpt/triangulator.hpp | 82 +-- 3 files changed, 832 insertions(+), 803 deletions(-) diff --git a/media/RESULTS.md b/media/RESULTS.md index 962af93..dfa4eba 100644 --- a/media/RESULTS.md +++ b/media/RESULTS.md @@ -294,47 +294,47 @@ Results of the model in various experiments on different datasets. \ ```json { - "img_loading": 0.0420667, - "demosaicing": 0.000741598, - "avg_time_2d": 0.0148536, - "avg_time_3d": 0.000350915, - "fps": 62.7112 + "img_loading": 0.0422137, + "demosaicing": 0.000738378, + "avg_time_2d": 0.0148837, + "avg_time_3d": 0.000220359, + "fps": 63.1215 } { "triangulator_calls": 301, - "init_time": 9.29104e-06, - "undistort_time": 2.87243e-05, - "project_time": 3.63974e-05, - "match_time": 2.42599e-05, - "pairs_time": 3.40423e-05, - "pair_scoring_time": 7.63089e-05, - "grouping_time": 1.0948e-05, - "full_time": 6.34768e-05, - "merge_time": 9.62578e-06, - "post_time": 1.23629e-05, - "convert_time": 5.63644e-07, - "total_time": 0.000306359 + "init_time": 7.04818e-06, + "undistort_time": 3.18955e-05, + "project_time": 3.92248e-06, + "match_time": 1.17322e-05, + "pairs_time": 6.12719e-06, + "pair_scoring_time": 4.9701e-05, + "grouping_time": 8.03308e-06, + "full_time": 5.63869e-05, + "merge_time": 1.32789e-05, + "post_time": 9.21993e-06, + "convert_time": 2.10834e-07, + "total_time": 0.000197817 } { "person_nums": { "total_frames": 301, "total_labels": 477, - "total_preds": 835, + "total_preds": 828, "considered_empty": 0, "valid_preds": 477, - "invalid_preds": 358, + "invalid_preds": 351, "missing": 0, - "invalid_fraction": 0.42874, - "precision": 0.57126, + "invalid_fraction": 0.42391, + "precision": 0.57609, "recall": 1.0, - "f1": 0.72713, - "non_empty": 835 + "f1": 0.73103, + "non_empty": 828 }, "mpjpe": { "count": 477, - "mean": 0.047948, + "mean": 0.047957, "median": 0.042546, - "std": 0.014903, + "std": 0.014906, "sem": 0.000683, "min": 0.03014, "max": 0.12344, @@ -346,22 +346,22 @@ Results of the model in various experiments on different datasets. \ "recall-0.5": 1.0, "num_labels": 477, "ap-0.025": 0.0, - "ap-0.05": 0.386198, - "ap-0.1": 0.725994, - "ap-0.15": 0.741431, - "ap-0.25": 0.741431, - "ap-0.5": 0.741431 + "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 }, "head": { "count": 477, - "mean": 0.054142, - "median": 0.050182, - "std": 0.024877, - "sem": 0.00114, + "mean": 0.054194, + "median": 0.050192, + "std": 0.024884, + "sem": 0.001141, "min": 0.005604, "max": 0.180414, "recall-0.025": 0.083857, - "recall-0.05": 0.498952, + "recall-0.05": 0.496855, "recall-0.1": 0.937107, "recall-0.15": 0.995807, "recall-0.25": 1.0, @@ -402,14 +402,14 @@ Results of the model in various experiments on different datasets. \ }, "elbow_left": { "count": 477, - "mean": 0.040702, + "mean": 0.040748, "median": 0.032111, - "std": 0.029146, - "sem": 0.001336, + "std": 0.02924, + "sem": 0.00134, "min": 0.003362, "max": 0.326353, - "recall-0.025": 0.312369, - "recall-0.05": 0.75891, + "recall-0.025": 0.314465, + "recall-0.05": 0.756813, "recall-0.1": 0.953878, "recall-0.15": 0.997904, "recall-0.25": 0.997904, @@ -434,7 +434,7 @@ Results of the model in various experiments on different datasets. \ }, "wrist_left": { "count": 477, - "mean": 0.060087, + "mean": 0.060086, "median": 0.053969, "std": 0.038695, "sem": 0.001774, @@ -450,13 +450,13 @@ Results of the model in various experiments on different datasets. \ }, "wrist_right": { "count": 477, - "mean": 0.059116, - "median": 0.054488, - "std": 0.033516, + "mean": 0.059129, + "median": 0.054282, + "std": 0.033512, "sem": 0.001536, "min": 0.009831, "max": 0.371597, - "recall-0.025": 0.111111, + "recall-0.025": 0.109015, "recall-0.05": 0.419287, "recall-0.1": 0.899371, "recall-0.15": 0.981132, @@ -563,7 +563,7 @@ Results of the model in various experiments on different datasets. \ "joint_recalls": { "num_labels": 6201, "recall-0.025": 0.20997, - "recall-0.05": 0.61635, + "recall-0.05": 0.61571, "recall-0.1": 0.94243, "recall-0.15": 0.98645, "recall-0.25": 0.99839, @@ -1091,26 +1091,26 @@ Results of the model in various experiments on different datasets. \ ```json { - "img_loading": 0.0479787, - "demosaicing": 0.00105853, - "avg_time_2d": 0.0168711, - "avg_time_3d": 0.000490739, - "fps": 54.2878 + "img_loading": 0.0460374, + "demosaicing": 0.00103523, + "avg_time_2d": 0.0168158, + "avg_time_3d": 0.00023421, + "fps": 55.2936 } { "triangulator_calls": 420, - "init_time": 1.06278e-05, - "undistort_time": 2.61993e-05, - "project_time": 5.91157e-05, - "match_time": 7.7537e-05, - "pairs_time": 4.91138e-05, - "pair_scoring_time": 9.39502e-05, - "grouping_time": 1.86132e-05, - "full_time": 8.07886e-05, - "merge_time": 9.90888e-06, - "post_time": 1.35556e-05, - "convert_time": 7.28681e-07, - "total_time": 0.000440487 + "init_time": 6.60958e-06, + "undistort_time": 3.08934e-05, + "project_time": 5.10071e-06, + "match_time": 1.76305e-05, + "pairs_time": 1.31107e-05, + "pair_scoring_time": 5.49491e-05, + "grouping_time": 1.05505e-05, + "full_time": 4.52758e-05, + "merge_time": 1.57141e-05, + "post_time": 1.02826e-05, + "convert_time": 2.49755e-07, + "total_time": 0.000210623 } { "person_nums": { @@ -1129,35 +1129,35 @@ Results of the model in various experiments on different datasets. \ }, "mpjpe": { "count": 1462, - "mean": 0.032346, - "median": 0.029628, - "std": 0.014512, - "sem": 0.00038, + "mean": 0.032408, + "median": 0.029662, + "std": 0.014572, + "sem": 0.000381, "min": 0.010671, - "max": 0.136736, - "recall-0.025": 0.339018, - "recall-0.05": 0.900409, + "max": 0.135869, + "recall-0.025": 0.337653, + "recall-0.05": 0.899045, "recall-0.1": 0.99045, "recall-0.15": 0.997271, "recall-0.25": 0.997271, "recall-0.5": 0.997271, "num_labels": 1466, - "ap-0.025": 0.181063, - "ap-0.05": 0.865292, - "ap-0.1": 0.979068, - "ap-0.15": 0.989802, - "ap-0.25": 0.989802, - "ap-0.5": 0.989802 + "ap-0.025": 0.17872, + "ap-0.05": 0.863429, + "ap-0.1": 0.979024, + "ap-0.15": 0.989805, + "ap-0.25": 0.989805, + "ap-0.5": 0.989805 }, "nose": { "count": 1461, - "mean": 0.01573, - "median": 0.011646, - "std": 0.018004, - "sem": 0.000471, + "mean": 0.015719, + "median": 0.011701, + "std": 0.017968, + "sem": 0.00047, "min": 0.001311, "max": 0.276143, - "recall-0.025": 0.899522, + "recall-0.025": 0.898838, "recall-0.05": 0.963773, "recall-0.1": 0.992481, "recall-0.15": 0.995899, @@ -1167,14 +1167,14 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_left": { "count": 1462, - "mean": 0.016823, - "median": 0.014758, - "std": 0.010996, - "sem": 0.000288, + "mean": 0.016838, + "median": 0.014756, + "std": 0.011066, + "sem": 0.00029, "min": 0.000954, "max": 0.103637, - "recall-0.025": 0.8397, - "recall-0.05": 0.982947, + "recall-0.025": 0.839018, + "recall-0.05": 0.982265, "recall-0.1": 0.996589, "recall-0.15": 0.997271, "recall-0.25": 0.997271, @@ -1183,13 +1183,13 @@ Results of the model in various experiments on different datasets. \ }, "shoulder_right": { "count": 1461, - "mean": 0.016777, - "median": 0.014699, - "std": 0.011399, - "sem": 0.000298, + "mean": 0.016814, + "median": 0.014713, + "std": 0.011424, + "sem": 0.000299, "min": 0.001164, "max": 0.156188, - "recall-0.025": 0.833447, + "recall-0.025": 0.83413, "recall-0.05": 0.983618, "recall-0.1": 0.996587, "recall-0.15": 0.996587, @@ -1199,30 +1199,30 @@ Results of the model in various experiments on different datasets. \ }, "elbow_left": { "count": 1461, - "mean": 0.022172, - "median": 0.016399, - "std": 0.019077, - "sem": 0.000499, + "mean": 0.022249, + "median": 0.016379, + "std": 0.019622, + "sem": 0.000514, "min": 0.000543, - "max": 0.210066, - "recall-0.025": 0.735154, - "recall-0.05": 0.916724, - "recall-0.1": 0.990444, - "recall-0.15": 0.996587, + "max": 0.210065, + "recall-0.025": 0.736519, + "recall-0.05": 0.916041, + "recall-0.1": 0.989761, + "recall-0.15": 0.995904, "recall-0.25": 0.99727, "recall-0.5": 0.99727, "num_labels": 1465 }, "elbow_right": { "count": 1461, - "mean": 0.021149, + "mean": 0.021067, "median": 0.015999, - "std": 0.016746, - "sem": 0.000438, + "std": 0.016565, + "sem": 0.000434, "min": 0.001472, "max": 0.162788, "recall-0.025": 0.780588, - "recall-0.05": 0.926863, + "recall-0.05": 0.928913, "recall-0.1": 0.995899, "recall-0.15": 0.997949, "recall-0.25": 0.998633, @@ -1231,14 +1231,14 @@ Results of the model in various experiments on different datasets. \ }, "wrist_left": { "count": 1432, - "mean": 0.035971, - "median": 0.016823, - "std": 0.055313, - "sem": 0.001462, + "mean": 0.036145, + "median": 0.016837, + "std": 0.055856, + "sem": 0.001477, "min": 0.000898, "max": 0.450938, - "recall-0.025": 0.67364, - "recall-0.05": 0.843794, + "recall-0.025": 0.672245, + "recall-0.05": 0.842399, "recall-0.1": 0.904463, "recall-0.15": 0.953975, "recall-0.25": 0.974895, @@ -1247,29 +1247,29 @@ Results of the model in various experiments on different datasets. \ }, "wrist_right": { "count": 1455, - "mean": 0.026926, - "median": 0.016801, - "std": 0.033427, - "sem": 0.000877, + "mean": 0.026992, + "median": 0.016745, + "std": 0.033905, + "sem": 0.000889, "min": 0.001361, "max": 0.280646, - "recall-0.025": 0.690934, - "recall-0.05": 0.888736, + "recall-0.025": 0.692995, + "recall-0.05": 0.888049, "recall-0.1": 0.964973, - "recall-0.15": 0.980769, - "recall-0.25": 0.997253, + "recall-0.15": 0.980082, + "recall-0.25": 0.996566, "recall-0.5": 0.999313, "num_labels": 1456 }, "hip_left": { "count": 1461, - "mean": 0.034771, - "median": 0.031898, - "std": 0.019216, + "mean": 0.034766, + "median": 0.031916, + "std": 0.019208, "sem": 0.000503, "min": 0.00101, "max": 0.181992, - "recall-0.025": 0.32628, + "recall-0.025": 0.324915, "recall-0.05": 0.845051, "recall-0.1": 0.989761, "recall-0.15": 0.995904, @@ -1279,13 +1279,13 @@ Results of the model in various experiments on different datasets. \ }, "hip_right": { "count": 1462, - "mean": 0.037383, - "median": 0.032592, - "std": 0.024222, - "sem": 0.000634, + "mean": 0.037408, + "median": 0.032634, + "std": 0.024199, + "sem": 0.000633, "min": 0.002509, "max": 0.281736, - "recall-0.025": 0.318554, + "recall-0.025": 0.316508, "recall-0.05": 0.802183, "recall-0.1": 0.976126, "recall-0.15": 0.993861, @@ -1295,15 +1295,15 @@ Results of the model in various experiments on different datasets. \ }, "knee_left": { "count": 1461, - "mean": 0.038795, - "median": 0.032848, - "std": 0.034462, - "sem": 0.000902, + "mean": 0.038815, + "median": 0.032849, + "std": 0.034505, + "sem": 0.000903, "min": 0.003309, "max": 0.473605, "recall-0.025": 0.287372, "recall-0.05": 0.799317, - "recall-0.1": 0.978157, + "recall-0.1": 0.976792, "recall-0.15": 0.9843, "recall-0.25": 0.990444, "recall-0.5": 0.99727, @@ -1311,13 +1311,13 @@ Results of the model in various experiments on different datasets. \ }, "knee_right": { "count": 1455, - "mean": 0.038488, - "median": 0.031543, - "std": 0.026577, + "mean": 0.038477, + "median": 0.031533, + "std": 0.026575, "sem": 0.000697, "min": 0.003512, "max": 0.275123, - "recall-0.025": 0.349554, + "recall-0.025": 0.35024, "recall-0.05": 0.749829, "recall-0.1": 0.964359, "recall-0.15": 0.993146, @@ -1327,50 +1327,50 @@ Results of the model in various experiments on different datasets. \ }, "ankle_left": { "count": 1458, - "mean": 0.056122, - "median": 0.034021, - "std": 0.062141, - "sem": 0.001628, + "mean": 0.056359, + "median": 0.034166, + "std": 0.062188, + "sem": 0.001629, "min": 0.003035, "max": 0.432301, "recall-0.025": 0.347915, - "recall-0.05": 0.666439, - "recall-0.1": 0.855776, - "recall-0.15": 0.917977, + "recall-0.05": 0.663021, + "recall-0.1": 0.853725, + "recall-0.15": 0.91661, "recall-0.25": 0.971292, "recall-0.5": 0.996582, "num_labels": 1463 }, "ankle_right": { "count": 1445, - "mean": 0.053755, - "median": 0.030905, - "std": 0.067212, - "sem": 0.001769, + "mean": 0.054031, + "median": 0.030928, + "std": 0.067536, + "sem": 0.001777, "min": 0.001698, "max": 0.489965, - "recall-0.025": 0.378082, - "recall-0.05": 0.736301, - "recall-0.1": 0.856849, - "recall-0.15": 0.903425, - "recall-0.25": 0.962329, + "recall-0.025": 0.376027, + "recall-0.05": 0.734247, + "recall-0.1": 0.856164, + "recall-0.15": 0.90274, + "recall-0.25": 0.961644, "recall-0.5": 0.989726, "num_labels": 1460 }, "joint_recalls": { "num_labels": 18990, - "recall-0.025": 0.57341, - "recall-0.05": 0.85408, - "recall-0.1": 0.9584, - "recall-0.15": 0.9772, - "recall-0.25": 0.99005, + "recall-0.025": 0.57325, + "recall-0.05": 0.8535, + "recall-0.1": 0.95793, + "recall-0.15": 0.97694, + "recall-0.25": 0.98994, "recall-0.5": 0.99674 } } { "total_parts": 20444, - "correct_parts": 20204, - "pcp": 0.988261 + "correct_parts": 20203, + "pcp": 0.988212 } ``` diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index eaca842..b6babe4 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -49,6 +49,48 @@ // ================================================================================================= +[[maybe_unused]] static void print_2d_poses(const std::vector> &poses) +{ + std::cout << "Poses: (" << poses.size() << ", 3)[" << std::endl; + for (const auto &pose : poses) + { + std::cout << " ["; + for (size_t i = 0; i < 3; ++i) + { + std::cout << std::fixed << std::setprecision(3) << pose[i]; + if (i < 2) + { + std::cout << ", "; + } + } + std::cout << "]" << std::endl; + } + std::cout << "]" << std::endl; +} + +// ================================================================================================= + +[[maybe_unused]] static void print_3d_poses(const std::vector> &poses) +{ + std::cout << "Poses: (" << poses.size() << ", 4)[" << std::endl; + for (const auto &pose : poses) + { + std::cout << " ["; + for (size_t i = 0; i < 4; ++i) + { + std::cout << std::fixed << std::setprecision(3) << pose[i]; + if (i < 3) + { + std::cout << ", "; + } + } + std::cout << "]" << std::endl; + } + std::cout << "]" << std::endl; +} + +// ================================================================================================= + [[maybe_unused]] static void print_allpairs( const std::vector, std::pair>> &all_pairs) { @@ -116,9 +158,9 @@ std::vector>> TriangulatorInternal::triangulate const std::array, 2> &roomparams, const std::vector &joint_names) { - auto ttime = std::chrono::high_resolution_clock::now(); - auto stime = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed; + auto ttime = std::chrono::steady_clock::now(); + auto stime = std::chrono::steady_clock::now(); + std::chrono::duration elapsed; // Check inputs if (poses_2d.empty()) @@ -142,33 +184,7 @@ std::vector>> TriangulatorInternal::triangulate } // Convert inputs to internal formats - std::vector> poses_2d_mats; - poses_2d_mats.reserve(cameras.size()); - for (size_t i = 0; i < cameras.size(); ++i) - { - size_t num_persons = poses_2d[i].size(); - size_t num_joints = poses_2d[i][0].size(); - std::vector camera_poses; - camera_poses.reserve(num_persons); - - for (size_t j = 0; j < num_persons; ++j) - { - std::vector dims = {(int)num_joints, 3}; - cv::Mat pose_mat = cv::Mat(dims, CV_32F); - - // Use pointer to copy data efficiently - for (size_t k = 0; k < num_joints; ++k) - { - float *mat_ptr = pose_mat.ptr(k); - const auto &joint = poses_2d[i][j][k]; - mat_ptr[0] = joint[0]; - mat_ptr[1] = joint[1]; - mat_ptr[2] = joint[2]; - } - camera_poses.push_back(std::move(pose_mat)); - } - poses_2d_mats.push_back(std::move(camera_poses)); - } + std::vector>>> i_poses_2d = poses_2d; std::vector internal_cameras; for (size_t i = 0; i < cameras.size(); ++i) { @@ -189,46 +205,45 @@ std::vector>> TriangulatorInternal::triangulate (size_t)std::distance(core_joints.begin(), it2)}); } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; init_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Undistort 2D poses #pragma omp parallel for for (size_t i = 0; i < cameras.size(); ++i) { - undistort_poses(poses_2d_mats[i], internal_cameras[i]); + undistort_poses(i_poses_2d[i], internal_cameras[i]); internal_cameras[i].update_projection_matrix(); } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; undistort_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Project last 3D poses to 2D - std::vector, std::vector>> last_poses_2d; + std::vector>>, + std::vector>>> + last_poses_2d; if (!last_poses_3d.empty()) { // Select core joints - std::vector last_core_poses; + std::vector>> last_core_poses; last_core_poses.resize(last_poses_3d.size()); - #pragma omp parallel for for (size_t i = 0; i < last_poses_3d.size(); ++i) { - cv::Mat &pose = last_poses_3d[i]; - std::vector dims = {(int)core_joint_idx.size(), 4}; - cv::Mat last_poses_core(dims, pose.type()); - + last_core_poses[i].resize(core_joint_idx.size()); for (size_t j = 0; j < core_joint_idx.size(); ++j) { - pose.row(core_joint_idx[j]).copyTo(last_poses_core.row(j)); + for (size_t k = 0; k < 4; ++k) + { + last_core_poses[i][j][k] = last_poses_3d[i][core_joint_idx[j]][k]; + } } - last_core_poses[i] = last_poses_core; } // Project last_poses_2d.resize(cameras.size()); - #pragma omp parallel for for (size_t i = 0; i < cameras.size(); ++i) { auto [poses2d, dists] = project_poses(last_core_poses, internal_cameras[i], true); @@ -236,9 +251,9 @@ std::vector>> TriangulatorInternal::triangulate } } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; project_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Check matches to old poses float threshold = min_match_score - 0.2; @@ -254,7 +269,7 @@ std::vector>> TriangulatorInternal::triangulate for (size_t j = 0; j < num_last_persons; ++j) { - size_t num_new_persons = poses_2d_mats[i].size(); + size_t num_new_persons = poses_2d[i].size(); scored_pasts[i][j] = std::vector(); for (size_t k = 0; k < num_new_persons; ++k) @@ -266,7 +281,7 @@ std::vector>> TriangulatorInternal::triangulate std::vector> indices_ik; for (size_t i = 0; i < cameras.size(); ++i) { - size_t num_new_persons = poses_2d_mats[i].size(); + size_t num_new_persons = poses_2d[i].size(); for (size_t k = 0; k < num_new_persons; ++k) { indices_ik.push_back({i, k}); @@ -274,50 +289,45 @@ std::vector>> TriangulatorInternal::triangulate } // Precalculate core poses - std::vector poses_2d_mats_core_list; - poses_2d_mats_core_list.resize(indices_ik.size()); + std::vector>> poses_2d_core_list; + poses_2d_core_list.resize(indices_ik.size()); std::vector> mats_core_map; mats_core_map.resize(cameras.size()); for (size_t i = 0; i < cameras.size(); ++i) { - size_t num_new_persons = poses_2d_mats[i].size(); + size_t num_new_persons = poses_2d[i].size(); for (size_t k = 0; k < num_new_persons; ++k) { mats_core_map[i].push_back(0); } } - #pragma omp parallel for for (size_t e = 0; e < indices_ik.size(); ++e) { const auto [i, k] = indices_ik[e]; - - const cv::Mat &pose = poses_2d_mats[i][k]; - std::vector dims = {(int)core_joint_idx.size(), 3}; - cv::Mat pose_core(dims, pose.type()); - + const auto &pose = i_poses_2d[i][k]; + std::vector> pose_core; + pose_core.resize(core_joint_idx.size()); for (size_t j = 0; j < core_joint_idx.size(); ++j) { - pose.row(core_joint_idx[j]).copyTo(pose_core.row(j)); + size_t idx = core_joint_idx[j]; + pose_core[j] = pose[idx]; } - - poses_2d_mats_core_list[e] = pose_core; + poses_2d_core_list[e] = pose_core; mats_core_map[i][k] = e; } // Calculate matching score - #pragma omp parallel for for (size_t e = 0; e < indices_ijk.size(); ++e) { const auto [i, j, k] = indices_ijk[e]; - const cv::Mat &last_pose = std::get<0>(last_poses_2d[i])[j]; - const cv::Mat &last_dist = std::get<1>(last_poses_2d[i])[j]; - const cv::Mat &new_pose = poses_2d_mats_core_list[mats_core_map[i][k]]; + const auto &last_pose = std::get<0>(last_poses_2d[i])[j]; + const auto &last_dist = std::get<1>(last_poses_2d[i])[j]; + const auto &new_pose = poses_2d_core_list[mats_core_map[i][k]]; float score = calc_pose_score(new_pose, last_pose, last_dist, internal_cameras[i]); if (score > threshold) { - #pragma omp critical { scored_pasts[i][j].push_back(k); } @@ -325,9 +335,9 @@ std::vector>> TriangulatorInternal::triangulate } } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; match_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Create pairs of persons // Checks if the person was already matched to the last frame and if so only creates pairs @@ -357,7 +367,6 @@ std::vector>> TriangulatorInternal::triangulate } } } - #pragma omp parallel for ordered schedule(dynamic) for (size_t e = 0; e < indices.size(); ++e) { const auto [i, j, k, l] = indices[e]; @@ -380,8 +389,6 @@ std::vector>> TriangulatorInternal::triangulate match = true; auto item = std::make_pair( std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2)); - - #pragma omp ordered all_pairs.push_back(item); } else if (in_smi || in_smj) @@ -395,19 +402,16 @@ std::vector>> TriangulatorInternal::triangulate { auto item = std::make_pair( std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2)); - - // Needed to prevent randomized grouping/merging with slightly different results - #pragma omp ordered all_pairs.push_back(item); } } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; pairs_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Calculate pair scores - std::vector> all_scored_poses; + std::vector>, float>> all_scored_poses; all_scored_poses.resize(all_pairs.size()); #pragma omp parallel for for (size_t i = 0; i < all_pairs.size(); ++i) @@ -419,29 +423,29 @@ std::vector>> TriangulatorInternal::triangulate const auto &cam2 = internal_cameras[std::get<1>(pids)]; // Extract 2D poses - const cv::Mat &pose1 = poses_2d_mats[std::get<0>(pids)][std::get<2>(pids)]; - const cv::Mat &pose2 = poses_2d_mats[std::get<1>(pids)][std::get<3>(pids)]; + const auto &pose1 = i_poses_2d[std::get<0>(pids)][std::get<2>(pids)]; + const auto &pose2 = i_poses_2d[std::get<1>(pids)][std::get<3>(pids)]; // Select core joints - std::vector dims = {(int)core_joint_idx.size(), 3}; - cv::Mat pose1_core(dims, pose1.type()); - cv::Mat pose2_core(dims, pose2.type()); + std::vector> pose1_core, pose2_core; + pose1_core.resize(core_joint_idx.size()); + pose2_core.resize(core_joint_idx.size()); for (size_t j = 0; j < core_joint_idx.size(); ++j) { size_t idx = core_joint_idx[j]; - pose1.row(idx).copyTo(pose1_core.row(j)); - pose2.row(idx).copyTo(pose2_core.row(j)); + pose1_core[j] = pose1[idx]; + pose2_core[j] = pose2[idx]; } // Triangulate and score auto [pose3d, score] = triangulate_and_score( pose1_core, pose2_core, cam1, cam2, roomparams, core_limbs_idx); - all_scored_poses[i] = std::make_pair(pose3d, score); + all_scored_poses[i] = std::move(std::make_pair(pose3d, score)); } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; pair_scoring_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Drop low scoring poses size_t num_poses = all_scored_poses.size(); @@ -455,8 +459,9 @@ std::vector>> TriangulatorInternal::triangulate } // Group pairs that share a person - std::vector>> groups; - groups = calc_grouping(all_pairs, all_scored_poses, min_match_score); + std::vector, std::vector>, std::vector>> + groups = calc_grouping(all_pairs, all_scored_poses, min_match_score); // Drop groups with too few matches size_t num_groups = groups.size(); @@ -468,12 +473,12 @@ std::vector>> TriangulatorInternal::triangulate } } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; grouping_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Calculate full 3D poses - std::vector all_full_poses; + std::vector>> all_full_poses; all_full_poses.resize(all_pairs.size()); #pragma omp parallel for for (size_t i = 0; i < all_pairs.size(); ++i) @@ -481,26 +486,25 @@ std::vector>> TriangulatorInternal::triangulate const auto &pids = all_pairs[i].first; const auto &cam1 = internal_cameras[std::get<0>(pids)]; const auto &cam2 = internal_cameras[std::get<1>(pids)]; - const auto &pose1 = poses_2d_mats[std::get<0>(pids)][std::get<2>(pids)]; - const auto &pose2 = poses_2d_mats[std::get<1>(pids)][std::get<3>(pids)]; + const auto &pose1 = i_poses_2d[std::get<0>(pids)][std::get<2>(pids)]; + const auto &pose2 = i_poses_2d[std::get<1>(pids)][std::get<3>(pids)]; auto [pose3d, score] = triangulate_and_score( pose1, pose2, cam1, cam2, roomparams, {}); - all_full_poses[i] = (pose3d); + all_full_poses[i] = std::move(pose3d); } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; full_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Merge groups - std::vector all_merged_poses; + std::vector>> all_merged_poses; all_merged_poses.resize(groups.size()); - #pragma omp parallel for for (size_t i = 0; i < groups.size(); ++i) { const auto &group = groups[i]; - std::vector poses; + std::vector>> poses; poses.reserve(std::get<2>(group).size()); for (const auto &idx : std::get<2>(group)) @@ -509,42 +513,42 @@ std::vector>> TriangulatorInternal::triangulate } auto merged_pose = merge_group(poses, min_match_score); - all_merged_poses[i] = (merged_pose); + all_merged_poses[i] = std::move(merged_pose); } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; merge_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Run post-processing steps - std::vector final_poses_3d = all_merged_poses; + std::vector>> final_poses_3d = std::move(all_merged_poses); add_extra_joints(final_poses_3d, joint_names); filter_poses(final_poses_3d, roomparams, core_joint_idx, core_limbs_idx); add_missing_joints(final_poses_3d, joint_names); + + // Store final result for use in the next frame. last_poses_3d = final_poses_3d; - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; post_time += elapsed.count(); - stime = std::chrono::high_resolution_clock::now(); + stime = std::chrono::steady_clock::now(); // Convert to output format std::vector>> poses_3d; poses_3d.reserve(final_poses_3d.size()); for (size_t i = 0; i < final_poses_3d.size(); ++i) { - const auto &mat = final_poses_3d[i]; + const size_t num_joints = final_poses_3d[i].size(); std::vector> pose; - size_t num_joints = mat.rows; pose.reserve(num_joints); size_t num_valid = 0; for (size_t j = 0; j < num_joints; ++j) { - const float *mat_ptr = mat.ptr(j); std::array point; for (size_t k = 0; k < 4; ++k) { - point[k] = mat_ptr[k]; + point[k] = final_poses_3d[i][j][k]; } pose.push_back(point); @@ -560,10 +564,10 @@ std::vector>> TriangulatorInternal::triangulate } } - elapsed = std::chrono::high_resolution_clock::now() - stime; + elapsed = std::chrono::steady_clock::now() - stime; convert_time += elapsed.count(); - elapsed = std::chrono::high_resolution_clock::now() - ttime; + elapsed = std::chrono::steady_clock::now() - ttime; total_time += elapsed.count(); num_calls++; @@ -600,7 +604,8 @@ void TriangulatorInternal::print_stats() // ================================================================================================= -void TriangulatorInternal::undistort_poses(std::vector &poses, CameraInternal &icam) +void TriangulatorInternal::undistort_poses( + std::vector>> &poses_2d, CameraInternal &icam) { int width = icam.cam.width; int height = icam.cam.height; @@ -619,6 +624,27 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); } + // Convert vectors to mats + size_t num_persons = poses_2d.size(); + size_t num_joints = poses_2d[0].size(); + std::vector poses; + for (size_t j = 0; j < num_persons; ++j) + { + std::vector dims = {(int)num_joints, 3}; + cv::Mat pose_mat = cv::Mat(dims, CV_32F); + + // Use pointer to copy data efficiently + for (size_t k = 0; k < num_joints; ++k) + { + float *mat_ptr = pose_mat.ptr(k); + const auto &joint = poses_2d[j][k]; + mat_ptr[0] = joint[0]; + mat_ptr[1] = joint[1]; + mat_ptr[2] = joint[2]; + } + poses.push_back(std::move(pose_mat)); + } + for (size_t p = 0; p < poses.size(); ++p) { // Extract the (x, y) coordinates @@ -658,138 +684,168 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn } } + // Convert mats to vectors + poses_2d.clear(); + poses_2d.reserve(poses.size()); + for (size_t i = 0; i < poses.size(); ++i) + { + std::vector> pose; + size_t num_joints = poses[i].rows; + pose.reserve(num_joints); + + for (size_t j = 0; j < num_joints; ++j) + { + const float *mat_ptr = poses[i].ptr(j); + std::array point; + for (size_t k = 0; k < 3; ++k) + { + point[k] = mat_ptr[k]; + } + pose.push_back(point); + } + + poses_2d.push_back(std::move(pose)); + } + // 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); + } + } 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}; } } // ================================================================================================= -std::tuple, std::vector> TriangulatorInternal::project_poses( - const std::vector &bodies3D, const CameraInternal &icam, bool calc_dists) +std::tuple>>, std::vector>> +TriangulatorInternal::project_poses( + const std::vector>> &poses_3d, + const CameraInternal &icam, + bool calc_dists) { - size_t num_persons = bodies3D.size(); - size_t num_joints = bodies3D[0].rows; + const size_t num_persons = poses_3d.size(); + const size_t num_joints = poses_3d[0].size(); - std::vector bodies2D_list(num_persons); - std::vector dists_list(num_persons); + // Prepare output vectors + std::vector>> poses_2d; + std::vector> all_dists; + poses_2d.resize(num_persons); + all_dists.resize(num_persons); - cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints).t(); - cv::Mat R_transposed = icam.R.t(); + // Get camera parameters + const std::array, 3> &K = icam.cam.K; + const std::array, 3> &R = icam.cam.R; + const std::array, 3> &T = icam.cam.T; for (size_t i = 0; i < num_persons; ++i) { - const cv::Mat &body3D = bodies3D[i]; + const std::vector> &body3D = poses_3d[i]; + std::vector> body2D(num_joints, std::array{0.0, 0.0, 0.0}); + std::vector dists(num_joints, 0.0); - // Extract coordinates - const cv::Mat &points3d = body3D.colRange(0, 3); - - // Project from world to camera coordinate system - cv::Mat xyz = (points3d - T_repeated) * R_transposed; - - // Set points behind the camera to zero for (size_t j = 0; j < num_joints; ++j) { - float *xyz_row_ptr = xyz.ptr(j); - float z = xyz_row_ptr[2]; - if (z <= 0) + float x = body3D[j][0]; + float y = body3D[j][1]; + float z = body3D[j][2]; + float score = body3D[j][3]; + + // Project from world to camera coordinate system + float xt = x - T[0][0]; + float yt = y - T[1][0]; + float zt = z - T[2][0]; + float x_cam = xt * R[0][0] + yt * R[0][1] + zt * R[0][2]; + float y_cam = xt * R[1][0] + yt * R[1][1] + zt * R[1][2]; + float z_cam = xt * R[2][0] + yt * R[2][1] + zt * R[2][2]; + + // Set points behind the camera to zero + if (z_cam <= 0.0) { - xyz_row_ptr[0] = 0.0; - xyz_row_ptr[1] = 0.0; - xyz_row_ptr[2] = 0.0; + x_cam = y_cam = z_cam = 0.0; } - } - // Calculate distance from camera center if required - cv::Mat dists; - if (calc_dists) - { - cv::multiply(xyz, xyz, dists); - cv::reduce(dists, dists, 1, cv::REDUCE_SUM, CV_32F); - cv::sqrt(dists, dists); - } - else - { - dists = cv::Mat::zeros(num_joints, 1, CV_32F); - } + // Calculate distance from camera center if required + if (calc_dists) + dists[j] = std::sqrt(x_cam * x_cam + y_cam * y_cam + z_cam * z_cam); + else + dists[j] = 0.0; - // Project points to image plane - // Since images are already undistorted, use the standard projection for all camera types - cv::Mat uv; - cv::projectPoints( - xyz, cv::Mat::zeros(3, 1, CV_32F), cv::Mat::zeros(3, 1, CV_32F), - icam.K, cv::Mat::zeros(5, 1, CV_32F), uv); - uv = uv.reshape(1, {xyz.rows, 2}); - - // Add scores again - std::vector dimsB = {(int)num_joints, 3}; - cv::Mat bodies2D = cv::Mat(dimsB, CV_32F); - for (size_t j = 0; j < num_joints; ++j) - { - float *bodies2D_row_ptr = bodies2D.ptr(j); - const float *uv_row_ptr = uv.ptr(j); - const float *bodies3D_row_ptr = body3D.ptr(j); - - bodies2D_row_ptr[0] = uv_row_ptr[0]; - bodies2D_row_ptr[1] = uv_row_ptr[1]; - bodies2D_row_ptr[2] = bodies3D_row_ptr[3]; - } - - // Filter invalid projections - for (size_t j = 0; j < num_joints; ++j) - { - float *bodies2D_row_ptr = bodies2D.ptr(j); - float x = bodies2D_row_ptr[0]; - float y = bodies2D_row_ptr[1]; - bool in_x = x >= 0.0 && x < icam.cam.width; - bool in_y = y >= 0.0 && y < icam.cam.height; - if (!in_x || !in_y) + // Project points to image plane + // Since images are already undistorted, use the pinhole projection for all camera types + float u = 0.0, v = 0.0; + if (z_cam > 0.0) { - bodies2D_row_ptr[0] = 0.0; - bodies2D_row_ptr[1] = 0.0; - bodies2D_row_ptr[2] = 0.0; + u = (K[0][0] * x_cam + K[0][1] * y_cam + K[0][2] * z_cam) / z_cam; + v = (K[1][0] * x_cam + K[1][1] * y_cam + K[1][2] * z_cam) / z_cam; } + + // Filter invalid projections + if (u < 0.0 || u >= icam.cam.width || v < 0.0 || v >= icam.cam.height) + { + u = 0.0; + v = 0.0; + score = 0.0; + } + + body2D[j] = {u, v, score}; } // Store results - bodies2D_list[i] = bodies2D; - dists_list[i] = dists; + poses_2d[i] = std::move(body2D); + all_dists[i] = std::move(dists); } - return std::make_tuple(bodies2D_list, dists_list); + return std::make_tuple(poses_2d, all_dists); } // ================================================================================================= float TriangulatorInternal::calc_pose_score( - const cv::Mat &pose1, - const cv::Mat &pose2, - const cv::Mat &dist1, + const std::vector> &pose1, + const std::vector> &pose2, + const std::vector &dist1, const CameraInternal &icam) { const float min_score = 0.1; + const size_t num_joints = pose1.size(); // Create mask for valid points - size_t num_joints = pose1.rows; - cv::Mat mask(num_joints, 1, CV_8U); + std::vector mask; + mask.resize(num_joints); for (size_t i = 0; i < num_joints; ++i) { - u_char *mask_ptr = mask.ptr(i); - const float *pose1_ptr = pose1.ptr(i); - const float *pose2_ptr = pose2.ptr(i); - - mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score); + if (pose1[i][2] <= min_score || pose2[i][2] <= min_score) + { + mask[i] = false; + } + else + { + mask[i] = true; + } } // Drop if not enough valid points - int valid_count = cv::countNonZero(mask); + int valid_count = 0; + for (size_t i = 0; i < num_joints; ++i) + { + if (mask[i]) + { + valid_count++; + } + } if (valid_count < 3) { return 0.0; @@ -797,20 +853,18 @@ float TriangulatorInternal::calc_pose_score( // Calculate scores float iscale = (icam.cam.width + icam.cam.height) / 2; - cv::Mat scores = score_projection(pose1, pose2, dist1, mask, iscale); + std::vector scores = score_projection(pose1, pose2, dist1, mask, iscale); // Drop lowest scores - size_t drop_k = static_cast(pose1.rows * 0.2); + size_t drop_k = static_cast(num_joints * 0.2); const size_t min_k = 3; std::vector scores_vec; scores_vec.reserve(valid_count); - for (int i = 0; i < scores.rows; ++i) + for (size_t i = 0; i < scores.size(); ++i) { - const float *scores_ptr = scores.ptr(i); - const u_char *mask_ptr = mask.ptr(i); - if (mask_ptr[0] > 0) + if (mask[i] > 0) { - scores_vec.push_back(scores_ptr[0]); + scores_vec.push_back(scores[i]); } } size_t scores_size = scores_vec.size(); @@ -835,39 +889,32 @@ float TriangulatorInternal::calc_pose_score( // ================================================================================================= -cv::Mat TriangulatorInternal::score_projection( - const cv::Mat &pose, - const cv::Mat &repro, - const cv::Mat &dists, - const cv::Mat &mask, +std::vector TriangulatorInternal::score_projection( + const std::vector> &pose, + const std::vector> &repro, + const std::vector &dists, + const std::vector &mask, float iscale) { const float min_score = 0.1; - const size_t num_joints = pose.rows; + const size_t num_joints = pose.size(); // Calculate error - cv::Mat error = cv::Mat::zeros(num_joints, 1, CV_32F); + std::vector error(num_joints, 0.0); for (size_t i = 0; i < num_joints; ++i) { - const u_char *mask_ptr = mask.ptr(i); - const float *pose_ptr = pose.ptr(i); - const float *repro_ptr = repro.ptr(i); - float *error_ptr = error.ptr(i); - - if (mask_ptr) + if (mask[i]) { - float dx = pose_ptr[0] - repro_ptr[0]; - float dy = pose_ptr[1] - repro_ptr[1]; + float dx = pose[i][0] - repro[i][0]; + float dy = pose[i][1] - repro[i][1]; float err = std::sqrt(dx * dx + dy * dy); // Set errors of invisible reprojections to a high value - float score = repro_ptr[2]; - if (score < min_score) + if (repro[i][2] < min_score) { err = iscale; } - - error_ptr[0] = err; + error[i] = err; } } @@ -876,11 +923,9 @@ cv::Mat TriangulatorInternal::score_projection( const float iscale_quarter = iscale / 4.0; for (size_t i = 0; i < num_joints; ++i) { - float *error_ptr = error.ptr(i); - - float err = error_ptr[0]; + float err = error[i]; err = std::max(0.0f, std::min(err, iscale_quarter)) * inv_iscale; - error_ptr[0] = err; + error[i] = err; } // Scale error by distance to camera @@ -888,13 +933,10 @@ cv::Mat TriangulatorInternal::score_projection( int count = 0; for (size_t i = 0; i < num_joints; ++i) { - const u_char *mask_ptr = mask.ptr(i); - const float *dists_ptr = dists.ptr(i); - - if (mask_ptr) + if (mask[i]) { - mean_dist += dists_ptr[0]; - count++; + mean_dist += dists[i]; + ++count; } } if (count > 0) @@ -904,19 +946,14 @@ cv::Mat TriangulatorInternal::score_projection( const float dscale = std::sqrt(mean_dist / 3.5); for (size_t i = 0; i < num_joints; ++i) { - float *error_ptr = error.ptr(i); - - float err = error_ptr[0]; - err *= dscale; - error_ptr[0] = err; + error[i] *= dscale; } // Convert error to score - cv::Mat score = error; + std::vector score(num_joints, 0.0); for (size_t i = 0; i < num_joints; ++i) { - float *score_ptr = score.ptr(i); - score_ptr[0] = 1.0 / (1.0 + score_ptr[0] * 10.0); + score[i] = 1.0 / (1.0 + error[i] * 10.0); } return score; @@ -924,81 +961,105 @@ cv::Mat TriangulatorInternal::score_projection( // ================================================================================================= -std::pair TriangulatorInternal::triangulate_and_score( - const cv::Mat &pose1, - const cv::Mat &pose2, +std::pair>, float> TriangulatorInternal::triangulate_and_score( + const std::vector> &pose1, + const std::vector> &pose2, const CameraInternal &cam1, const CameraInternal &cam2, const std::array, 2> &roomparams, const std::vector> &core_limbs_idx) { const float min_score = 0.1; - const size_t num_joints = pose1.rows; + const size_t num_joints = pose1.size(); // Create mask for valid points - cv::Mat mask(num_joints, 1, CV_8U); + std::vector mask; + mask.resize(num_joints); for (size_t i = 0; i < num_joints; ++i) { - u_char *mask_ptr = mask.ptr(i); - const float *pose1_ptr = pose1.ptr(i); - const float *pose2_ptr = pose2.ptr(i); - - mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score); - } - - // If too few joints are visible, return a low score - int num_visible = cv::countNonZero(mask); - if (num_visible < 3) - { - cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_32F); - float score = 0.0; - return std::make_pair(pose3d, score); - } - - // Extract coordinates of visible joints - std::vector dims = {2, num_visible}; - cv::Mat points1 = cv::Mat(dims, CV_32F); - cv::Mat points2 = cv::Mat(dims, CV_32F); - int idx = 0; - float *points1_ptr = points1.ptr(0); - float *points2_ptr = points2.ptr(0); - for (size_t i = 0; i < num_joints; ++i) - { - const u_char *mask_ptr = mask.ptr(i); - const float *pose1_ptr = pose1.ptr(i); - const float *pose2_ptr = pose2.ptr(i); - - if (mask_ptr[0]) + if (pose1[i][2] <= min_score || pose2[i][2] <= min_score) { - points1_ptr[idx + 0 * num_visible] = pose1_ptr[0]; - points1_ptr[idx + 1 * num_visible] = pose1_ptr[1]; - points2_ptr[idx + 0 * num_visible] = pose2_ptr[0]; - points2_ptr[idx + 1 * num_visible] = pose2_ptr[1]; - idx++; + mask[i] = false; + } + else + { + mask[i] = true; } } - // Triangulate points - cv::Mat points4d_h, points3d; - cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points4d_h); - cv::convertPointsFromHomogeneous(points4d_h.t(), points3d); - - // Create the 3D pose matrix - cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_32F); - idx = 0; + // If too few joints are visible, return a low score + int num_visible = 0; for (size_t i = 0; i < num_joints; ++i) { - const u_char *mask_ptr = mask.ptr(i); - float *pose3d_ptr = pose3d.ptr(i); - const float *points3d_ptr = points3d.ptr(idx); - - if (mask_ptr[0]) + if (mask[i]) { - pose3d_ptr[0] = points3d_ptr[0]; - pose3d_ptr[1] = points3d_ptr[1]; - pose3d_ptr[2] = points3d_ptr[2]; - pose3d_ptr[3] = 1.0; - idx++; + num_visible++; + } + } + if (num_visible < 3) + { + std::vector> empty(num_joints, {0.0, 0.0, 0.0, 0.0}); + float score = 0.0; + return std::make_pair(empty, score); + } + + // Extract coordinates of visible joints + std::vector> points1; + std::vector> points2; + points1.reserve(num_visible); + points2.reserve(num_visible); + for (size_t i = 0; i < num_joints; ++i) + { + if (mask[i]) + { + points1.push_back({pose1[i][0], pose1[i][1]}); + points2.push_back({pose2[i][0], pose2[i][1]}); + } + } + + // Convert vectors to mats + cv::Mat points1_mat(2, num_visible, CV_32F); + cv::Mat points2_mat(2, num_visible, CV_32F); + float *p1_ptr = points1_mat.ptr(0); + float *p2_ptr = points2_mat.ptr(0); + for (int i = 0; i < num_visible; ++i) + { + p1_ptr[i + 0 * num_visible] = points1[i][0]; + p1_ptr[i + 1 * num_visible] = points1[i][1]; + p2_ptr[i + 0 * num_visible] = points2[i][0]; + p2_ptr[i + 1 * num_visible] = points2[i][1]; + } + + // Triangulate points + cv::Mat points4d_h; + cv::triangulatePoints(cam1.P, cam2.P, points1_mat, points2_mat, points4d_h); + + // Convert homogeneous coordinates to 3D + std::vector> points_3d; + points_3d.reserve(num_visible); + const float *p4_ptr = points4d_h.ptr(0); + for (int i = 0; i < points4d_h.cols; ++i) + { + float w = p4_ptr[i + 3 * num_visible]; + std::array pt = { + p4_ptr[i + 0 * num_visible] / w, + p4_ptr[i + 1 * num_visible] / w, + p4_ptr[i + 2 * num_visible] / w}; + points_3d.push_back(std::move(pt)); + } + + // Create the 3D pose + std::vector> pose3d(num_joints, {0.0, 0.0, 0.0, 0.0}); + int idx = 0; + for (size_t i = 0; i < num_joints; ++i) + { + if (mask[i]) + { + pose3d[i][0] = points_3d[idx][0]; + pose3d[i][1] = points_3d[idx][1]; + pose3d[i][2] = points_3d[idx][2]; + pose3d[i][3] = 1.0; + ++idx; } } @@ -1006,14 +1067,11 @@ std::pair TriangulatorInternal::triangulate_and_score( std::array mean = {0.0, 0.0, 0.0}; for (size_t i = 0; i < num_joints; ++i) { - const u_char *mask_ptr = mask.ptr(i); - const float *pose3d_ptr = pose3d.ptr(i); - - if (mask_ptr[0]) + if (mask[i]) { - mean[0] += pose3d_ptr[0]; - mean[1] += pose3d_ptr[1]; - mean[2] += pose3d_ptr[2]; + mean[0] += pose3d[i][0]; + mean[1] += pose3d[i][1]; + mean[2] += pose3d[i][2]; } } float inv_num_vis = 1.0 / num_visible; @@ -1021,51 +1079,46 @@ std::pair TriangulatorInternal::triangulate_and_score( { mean[j] *= inv_num_vis; } - const std::array &size = roomparams[0]; - const std::array ¢er = roomparams[1]; + const std::array &room_size = roomparams[0]; + const std::array &room_center = roomparams[1]; for (int j = 0; j < 3; ++j) { - if (mean[j] > center[j] + size[j] / 2.0 || - mean[j] < center[j] - size[j] / 2.0) + if (mean[j] > room_center[j] + room_size[j] / 2.0 || + mean[j] < room_center[j] - room_size[j] / 2.0) { // Very low score if outside room for (size_t i = 0; i < num_joints; ++i) { - float *pose3d_ptr = pose3d.ptr(i); - pose3d_ptr[3] = 0.001; + if (mask[i]) + { + pose3d[i][3] = 0.001; + } } - return {pose3d, 0.001}; + return std::make_pair(pose3d, 0.001); } } // Calculate reprojections - std::vector poses_3d = {pose3d}; - cv::Mat repro1, dists1, repro2, dists2; + std::vector>> poses_3d = {pose3d}; auto [repro1s, dists1s] = project_poses(poses_3d, cam1, true); auto [repro2s, dists2s] = project_poses(poses_3d, cam2, true); - repro1 = repro1s[0]; - dists1 = dists1s[0]; - repro2 = repro2s[0]; - dists2 = dists2s[0]; + auto repro1 = repro1s[0]; + auto dists1 = dists1s[0]; + auto repro2 = repro2s[0]; + auto dists2 = dists2s[0]; // Calculate scores for each view float iscale = (cam1.cam.width + cam1.cam.height) / 2.0; - cv::Mat score1 = score_projection(pose1, repro1, dists1, mask, iscale); - cv::Mat score2 = score_projection(pose2, repro2, dists2, mask, iscale); - - // Combine scores - cv::Mat scores = (score1 + score2) * 0.5; + std::vector score1 = score_projection(pose1, repro1, dists1, mask, iscale); + std::vector score2 = score_projection(pose2, repro2, dists2, mask, iscale); // Add scores to 3D pose for (size_t i = 0; i < num_joints; ++i) { - const u_char *mask_ptr = mask.ptr(i); - float *pose3d_ptr = pose3d.ptr(i); - const float *scores_ptr = scores.ptr(i); - - if (mask_ptr[0]) + if (mask[i]) { - pose3d_ptr[3] = scores_ptr[0]; + float score = 0.5 * (score1[i] + score2[i]); + pose3d[i][3] = score; } } @@ -1073,17 +1126,14 @@ std::pair TriangulatorInternal::triangulate_and_score( const float wdist = 0.1; for (size_t i = 0; i < num_joints; ++i) { - const u_char *mask_ptr = mask.ptr(i); - float *pose3d_ptr = pose3d.ptr(i); - - if (mask_ptr[0]) + if (mask[i]) { for (int j = 0; j < 3; ++j) { - if (pose3d_ptr[j] > center[j] + size[j] / 2.0 + wdist || - pose3d_ptr[j] < center[j] - size[j] / 2.0 - wdist) + if (pose3d[i][j] > room_center[j] + room_size[j] / 2.0 + wdist || + pose3d[i][j] < room_center[j] - room_size[j] / 2.0 - wdist) { - pose3d_ptr[3] = 0.001; + pose3d[i][3] = 0.001; break; } } @@ -1098,19 +1148,17 @@ std::pair TriangulatorInternal::triangulate_and_score( { size_t limb1 = core_limbs_idx[i][0]; size_t limb2 = core_limbs_idx[i][1]; - float *pose3d_ptr1 = pose3d.ptr(limb1); - float *pose3d_ptr2 = pose3d.ptr(limb2); - if (pose3d_ptr1[3] > min_score && pose3d_ptr2[3] > min_score) + if (pose3d[limb1][3] > min_score && pose3d[limb2][3] > min_score) { - float dx = pose3d_ptr1[0] - pose3d_ptr2[0]; - float dy = pose3d_ptr1[1] - pose3d_ptr2[1]; - float dz = pose3d_ptr1[2] - pose3d_ptr2[2]; + float dx = pose3d[limb1][0] - pose3d[limb2][0]; + float dy = pose3d[limb1][1] - pose3d[limb2][1]; + float dz = pose3d[limb1][2] - pose3d[limb2][2]; float length_sq = dx * dx + dy * dy + dz * dz; if (length_sq > max_length_sq) { - pose3d_ptr2[3] = 0.001; + pose3d[limb2][3] = 0.001; } } } @@ -1119,40 +1167,39 @@ std::pair TriangulatorInternal::triangulate_and_score( // Drop lowest scores size_t drop_k = static_cast(num_joints * 0.2); const size_t min_k = 3; - std::vector scores_vec; + std::vector valid_scores; for (size_t i = 0; i < num_joints; ++i) { - const float *pose3d_ptr = pose3d.ptr(i); - if (pose3d_ptr[3] > min_score) + if (pose3d[i][3] > min_score) { - scores_vec.push_back(pose3d_ptr[3]); + valid_scores.push_back(pose3d[i][3]); } } - size_t scores_size = scores_vec.size(); + size_t scores_size = valid_scores.size(); if (scores_size >= min_k) { drop_k = std::min(drop_k, scores_size - min_k); - std::partial_sort(scores_vec.begin(), scores_vec.begin() + drop_k, scores_vec.end()); - scores_vec.erase(scores_vec.begin(), scores_vec.begin() + drop_k); + std::partial_sort(valid_scores.begin(), valid_scores.begin() + drop_k, valid_scores.end()); + valid_scores.erase(valid_scores.begin(), valid_scores.begin() + drop_k); } // Calculate final score - float score = 0.0; - size_t n_items = scores_vec.size(); - if (n_items > 0) + float final_score = 0.0; + if (!valid_scores.empty()) { - float sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); - score = sum_scores / static_cast(n_items); + float sum_scores = std::accumulate(valid_scores.begin(), valid_scores.end(), 0.0); + final_score = sum_scores / static_cast(valid_scores.size()); } - return std::make_pair(pose3d, score); + return std::make_pair(pose3d, final_score); } // ================================================================================================= -std::vector>> TriangulatorInternal::calc_grouping( +std::vector, std::vector>, std::vector>> +TriangulatorInternal::calc_grouping( const std::vector, std::pair>> &all_pairs, - const std::vector> &all_scored_poses, + const std::vector>, float>> &all_scored_poses, float min_score) { float max_center_dist_sq = 0.6 * 0.6; @@ -1160,83 +1207,77 @@ std::vector>> TriangulatorInte size_t num_pairs = all_pairs.size(); // Calculate pose centers - std::vector centers; + std::vector> centers; centers.resize(num_pairs); for (size_t i = 0; i < num_pairs; ++i) { - const cv::Mat &pose_3d = all_scored_poses[i].first; - size_t num_joints = pose_3d.rows; + const auto &pose_3d = all_scored_poses[i].first; + const size_t num_joints = pose_3d.size(); - cv::Point3f center(0, 0, 0); + std::array center = {0.0, 0.0, 0.0}; size_t num_valid = 0; for (size_t j = 0; j < num_joints; ++j) { - const float *pose_3d_ptr = pose_3d.ptr(j); - - float score = pose_3d_ptr[3]; + float score = pose_3d[j][3]; if (score > min_score) { - center.x += pose_3d_ptr[0]; - center.y += pose_3d_ptr[1]; - center.z += pose_3d_ptr[2]; - num_valid++; + center[0] += pose_3d[j][0]; + center[1] += pose_3d[j][1]; + center[2] += pose_3d[j][2]; + ++num_valid; } } if (num_valid > 0) { - float inv_num_valid = 1.0 / num_valid; - center.x *= inv_num_valid; - center.y *= inv_num_valid; - center.z *= inv_num_valid; + float inv = 1.0 / num_valid; + center[0] *= inv; + center[1] *= inv; + center[2] *= inv; } centers[i] = center; } // Calculate Groups // defined as a tuple of center, pose, and all-pairs-indices of members - std::vector>> groups; + std::vector, std::vector>, std::vector>> groups; std::vector> per_group_visible_counts; for (size_t i = 0; i < num_pairs; ++i) { - const cv::Mat &pose_3d = all_scored_poses[i].first; - size_t num_joints = pose_3d.rows; + const auto &pose_3d = all_scored_poses[i].first; + size_t num_joints = pose_3d.size(); - const cv::Point3f ¢er = centers[i]; + const std::array ¢er = centers[i]; float best_dist = std::numeric_limits::infinity(); int best_group = -1; for (size_t j = 0; j < groups.size(); ++j) { auto &group = groups[j]; - cv::Point3f &group_center = std::get<0>(group); + std::array &group_center = std::get<0>(group); // Check if the center is close enough - float dx = group_center.x - center.x; - float dy = group_center.y - center.y; - float dz = group_center.z - center.z; + float dx = group_center[0] - center[0]; + float dy = group_center[1] - center[1]; + float dz = group_center[2] - center[2]; float center_dist_sq = dx * dx + dy * dy + dz * dz; if (center_dist_sq < max_center_dist_sq) { - cv::Mat &group_pose = std::get<1>(group); + const auto &group_pose = std::get<1>(group); // Calculate average joint distance std::vector dists; for (size_t row = 0; row < num_joints; ++row) { - const float *pose_3d_ptr = pose_3d.ptr(row); - const float *group_pose_ptr = group_pose.ptr(row); - - float score1 = pose_3d_ptr[3]; - float score2 = group_pose_ptr[3]; - + float score1 = pose_3d[row][3]; + float score2 = group_pose[row][3]; if (score1 > min_score && score2 > min_score) { - float dx = pose_3d_ptr[0] - group_pose_ptr[0]; - float dy = pose_3d_ptr[1] - group_pose_ptr[1]; - float dz = pose_3d_ptr[2] - group_pose_ptr[2]; - float dist_sq = dx * dx + dy * dy + dz * dz; - dists.push_back(std::sqrt(dist_sq)); + float dx = pose_3d[row][0] - group_pose[row][0]; + float dy = pose_3d[row][1] - group_pose[row][1]; + float dz = pose_3d[row][2] - group_pose[row][2]; + float dist = std::sqrt(dx * dx + dy * dy + dz * dz); + dists.push_back(dist); } } if (dists.size() >= 5) @@ -1263,14 +1304,14 @@ std::vector>> TriangulatorInte { // Create a new group std::vector new_indices{static_cast(i)}; - groups.emplace_back(center, pose_3d.clone(), std::move(new_indices)); + std::vector> group_pose = pose_3d; + groups.emplace_back(center, group_pose, new_indices); // Update per joint counts std::vector new_valid_joint_counts(num_joints, 0); for (size_t row = 0; row < num_joints; ++row) { - const float *pose_3d_ptr = pose_3d.ptr(row); - if (pose_3d_ptr[3] > min_score) + if (pose_3d[row][3] > min_score) { new_valid_joint_counts[row] = 1; } @@ -1281,33 +1322,30 @@ std::vector>> TriangulatorInte { // Update existing group auto &group = groups[best_group]; - cv::Point3f &group_center = std::get<0>(group); - cv::Mat &group_pose = std::get<1>(group); + std::array &group_center = std::get<0>(group); + std::vector> &group_pose = std::get<1>(group); std::vector &group_indices = std::get<2>(group); float n_elems = static_cast(group_indices.size()); - float inv_n1 = 1.0 / (n_elems + 1.0); + float inv_n = 1.0 / (n_elems + 1.0); // Update group center - group_center.x = (group_center.x * n_elems + center.x) * inv_n1; - group_center.y = (group_center.y * n_elems + center.y) * inv_n1; - group_center.z = (group_center.z * n_elems + center.z) * inv_n1; + group_center[0] = (group_center[0] * n_elems + center[0]) * inv_n; + group_center[1] = (group_center[1] * n_elems + center[1]) * inv_n; + group_center[2] = (group_center[2] * n_elems + center[2]) * inv_n; // Update group pose for (size_t row = 0; row < num_joints; ++row) { - const float *pose_3d_ptr = pose_3d.ptr(row); - float *group_pose_ptr = group_pose.ptr(row); - - if (pose_3d_ptr[3] > min_score) + if (pose_3d[row][3] > min_score) { float j_elems = static_cast(per_group_visible_counts[best_group][row]); - float inv_j1 = 1.0 / (j_elems + 1.0); + float inv_j = 1.0 / (j_elems + 1.0); - group_pose_ptr[0] = (group_pose_ptr[0] * j_elems + pose_3d_ptr[0]) * inv_j1; - group_pose_ptr[1] = (group_pose_ptr[1] * j_elems + pose_3d_ptr[1]) * inv_j1; - group_pose_ptr[2] = (group_pose_ptr[2] * j_elems + pose_3d_ptr[2]) * inv_j1; - group_pose_ptr[3] = (group_pose_ptr[3] * j_elems + pose_3d_ptr[3]) * inv_j1; + group_pose[row][0] = (group_pose[row][0] * j_elems + pose_3d[row][0]) * inv_j; + group_pose[row][1] = (group_pose[row][1] * j_elems + pose_3d[row][1]) * inv_j; + group_pose[row][2] = (group_pose[row][2] * j_elems + pose_3d[row][2]) * inv_j; + group_pose[row][3] = (group_pose[row][3] * j_elems + pose_3d[row][3]) * inv_j; per_group_visible_counts[best_group][row]++; } } @@ -1319,10 +1357,12 @@ std::vector>> TriangulatorInte // Depending on the inital group creation, one or more groups can be created that in the end // share the same persons, even if they had a larger distance at the beginning // So merge them similar to the group assignment before - std::vector>> merged_groups; + std::vector, std::vector>, std::vector>> + merged_groups; for (size_t i = 0; i < groups.size(); ++i) { - size_t num_joints = std::get<1>(groups[i]).rows; + size_t num_joints = std::get<1>(groups[i]).size(); auto &group = groups[i]; auto &group_visible_counts = per_group_visible_counts[i]; @@ -1331,25 +1371,22 @@ std::vector>> TriangulatorInte for (size_t j = 0; j < merged_groups.size(); ++j) { - auto &merged_group = merged_groups[j]; + const auto &group_pose = std::get<1>(group); + const auto &merged_pose = std::get<1>(merged_groups[j]); // Calculate average joint distance std::vector dists; for (size_t row = 0; row < num_joints; ++row) { - const float *group_pose_ptr = std::get<1>(group).ptr(row); - const float *merged_pose_ptr = std::get<1>(merged_group).ptr(row); - - float score1 = group_pose_ptr[3]; - float score2 = merged_pose_ptr[3]; - + float score1 = group_pose[row][3]; + float score2 = merged_pose[row][3]; if (score1 > min_score && score2 > min_score) { - float dx = group_pose_ptr[0] - merged_pose_ptr[0]; - float dy = group_pose_ptr[1] - merged_pose_ptr[1]; - float dz = group_pose_ptr[2] - merged_pose_ptr[2]; - float dist_sq = dx * dx + dy * dy + dz * dz; - dists.push_back(std::sqrt(dist_sq)); + float dx = group_pose[row][0] - merged_pose[row][0]; + float dy = group_pose[row][1] - merged_pose[row][1]; + float dz = group_pose[row][2] - merged_pose[row][2]; + float dist = std::sqrt(dx * dx + dy * dy + dz * dz); + dists.push_back(dist); } } if (dists.size() >= 5) @@ -1380,8 +1417,8 @@ std::vector>> TriangulatorInte { // Update existing group auto &merged_group = merged_groups[best_group]; - cv::Point3f &merged_center = std::get<0>(merged_group); - cv::Mat &merged_group_pose = std::get<1>(merged_group); + std::array &merged_center = std::get<0>(merged_group); + std::array &group_center = std::get<0>(group); std::vector &merged_group_indices = std::get<2>(merged_group); float n_elems1 = static_cast(merged_group_indices.size()); @@ -1390,27 +1427,27 @@ std::vector>> TriangulatorInte float inv2 = n_elems2 / (n_elems1 + n_elems2); // Update group center - merged_center.x = (merged_center.x * inv1 + std::get<0>(group).x * inv2); - merged_center.y = (merged_center.y * inv1 + std::get<0>(group).y * inv2); - merged_center.z = (merged_center.z * inv1 + std::get<0>(group).z * inv2); + merged_center[0] = merged_center[0] * inv1 + group_center[0] * inv2; + merged_center[1] = merged_center[1] * inv1 + group_center[1] * inv2; + merged_center[2] = merged_center[2] * inv1 + group_center[2] * inv2; // Update group pose for (size_t row = 0; row < num_joints; ++row) { - const float *group_pose_ptr = std::get<1>(group).ptr(row); - float *merged_pose_ptr = merged_group_pose.ptr(row); + auto &group_pose = std::get<1>(group); + auto &merged_pose = std::get<1>(merged_group); - if (group_pose_ptr[3] > min_score) + if (group_pose[row][3] > min_score) { float j_elems1 = static_cast(group_visible_counts[row]); float j_elems2 = static_cast(per_group_visible_counts[best_group][row]); float inv1 = j_elems1 / (j_elems1 + j_elems2); float inv2 = j_elems2 / (j_elems1 + j_elems2); - merged_pose_ptr[0] = (merged_pose_ptr[0] * inv1 + group_pose_ptr[0] * inv2); - merged_pose_ptr[1] = (merged_pose_ptr[1] * inv1 + group_pose_ptr[1] * inv2); - merged_pose_ptr[2] = (merged_pose_ptr[2] * inv1 + group_pose_ptr[2] * inv2); - merged_pose_ptr[3] = (merged_pose_ptr[3] * inv1 + group_pose_ptr[3] * inv2); + merged_pose[row][0] = merged_pose[row][0] * inv1 + group_pose[row][0] * inv2; + merged_pose[row][1] = merged_pose[row][1] * inv1 + group_pose[row][1] * inv2; + merged_pose[row][2] = merged_pose[row][2] * inv1 + group_pose[row][2] * inv2; + merged_pose[row][3] = merged_pose[row][3] * inv1 + group_pose[row][3] * inv2; group_visible_counts[row]++; } } @@ -1426,179 +1463,171 @@ std::vector>> TriangulatorInte // ================================================================================================= -cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, float min_score) +std::vector> TriangulatorInternal::merge_group( + const std::vector>> &poses_3d, + float min_score) { - int num_poses = poses_3d.size(); - int num_joints = poses_3d[0].rows; + size_t num_poses = poses_3d.size(); + size_t num_joints = poses_3d[0].size(); // Merge poses to create initial pose // Use only those triangulations with a high score - cv::Mat sum_poses = cv::Mat::zeros(num_joints, 4, CV_32F); - std::vector sum_mask(num_joints, 0); - for (int i = 0; i < num_poses; ++i) + std::vector> sum_poses(num_joints, {0.0, 0.0, 0.0, 0.0}); + std::vector sum_mask1(num_joints, 0); + for (size_t i = 0; i < num_poses; ++i) { - const cv::Mat &pose = poses_3d[i]; + const auto &pose = poses_3d[i]; - for (int j = 0; j < num_joints; ++j) + for (size_t j = 0; j < num_joints; ++j) { - const float *pose_ptr = pose.ptr(j); - float *sum_ptr = sum_poses.ptr(j); - - float score = pose_ptr[3]; + float score = pose[j][3]; if (score > min_score) { - sum_ptr[0] += pose_ptr[0]; - sum_ptr[1] += pose_ptr[1]; - sum_ptr[2] += pose_ptr[2]; - sum_ptr[3] += pose_ptr[3]; - sum_mask[j]++; + sum_poses[j][0] += pose[j][0]; + sum_poses[j][1] += pose[j][1]; + sum_poses[j][2] += pose[j][2]; + sum_poses[j][3] += score; + sum_mask1[j]++; } } } - cv::Mat initial_pose_3d = cv::Mat::zeros(num_joints, 4, CV_32F); - for (int j = 0; j < num_joints; ++j) + std::vector> initial_pose_3d(num_joints, {0.0, 0.0, 0.0, 0.0}); + for (size_t j = 0; j < num_joints; ++j) { - if (sum_mask[j] > 0) + if (sum_mask1[j] > 0) { - float *initial_ptr = initial_pose_3d.ptr(j); - const float *sum_ptr = sum_poses.ptr(j); - - float inv_count = 1.0 / sum_mask[j]; - initial_ptr[0] = sum_ptr[0] * inv_count; - initial_ptr[1] = sum_ptr[1] * inv_count; - initial_ptr[2] = sum_ptr[2] * inv_count; - initial_ptr[3] = sum_ptr[3] * inv_count; + float inv = 1.0 / sum_mask1[j]; + initial_pose_3d[j][0] = sum_poses[j][0] * inv; + initial_pose_3d[j][1] = sum_poses[j][1] * inv; + initial_pose_3d[j][2] = sum_poses[j][2] * inv; + initial_pose_3d[j][3] = sum_poses[j][3] * inv; } } // Use center as default if the initial pose is empty - std::vector jmask(num_joints, 0); - cv::Point3f center(0, 0, 0); + std::vector jmask(num_joints, false); + std::array center = {0.0, 0.0, 0.0}; int valid_joints = 0; - for (int j = 0; j < num_joints; ++j) + for (size_t j = 0; j < num_joints; ++j) { - const float *initial_ptr = initial_pose_3d.ptr(j); - float score = initial_ptr[3]; - + float score = initial_pose_3d[j][3]; if (score > min_score) { - jmask[j] = 1; - center.x += initial_ptr[0]; - center.y += initial_ptr[1]; - center.z += initial_ptr[2]; + jmask[j] = true; + center[0] += initial_pose_3d[j][0]; + center[1] += initial_pose_3d[j][1]; + center[2] += initial_pose_3d[j][2]; valid_joints++; } } if (valid_joints > 0) { - center *= 1.0 / valid_joints; + float inv_valid = 1.0 / valid_joints; + center[0] *= inv_valid; + center[1] *= inv_valid; + center[2] *= inv_valid; } - for (int j = 0; j < num_joints; ++j) + for (size_t j = 0; j < num_joints; ++j) { if (!jmask[j]) { - float *initial_ptr = initial_pose_3d.ptr(j); - initial_ptr[0] = center.x; - initial_ptr[1] = center.y; - initial_ptr[2] = center.z; + initial_pose_3d[j][0] = center[0]; + initial_pose_3d[j][1] = center[1]; + initial_pose_3d[j][2] = center[2]; + initial_pose_3d[j][3] = 0.0; } } // Drop joints with low scores and filter outlying joints using distance threshold float offset = 0.1; float max_dist_sq = 1.2 * 1.2; - cv::Mat mask = cv::Mat::zeros(num_poses, num_joints, CV_8U); - cv::Mat distances = cv::Mat::zeros(num_poses, num_joints, CV_32F); - for (int i = 0; i < num_poses; ++i) + std::vector> mask(num_poses, std::vector(num_joints, false)); + std::vector> distances(num_poses, std::vector(num_joints, 0.0)); + for (size_t i = 0; i < num_poses; ++i) { - float *distances_ptr = distances.ptr(i); - u_char *mask_ptr = mask.ptr(i); - const cv::Mat &pose = poses_3d[i]; - - for (int j = 0; j < num_joints; ++j) + const auto &pose = poses_3d[i]; + for (size_t j = 0; j < num_joints; ++j) { - const float *initial_ptr = initial_pose_3d.ptr(j); - const float *pose_ptr = pose.ptr(j); - - float dx = pose_ptr[0] - initial_ptr[0]; - float dy = pose_ptr[1] - initial_ptr[1]; - float dz = pose_ptr[2] - initial_ptr[2]; + float dx = pose[j][0] - initial_pose_3d[j][0]; + float dy = pose[j][1] - initial_pose_3d[j][1]; + float dz = pose[j][2] - initial_pose_3d[j][2]; float dist_sq = dx * dx + dy * dy + dz * dz; - distances_ptr[j] = dist_sq; + distances[i][j] = dist_sq; - float score = pose_ptr[3]; + float score = pose[j][3]; if (dist_sq <= max_dist_sq && score > (min_score - offset)) { - mask_ptr[j] = 1; + mask[i][j] = true; } } } // Select the best-k proposals for each joint that are closest to the initial pose - int keep_best = std::max(3, num_poses / 3); - cv::Mat best_k_mask = cv::Mat::zeros(num_poses, num_joints, CV_8U); - for (int j = 0; j < num_joints; ++j) + int keep_best = std::max(3, (int)num_poses / 3); + std::vector> best_k_mask(num_poses, std::vector(num_joints, false)); + for (size_t j = 0; j < num_joints; ++j) { std::vector> valid_indices; valid_indices.reserve(num_poses); - for (int i = 0; i < num_poses; ++i) + for (size_t i = 0; i < num_poses; ++i) { - if (mask.at(i, j)) + if (mask[i][j]) { - valid_indices.emplace_back(distances.at(i, j), i); + valid_indices.emplace_back(distances[i][j], i); } } - std::partial_sort( - valid_indices.begin(), - valid_indices.begin() + std::min(keep_best, static_cast(valid_indices.size())), - valid_indices.end()); - - for (int k = 0; k < std::min(keep_best, static_cast(valid_indices.size())); ++k) + int num_valid = std::min(keep_best, static_cast(valid_indices.size())); + if (num_valid > 0) { - best_k_mask.at(valid_indices[k].second, j) = 1; + std::partial_sort(valid_indices.begin(), valid_indices.begin() + num_valid, + valid_indices.end()); + + for (int k = 0; k < num_valid; ++k) + { + int idx = valid_indices[k].second; + best_k_mask[idx][j] = true; + } } } // Combine masks - mask &= best_k_mask; + for (size_t i = 0; i < num_poses; ++i) + { + for (size_t j = 0; j < num_joints; ++j) + { + mask[i][j] = mask[i][j] && best_k_mask[i][j]; + } + } // Compute the final pose - sum_poses = cv::Mat::zeros(num_joints, 4, CV_32F); - sum_mask.assign(num_joints, 0); - for (int i = 0; i < num_poses; ++i) + std::vector> sum_poses2(num_joints, {0.0, 0.0, 0.0, 0.0}); + std::vector sum_mask2(num_joints, 0); + for (size_t i = 0; i < num_poses; ++i) { - const u_char *mask_row_ptr = mask.ptr(i); - const cv::Mat &pose = poses_3d[i]; + const auto &pose = poses_3d[i]; - for (int j = 0; j < num_joints; ++j) + for (size_t j = 0; j < num_joints; ++j) { - if (mask_row_ptr[j]) + if (mask[i][j]) { - float *sum_ptr = sum_poses.ptr(j); - const float *pose_ptr = pose.ptr(j); - - sum_ptr[0] += pose_ptr[0]; - sum_ptr[1] += pose_ptr[1]; - sum_ptr[2] += pose_ptr[2]; - sum_ptr[3] += pose_ptr[3]; - - sum_mask[j]++; + sum_poses2[j][0] += pose[j][0]; + sum_poses2[j][1] += pose[j][1]; + sum_poses2[j][2] += pose[j][2]; + sum_poses2[j][3] += pose[j][3]; + sum_mask2[j]++; } } } - cv::Mat final_pose_3d = cv::Mat::zeros(num_joints, 4, CV_32F); - for (int j = 0; j < num_joints; ++j) + std::vector> final_pose_3d(num_joints, {0.0, 0.0, 0.0, 0.0}); + for (size_t j = 0; j < num_joints; ++j) { - if (sum_mask[j] > 0) + if (sum_mask2[j] > 0) { - float *final_pose_ptr = final_pose_3d.ptr(j); - const float *sum_ptr = sum_poses.ptr(j); - - float inv_count = 1.0 / sum_mask[j]; - final_pose_ptr[0] = sum_ptr[0] * inv_count; - final_pose_ptr[1] = sum_ptr[1] * inv_count; - final_pose_ptr[2] = sum_ptr[2] * inv_count; - final_pose_ptr[3] = sum_ptr[3] * inv_count; + float inv = 1.0 / sum_mask2[j]; + final_pose_3d[j][0] = sum_poses2[j][0] * inv; + final_pose_3d[j][1] = sum_poses2[j][1] * inv; + final_pose_3d[j][2] = sum_poses2[j][2] * inv; + final_pose_3d[j][3] = sum_poses2[j][3] * inv; } } @@ -1608,7 +1637,8 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, // ================================================================================================= void TriangulatorInternal::add_extra_joints( - std::vector &poses, const std::vector &joint_names) + std::vector>> &poses, + const std::vector &joint_names) { // Find indices of "head", "ear_left", and "ear_right" auto it_head = std::find(joint_names.begin(), joint_names.end(), "head"); @@ -1621,20 +1651,15 @@ void TriangulatorInternal::add_extra_joints( // If the confidence score of "head" is zero compute it as the average of the ears for (size_t i = 0; i < poses.size(); ++i) { - cv::Mat &pose = poses[i]; - float *pose_ptr_h = pose.ptr(idx_h); - - if (pose_ptr_h[3] == 0.0f) + auto &pose = poses[i]; + if (pose[idx_h][3] == 0.0) { - float *pose_ptr_el = pose.ptr(idx_el); - float *pose_ptr_er = pose.ptr(idx_er); - - if (pose_ptr_el[3] > 0.0f && pose_ptr_er[3] > 0.0f) + if (pose[idx_el][3] > 0.0 && pose[idx_er][3] > 0.0) { - pose_ptr_h[0] = (pose_ptr_el[0] + pose_ptr_er[0]) * 0.5f; - pose_ptr_h[1] = (pose_ptr_el[1] + pose_ptr_er[1]) * 0.5f; - pose_ptr_h[2] = (pose_ptr_el[2] + pose_ptr_er[2]) * 0.5f; - pose_ptr_h[3] = std::min(pose_ptr_el[3], pose_ptr_er[3]); + pose[idx_h][0] = (pose[idx_el][0] + pose[idx_er][0]) * 0.5; + pose[idx_h][1] = (pose[idx_el][1] + pose[idx_er][1]) * 0.5; + pose[idx_h][2] = (pose[idx_el][2] + pose[idx_er][2]) * 0.5; + pose[idx_h][3] = std::min(pose[idx_el][3], pose[idx_er][3]); } } } @@ -1643,7 +1668,7 @@ void TriangulatorInternal::add_extra_joints( // ================================================================================================= void TriangulatorInternal::filter_poses( - std::vector &poses, + std::vector>> &poses, const std::array, 2> &roomparams, const std::vector &core_joint_idx, const std::vector> &core_limbs_idx) @@ -1653,15 +1678,14 @@ void TriangulatorInternal::filter_poses( for (size_t i = 0; i < poses.size(); ++i) { - cv::Mat &pose = poses[i]; - const size_t num_joints = pose.rows; + auto &pose = poses[i]; + size_t num_joints = pose.size(); // Collect valid joint indices std::vector valid_joint_idx; for (size_t j = 0; j < num_joints; ++j) { - float *pose_ptr = pose.ptr(j); - if (pose_ptr[3] > min_score) + if (pose[j][3] > min_score) { valid_joint_idx.push_back(j); } @@ -1686,12 +1710,12 @@ void TriangulatorInternal::filter_poses( std::numeric_limits::lowest()}; for (size_t j = 0; j < valid_joint_idx.size(); ++j) { - float *joint_ptr = pose.ptr(valid_joint_idx[j]); + size_t idx = valid_joint_idx[j]; for (size_t k = 0; k < 3; ++k) { - mins[k] = std::min(mins[k], joint_ptr[k]); - maxs[k] = std::max(maxs[k], joint_ptr[k]); - mean[k] += joint_ptr[k]; + mins[k] = std::min(mins[k], pose[idx][k]); + maxs[k] = std::max(maxs[k], pose[idx][k]); + mean[k] += pose[idx][k]; } } for (size_t k = 0; k < 3; ++k) @@ -1700,9 +1724,9 @@ void TriangulatorInternal::filter_poses( } // Drop poses that are too large or too small or too flat - float min_flatness = 0.2f; - float max_size = 2.3f; - float min_size = 0.3f; + float min_flatness = 0.2; + float max_size = 2.3; + float min_size = 0.3; std::array diff; for (int j = 0; j < 3; ++j) { @@ -1727,7 +1751,7 @@ void TriangulatorInternal::filter_poses( } // Drop persons outside room - const float wdist = 0.1f; + const float wdist = 0.1; const auto &room_size = roomparams[0]; const auto &room_center = roomparams[1]; const std::array room_half_size = { @@ -1762,26 +1786,23 @@ void TriangulatorInternal::filter_poses( } // Calculate total limb length and average limb length - const float max_avg_length = 0.5f; - const float min_avg_length = 0.1f; - float total_length = 0.0f; + const float max_avg_length = 0.5; + const float min_avg_length = 0.1; + float total_length = 0.0; int total_limbs = 0; for (size_t j = 0; j < core_limbs_idx.size(); ++j) { size_t start_idx = core_joint_idx[core_limbs_idx[j][0]]; size_t end_idx = core_joint_idx[core_limbs_idx[j][1]]; - float *joint_start_ptr = pose.ptr(start_idx); - float *joint_end_ptr = pose.ptr(end_idx); - - if (joint_start_ptr[3] < min_score || joint_end_ptr[3] < min_score) + if (pose[start_idx][3] < min_score || pose[end_idx][3] < min_score) { continue; } - float dx = joint_end_ptr[0] - joint_start_ptr[0]; - float dy = joint_end_ptr[1] - joint_start_ptr[1]; - float dz = joint_end_ptr[2] - joint_start_ptr[2]; + float dx = pose[end_idx][0] - pose[start_idx][0]; + float dy = pose[end_idx][1] - pose[start_idx][1]; + float dz = pose[end_idx][2] - pose[start_idx][2]; float length = std::sqrt(dx * dx + dy * dy + dz * dz); total_length += length; @@ -1808,11 +1829,10 @@ void TriangulatorInternal::filter_poses( // Set confidences of invalid poses to a low value for (size_t i = 0; i < drop_indices.size(); ++i) { - cv::Mat &pose = poses[drop_indices[i]]; - for (int j = 0; j < pose.rows; ++j) + auto &pose = poses[drop_indices[i]]; + for (size_t j = 0; j < pose.size(); ++j) { - float *joint_ptr = pose.ptr(j); - joint_ptr[3] = 0.001f; + pose[j][3] = 0.001; } } } @@ -1820,7 +1840,8 @@ void TriangulatorInternal::filter_poses( // ================================================================================================= void TriangulatorInternal::add_missing_joints( - std::vector &poses, const std::vector &joint_names) + std::vector>> &poses, + const std::vector &joint_names) { // Map joint names to their indices for quick lookup std::unordered_map joint_name_to_idx; @@ -1853,15 +1874,14 @@ void TriangulatorInternal::add_missing_joints( for (size_t i = 0; i < poses.size(); ++i) { - cv::Mat &pose = poses[i]; - const size_t num_joints = pose.rows; + auto &pose = poses[i]; + size_t num_joints = pose.size(); // Collect valid joint indices std::vector valid_joint_idx; for (size_t j = 0; j < num_joints; ++j) { - float *pose_ptr = pose.ptr(j); - if (pose_ptr[3] > min_match_score) + if (pose[j][3] > min_match_score) { valid_joint_idx.push_back(j); } @@ -1873,13 +1893,12 @@ void TriangulatorInternal::add_missing_joints( } // Compute body center as the mean of valid joints - std::array body_center = {0.0f, 0.0f, 0.0f}; + std::array body_center = {0.0, 0.0, 0.0}; for (size_t idx : valid_joint_idx) { - float *joint_ptr = pose.ptr(idx); - body_center[0] += joint_ptr[0]; - body_center[1] += joint_ptr[1]; - body_center[2] += joint_ptr[2]; + body_center[0] += pose[idx][0]; + body_center[1] += pose[idx][1]; + body_center[2] += pose[idx][2]; } for (int j = 0; j < 3; ++j) { @@ -1924,13 +1943,12 @@ void TriangulatorInternal::add_missing_joints( continue; } - float *joint_ptr = pose.ptr(j); - if (joint_ptr[3] == 0.0f) + if (pose[j][3] == 0.0) { if (adjacents.find(adname) != adjacents.end()) { // Joint is missing; attempt to estimate its position based on adjacent joints - std::array adjacent_position = {0.0f, 0.0f, 0.0f}; + std::array adjacent_position = {0.0, 0.0, 0.0}; size_t adjacent_count = 0; const std::vector &adjacent_joint_names = adjacents[adname]; @@ -1940,13 +1958,12 @@ void TriangulatorInternal::add_missing_joints( if (it != joint_name_to_idx.end()) { size_t adj_idx = it->second; - float *adj_joint_ptr = pose.ptr(adj_idx); - if (adj_joint_ptr[3] > 0.1f) + if (adj_idx < pose.size() && pose[adj_idx][3] > 0.1) { - adjacent_position[0] += adj_joint_ptr[0]; - adjacent_position[1] += adj_joint_ptr[1]; - adjacent_position[2] += adj_joint_ptr[2]; - adjacent_count++; + adjacent_position[0] += pose[adj_idx][0]; + adjacent_position[1] += pose[adj_idx][1]; + adjacent_position[2] += pose[adj_idx][2]; + ++adjacent_count; } } } @@ -1959,28 +1976,28 @@ void TriangulatorInternal::add_missing_joints( } // Update the missing joint's position - joint_ptr[0] = adjacent_position[0]; - joint_ptr[1] = adjacent_position[1]; - joint_ptr[2] = adjacent_position[2]; + pose[j][0] = adjacent_position[0]; + pose[j][1] = adjacent_position[1]; + pose[j][2] = adjacent_position[2]; } else { // No valid adjacent joints, use body center - joint_ptr[0] = body_center[0]; - joint_ptr[1] = body_center[1]; - joint_ptr[2] = body_center[2]; + pose[j][0] = body_center[0]; + pose[j][1] = body_center[1]; + pose[j][2] = body_center[2]; } } else { // Adjacent joints not defined, use body center - joint_ptr[0] = body_center[0]; - joint_ptr[1] = body_center[1]; - joint_ptr[2] = body_center[2]; + pose[j][0] = body_center[0]; + pose[j][1] = body_center[1]; + pose[j][2] = body_center[2]; } // Set a low confidence score - joint_ptr[3] = 0.1f; + pose[j][3] = 0.1; } } } diff --git a/rpt/triangulator.hpp b/rpt/triangulator.hpp index 8e5be45..5ce940d 100644 --- a/rpt/triangulator.hpp +++ b/rpt/triangulator.hpp @@ -70,64 +70,76 @@ private: {"shoulder_left", "elbow_left"}, {"shoulder_right", "elbow_right"}, }; - std::vector last_poses_3d; + std::vector>> last_poses_3d; - void undistort_poses(std::vector &poses, CameraInternal &icam); + void undistort_poses( + std::vector>> &poses_2d, CameraInternal &icam); - std::tuple, std::vector> project_poses( - const std::vector &bodies3D, const CameraInternal &icam, bool calc_dists); + std::tuple>>, std::vector>> + project_poses( + const std::vector>> &poses_3d, + const CameraInternal &icam, + bool calc_dists); float calc_pose_score( - const cv::Mat &pose1, - const cv::Mat &pose2, - const cv::Mat &dist1, + const std::vector> &pose1, + const std::vector> &pose2, + const std::vector &dist1, const CameraInternal &icam); - cv::Mat score_projection( - const cv::Mat &pose1, - const cv::Mat &repro1, - const cv::Mat &dists1, - const cv::Mat &mask, + std::vector score_projection( + const std::vector> &pose, + const std::vector> &repro, + const std::vector &dists, + const std::vector &mask, float iscale); - std::pair triangulate_and_score( - const cv::Mat &pose1, - const cv::Mat &pose2, + std::pair>, float> triangulate_and_score( + const std::vector> &pose1, + const std::vector> &pose2, const CameraInternal &cam1, const CameraInternal &cam2, const std::array, 2> &roomparams, const std::vector> &core_limbs_idx); - std::vector>> calc_grouping( - const std::vector, std::pair>> &all_pairs, - const std::vector> &all_scored_poses, + std::vector, std::vector>, std::vector>> + calc_grouping( + const std::vector, std::pair>> &all_pairs, + const std::vector>, float>> &all_scored_poses, float min_score); - cv::Mat merge_group(const std::vector &poses_3d, float min_score); + std::vector> merge_group( + const std::vector>> &poses_3d, + float min_score); - void add_extra_joints(std::vector &poses, const std::vector &joint_names); + void add_extra_joints( + std::vector>> &poses, + const std::vector &joint_names); void filter_poses( - std::vector &poses, + std::vector>> &poses, const std::array, 2> &roomparams, const std::vector &core_joint_idx, const std::vector> &core_limbs_idx); void add_missing_joints( - std::vector &poses, const std::vector &joint_names); + std::vector>> &poses, + const std::vector &joint_names); // Statistics - float num_calls = 0; - float total_time = 0; - float init_time = 0; - float undistort_time = 0; - float project_time = 0; - float match_time = 0; - float pairs_time = 0; - float pair_scoring_time = 0; - float grouping_time = 0; - float full_time = 0; - float merge_time = 0; - float post_time = 0; - float convert_time = 0; + double num_calls = 0; + double total_time = 0; + double init_time = 0; + double undistort_time = 0; + double project_time = 0; + double match_time = 0; + double pairs_time = 0; + double pair_scoring_time = 0; + double grouping_time = 0; + double full_time = 0; + double merge_time = 0; + double post_time = 0; + double convert_time = 0; };