diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index 25cbf47..e18a1ba 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "camera.hpp" @@ -803,8 +804,106 @@ std::vector>> 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(); - for (size_t i = num_groups; i > 0; --i) + size_t num_groups_1 = groups.size(); + 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::vector> 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 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 drop_indices; + for (auto &pair : pairs_map) + { + auto &indices = pair.second; + if (indices.size() > 1) + { + std::vector group_sizes; + std::vector 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 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 drop_list(drop_indices.begin(), drop_indices.end()); + std::sort(drop_list.begin(), drop_list.end(), std::greater()); + 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) {