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

Prevent duplicate track assignments.

Browse Source
This commit is contained in:
Daniel
2025-04-23 10:41:47 +02:00
parent cc8b75b8bf
commit e90d7c53f4
1 changed files with 37 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
rpt/tracker.hpp
View File

@ -56,7 +56,7 @@ private:
"ankle_right", "ankle_right",
}; };
int match_to_track(const std::vector<std::array<float, 4>> &core_pose_3d); std::tuple<int, float> match_to_track(const std::vector<std::array<float, 4>> &core_pose_3d);
std::vector<std::array<float, 4>> refine_pose(const Track &track); std::vector<std::array<float, 4>> refine_pose(const Track &track);
}; };
@ -107,15 +107,41 @@ std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> PoseTracker::
} }
// Match core poses to tracks // Match core poses to tracks
std::vector<std::tuple<size_t, int, float>> matches;
for (size_t i = 0; i < core_poses.size(); ++i) for (size_t i = 0; i < core_poses.size(); ++i)
{ {
int track_idx = match_to_track(core_poses[i]); auto [track_idx, distance_sq] = match_to_track(core_poses[i]);
matches.emplace_back(i, track_idx, distance_sq);
}
std::sort(matches.begin(), matches.end(),
[](const auto &a, const auto &b)
{ return std::get<2>(a) < std::get<2>(b); });
// If track is matched multiple times, only add the best and create new tracks for the rest
std::vector<bool> used(pose_tracks.size(), false);
for (size_t i = 0; i < matches.size(); ++i)
{
auto [pose_idx, track_idx, distance_sq] = matches[i];
if (track_idx != -1 && !used[track_idx])
{
used[track_idx] = true;
}
else
{
std::get<1>(matches[i]) = -1;
}
}
// Update tracks
for (size_t i = 0; i < matches.size(); ++i)
{
auto [pose_idx, track_idx, distance_sq] = matches[i];
if (track_idx == -1) if (track_idx == -1)
{ {
// Create a new track // Create a new track
Track new_track; Track new_track;
new_track.core_poses.push_back(core_poses[i]); new_track.core_poses.push_back(core_poses[pose_idx]);
new_track.full_poses.push_back(poses_3d[i]); new_track.full_poses.push_back(poses_3d[pose_idx]);
new_track.timestamps.push_back(timestamp); new_track.timestamps.push_back(timestamp);
new_track.id = pose_tracks.size(); new_track.id = pose_tracks.size();
pose_tracks.push_back(new_track); pose_tracks.push_back(new_track);
@ -124,8 +150,8 @@ std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> PoseTracker::
{ {
// Update existing track // Update existing track
auto &track = pose_tracks[track_idx]; auto &track = pose_tracks[track_idx];
track.core_poses.push_back(core_poses[i]); track.core_poses.push_back(core_poses[pose_idx]);
track.full_poses.push_back(poses_3d[i]); track.full_poses.push_back(poses_3d[pose_idx]);
track.timestamps.push_back(timestamp); track.timestamps.push_back(timestamp);
} }
} }
@ -179,7 +205,7 @@ std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> PoseTracker::
// ================================================================================================= // =================================================================================================
int PoseTracker::match_to_track(const std::vector<std::array<float, 4>> &core_pose_3d) std::tuple<int, float> PoseTracker::match_to_track(const std::vector<std::array<float, 4>> &core_pose_3d)
{ {
int best_track = -1; int best_track = -1;
float best_distance_sq = max_distance * max_distance; float best_distance_sq = max_distance * max_distance;
@ -188,7 +214,9 @@ int PoseTracker::match_to_track(const std::vector<std::array<float, 4>> &core_po
{ {
const auto &track = pose_tracks[i]; const auto &track = pose_tracks[i];
if (track.core_poses.size() == 0) if (track.core_poses.size() == 0)
{
continue; continue;
}
// Calculate distance to the last pose in the track // Calculate distance to the last pose in the track
const auto &last_pose = track.core_poses.back(); const auto &last_pose = track.core_poses.back();
@ -208,7 +236,8 @@ int PoseTracker::match_to_track(const std::vector<std::array<float, 4>> &core_po
best_track = static_cast<int>(i); best_track = static_cast<int>(i);
} }
} }
return best_track;
return {best_track, best_distance_sq};
} }
// ================================================================================================= // =================================================================================================