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

Filter only by corejoints outside room.

Browse Source
This commit is contained in:
Daniel
2025-03-19 14:37:15 +01:00
parent ccc7f832c4
commit 6e68dcdf34
1 changed files with 23 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
rpt/triangulator.cpp
View File

@ -1996,15 +1996,17 @@ void TriangulatorInternal::filter_poses(
for (size_t i = 0; i < poses.size(); ++i) for (size_t i = 0; i < poses.size(); ++i)
{ {
auto &pose = poses[i]; auto &pose = poses[i];
size_t num_joints = pose.size(); size_t num_core_joints = core_joint_idx.size();
size_t num_full_joints = pose.size();
// Collect valid joint indices // Collect valid joint indices
std::vector<size_t> valid_joint_idx; std::vector<size_t> valid_joint_idx;
for (size_t j = 0; j < num_joints; ++j) for (size_t j = 0; j < num_core_joints; ++j)
{ {
if (pose[j][3] > min_score) size_t idx = core_joint_idx[j];
if (pose[idx][3] > min_score)
{ {
valid_joint_idx.push_back(j); valid_joint_idx.push_back(idx);
} }
} }
@ -2102,6 +2104,23 @@ void TriangulatorInternal::filter_poses(
continue; continue;
} }
// Set joint scores outside the room to a low value
for (size_t j = 0; j < num_full_joints; ++j)
{
if (pose[j][3] > min_score)
{
for (int k = 0; k < 3; ++k)
{
if (pose[j][k] > room_half_size[k] + room_center[k] + wdist ||
pose[j][k] < -room_half_size[k] + room_center[k] - wdist)
{
pose[j][3] = 0.001;
break;
}
}
}
}
// Calculate total limb length and average limb length // Calculate total limb length and average limb length
const float max_avg_length = 0.5; const float max_avg_length = 0.5;
const float min_avg_length = 0.1; const float min_avg_length = 0.1;