From e90d7c53f4e4e7ac5133f20e98880ef61cea3a1a Mon Sep 17 00:00:00 2001
From: Daniel <daniel@mail.de>
Date: Wed, 23 Apr 2025 10:41:47 +0200
Subject: [PATCH] Prevent duplicate track assignments.

---
 rpt/tracker.hpp | 45 +++++++++++++++++++++++++++++++++++++--------
 1 file changed, 37 insertions(+), 8 deletions(-)

diff --git a/rpt/tracker.hpp b/rpt/tracker.hpp
index b791f1b..d8049da 100644
--- a/rpt/tracker.hpp
+++ b/rpt/tracker.hpp
@@ -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};
 }
 
 // =================================================================================================