From 91a502811f69734ff270affa8114f5543aa88b8e Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 13 Sep 2024 10:45:52 +0200 Subject: [PATCH] Fix a few conversion errors to make the results close to the python version. --- spt/triangulator.cpp | 144 +++++++++++++++++++++++++++---------------- 1 file changed, 91 insertions(+), 53 deletions(-) diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index 2d025e3..d8caeab 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -147,7 +147,7 @@ std::vector>> TriangulatorInternal::triangulat for (size_t j = 0; j < num_persons; ++j) { std::vector dims = {(int)num_joints, 3}; - cv::Mat pose_mat(dims, CV_64F); + cv::Mat pose_mat = cv::Mat(dims, CV_64F); for (size_t k = 0; k < num_joints; ++k) { for (size_t l = 0; l < 3; ++l) @@ -391,6 +391,7 @@ std::vector>> TriangulatorInternal::triangulat 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) { @@ -400,9 +401,17 @@ std::vector>> TriangulatorInternal::triangulat point[k] = mat.at(j, k); } pose.push_back(point); + + if (point[3] > min_score) + { + num_valid++; + } } - poses_3d.push_back(std::move(pose)); + if (num_valid > 0) + { + poses_3d.push_back(std::move(pose)); + } } last_poses_3d = all_merged_poses; @@ -443,7 +452,7 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn // Extract the (x, y) coordinates std::vector dims = {num_joints, 2}; - cv::Mat points(dims, CV_64F); + cv::Mat points = cv::Mat(dims, CV_64F); for (int j = 0; j < num_joints; ++j) { points.at(j, 0) = poses[p].at(j, 0); @@ -500,7 +509,7 @@ std::tuple, std::vector> TriangulatorInternal::pro // Split up vector std::vector dimsA = {(int)num_joints, 3}; - cv::Mat points3d(dimsA, CV_64F); + cv::Mat points3d = cv::Mat(dimsA, CV_64F); for (size_t i = 0; i < num_joints; ++i) { points3d.at(i, 0) = body3D.at(i, 0); @@ -547,7 +556,7 @@ std::tuple, std::vector> TriangulatorInternal::pro // Add scores again std::vector dimsB = {(int)num_joints, 3}; - cv::Mat bodies2D(dimsB, CV_64F); + cv::Mat bodies2D = cv::Mat(dimsB, CV_64F); for (size_t i = 0; i < num_joints; ++i) { bodies2D.at(i, 0) = uv.at(i, 0); @@ -598,20 +607,17 @@ double TriangulatorInternal::calc_pose_score( // Calculate scores double iscale = (icam.cam.width + icam.cam.height) / 2; - cv::Mat scores = score_projection( - pose1, pose2, dist1, mask, iscale); + cv::Mat scores = score_projection(pose1, pose2, dist1, mask, iscale); - // Drop lowest scores - int drop_k = static_cast(pose1.rows * 0.2); - std::sort(scores.begin(), scores.end()); - - // Calculate final score double score = 0.0; - for (int i = drop_k; i < scores.rows; i++) + for (int i = 0; i < scores.rows; ++i) { - score += scores.at(i); + if (mask.at(i) > 0) + { + score += scores.at(i); + } } - score /= (pose1.rows - drop_k); + score /= valid_count; return score; } @@ -626,7 +632,6 @@ cv::Mat TriangulatorInternal::score_projection( double iscale) { double min_score = 0.1; - double penalty = iscale; // Calculate error cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2); @@ -637,16 +642,14 @@ cv::Mat TriangulatorInternal::score_projection( error1.setTo(0.0, ~mask); // Set errors of invisible reprojections to a high value - cv::Mat mask_invisible = (repro1.col(2) < min_score); + double penalty = iscale; + cv::Mat mask_invisible = (repro1.col(2) < min_score) & mask; error1.setTo(penalty, mask_invisible); // Scale error by image size and distance to the camera - error1 = cv::min(error1, (iscale / 4.0)) / iscale; - - // Compute scaling factor - double dscale1 = std::sqrt(cv::mean(dists1).val[0] / 3.5); - - // Scale errors + error1 = cv::max(0, cv::min(error1, (iscale / 4.0))) / iscale; + cv::Scalar mean_dist = cv::mean(dists1, mask); + double dscale1 = std::sqrt(mean_dist[0] / 3.5); error1 *= dscale1; // Convert errors to a score @@ -668,22 +671,22 @@ std::pair TriangulatorInternal::triangulate_and_score( double min_score = 0.1; cv::Mat mask1a = (pose1.col(2) >= min_score); cv::Mat mask2a = (pose2.col(2) >= min_score); - cv::Mat mask = mask1a & mask2a; + const cv::Mat mask = mask1a & mask2a; // If too few joints are visible, return a low score int num_visible = cv::countNonZero(mask); if (num_visible < 3) { std::vector dims = {(int)pose1.rows, 4}; - cv::Mat pose3d(dims, CV_64F); + cv::Mat pose3d(dims, CV_64F, cv::Scalar(0)); double score = 0.0; return std::make_pair(pose3d, score); } // Triangulate points std::vector dimsA = {2, num_visible}; - cv::Mat points1(dimsA, CV_64F); - cv::Mat points2(dimsA, CV_64F); + cv::Mat points1 = cv::Mat(dimsA, CV_64F); + cv::Mat points2 = cv::Mat(dimsA, CV_64F); int idx = 0; for (int i = 0; i < pose1.rows; ++i) { @@ -703,34 +706,55 @@ std::pair TriangulatorInternal::triangulate_and_score( // Create the 3D pose matrix std::vector dimsB = {(int)pose1.rows, 4}; - cv::Mat pose3d(dimsB, CV_64F); + cv::Mat pose3d = cv::Mat(dimsB, CV_64F, cv::Scalar(0)); + idx = 0; for (int i = 0; i < pose1.rows; ++i) { if (mask.at(i) > 0) { - pose3d.at(i, 0) = points3d.at(i, 0); - pose3d.at(i, 1) = points3d.at(i, 1); - pose3d.at(i, 2) = points3d.at(i, 2); + 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; + idx++; } } // Check if points are inside the room bounds - cv::Mat mean, mins, maxs; - cv::reduce(pose3d.colRange(0, 3), mean, 0, cv::REDUCE_AVG); - cv::reduce(pose3d.colRange(0, 3), mins, 0, cv::REDUCE_MIN); - cv::reduce(pose3d.colRange(0, 3), maxs, 0, cv::REDUCE_MAX); + std::array mean = {0, 0, 0}; + std::array mins = {std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()}; + std::array maxs = {std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()}; + for (int i = 0; i < pose1.rows; ++i) + { + if (mask.at(i) > 0) + { + for (int j = 0; j < 3; ++j) + { + mean[j] += pose3d.at(i, j); + mins[j] = std::min(mins[j], pose3d.at(i, j)); + maxs[j] = std::max(maxs[j], pose3d.at(i, j)); + } + } + } + for (int j = 0; j < 3; ++j) + { + mean[j] /= num_visible; + } double wdist = 0.1; std::array center = roomparams[0]; std::array size = roomparams[1]; for (int i = 0; i < 3; ++i) { - if (mean.at(i) > center[i] + size[i] / 2 || - mean.at(i) < center[i] - size[i] / 2 || - maxs.at(i) > center[i] + size[i] / 2 + wdist || - mins.at(i) < center[i] - size[i] / 2 - wdist) + if (mean[i] > center[i] + size[i] / 2 || + mean[i] < center[i] - size[i] / 2 || + maxs[i] > center[i] + size[i] / 2 + wdist || + mins[i] < center[i] - size[i] / 2 - wdist) { // Very low score if outside room + for (int j = 0; j < pose1.rows; ++j) + { + pose3d.at(j, 3) = 0.001; + } return {pose3d, 0.001}; } } @@ -764,15 +788,28 @@ std::pair TriangulatorInternal::triangulate_and_score( // Drop lowest scores int drop_k = static_cast(pose1.rows * 0.2); - std::sort(scores.begin(), scores.end()); + std::vector scores_vec; + for (int i = 0; i < pose1.rows; ++i) + { + if (mask.at(i) > 0) + { + scores_vec.push_back(scores.at(i)); + } + } + std::sort(scores_vec.begin(), scores_vec.end()); // Calculate final score double score = 0.0; - for (int i = drop_k; i < scores.rows; i++) + size_t items = 0; + for (size_t i = drop_k; i < scores_vec.size(); i++) { - score += scores.at(i); + score += scores_vec[i]; + items++; + } + if (items > 0) + { + score /= (double)items; } - score /= (pose1.rows - drop_k); return std::make_pair(pose3d, score); } @@ -902,24 +939,25 @@ 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()); - cv::Mat sum_mask = cv::Mat::zeros(poses_3d[0].size(), CV_32S); - for (const auto &pose : poses_3d) + std::vector sum_mask(num_joints, 0); + for (int i = 0; i < num_poses; ++i) { + const cv::Mat &pose = poses_3d[i]; for (int j = 0; j < num_joints; ++j) { if (pose.at(j, 3) > min_score) { sum_poses.row(j) += pose.row(j); - sum_mask.at(j, 3) += 1; + sum_mask[j]++; } } } - cv::Mat initial_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); + cv::Mat initial_pose_3d = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type()); for (int j = 0; j < num_joints; ++j) { - if (sum_mask.at(j, 3) > 0) + if (sum_mask[j] > 0) { - initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask.at(j, 3); + initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j]; } } @@ -995,7 +1033,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, // Compute the final pose sum_poses = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); - sum_mask = cv::Mat::zeros(sum_mask.size(), CV_32S); + sum_mask = std::vector(num_joints, 0); for (int i = 0; i < num_poses; ++i) { for (int j = 0; j < num_joints; ++j) @@ -1003,16 +1041,16 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, if (mask.at(i, j)) { sum_poses.row(j) += poses_3d[i].row(j); - sum_mask.at(j, 3) += 1; + sum_mask[j]++; } } } cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); for (int j = 0; j < num_joints; ++j) { - if (sum_mask.at(j, 3) > 0) + if (sum_mask[j] > 0) { - final_pose_3d.row(j) = sum_poses.row(j) / sum_mask.at(j, 3); + final_pose_3d.row(j) = sum_poses.row(j) / sum_mask[j]; } }