From 57cf91ae95713f312549b5d7bc72509def701b68 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 16 Sep 2024 17:57:46 +0200 Subject: [PATCH] Improving triangulation speed. --- media/RESULTS.md | 6 +- spt/triangulator.cpp | 481 +++++++++++++++++++++++++------------------ spt/triangulator.hpp | 1 - 3 files changed, 279 insertions(+), 209 deletions(-) diff --git a/media/RESULTS.md b/media/RESULTS.md index 9219abc..2066daf 100644 --- a/media/RESULTS.md +++ b/media/RESULTS.md @@ -278,9 +278,9 @@ Results of the model in various experiments on different datasets. (duration 00:00:56) ```json { - "avg_time_2d": 0.10053871915102824, - "avg_time_3d": 0.0020945760392651115, - "avg_fps": 9.743426810431163 + "avg_time_2d": 0.101639888540576, + "avg_time_3d": 0.0018720938168030833, + "avg_fps": 9.660717312392508 } { "person_nums": { diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index 82a9fe4..dfd7e1d 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -149,16 +149,19 @@ std::vector>> TriangulatorInternal::triangulat { std::vector dims = {(int)num_joints, 3}; cv::Mat pose_mat = cv::Mat(dims, CV_64F); + + // Use pointer to copy data efficiently + double *mat_ptr = pose_mat.ptr(0); for (size_t k = 0; k < num_joints; ++k) { - for (size_t l = 0; l < 3; ++l) - { - pose_mat.at(k, l) = poses_2d[i][j][k][l]; - } + const auto &joint = poses_2d[i][j][k]; + mat_ptr[k * 3 + 0] = joint[0]; + mat_ptr[k * 3 + 1] = joint[1]; + mat_ptr[k * 3 + 2] = joint[2]; } - camera_poses.push_back(pose_mat); + camera_poses.push_back(std::move(pose_mat)); } - poses_2d_mats.push_back(camera_poses); + poses_2d_mats.push_back(std::move(camera_poses)); } std::vector internal_cameras; for (size_t i = 0; i < cameras.size(); ++i) @@ -194,9 +197,11 @@ std::vector>> TriangulatorInternal::triangulat { // Select core joints 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]; + cv::Mat &pose = last_poses_3d[i]; std::vector dims = {(int)core_joint_idx.size(), 4}; cv::Mat last_poses_core(dims, pose.type()); @@ -204,7 +209,7 @@ std::vector>> TriangulatorInternal::triangulat { pose.row(core_joint_idx[j]).copyTo(last_poses_core.row(j)); } - last_core_poses.push_back(last_poses_core); + last_core_poses[i] = last_poses_core; } // Project @@ -223,50 +228,81 @@ std::vector>> TriangulatorInternal::triangulat if (!last_poses_3d.empty()) { // Calculate index pairs and initialize vectors - std::vector> indices; + std::vector> indices_ijk; for (size_t i = 0; i < cameras.size(); ++i) { - size_t num_persons = std::get<0>(last_poses_2d[i]).size(); + size_t num_last_persons = std::get<0>(last_poses_2d[i]).size(); scored_pasts[i] = std::map>(); - for (size_t j = 0; j < num_persons; ++j) + for (size_t j = 0; j < num_last_persons; ++j) { + size_t num_new_persons = poses_2d_mats[i].size(); scored_pasts[i][j] = std::vector(); - for (size_t k = 0; k < poses_2d_mats[i].size(); ++k) + + for (size_t k = 0; k < num_new_persons; ++k) { - indices.push_back({i, j, k}); + indices_ijk.push_back({i, j, k}); } } } - - #pragma omp parallel for ordered schedule(dynamic) - for (size_t e = 0; e < indices.size(); ++e) + std::vector> indices_ik; + for (size_t i = 0; i < cameras.size(); ++i) { - const auto &i = indices[e][0]; - const auto &j = indices[e][1]; - const auto &k = indices[e][2]; + size_t num_new_persons = poses_2d_mats[i].size(); + for (size_t k = 0; k < num_new_persons; ++k) + { + indices_ik.push_back({i, k}); + } + } - const std::vector &poses = poses_2d_mats[i]; - const std::vector &last_poses = std::get<0>(last_poses_2d[i]); - const std::vector &dists = std::get<1>(last_poses_2d[i]); + // Precalculate core poses + std::vector poses_2d_mats_core_list; + poses_2d_mats_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(); + 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]; - // Select core joints - const cv::Mat &last_pose = last_poses[j]; - const cv::Mat &pose = poses[k]; + 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()); - for (size_t l = 0; l < core_joint_idx.size(); ++l) + + for (size_t j = 0; j < core_joint_idx.size(); ++j) { - size_t idx = core_joint_idx[l]; - pose.row(idx).copyTo(pose_core.row(l)); + pose.row(core_joint_idx[j]).copyTo(pose_core.row(j)); } - // Calculate score - double score = calc_pose_score(pose_core, last_pose, dists[j], internal_cameras[i]); + poses_2d_mats_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]]; + + double score = calc_pose_score(new_pose, last_pose, last_dist, internal_cameras[i]); if (score > threshold) { - #pragma omp ordered - scored_pasts[i][j].push_back(k); + #pragma omp critical + { + scored_pasts[i][j].push_back(k); + } } } } @@ -274,10 +310,15 @@ std::vector>> TriangulatorInternal::triangulat // Create pairs of persons // Checks if the person was already matched to the last frame and if so only creates pairs // with those, else it creates all possible pairs - std::vector num_persons; + std::vector num_persons_sum; for (size_t i = 0; i < cameras.size(); ++i) { - num_persons.push_back(poses_2d[i].size()); + int nsum = poses_2d[i].size(); + if (i > 0) + { + nsum += num_persons_sum[i - 1]; + } + num_persons_sum.push_back(nsum); } std::vector, std::pair>> all_pairs; std::vector> indices; @@ -299,8 +340,8 @@ std::vector>> TriangulatorInternal::triangulat { const auto [i, j, k, l] = indices[e]; - int pid1 = std::accumulate(num_persons.begin(), num_persons.begin() + i, 0) + k; - int pid2 = std::accumulate(num_persons.begin(), num_persons.begin() + j, 0) + l; + int pid1 = num_persons_sum[i] + k; + int pid2 = num_persons_sum[k] + l; bool match = false; if (!last_poses_3d.empty()) @@ -309,16 +350,19 @@ std::vector>> TriangulatorInternal::triangulat { auto &smi = scored_pasts[i][m]; auto &smj = scored_pasts[j][m]; - if (std::find(smi.begin(), smi.end(), k) != smi.end() && - std::find(smj.begin(), smj.end(), l) != smj.end()) + bool in_smi = std::find(smi.begin(), smi.end(), k) != smi.end(); + bool in_smj = std::find(smj.begin(), smj.end(), l) != smj.end(); + + if (in_smi && in_smj) { match = true; - #pragma omp ordered - all_pairs.emplace_back( + 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 (std::find(smi.begin(), smi.end(), k) != smi.end() || - std::find(smj.begin(), smj.end(), l) != smj.end()) + else if (in_smi || in_smj) { match = true; } @@ -327,9 +371,12 @@ std::vector>> TriangulatorInternal::triangulat if (!match) { - #pragma omp ordered - all_pairs.emplace_back( + 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); } } @@ -413,6 +460,8 @@ std::vector>> TriangulatorInternal::triangulat { const auto &group = groups[i]; std::vector poses; + poses.reserve(std::get<2>(group).size()); + for (const auto &idx : std::get<2>(group)) { poses.push_back(all_full_poses[idx]); @@ -421,6 +470,7 @@ std::vector>> TriangulatorInternal::triangulat auto merged_pose = merge_group(poses, min_score); all_merged_poses[i] = (merged_pose); } + last_poses_3d = all_merged_poses; // Convert to output format std::vector>> poses_3d; @@ -428,6 +478,7 @@ std::vector>> TriangulatorInternal::triangulat for (size_t i = 0; i < all_merged_poses.size(); ++i) { const auto &mat = all_merged_poses[i]; + const double *mat_ptr = mat.ptr(0); std::vector> pose; size_t num_joints = mat.rows; pose.reserve(num_joints); @@ -438,7 +489,7 @@ std::vector>> TriangulatorInternal::triangulat std::array point; for (size_t k = 0; k < 4; ++k) { - point[k] = mat.at(j, k); + point[k] = mat_ptr[j * 4 + k]; } pose.push_back(point); @@ -454,7 +505,6 @@ std::vector>> TriangulatorInternal::triangulat } } - last_poses_3d = all_merged_poses; return poses_3d; } @@ -467,7 +517,7 @@ void TriangulatorInternal::reset() // ================================================================================================= -cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam) +void TriangulatorInternal::undistort_poses(std::vector &poses, CameraInternal &icam) { int width = icam.cam.width; int height = icam.cam.height; @@ -476,55 +526,41 @@ cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal & cv::Mat newK = cv::getOptimalNewCameraMatrix( icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); - // Undistort points - cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); - - return newK; -} - -// ================================================================================================= - -void TriangulatorInternal::undistort_poses(std::vector &poses, CameraInternal &icam) -{ for (size_t p = 0; p < poses.size(); ++p) { - int num_joints = poses[p].rows; - // Extract the (x, y) coordinates cv::Mat points = poses[p].colRange(0, 2).clone(); + points = points.reshape(2); // Undistort the points - cv::Mat newK = undistort_points(points, icam); - if (p == poses.size() - 1) - { - // Update the camera matrix as well - icam.K = newK; - icam.DC = cv::Mat::zeros(5, 1, CV_64F); - } + cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); // Update the original poses with the undistorted points - for (int j = 0; j < num_joints; ++j) - { - poses[p].at(j, 0) = points.at(j, 0); - poses[p].at(j, 1) = points.at(j, 1); - } + points = points.reshape(1); + points.copyTo(poses[p].colRange(0, 2)); // Mask out points that are far outside the image (points slightly outside are still valid) - double offset = (icam.cam.width + icam.cam.height) / 40.0; + double mask_offset = (width + height) / 40.0; + int num_joints = poses[p].rows; + double *poses_ptr = poses[p].ptr(0); for (int j = 0; j < num_joints; ++j) { - double x = poses[p].at(j, 0); - double y = poses[p].at(j, 1); - bool in_x = x >= -offset && x < icam.cam.width + offset; - bool in_y = y >= -offset && y < icam.cam.height + offset; + double x = poses_ptr[j * 3 + 0]; + double y = poses_ptr[j * 3 + 1]; + bool in_x = x >= -mask_offset && x < width + mask_offset; + bool in_y = y >= -mask_offset && y < height + mask_offset; if (!in_x || !in_y) { - poses[p].at(j, 0) = 0; - poses[p].at(j, 1) = 0; - poses[p].at(j, 2) = 0; + poses_ptr[j * 3 + 0] = 0; + poses_ptr[j * 3 + 1] = 0; + poses_ptr[j * 3 + 2] = 0; } } } + + // Update the camera matrix + icam.K = newK.clone(); + icam.DC = cv::Mat::zeros(5, 1, CV_64F); } // ================================================================================================= @@ -533,29 +569,31 @@ std::tuple, std::vector> TriangulatorInternal::pro const std::vector &bodies3D, const CameraInternal &icam, bool calc_dists) { size_t num_persons = bodies3D.size(); + size_t num_joints = bodies3D[0].rows; + std::vector bodies2D_list(num_persons); std::vector dists_list(num_persons); + cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints); for (size_t i = 0; i < num_persons; ++i) { const cv::Mat &body3D = bodies3D[i]; - size_t num_joints = body3D.size[0]; // Split up vector cv::Mat points3d = body3D.colRange(0, 3).clone(); // Project from world to camera coordinate system - cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints); cv::Mat xyz = (icam.R * (points3d.t() - T_repeated)).t(); // Set points behind the camera to zero + double *xyz_ptr = xyz.ptr(0); for (size_t i = 0; i < num_joints; ++i) { - if (xyz.at(i, 2) <= 0) + if (xyz_ptr[i * 3 + 2] <= 0) { - xyz.at(i, 0) = 0; - xyz.at(i, 1) = 0; - xyz.at(i, 2) = 0; + xyz_ptr[i * 3 + 0] = 0; + xyz_ptr[i * 3 + 1] = 0; + xyz_ptr[i * 3 + 2] = 0; } } @@ -584,24 +622,28 @@ std::tuple, std::vector> TriangulatorInternal::pro // Add scores again std::vector dimsB = {(int)num_joints, 3}; cv::Mat bodies2D = cv::Mat(dimsB, CV_64F); + double *bodies2D_ptr = bodies2D.ptr(0); + const double *uv_ptr = uv.ptr(0); + const double *bodies3D_ptr = body3D.ptr(0); for (size_t i = 0; i < num_joints; ++i) { - bodies2D.at(i, 0) = uv.at(i, 0); - bodies2D.at(i, 1) = uv.at(i, 1); - bodies2D.at(i, 2) = body3D.at(i, 3); + bodies2D_ptr[i * 3 + 0] = uv_ptr[i * 2 + 0]; + bodies2D_ptr[i * 3 + 1] = uv_ptr[i * 2 + 1]; + bodies2D_ptr[i * 3 + 2] = bodies3D_ptr[i * 4 + 3]; } // Filter invalid projections - cv::Mat valid_x = (bodies2D.col(0) >= 0) & (bodies2D.col(0) < icam.cam.width); - cv::Mat valid_y = (bodies2D.col(1) >= 0) & (bodies2D.col(1) < icam.cam.height); - cv::Mat vis = valid_x & valid_y; for (size_t i = 0; i < num_joints; ++i) { - if (!vis.at(i)) + double x = bodies2D_ptr[i * 3 + 0]; + double y = bodies2D_ptr[i * 3 + 1]; + bool in_x = x >= 0 && x < icam.cam.width; + bool in_y = y >= 0 && y < icam.cam.height; + if (!in_x || !in_y) { - bodies2D.at(i, 0) = 0; - bodies2D.at(i, 1) = 0; - bodies2D.at(i, 2) = 0; + bodies2D_ptr[i * 3 + 0] = 0; + bodies2D_ptr[i * 3 + 1] = 0; + bodies2D_ptr[i * 3 + 2] = 0; } } @@ -640,11 +682,13 @@ double TriangulatorInternal::calc_pose_score( size_t drop_k = static_cast(pose1.rows * 0.2); size_t min_k = 3; std::vector scores_vec; + const double *scores_ptr = scores.ptr(0); + const uchar *mask_ptr = mask.ptr(0); for (int i = 0; i < scores.rows; ++i) { - if (mask.at(i) > 0) + if (mask_ptr[i] > 0) { - scores_vec.push_back(scores.at(i)); + scores_vec.push_back(scores_ptr[i]); } } std::sort(scores_vec.begin(), scores_vec.end()); @@ -656,15 +700,14 @@ double TriangulatorInternal::calc_pose_score( // Calculate final score double score = 0.0; - size_t items = 0; - for (size_t i = 0; i < scores_vec.size(); i++) + size_t n_items = scores_vec.size(); + for (size_t i = 0; i < n_items; i++) { score += scores_vec[i]; - items++; } - if (items > 0) + if (n_items > 0) { - score /= (double)items; + score /= (double)n_items; } return score; @@ -682,9 +725,9 @@ cv::Mat TriangulatorInternal::score_projection( double min_score = 0.1; // Calculate error - cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2); - cv::Mat error1 = diff.mul(diff); - cv::reduce(error1, error1, 1, cv::REDUCE_SUM, CV_64F); + cv::Mat error1 = pose1.colRange(0, 2) - repro1.colRange(0, 2); + error1 = error1.mul(error1); + error1 = error1.col(0) + error1.col(1); cv::sqrt(error1, error1); error1.setTo(0.0, ~mask); @@ -720,6 +763,7 @@ std::pair TriangulatorInternal::triangulate_and_score( cv::Mat mask1a = (pose1.col(2) >= min_score); cv::Mat mask2a = (pose2.col(2) >= min_score); const cv::Mat mask = mask1a & mask2a; + const uchar *mask_ptr = mask.ptr(0); // If too few joints are visible, return a low score int num_visible = cv::countNonZero(mask); @@ -731,39 +775,46 @@ std::pair TriangulatorInternal::triangulate_and_score( return std::make_pair(pose3d, score); } - // Triangulate points + // Extract coordinates std::vector dimsA = {2, num_visible}; cv::Mat points1 = cv::Mat(dimsA, CV_64F); cv::Mat points2 = cv::Mat(dimsA, CV_64F); + const double *pose1_ptr = pose1.ptr(0); + const double *pose2_ptr = pose2.ptr(0); + double *points1_ptr = points1.ptr(0); + double *points2_ptr = points2.ptr(0); int idx = 0; for (int i = 0; i < pose1.rows; ++i) { - if (mask.at(i) > 0) + if (mask_ptr[i] > 0) { - points1.at(0, idx) = pose1.at(i, 0); - points1.at(1, idx) = pose1.at(i, 1); - points2.at(0, idx) = pose2.at(i, 0); - points2.at(1, idx) = pose2.at(i, 1); + points1_ptr[idx + 0 * num_visible] = pose1_ptr[i * 3 + 0]; + points1_ptr[idx + 1 * num_visible] = pose1_ptr[i * 3 + 1]; + points2_ptr[idx + 0 * num_visible] = pose2_ptr[i * 3 + 0]; + points2_ptr[idx + 1 * num_visible] = pose2_ptr[i * 3 + 1]; idx++; } } - cv::Mat points3d_h, points3d; - cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points3d_h); - cv::convertPointsFromHomogeneous(points3d_h.t(), points3d); + // 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 std::vector dimsB = {(int)pose1.rows, 4}; cv::Mat pose3d = cv::Mat(dimsB, CV_64F, cv::Scalar(0)); + const double *points3d_ptr = points3d.ptr(0); + double *pose3d_ptr = pose3d.ptr(0); idx = 0; for (int i = 0; i < pose1.rows; ++i) { - if (mask.at(i) > 0) + if (mask_ptr[i] > 0) { - pose3d.at(i, 0) = points3d.at(idx, 0); - pose3d.at(i, 1) = points3d.at(idx, 1); - pose3d.at(i, 2) = points3d.at(idx, 2); - pose3d.at(i, 3) = 1.0; + pose3d_ptr[i * 4 + 0] = points3d_ptr[idx * 3 + 0]; + pose3d_ptr[i * 4 + 1] = points3d_ptr[idx * 3 + 1]; + pose3d_ptr[i * 4 + 2] = points3d_ptr[idx * 3 + 2]; + pose3d_ptr[i * 4 + 3] = 1.0; idx++; } } @@ -772,11 +823,11 @@ std::pair TriangulatorInternal::triangulate_and_score( std::array mean = {0, 0, 0}; for (int i = 0; i < pose1.rows; ++i) { - if (mask.at(i) > 0) + if (mask_ptr[i] > 0) { for (int j = 0; j < 3; ++j) { - mean[j] += pose3d.at(i, j); + mean[j] += pose3d_ptr[i * 4 + j]; } } } @@ -792,10 +843,7 @@ std::pair TriangulatorInternal::triangulate_and_score( mean[i] < center[i] - size[i] / 2) { // Very low score if outside room - for (int j = 0; j < pose1.rows; ++j) - { - pose3d.at(j, 3) = 0.001; - } + pose3d.col(3).setTo(0.001); return {pose3d, 0.001}; } } @@ -817,13 +865,14 @@ std::pair TriangulatorInternal::triangulate_and_score( // Combine scores cv::Mat scores = (score1 + score2) / 2.0; + const double *scores_ptr = scores.ptr(0); // Add scores to 3D pose for (int i = 0; i < pose1.rows; ++i) { - if (mask.at(i) > 0) + if (mask_ptr[i] > 0) { - pose3d.at(i, 3) = scores.at(i); + pose3d_ptr[i * 4 + 3] = scores_ptr[i]; } } @@ -831,14 +880,14 @@ std::pair TriangulatorInternal::triangulate_and_score( double wdist = 0.1; for (int i = 0; i < pose1.rows; ++i) { - if (mask.at(i) > 0) + if (mask_ptr[i] > 0) { for (int j = 0; j < 3; ++j) { - if (pose3d.at(i, j) > center[j] + size[j] / 2 + wdist || - pose3d.at(i, j) < center[j] - size[j] / 2 - wdist) + if (pose3d_ptr[i * 4 + j] > center[j] + size[j] / 2 + wdist || + pose3d_ptr[i * 4 + j] < center[j] - size[j] / 2 - wdist) { - pose3d.at(i, 3) = 0.001; + pose3d_ptr[i * 4 + 3] = 0.001; } } } @@ -852,18 +901,14 @@ std::pair TriangulatorInternal::triangulate_and_score( for (size_t i = 0; i < core_limbs_idx.size(); ++i) { auto limb = core_limbs_idx[i]; - if (pose3d.at(limb[0], 3) > min_score && - pose3d.at(limb[1], 3) > min_score) + if (pose3d_ptr[limb[0] * 4 + 3] > min_score && + pose3d_ptr[limb[1] * 4 + 3] > min_score) { - cv::Point3d p1 = cv::Point3d( - pose3d.at(limb[0], 0), - pose3d.at(limb[0], 1), - pose3d.at(limb[0], 2)); - cv::Point3d p2 = cv::Point3d( - pose3d.at(limb[1], 0), - pose3d.at(limb[1], 1), - pose3d.at(limb[1], 2)); - double length = cv::norm(p1 - p2); + double dx = pose3d_ptr[limb[0] * 4 + 0] - pose3d_ptr[limb[1] * 4 + 0]; + double dy = pose3d_ptr[limb[0] * 4 + 1] - pose3d_ptr[limb[1] * 4 + 1]; + double dz = pose3d_ptr[limb[0] * 4 + 2] - pose3d_ptr[limb[1] * 4 + 2]; + double length = std::sqrt(dx * dx + dy * dy + dz * dz); + if (length > max_length) { invalid_joints.push_back(limb[1]); @@ -872,7 +917,7 @@ std::pair TriangulatorInternal::triangulate_and_score( } for (size_t i = 0; i < invalid_joints.size(); ++i) { - pose3d.at(invalid_joints[i], 3) = 0.001; + pose3d_ptr[invalid_joints[i] * 4 + 3] = 0.001; } } @@ -882,9 +927,9 @@ std::pair TriangulatorInternal::triangulate_and_score( std::vector scores_vec; for (int i = 0; i < pose1.rows; ++i) { - if (pose3d.at(i, 3) > min_score) + if (pose3d_ptr[i * 4 + 3] > min_score) { - scores_vec.push_back(pose3d.at(i, 3)); + scores_vec.push_back(pose3d_ptr[i * 4 + 3]); } } std::sort(scores_vec.begin(), scores_vec.end()); @@ -896,15 +941,14 @@ std::pair TriangulatorInternal::triangulate_and_score( // Calculate final score double score = 0.0; - size_t items = 0; - for (size_t i = 0; i < scores_vec.size(); i++) + size_t n_items = scores_vec.size(); + for (size_t i = 0; i < n_items; i++) { score += scores_vec[i]; - items++; } - if (items > 0) + if (n_items > 0) { - score /= (double)items; + score /= (double)n_items; } return std::make_pair(pose3d, score); @@ -926,15 +970,17 @@ std::vector>> TriangulatorInte { const cv::Mat &pose_3d = all_scored_poses[i].first; size_t num_joints = pose_3d.rows; + const double *pose_3d_ptr = pose_3d.ptr(0); + cv::Point3d center(0, 0, 0); size_t num_valid = 0; for (size_t j = 0; j < num_joints; ++j) { - if (pose_3d.at(j, 3) > min_score) + if (pose_3d_ptr[j * 4 + 3] > min_score) { - center.x += pose_3d.at(j, 0); - center.y += pose_3d.at(j, 1); - center.z += pose_3d.at(j, 2); + center.x += pose_3d_ptr[j * 4 + 0]; + center.y += pose_3d_ptr[j * 4 + 1]; + center.z += pose_3d_ptr[j * 4 + 2]; num_valid++; } } @@ -954,6 +1000,8 @@ std::vector>> TriangulatorInte { const cv::Mat &pose_3d = all_scored_poses[i].first; size_t num_joints = pose_3d.rows; + const double *pose_3d_ptr = pose_3d.ptr(0); + const cv::Point3d ¢er = centers[i]; double best_dist = std::numeric_limits::infinity(); int best_group = -1; @@ -963,6 +1011,7 @@ std::vector>> TriangulatorInte auto &group = groups[j]; cv::Point3d &group_center = std::get<0>(group); cv::Mat &group_pose = std::get<1>(group); + const double *group_pose_ptr = group_pose.ptr(0); // Check if the center is close enough if (cv::norm(group_center - center) < max_center_dist) @@ -971,18 +1020,14 @@ std::vector>> TriangulatorInte std::vector dists; for (size_t row = 0; row < num_joints; row++) { - if (pose_3d.at(row, 3) > min_score && - group_pose.at(row, 3) > min_score) + if (pose_3d_ptr[row * 4 + 3] > min_score && + group_pose_ptr[row * 4 + 3] > min_score) { - cv::Point3d p1 = cv::Point3d( - pose_3d.at(row, 0), - pose_3d.at(row, 1), - pose_3d.at(row, 2)); - cv::Point3d p2 = cv::Point3d( - group_pose.at(row, 0), - group_pose.at(row, 1), - group_pose.at(row, 2)); - dists.push_back(cv::norm(p1 - p2)); + double dx = pose_3d_ptr[row * 4 + 0] - group_pose_ptr[row * 4 + 0]; + double dy = pose_3d_ptr[row * 4 + 1] - group_pose_ptr[row * 4 + 1]; + double dz = pose_3d_ptr[row * 4 + 2] - group_pose_ptr[row * 4 + 2]; + double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + dists.push_back(dist); } } double dist = std::numeric_limits::infinity(); @@ -1018,20 +1063,23 @@ std::vector>> TriangulatorInte group_center = (group_center * n_elems + center) / (n_elems + 1); cv::Mat new_pose = group_pose.clone(); + const double *group_pose_ptr = group_pose.ptr(0); + const double *pose_3d_ptr = pose_3d.ptr(0); + double *new_pose_ptr = new_pose.ptr(0); + for (size_t row = 0; row < num_joints; row++) { - - new_pose.at(row, 0) = - (group_pose.at(row, 0) * n_elems + pose_3d.at(row, 0)) / + new_pose_ptr[row * 4 + 0] = + (group_pose_ptr[row * 4 + 0] * n_elems + pose_3d_ptr[row * 4 + 0]) / (n_elems + 1); - new_pose.at(row, 1) = - (group_pose.at(row, 1) * n_elems + pose_3d.at(row, 1)) / + new_pose_ptr[row * 4 + 1] = + (group_pose_ptr[row * 4 + 1] * n_elems + pose_3d_ptr[row * 4 + 1]) / (n_elems + 1); - new_pose.at(row, 2) = - (group_pose.at(row, 2) * n_elems + pose_3d.at(row, 2)) / + new_pose_ptr[row * 4 + 2] = + (group_pose_ptr[row * 4 + 2] * n_elems + pose_3d_ptr[row * 4 + 2]) / (n_elems + 1); - new_pose.at(row, 3) = - (group_pose.at(row, 3) * n_elems + pose_3d.at(row, 3)) / + new_pose_ptr[row * 4 + 3] = + (group_pose_ptr[row * 4 + 3] * n_elems + pose_3d_ptr[row * 4 + 3]) / (n_elems + 1); } group_pose = new_pose; @@ -1052,41 +1100,50 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, // Merge poses to create initial pose // Use only those triangulations with a high score cv::Mat sum_poses = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type()); + double *sum_poses_ptr = sum_poses.ptr(0); std::vector sum_mask(num_joints, 0); for (int i = 0; i < num_poses; ++i) { const cv::Mat &pose = poses_3d[i]; + const double *pose_ptr = pose.ptr(0); + for (int j = 0; j < num_joints; ++j) { - if (pose.at(j, 3) > min_score) + if (pose_ptr[j * 4 + 3] > min_score) { - sum_poses.row(j) += pose.row(j); + for (int k = 0; k < 4; ++k) + { + sum_poses_ptr[j * 4 + k] += pose_ptr[j * 4 + k]; + } sum_mask[j]++; } } } cv::Mat initial_pose_3d = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type()); + double *initial_pose_3d_ptr = initial_pose_3d.ptr(0); for (int j = 0; j < num_joints; ++j) { if (sum_mask[j] > 0) { - initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j]; + for (int k = 0; k < 4; ++k) + { + initial_pose_3d_ptr[j * 4 + k] = sum_poses_ptr[j * 4 + k] / sum_mask[j]; + } } } // Use center as default if the initial pose is empty - cv::Mat jmask = cv::Mat::zeros(num_joints, 1, CV_8U); + std::vector jmask(num_joints, false); cv::Point3d center(0, 0, 0); int valid_joints = 0; for (int j = 0; j < num_joints; ++j) { - if (initial_pose_3d.at(j, 3) > 0.0) + if (initial_pose_3d_ptr[j * 4 + 3] > min_score) { - jmask.at(j) = 1; - center += cv::Point3d( - initial_pose_3d.at(j, 0), - initial_pose_3d.at(j, 1), - initial_pose_3d.at(j, 2)); + jmask[j] = true; + center.x += initial_pose_3d_ptr[j * 4 + 0]; + center.y += initial_pose_3d_ptr[j * 4 + 1]; + center.z += initial_pose_3d_ptr[j * 4 + 2]; valid_joints++; } } @@ -1096,11 +1153,11 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, } for (int j = 0; j < num_joints; ++j) { - if (jmask.at(j) == 0) + if (!jmask[j]) { - initial_pose_3d.at(j, 0) = center.x; - initial_pose_3d.at(j, 1) = center.y; - initial_pose_3d.at(j, 2) = center.z; + initial_pose_3d_ptr[j * 4 + 0] = center.x; + initial_pose_3d_ptr[j * 4 + 1] = center.y; + initial_pose_3d_ptr[j * 4 + 2] = center.z; } } @@ -1109,23 +1166,24 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, double max_dist = 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_64F); + double *distances_ptr = distances.ptr(0); + u_char *mask_ptr = mask.ptr(0); for (int i = 0; i < num_poses; ++i) { + const cv::Mat &pose = poses_3d[i]; + const double *pose_ptr = pose.ptr(0); + for (int j = 0; j < num_joints; ++j) { - cv::Point3d joint_i = cv::Point3d( - poses_3d[i].at(j, 0), - poses_3d[i].at(j, 1), - poses_3d[i].at(j, 2)); - cv::Point3d joint_initial = cv::Point3d( - initial_pose_3d.at(j, 0), - initial_pose_3d.at(j, 1), - initial_pose_3d.at(j, 2)); - double distance = cv::norm(joint_i - joint_initial); - distances.at(i, j) = distance; - if (distance <= max_dist && poses_3d[i].at(j, 3) > (min_score - offset)) + double dx = pose_ptr[j * 4 + 0] - initial_pose_3d_ptr[j * 4 + 0]; + double dy = pose_ptr[j * 4 + 1] - initial_pose_3d_ptr[j * 4 + 1]; + double dz = pose_ptr[j * 4 + 2] - initial_pose_3d_ptr[j * 4 + 2]; + double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + distances_ptr[i * num_joints + j] = dist; + + if (dist <= max_dist && pose_ptr[j * 4 + 3] > (min_score - offset)) { - mask.at(i, j) = 1; + mask_ptr[i * num_joints + j] = 1; } } } @@ -1138,9 +1196,10 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, std::vector> valid_indices; for (int i = 0; i < num_poses; ++i) { - if (mask.at(i, j)) + if (mask_ptr[i * num_joints + j]) { - valid_indices.push_back({distances.at(i, j), i}); + auto item = std::make_pair(distances_ptr[i * num_joints + j], i); + valid_indices.push_back(item); } } std::sort(valid_indices.begin(), valid_indices.end()); @@ -1152,27 +1211,39 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, // Combine masks mask = mask & best_k_mask; + mask_ptr = mask.ptr(0); // Compute the final pose sum_poses = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); + sum_poses_ptr = sum_poses.ptr(0); sum_mask = std::vector(num_joints, 0); for (int i = 0; i < num_poses; ++i) { + const cv::Mat &pose = poses_3d[i]; + const double *pose_ptr = pose.ptr(0); + for (int j = 0; j < num_joints; ++j) { - if (mask.at(i, j)) + if (mask_ptr[i * num_joints + j] > 0) { - sum_poses.row(j) += poses_3d[i].row(j); + for (int k = 0; k < 4; ++k) + { + sum_poses_ptr[j * 4 + k] += pose_ptr[j * 4 + k]; + } sum_mask[j]++; } } } cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); + double *final_pose_3d_ptr = final_pose_3d.ptr(0); for (int j = 0; j < num_joints; ++j) { if (sum_mask[j] > 0) { - final_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j]; + for (int k = 0; k < 4; ++k) + { + final_pose_3d_ptr[j * 4 + k] = sum_poses_ptr[j * 4 + k] / sum_mask[j]; + } } } diff --git a/spt/triangulator.hpp b/spt/triangulator.hpp index d66bea8..33a05f7 100644 --- a/spt/triangulator.hpp +++ b/spt/triangulator.hpp @@ -70,7 +70,6 @@ private: std::vector last_poses_3d; - cv::Mat undistort_points(cv::Mat &points, CameraInternal &icam); void undistort_poses(std::vector &poses, CameraInternal &icam); std::tuple, std::vector> project_poses(