diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index dfd7e1d..00d91ce 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -151,13 +151,13 @@ std::vector>> TriangulatorInternal::triangulat 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) { + double *mat_ptr = pose_mat.ptr(k); 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]; + mat_ptr[0] = joint[0]; + mat_ptr[1] = joint[1]; + mat_ptr[2] = joint[2]; } camera_poses.push_back(std::move(pose_mat)); } @@ -478,7 +478,6 @@ 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); @@ -486,10 +485,11 @@ std::vector>> TriangulatorInternal::triangulat for (size_t j = 0; j < num_joints; ++j) { + const double *mat_ptr = mat.ptr(j); std::array point; for (size_t k = 0; k < 4; ++k) { - point[k] = mat_ptr[j * 4 + k]; + point[k] = mat_ptr[k]; } pose.push_back(point); @@ -542,18 +542,18 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn // Mask out points that are far outside the image (points slightly outside are still valid) 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_ptr[j * 3 + 0]; - double y = poses_ptr[j * 3 + 1]; + double *poses_ptr = poses[p].ptr(j); + double x = poses_ptr[0]; + double y = poses_ptr[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_ptr[j * 3 + 0] = 0; - poses_ptr[j * 3 + 1] = 0; - poses_ptr[j * 3 + 2] = 0; + poses_ptr[0] = 0.0; + poses_ptr[1] = 0.0; + poses_ptr[2] = 0.0; } } } @@ -573,47 +573,54 @@ std::tuple, std::vector> TriangulatorInternal::pro std::vector bodies2D_list(num_persons); std::vector dists_list(num_persons); - cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints); + + cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints).t(); + cv::Mat R_transposed = icam.R.t(); for (size_t i = 0; i < num_persons; ++i) { const cv::Mat &body3D = bodies3D[i]; - // Split up vector - cv::Mat points3d = body3D.colRange(0, 3).clone(); + // Extract coordinates + const cv::Mat points3d = body3D.colRange(0, 3); // Project from world to camera coordinate system - cv::Mat xyz = (icam.R * (points3d.t() - T_repeated)).t(); + cv::Mat xyz = (points3d - T_repeated) * R_transposed; // Set points behind the camera to zero - double *xyz_ptr = xyz.ptr(0); - for (size_t i = 0; i < num_joints; ++i) + for (size_t j = 0; j < num_joints; ++j) { - if (xyz_ptr[i * 3 + 2] <= 0) + double *xyz_row_ptr = xyz.ptr(j); + double z = xyz_row_ptr[2]; + if (z <= 0) { - xyz_ptr[i * 3 + 0] = 0; - xyz_ptr[i * 3 + 1] = 0; - xyz_ptr[i * 3 + 2] = 0; + xyz_row_ptr[0] = 0.0; + xyz_row_ptr[1] = 0.0; + xyz_row_ptr[2] = 0.0; } } - // Calculate distance from camera center + // Calculate distance from camera center if required cv::Mat dists; if (calc_dists) { - dists = xyz.mul(xyz); + cv::multiply(xyz, xyz, dists); cv::reduce(dists, dists, 1, cv::REDUCE_SUM, CV_64F); cv::sqrt(dists, dists); } + else + { + dists = cv::Mat::zeros(num_joints, 1, CV_64F); + } - // Project to camera system + // Project points to image plane cv::Mat uv; if (icam.cam.type == "fisheye") { } else { - cv::Mat DCc = icam.DC.rowRange(0, 5).clone(); + cv::Mat DCc = icam.DC.rowRange(0, 5); cv::projectPoints( xyz, cv::Mat::zeros(3, 1, CV_64F), cv::Mat::zeros(3, 1, CV_64F), icam.K, DCc, uv); } @@ -622,28 +629,30 @@ 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) + for (size_t j = 0; j < num_joints; ++j) { - 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]; + double *bodies2D_row_ptr = bodies2D.ptr(j); + const double *uv_row_ptr = uv.ptr(j); + const double *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 i = 0; i < num_joints; ++i) + for (size_t j = 0; j < num_joints; ++j) { - 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; + double *bodies2D_row_ptr = bodies2D.ptr(j); + double x = bodies2D_row_ptr[0]; + double 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) { - bodies2D_ptr[i * 3 + 0] = 0; - bodies2D_ptr[i * 3 + 1] = 0; - bodies2D_ptr[i * 3 + 2] = 0; + bodies2D_row_ptr[0] = 0.0; + bodies2D_row_ptr[1] = 0.0; + bodies2D_row_ptr[2] = 0.0; } } @@ -663,12 +672,22 @@ double TriangulatorInternal::calc_pose_score( const cv::Mat &dist1, const CameraInternal &icam) { + const double min_score = 0.1; + // Create mask for valid points - double min_score = 0.1; - cv::Mat mask = (pose1.col(2) > min_score) & (pose2.col(2) > min_score); - int valid_count = cv::countNonZero(mask); + size_t num_joints = pose1.rows; + cv::Mat mask(num_joints, 1, CV_8U); + for (size_t i = 0; i < num_joints; ++i) + { + u_char *mask_ptr = mask.ptr(i); + const double *pose1_ptr = pose1.ptr(i); + const double *pose2_ptr = pose2.ptr(i); + + mask_ptr[0] = (pose1_ptr[2] > min_score) && (pose2_ptr[2] > min_score); + } // Drop if not enough valid points + int valid_count = cv::countNonZero(mask); if (valid_count < 3) { return 0.0; @@ -680,34 +699,33 @@ double TriangulatorInternal::calc_pose_score( // Drop lowest scores size_t drop_k = static_cast(pose1.rows * 0.2); - size_t min_k = 3; + const size_t min_k = 3; std::vector scores_vec; - const double *scores_ptr = scores.ptr(0); - const uchar *mask_ptr = mask.ptr(0); + scores_vec.reserve(valid_count); for (int i = 0; i < scores.rows; ++i) { - if (mask_ptr[i] > 0) + const double *scores_ptr = scores.ptr(i); + const uchar *mask_ptr = mask.ptr(i); + if (mask_ptr[0] > 0) { - scores_vec.push_back(scores_ptr[i]); + scores_vec.push_back(scores_ptr[0]); } } - std::sort(scores_vec.begin(), scores_vec.end()); - drop_k = (scores_vec.size() >= min_k) ? std::min(drop_k, scores_vec.size() - min_k) : 0; - if (drop_k > 0) + size_t scores_size = scores_vec.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); } // Calculate final score double score = 0.0; size_t n_items = scores_vec.size(); - for (size_t i = 0; i < n_items; i++) - { - score += scores_vec[i]; - } if (n_items > 0) { - score /= (double)n_items; + double sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); + score = sum_scores / static_cast(n_items); } return score; @@ -716,36 +734,90 @@ double TriangulatorInternal::calc_pose_score( // ================================================================================================= cv::Mat TriangulatorInternal::score_projection( - const cv::Mat &pose1, - const cv::Mat &repro1, - const cv::Mat &dists1, + const cv::Mat &pose, + const cv::Mat &repro, + const cv::Mat &dists, const cv::Mat &mask, double iscale) { - double min_score = 0.1; + const double min_score = 0.1; + const size_t num_joints = pose.rows; // Calculate error - 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); + cv::Mat error = cv::Mat::zeros(num_joints, 1, CV_64F); + for (size_t i = 0; i < num_joints; ++i) + { + const uchar *mask_ptr = mask.ptr(i); + const double *pose_ptr = pose.ptr(i); + const double *repro_ptr = repro.ptr(i); + double *error_ptr = error.ptr(i); - // Set errors of invisible reprojections to a high value - double penalty = iscale; - cv::Mat mask_invisible = (repro1.col(2) < min_score) & mask; - error1.setTo(penalty, mask_invisible); + if (mask_ptr) + { + double dx = pose_ptr[0] - repro_ptr[0]; + double dy = pose_ptr[1] - repro_ptr[1]; + double err = std::sqrt(dx * dx + dy * dy); - // Scale error by image size and distance to the camera - 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; + // Set errors of invisible reprojections to a high value + double score = repro_ptr[2]; + if (score < min_score) + { + err = iscale; + } - // Convert errors to a score - cv::Mat score1 = 1.0 / (1.0 + error1 * 10); + error_ptr[0] = err; + } + } - return score1; + // Scale error by image size + const double inv_iscale = 1.0 / iscale; + const double iscale_quarter = iscale / 4.0; + for (size_t i = 0; i < num_joints; ++i) + { + double *error_ptr = error.ptr(i); + + double err = error_ptr[0]; + err = std::max(0.0, std::min(err, iscale_quarter)) * inv_iscale; + error_ptr[0] = err; + } + + // Scale error by distance to camera + double mean_dist = 0.0; + int count = 0; + for (size_t i = 0; i < num_joints; ++i) + { + const uchar *mask_ptr = mask.ptr(i); + const double *dists_ptr = dists.ptr(i); + + if (mask_ptr) + { + mean_dist += dists_ptr[0]; + count++; + } + } + if (count > 0) + { + mean_dist /= count; + } + const double dscale = std::sqrt(mean_dist / 3.5); + for (size_t i = 0; i < num_joints; ++i) + { + double *error_ptr = error.ptr(i); + + double err = error_ptr[0]; + err *= dscale; + error_ptr[0] = err; + } + + // Convert error to score + cv::Mat score = error; + for (size_t i = 0; i < num_joints; ++i) + { + double *score_ptr = score.ptr(i); + score_ptr[0] = 1.0 / (1.0 + score_ptr[0] * 10.0); + } + + return score; } // ================================================================================================= @@ -758,40 +830,48 @@ std::pair TriangulatorInternal::triangulate_and_score( const std::array, 2> &roomparams, const std::vector> &core_limbs_idx) { - // Mask out invisible joints - double min_score = 0.1; - 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); + const double min_score = 0.1; + const size_t num_joints = pose1.rows; + + // Create mask for valid points + cv::Mat mask(num_joints, 1, CV_8U); + for (size_t i = 0; i < num_joints; ++i) + { + u_char *mask_ptr = mask.ptr(i); + const double *pose1_ptr = pose1.ptr(i); + const double *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) { - std::vector dims = {(int)pose1.rows, 4}; - cv::Mat pose3d(dims, CV_64F, cv::Scalar(0)); + cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_64F); double score = 0.0; return std::make_pair(pose3d, score); } - // 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); + // Extract coordinates of visible joints + std::vector dims = {2, num_visible}; + cv::Mat points1 = cv::Mat(dims, CV_64F); + cv::Mat points2 = cv::Mat(dims, CV_64F); + int idx = 0; double *points1_ptr = points1.ptr(0); double *points2_ptr = points2.ptr(0); - int idx = 0; - for (int i = 0; i < pose1.rows; ++i) + for (size_t i = 0; i < num_joints; ++i) { - if (mask_ptr[i] > 0) + const uchar *mask_ptr = mask.ptr(i); + const double *pose1_ptr = pose1.ptr(i); + const double *pose2_ptr = pose2.ptr(i); + + if (mask_ptr[0]) { - 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]; + 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++; } } @@ -802,48 +882,56 @@ std::pair TriangulatorInternal::triangulate_and_score( 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); + cv::Mat pose3d = cv::Mat::zeros(num_joints, 4, CV_64F); idx = 0; - for (int i = 0; i < pose1.rows; ++i) + for (size_t i = 0; i < num_joints; ++i) { - if (mask_ptr[i] > 0) + const uchar *mask_ptr = mask.ptr(i); + double *pose3d_ptr = pose3d.ptr(i); + const double *points3d_ptr = points3d.ptr(idx); + + if (mask_ptr[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; + pose3d_ptr[0] = points3d_ptr[0]; + pose3d_ptr[1] = points3d_ptr[1]; + pose3d_ptr[2] = points3d_ptr[2]; + pose3d_ptr[3] = 1.0; idx++; } } // Check if mean of the points is inside the room bounds - std::array mean = {0, 0, 0}; - for (int i = 0; i < pose1.rows; ++i) + std::array mean = {0.0, 0.0, 0.0}; + for (size_t i = 0; i < num_joints; ++i) { - if (mask_ptr[i] > 0) + const uchar *mask_ptr = mask.ptr(i); + const double *pose3d_ptr = pose3d.ptr(i); + + if (mask_ptr[0]) { - for (int j = 0; j < 3; ++j) - { - mean[j] += pose3d_ptr[i * 4 + j]; - } + mean[0] += pose3d_ptr[0]; + mean[1] += pose3d_ptr[1]; + mean[2] += pose3d_ptr[2]; } } + double inv_num_vis = 1.0 / num_visible; for (int j = 0; j < 3; ++j) { - mean[j] /= num_visible; + mean[j] *= inv_num_vis; } - std::array center = roomparams[0]; - std::array size = roomparams[1]; - for (int i = 0; i < 3; ++i) + const std::array ¢er = roomparams[0]; + const std::array &size = roomparams[1]; + for (int j = 0; j < 3; ++j) { - if (mean[i] > center[i] + size[i] / 2 || - mean[i] < center[i] - size[i] / 2) + if (mean[j] > center[j] + size[j] / 2.0 || + mean[j] < center[j] - size[j] / 2.0) { // Very low score if outside room - pose3d.col(3).setTo(0.001); + for (size_t i = 0; i < num_joints; ++i) + { + double *pose3d_ptr = pose3d.ptr(i); + pose3d_ptr[3] = 0.001; + } return {pose3d, 0.001}; } } @@ -864,91 +952,95 @@ std::pair TriangulatorInternal::triangulate_and_score( cv::Mat score2 = score_projection(pose2, repro2, dists2, mask, iscale); // Combine scores - cv::Mat scores = (score1 + score2) / 2.0; - const double *scores_ptr = scores.ptr(0); + cv::Mat scores = (score1 + score2) * 0.5; // Add scores to 3D pose - for (int i = 0; i < pose1.rows; ++i) + for (size_t i = 0; i < num_joints; ++i) { - if (mask_ptr[i] > 0) + const uchar *mask_ptr = mask.ptr(i); + double *pose3d_ptr = pose3d.ptr(i); + const double *scores_ptr = scores.ptr(i); + + if (mask_ptr[0]) { - pose3d_ptr[i * 4 + 3] = scores_ptr[i]; + pose3d_ptr[3] = scores_ptr[0]; } } // Set scores outside the room to a low value - double wdist = 0.1; - for (int i = 0; i < pose1.rows; ++i) + const double wdist = 0.1; + for (size_t i = 0; i < num_joints; ++i) { - if (mask_ptr[i] > 0) + const uchar *mask_ptr = mask.ptr(i); + double *pose3d_ptr = pose3d.ptr(i); + + if (mask_ptr[0]) { for (int j = 0; j < 3; ++j) { - if (pose3d_ptr[i * 4 + j] > center[j] + size[j] / 2 + wdist || - pose3d_ptr[i * 4 + j] < center[j] - size[j] / 2 - wdist) + if (pose3d_ptr[j] > center[j] + size[j] / 2.0 + wdist || + pose3d_ptr[j] < center[j] - size[j] / 2.0 - wdist) { - pose3d_ptr[i * 4 + 3] = 0.001; + pose3d_ptr[3] = 0.001; + break; } } } } // Filter clearly wrong limbs - if (core_limbs_idx.size() > 0) + if (!core_limbs_idx.empty()) { - double max_length = 0.9; - std::vector invalid_joints; + const double max_length_sq = 0.9 * 0.9; for (size_t i = 0; i < core_limbs_idx.size(); ++i) { - auto limb = core_limbs_idx[i]; - if (pose3d_ptr[limb[0] * 4 + 3] > min_score && - pose3d_ptr[limb[1] * 4 + 3] > min_score) - { - 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); + size_t limb1 = core_limbs_idx[i][0]; + size_t limb2 = core_limbs_idx[i][1]; + double *pose3d_ptr1 = pose3d.ptr(limb1); + double *pose3d_ptr2 = pose3d.ptr(limb2); - if (length > max_length) + if (pose3d_ptr1[3] > min_score && pose3d_ptr2[3] > min_score) + { + double dx = pose3d_ptr1[0] - pose3d_ptr2[0]; + double dy = pose3d_ptr1[1] - pose3d_ptr2[1]; + double dz = pose3d_ptr1[2] - pose3d_ptr2[2]; + double length_sq = dx * dx + dy * dy + dz * dz; + + if (length_sq > max_length_sq) { - invalid_joints.push_back(limb[1]); + pose3d_ptr2[3] = 0.001; } } } - for (size_t i = 0; i < invalid_joints.size(); ++i) - { - pose3d_ptr[invalid_joints[i] * 4 + 3] = 0.001; - } } // Drop lowest scores - size_t drop_k = static_cast(pose1.rows * 0.2); - size_t min_k = 3; + size_t drop_k = static_cast(num_joints * 0.2); + const size_t min_k = 3; std::vector scores_vec; - for (int i = 0; i < pose1.rows; ++i) + for (size_t i = 0; i < num_joints; ++i) { - if (pose3d_ptr[i * 4 + 3] > min_score) + const double *pose3d_ptr = pose3d.ptr(i); + if (pose3d_ptr[3] > min_score) { - scores_vec.push_back(pose3d_ptr[i * 4 + 3]); + scores_vec.push_back(pose3d_ptr[3]); } } - std::sort(scores_vec.begin(), scores_vec.end()); - drop_k = (scores_vec.size() >= min_k) ? std::min(drop_k, scores_vec.size() - min_k) : 0; - if (drop_k > 0) + size_t scores_size = scores_vec.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); } // Calculate final score double score = 0.0; size_t n_items = scores_vec.size(); - for (size_t i = 0; i < n_items; i++) - { - score += scores_vec[i]; - } if (n_items > 0) { - score /= (double)n_items; + double sum_scores = std::accumulate(scores_vec.begin(), scores_vec.end(), 0.0); + score = sum_scores / static_cast(n_items); } return std::make_pair(pose3d, score); @@ -961,47 +1053,49 @@ std::vector>> TriangulatorInte const std::vector> &all_scored_poses, double min_score) { - double max_center_dist = 0.6; + double max_center_dist_sq = 0.6 * 0.6; double max_joint_avg_dist = 0.3; + size_t num_pairs = all_pairs.size(); + size_t num_joints = all_scored_poses[0].first.rows; + // Calculate pose centers std::vector centers; - for (size_t i = 0; i < all_pairs.size(); ++i) + 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 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_ptr[j * 4 + 3] > min_score) + const double *pose_3d_ptr = pose_3d.ptr(j); + + double score = pose_3d_ptr[3]; + if (score > min_score) { - center.x += pose_3d_ptr[j * 4 + 0]; - center.y += pose_3d_ptr[j * 4 + 1]; - center.z += pose_3d_ptr[j * 4 + 2]; + center.x += pose_3d_ptr[0]; + center.y += pose_3d_ptr[1]; + center.z += pose_3d_ptr[2]; num_valid++; } } if (num_valid > 0) { - center.x /= num_valid; - center.y /= num_valid; - center.z /= num_valid; + double inv_num_valid = 1.0 / num_valid; + center.x *= inv_num_valid; + center.y *= inv_num_valid; + center.z *= inv_num_valid; } - centers.push_back(center); + centers[i] = center; } // Calculate Groups // defined as a tuple of center, pose, and all-pairs-indices of members std::vector>> groups; - for (size_t i = 0; i < all_pairs.size(); ++i) + 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 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; @@ -1010,37 +1104,48 @@ 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) + double dx = group_center.x - center.x; + double dy = group_center.y - center.y; + double dz = group_center.z - center.z; + double 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); + // Calculate average joint distance - std::vector dists; - for (size_t row = 0; row < num_joints; row++) + double dist_sum = 0.0; + size_t count = 0; + for (size_t row = 0; row < num_joints; ++row) { - if (pose_3d_ptr[row * 4 + 3] > min_score && - group_pose_ptr[row * 4 + 3] > min_score) + const double *pose_3d_ptr = pose_3d.ptr(row); + const double *group_pose_ptr = group_pose.ptr(row); + + double score1 = pose_3d_ptr[3]; + double score2 = group_pose_ptr[3]; + + if (score1 > min_score && score2 > min_score) { - 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 dx = pose_3d_ptr[0] - group_pose_ptr[0]; + double dy = pose_3d_ptr[1] - group_pose_ptr[1]; + double dz = pose_3d_ptr[2] - group_pose_ptr[2]; + double dist_sq = dx * dx + dy * dy + dz * dz; + dist_sum += std::sqrt(dist_sq); + count++; } } - double dist = std::numeric_limits::infinity(); - if (!dists.empty()) - { - dist = std::accumulate(dists.begin(), dists.end(), 0.0) / dists.size(); - } - // Check if the average joint distance is close enough - if (dist < max_joint_avg_dist && dist < best_dist) + if (count > 0) { - best_dist = dist; - best_group = j; + // Check if the average joint distance is close enough + double avg_dist = dist_sum / count; + if (avg_dist < max_joint_avg_dist && avg_dist < best_dist) + { + best_dist = avg_dist; + best_group = static_cast(j); + } } } } @@ -1049,7 +1154,7 @@ std::vector>> TriangulatorInte { // Create a new group std::vector new_indices{static_cast(i)}; - groups.emplace_back(center, pose_3d.clone(), new_indices); + groups.emplace_back(center, pose_3d.clone(), std::move(new_indices)); } else { @@ -1059,31 +1164,26 @@ std::vector>> TriangulatorInte cv::Mat &group_pose = std::get<1>(group); std::vector &group_indices = std::get<2>(group); - double n_elems = group_indices.size(); - group_center = (group_center * n_elems + center) / (n_elems + 1); + double n_elems = static_cast(group_indices.size()); + double inv_n1 = 1.0 / (n_elems + 1.0); - 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); + // 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; - for (size_t row = 0; row < num_joints; row++) + // Update group pose + for (size_t row = 0; row < num_joints; ++row) { - 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_ptr[row * 4 + 1] = - (group_pose_ptr[row * 4 + 1] * n_elems + pose_3d_ptr[row * 4 + 1]) / - (n_elems + 1); - 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_ptr[row * 4 + 3] = - (group_pose_ptr[row * 4 + 3] * n_elems + pose_3d_ptr[row * 4 + 3]) / - (n_elems + 1); + const double *pose_3d_ptr = pose_3d.ptr(row); + double *group_pose_ptr = group_pose.ptr(row); + + group_pose_ptr[0] = (group_pose_ptr[0] * n_elems + pose_3d_ptr[0]) * inv_n1; + group_pose_ptr[1] = (group_pose_ptr[1] * n_elems + pose_3d_ptr[1]) * inv_n1; + group_pose_ptr[2] = (group_pose_ptr[2] * n_elems + pose_3d_ptr[2]) * inv_n1; + group_pose_ptr[3] = (group_pose_ptr[3] * n_elems + pose_3d_ptr[3]) * inv_n1; } - group_pose = new_pose; - group_indices.push_back(i); + group_indices.push_back(static_cast(i)); } } @@ -1099,51 +1199,59 @@ 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); + cv::Mat sum_poses = cv::Mat::zeros(num_joints, 4, CV_64F); 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_ptr[j * 4 + 3] > min_score) + const double *pose_ptr = pose.ptr(j); + double *sum_ptr = sum_poses.ptr(j); + + double score = pose_ptr[3]; + if (score > min_score) { - for (int k = 0; k < 4; ++k) - { - sum_poses_ptr[j * 4 + k] += pose_ptr[j * 4 + k]; - } + 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]++; } } } - 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); + cv::Mat initial_pose_3d = cv::Mat::zeros(num_joints, 4, CV_64F); for (int j = 0; j < num_joints; ++j) { if (sum_mask[j] > 0) { - for (int k = 0; k < 4; ++k) - { - initial_pose_3d_ptr[j * 4 + k] = sum_poses_ptr[j * 4 + k] / sum_mask[j]; - } + double *initial_ptr = initial_pose_3d.ptr(j); + const double *sum_ptr = sum_poses.ptr(j); + + double 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; } } // Use center as default if the initial pose is empty - std::vector jmask(num_joints, false); + std::vector jmask(num_joints, 0); cv::Point3d center(0, 0, 0); int valid_joints = 0; for (int j = 0; j < num_joints; ++j) { - if (initial_pose_3d_ptr[j * 4 + 3] > min_score) + const double *initial_ptr = initial_pose_3d.ptr(j); + double score = initial_ptr[3]; + + if (score > min_score) { - 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]; + jmask[j] = 1; + center.x += initial_ptr[0]; + center.y += initial_ptr[1]; + center.z += initial_ptr[2]; valid_joints++; } } @@ -1155,35 +1263,39 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, { if (!jmask[j]) { - 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; + double *initial_ptr = initial_pose_3d.ptr(j); + initial_ptr[0] = center.x; + initial_ptr[1] = center.y; + initial_ptr[2] = center.z; } } // Drop joints with low scores and filter outlying joints using distance threshold double offset = 0.1; - double max_dist = 1.2; + double 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_64F); - double *distances_ptr = distances.ptr(0); - u_char *mask_ptr = mask.ptr(0); for (int i = 0; i < num_poses; ++i) { + double *distances_ptr = distances.ptr(i); + u_char *mask_ptr = mask.ptr(i); const cv::Mat &pose = poses_3d[i]; - const double *pose_ptr = pose.ptr(0); for (int j = 0; j < num_joints; ++j) { - 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; + const double *initial_ptr = initial_pose_3d.ptr(j); + const double *pose_ptr = pose.ptr(j); - if (dist <= max_dist && pose_ptr[j * 4 + 3] > (min_score - offset)) + double dx = pose_ptr[0] - initial_ptr[0]; + double dy = pose_ptr[1] - initial_ptr[1]; + double dz = pose_ptr[2] - initial_ptr[2]; + double dist_sq = dx * dx + dy * dy + dz * dz; + distances_ptr[j] = dist_sq; + + double score = pose_ptr[3]; + if (dist_sq <= max_dist_sq && score > (min_score - offset)) { - mask_ptr[i * num_joints + j] = 1; + mask_ptr[j] = 1; } } } @@ -1194,56 +1306,65 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, for (int j = 0; j < num_joints; ++j) { std::vector> valid_indices; + valid_indices.reserve(num_poses); for (int i = 0; i < num_poses; ++i) { - if (mask_ptr[i * num_joints + j]) + if (mask.at(i, j)) { - auto item = std::make_pair(distances_ptr[i * num_joints + j], i); - valid_indices.push_back(item); + valid_indices.emplace_back(distances.at(i, j), i); } } - std::sort(valid_indices.begin(), valid_indices.end()); + 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) { - best_k_mask.at(valid_indices[k].second, j) = 1; + best_k_mask.at(valid_indices[k].second, j) = 1; } } // Combine masks - mask = mask & best_k_mask; - mask_ptr = mask.ptr(0); + mask &= best_k_mask; // 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); + sum_poses = cv::Mat::zeros(num_joints, 4, CV_64F); + sum_mask.assign(num_joints, 0); for (int i = 0; i < num_poses; ++i) { + const u_char *mask_row_ptr = mask.ptr(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_ptr[i * num_joints + j] > 0) + if (mask_row_ptr[j]) { - for (int k = 0; k < 4; ++k) - { - sum_poses_ptr[j * 4 + k] += pose_ptr[j * 4 + k]; - } + double *sum_ptr = sum_poses.ptr(j); + const double *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]++; } } } - cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); - double *final_pose_3d_ptr = final_pose_3d.ptr(0); + cv::Mat final_pose_3d = cv::Mat::zeros(num_joints, 4, CV_64F); for (int j = 0; j < num_joints; ++j) { if (sum_mask[j] > 0) { - for (int k = 0; k < 4; ++k) - { - final_pose_3d_ptr[j * 4 + k] = sum_poses_ptr[j * 4 + k] / sum_mask[j]; - } + double *final_pose_ptr = final_pose_3d.ptr(j); + const double *sum_ptr = sum_poses.ptr(j); + + double 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; } }