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

Slight performance improvements.

Browse Source
This commit is contained in:
Daniel
2025-07-07 11:43:27 +02:00
parent 53f13d6f97
commit ed5d25f595
4 changed files with 3834 additions and 3827 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
rpt/triangulator.cpp
View File

@ -1405,7 +1405,7 @@ std::array<float, 3> calc_ray_dir(const CameraInternal &icam, const std::array<f
return ray_dir;
}
std::array<float, 3> triangulate_midpoint(
std::array<float, 4> triangulate_midpoint(
const CameraInternal &icam1,
const CameraInternal &icam2,
const std::array<float, 2> &pt1,
@ -1435,7 +1435,10 @@ std::array<float, 3> triangulate_midpoint(
// Compute midpoint between c1 and c2.
std::array<float, 3> midpoint = multiply(add(c1, c2), 0.5);
return midpoint;
float dist = std::sqrt(dot(subtract(c1, c2), subtract(c1, c2)));
std::array<float, 4> result = {midpoint[0], midpoint[1], midpoint[2], dist};
return result;
}
// =================================================================================================
@ -1491,9 +1494,11 @@ std::pair<std::vector<std::array<float, 4>>, float> TriangulatorInternal::triang
{
auto &pt1 = pose1[i];
auto &pt2 = pose2[i];
std::array<float, 3> pt3d = triangulate_midpoint(
std::array<float, 4> pt3d = triangulate_midpoint(
cam1, cam2, {pt1[0], pt1[1]}, {pt2[0], pt2[1]});
pose3d[i] = {pt3d[0], pt3d[1], pt3d[2], 1.0};
float score = std::max(0.0, 1.0 - pt3d[3]);
pose3d[i] = {pt3d[0], pt3d[1], pt3d[2], score};
}
}
@ -1546,13 +1551,15 @@ std::pair<std::vector<std::array<float, 4>>, float> TriangulatorInternal::triang
std::vector<float> score1 = score_projection(pose1, repro1, dists1, mask, iscale);
std::vector<float> score2 = score_projection(pose2, repro2, dists2, mask, iscale);
// Add scores to 3D pose
// Update scores of 3D pose
for (size_t i = 0; i < num_joints; ++i)
{
if (mask[i])
{
// Using mainly the reprojection score leads to slightly better average results,
// but the triangulation score is used to penalize some bad outliers.
float scoreT = 0.5 * (score1[i] + score2[i]);
pose3d[i][3] = scoreT;
pose3d[i][3] = 0.9 * scoreT + 0.1 * pose3d[i][3];
}
}