Drop triangulation pairs with double associations.

rpt/triangulator.cpp

@@ -4,6 +4,7 @@
 #include <iomanip>
 #include <map>
 #include <numeric>
+#include <set>
 #include <unordered_map>
 
 #include "camera.hpp"
@@ -803,8 +804,106 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         groups = calc_grouping(all_pairs, all_scored_poses, min_match_score);
 
     // Drop groups with too few matches
-    size_t num_groups = groups.size();
+    size_t num_groups_1 = groups.size();
-    for (size_t i = num_groups; i > 0; --i)
+    for (size_t i = num_groups_1; i > 0; --i)
+    {
+        if (std::get<2>(groups[i - 1]).size() < this->min_group_size)
+        {
+            groups.erase(groups.begin() + i - 1);
+        }
+    }
+
+    // Drop pairs with double associations, where the same 2D person appears in multiple pairs.
+    // If two not-same persons have relatively similar poses, the triangulation could create a
+    // false positive virtual person. This can lead to a single 2D skeleton being associated with
+    // multiple 3D skeletons. To avoid this, check if the same 2D person appears in multiple valid
+    // pairs. If so, drop the pairs in less populated groups or with the lower scores.
+    std::map<std::tuple<int, int, int>, std::vector<size_t>> pairs_map;
+    for (size_t i = 0; i < all_pairs.size(); ++i)
+    {
+        const auto &p = all_pairs[i];
+        const auto &mid1 = std::make_tuple(
+            std::get<0>(p.first), std::get<1>(p.first), std::get<0>(p.second));
+        const auto &mid2 = std::make_tuple(
+            std::get<0>(p.first), std::get<1>(p.first), std::get<1>(p.second));
+        pairs_map[mid1].push_back(i);
+        pairs_map[mid2].push_back(i);
+    }
+    std::vector<size_t> group_map;
+    group_map.resize(all_pairs.size());
+    for (size_t i = 0; i < groups.size(); ++i)
+    {
+        const auto &group = groups[i];
+        const auto &indices = std::get<2>(group);
+        for (const auto &idx : indices)
+        {
+            group_map[idx] = i;
+        }
+    }
+    std::set<size_t> drop_indices;
+    for (auto &pair : pairs_map)
+    {
+        auto &indices = pair.second;
+        if (indices.size() > 1)
+        {
+            std::vector<size_t> group_sizes;
+            std::vector<float> pair_scores;
+            for (auto &idx : indices)
+            {
+                group_sizes.push_back(std::get<2>(groups[group_map[idx]]).size());
+                pair_scores.push_back(all_scored_poses[idx].second);
+            }
+
+            // Sort indices by group size (prio-1) and pair score (prio-2)
+            std::vector<size_t> indices_sorted(indices.size());
+            std::iota(indices_sorted.begin(), indices_sorted.end(), 0);
+            std::sort(indices_sorted.begin(), indices_sorted.end(),
+                      [&group_sizes, &pair_scores](size_t a, size_t b)
+                      {
+                          if (group_sizes[a] != group_sizes[b])
+                          {
+                              return group_sizes[a] > group_sizes[b];
+                          }
+                          return pair_scores[a] > pair_scores[b];
+                      });
+
+            // Drop all but the first index
+            for (size_t j = 1; j < indices_sorted.size(); ++j)
+            {
+                size_t drop_idx = indices[indices_sorted[j]];
+                drop_indices.insert(drop_idx);
+            }
+        }
+    }
+    std::vector<size_t> drop_list(drop_indices.begin(), drop_indices.end());
+    std::sort(drop_list.begin(), drop_list.end(), std::greater<size_t>());
+    for (size_t i = 0; i < drop_list.size(); ++i)
+    {
+        all_scored_poses.erase(all_scored_poses.begin() + drop_list[i]);
+        all_pairs.erase(all_pairs.begin() + drop_list[i]);
+
+        // Remove pairs from groups and update indices of the remaining pairs
+        for (size_t j = 0; j < groups.size(); ++j)
+        {
+            auto &indices = std::get<2>(groups[j]);
+            auto it = std::find(indices.begin(), indices.end(), drop_list[i]);
+            if (it != indices.end())
+            {
+                indices.erase(it);
+            }
+            for (size_t k = 0; k < std::get<2>(groups[j]).size(); ++k)
+            {
+                if ((size_t)std::get<2>(groups[j])[k] > drop_list[i])
+                {
+                    std::get<2>(groups[j])[k] -= 1;
+                }
+            }
+        }
+    }
+
+    // Drop groups with too few matches again
+    size_t num_groups_2 = groups.size();
+    for (size_t i = num_groups_2; i > 0; --i)
     {
         if (std::get<2>(groups[i - 1]).size() < this->min_group_size)
         {