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",
};
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);
};
@ -107,15 +107,41 @@ std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> PoseTracker::
}
// Match core poses to tracks
std::vector<std::tuple<size_t, int, float>> matches;
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)
{
// Create a new track
Track new_track;
new_track.core_poses.push_back(core_poses[i]);
new_track.full_poses.push_back(poses_3d[i]);
new_track.core_poses.push_back(core_poses[pose_idx]);
new_track.full_poses.push_back(poses_3d[pose_idx]);
new_track.timestamps.push_back(timestamp);
new_track.id = pose_tracks.size();
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
auto &track = pose_tracks[track_idx];
track.core_poses.push_back(core_poses[i]);
track.full_poses.push_back(poses_3d[i]);
track.core_poses.push_back(core_poses[pose_idx]);
track.full_poses.push_back(poses_3d[pose_idx]);
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;
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];
if (track.core_poses.size() == 0)
{
continue;
}
// Calculate distance to the last pose in the track
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);
}
}
return best_track;
return {best_track, best_distance_sq};
}
// =================================================================================================