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

Made some thresholds dynamic.

Browse Source
This commit is contained in:
Daniel
2025-03-25 10:26:09 +01:00
parent fa0a4da72d
commit 71968d06fd
3 changed files with 152 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
rpt/triangulator.cpp
View File

@ -525,6 +525,7 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
}
// Convert inputs to internal formats
this->num_cams = cameras.size();
std::vector<std::vector<std::vector<std::array<float, 3>>>> i_poses_2d = poses_2d;
std::vector<CameraInternal> internal_cameras;
for (size_t i = 0; i < cameras.size(); ++i)
@ -1809,6 +1810,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
// 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<float> sum_mask1(num_joints, 0);
float offset1 = (1.0 - min_score) / 2.0;
for (size_t i = 0; i < num_poses; ++i)
{
const auto &pose = poses_3d[i];
@ -1816,7 +1818,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
for (size_t j = 0; j < num_joints; ++j)
{
float score = pose[j][3];
if (score > min_score)
if (score > min_score - offset1)
{
float weight = std::pow(score, 2);
sum_poses[j][0] += pose[j][0] * weight;
@ -1847,7 +1849,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
for (size_t j = 0; j < num_joints; ++j)
{
float score = initial_pose_3d[j][3];
if (score > min_score)
if (score > min_score - offset1)
{
jmask[j] = true;
center[0] += initial_pose_3d[j][0];
@ -1875,7 +1877,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
}
// Drop joints with low scores and filter outlying joints using distance threshold
float offset = 0.1;
float offset2 = (1.0 - min_score) * 2.0;
float max_dist_sq = 1.2 * 1.2;
std::vector<std::vector<bool>> mask(num_poses, std::vector<bool>(num_joints, false));
std::vector<std::vector<float>> distances(num_poses, std::vector<float>(num_joints, 0.0));
@ -1891,7 +1893,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
distances[i][j] = dist_sq;
float score = pose[j][3];
if (dist_sq <= max_dist_sq && score > (min_score - offset))
if (dist_sq <= max_dist_sq && score > (min_score - offset2))
{
mask[i][j] = true;
}
@ -1899,7 +1901,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
}
// Select the best-k proposals for each joint that are closest to the initial pose
int keep_best = std::max(3, (int)num_poses / 3);
int keep_best = std::max((int)std::ceil(this->num_cams / 2.0), (int)std::ceil(num_poses / 3.0));
std::vector<std::vector<bool>> best_k_mask(num_poses, std::vector<bool>(num_joints, false));
for (size_t j = 0; j < num_joints; ++j)
{