crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improved group average calculation.

Browse Source
This commit is contained in:
Daniel
2024-10-08 17:21:01 +02:00
parent 99aa3bb301
commit fa2af31349
4 changed files with 481 additions and 461 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
spt/triangulator.cpp
View File

@ -455,7 +455,7 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
}
// Group pairs that share a person
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> groups;
std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> groups;
groups = calc_grouping(all_pairs, all_scored_poses, min_score);
// Drop groups with too few matches
@ -1149,7 +1149,7 @@ std::pair<cv::Mat, float> TriangulatorInternal::triangulate_and_score(
// =================================================================================================
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInternal::calc_grouping(
std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> TriangulatorInternal::calc_grouping(
const std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
const std::vector<std::pair<cv::Mat, float>> &all_scored_poses,
float min_score)
@ -1159,14 +1159,14 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
size_t num_pairs = all_pairs.size();
// Calculate pose centers
std::vector<cv::Point3d> centers;
std::vector<cv::Point3f> 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;
cv::Point3d center(0, 0, 0);
cv::Point3f center(0, 0, 0);
size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j)
{
@ -1193,20 +1193,21 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
// Calculate Groups
// defined as a tuple of center, pose, and all-pairs-indices of members
std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> groups;
std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> groups;
std::vector<std::vector<size_t>> 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 cv::Point3d &center = centers[i];
const cv::Point3f &center = centers[i];
float best_dist = std::numeric_limits<float>::infinity();
int best_group = -1;
for (size_t j = 0; j < groups.size(); ++j)
{
auto &group = groups[j];
cv::Point3d &group_center = std::get<0>(group);
cv::Point3f &group_center = std::get<0>(group);
// Check if the center is close enough
float dx = group_center.x - center.x;
@ -1258,12 +1259,24 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
// Create a new group
std::vector<int> new_indices{static_cast<int>(i)};
groups.emplace_back(center, pose_3d.clone(), std::move(new_indices));
// Update per joint counts
std::vector<size_t> new_valid_joint_counts(num_joints, 0);
for (size_t row = 0; row < num_joints; ++row)
{
const float *pose_3d_ptr = pose_3d.ptr<float>(row);
if (pose_3d_ptr[3] > min_score)
{
new_valid_joint_counts[row] = 1;
}
}
per_group_visible_counts.push_back(std::move(new_valid_joint_counts));
}
else
{
// Update existing group
auto &group = groups[best_group];
cv::Point3d &group_center = std::get<0>(group);
cv::Point3f &group_center = std::get<0>(group);
cv::Mat &group_pose = std::get<1>(group);
std::vector<int> &group_indices = std::get<2>(group);
@ -1281,10 +1294,17 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
const float *pose_3d_ptr = pose_3d.ptr<float>(row);
float *group_pose_ptr = group_pose.ptr<float>(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;
if (pose_3d_ptr[3] > min_score)
{
float j_elems = static_cast<float>(per_group_visible_counts[best_group][row]);
float inv_j1 = 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;
per_group_visible_counts[best_group][row]++;
}
}
group_indices.push_back(static_cast<int>(i));
}
@ -1342,7 +1362,7 @@ cv::Mat TriangulatorInternal::merge_group(const std::vector<cv::Mat> &poses_3d,
// Use center as default if the initial pose is empty
std::vector<u_char> jmask(num_joints, 0);
cv::Point3d center(0, 0, 0);
cv::Point3f center(0, 0, 0);
int valid_joints = 0;
for (int j = 0; j < num_joints; ++j)
{