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

Weigh positions at merge by exponential score.

Browse Source
This commit is contained in:
Daniel
2025-03-10 11:48:51 +01:00
parent 63f373e7ec
commit e9eb4c5d96
2 changed files with 592 additions and 589 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1154
media/RESULTS.md
View File

File diff suppressed because it is too large Load Diff

27
rpt/triangulator.cpp
View File

@ -1787,7 +1787,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
// Merge poses to create initial pose
// Use only those triangulations with a high score
std::vector<std::array<float, 4>> sum_poses(num_joints, {0.0, 0.0, 0.0, 0.0});
std::vector<int> sum_mask1(num_joints, 0);
std::vector<float> sum_mask1(num_joints, 0);
for (size_t i = 0; i < num_poses; ++i)
{
const auto &pose = poses_3d[i];
@ -1797,11 +1797,12 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
float score = pose[j][3];
if (score > min_score)
{
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]++;
float weight = std::pow(score, 2);
sum_poses[j][0] += pose[j][0] * weight;
sum_poses[j][1] += pose[j][1] * weight;
sum_poses[j][2] += pose[j][2] * weight;
sum_poses[j][3] += score * weight;
sum_mask1[j] += weight;
}
}
}
@ -1915,7 +1916,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
// Compute the final pose
std::vector<std::array<float, 4>> sum_poses2(num_joints, {0.0, 0.0, 0.0, 0.0});
std::vector<int> sum_mask2(num_joints, 0);
std::vector<float> sum_mask2(num_joints, 0);
for (size_t i = 0; i < num_poses; ++i)
{
const auto &pose = poses_3d[i];
@ -1924,11 +1925,13 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
{
if (mask[i][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]++;
// Use an exponential weighting to give more importance to higher scores
float weight = std::pow(pose[j][3], 4);
sum_poses2[j][0] += pose[j][0] * weight;
sum_poses2[j][1] += pose[j][1] * weight;
sum_poses2[j][2] += pose[j][2] * weight;
sum_poses2[j][3] += pose[j][3] * weight;
sum_mask2[j] += weight;
}
}
}