Add tracked triangulation association reports

rpt/interface.cpp

@@ -43,6 +43,16 @@ const float &PoseBatch3DView::at(size_t person, size_t joint, size_t coord) cons
     return data[pose3d_offset(person, joint, coord, num_joints)];
 }
 
+int64_t TrackedPoseBatch3DView::track_id(size_t person) const
+{
+    return track_ids[person];
+}
+
+const float &TrackedPoseBatch3DView::at(size_t person, size_t joint, size_t coord) const
+{
+    return data[pose3d_offset(person, joint, coord, num_joints)];
+}
+
 const float &PoseBatch2D::at(size_t view, size_t person, size_t joint, size_t coord) const
 {
     return data[pose2d_offset(view, person, joint, coord, max_persons, num_joints)];

rpt/interface.hpp

@@ -34,6 +34,17 @@ struct PoseBatch3DView
     const float &at(size_t person, size_t joint, size_t coord) const;
 };
 
+struct TrackedPoseBatch3DView
+{
+    const int64_t *track_ids = nullptr;
+    const float *data = nullptr;
+    size_t num_persons = 0;
+    size_t num_joints = 0;
+
+    int64_t track_id(size_t person) const;
+    const float &at(size_t person, size_t joint, size_t coord) const;
+};
+
 struct PoseBatch2D
 {
     std::vector<float> data;
@@ -78,6 +89,7 @@ struct PairCandidate
 struct PreviousPoseMatch
 {
     int previous_pose_index = -1;
+    int64_t previous_track_id = -1;
     float score_view1 = 0.0f;
     float score_view2 = 0.0f;
     bool matched_view1 = false;
@@ -131,6 +143,38 @@ struct MergeDebug
     std::vector<std::vector<int>> group_proposal_indices;
 };
 
+enum class AssociationStatus
+{
+    Matched,
+    New,
+    Ambiguous,
+};
+
+struct AssociationReport
+{
+    std::vector<int> pose_previous_indices;
+    std::vector<int64_t> pose_previous_track_ids;
+    std::vector<AssociationStatus> pose_status;
+    std::vector<std::vector<int>> pose_candidate_previous_indices;
+    std::vector<std::vector<int64_t>> pose_candidate_previous_track_ids;
+    std::vector<int> unmatched_previous_indices;
+    std::vector<int64_t> unmatched_previous_track_ids;
+    std::vector<int> new_pose_indices;
+    std::vector<int> ambiguous_pose_indices;
+};
+
+struct FinalPoseAssociationDebug
+{
+    int final_pose_index = -1;
+    std::vector<int> source_core_proposal_indices;
+    std::vector<int> source_pair_indices;
+    std::vector<int> candidate_previous_indices;
+    std::vector<int64_t> candidate_previous_track_ids;
+    int resolved_previous_index = -1;
+    int64_t resolved_previous_track_id = -1;
+    AssociationStatus status = AssociationStatus::New;
+};
+
 struct TriangulationTrace
 {
     std::vector<PairCandidate> pairs;
@@ -139,9 +183,17 @@ struct TriangulationTrace
     GroupingDebug grouping;
     std::vector<FullProposalDebug> full_proposals;
     MergeDebug merge;
+    AssociationReport association;
+    std::vector<FinalPoseAssociationDebug> final_pose_associations;
     PoseBatch3D final_poses;
 };
 
+struct TriangulationResult
+{
+    PoseBatch3D poses;
+    AssociationReport association;
+};
+
 // =================================================================================================
 
 struct TriangulationOptions
@@ -165,40 +217,15 @@ std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d
 PreviousPoseFilterDebug filter_pairs_with_previous_poses(
     const PoseBatch2DView &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3DView &previous_poses_3d,
+    const TrackedPoseBatch3DView &previous_poses_3d,
     const TriangulationOptions *options_override = nullptr);
 
-inline PreviousPoseFilterDebug filter_pairs_with_previous_poses(
-    const PoseBatch2D &poses_2d,
-    const TriangulationConfig &config,
-    const PoseBatch3D &previous_poses_3d,
-    const TriangulationOptions *options_override = nullptr)
-{
-    return filter_pairs_with_previous_poses(poses_2d.view(), config, previous_poses_3d.view(), options_override);
-}
-
 TriangulationTrace triangulate_debug(
     const PoseBatch2DView &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3DView *previous_poses_3d = nullptr,
+    const TrackedPoseBatch3DView *previous_poses_3d = nullptr,
     const TriangulationOptions *options_override = nullptr);
 
-inline TriangulationTrace triangulate_debug(
-    const PoseBatch2D &poses_2d,
-    const TriangulationConfig &config,
-    const PoseBatch3D *previous_poses_3d = nullptr,
-    const TriangulationOptions *options_override = nullptr)
-{
-    PoseBatch3DView previous_view_storage;
-    const PoseBatch3DView *previous_view = nullptr;
-    if (previous_poses_3d != nullptr)
-    {
-        previous_view_storage = previous_poses_3d->view();
-        previous_view = &previous_view_storage;
-    }
-    return triangulate_debug(poses_2d.view(), config, previous_view, options_override);
-}
-
 // =================================================================================================
 
 /**
@@ -213,21 +240,35 @@ inline TriangulationTrace triangulate_debug(
 PoseBatch3D triangulate_poses(
     const PoseBatch2DView &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3DView *previous_poses_3d = nullptr,
+    const TriangulationOptions *options_override = nullptr);
+
+TriangulationResult triangulate_with_report(
+    const PoseBatch2DView &poses_2d,
+    const TriangulationConfig &config,
+    const TrackedPoseBatch3DView &previous_poses_3d,
     const TriangulationOptions *options_override = nullptr);
 
 inline PoseBatch3D triangulate_poses(
     const PoseBatch2D &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3D *previous_poses_3d = nullptr,
     const TriangulationOptions *options_override = nullptr)
 {
-    PoseBatch3DView previous_view_storage;
-    const PoseBatch3DView *previous_view = nullptr;
-    if (previous_poses_3d != nullptr)
-    {
-        previous_view_storage = previous_poses_3d->view();
-        previous_view = &previous_view_storage;
-    }
-    return triangulate_poses(poses_2d.view(), config, previous_view, options_override);
+    return triangulate_poses(poses_2d.view(), config, options_override);
+}
+
+inline TriangulationTrace triangulate_debug(
+    const PoseBatch2D &poses_2d,
+    const TriangulationConfig &config,
+    const TriangulationOptions *options_override = nullptr)
+{
+    return triangulate_debug(poses_2d.view(), config, nullptr, options_override);
+}
+
+inline TriangulationResult triangulate_with_report(
+    const PoseBatch2D &poses_2d,
+    const TriangulationConfig &config,
+    const TrackedPoseBatch3DView &previous_poses_3d,
+    const TriangulationOptions *options_override = nullptr)
+{
+    return triangulate_with_report(poses_2d.view(), config, previous_poses_3d, options_override);
 }

rpt/triangulator.cpp

@@ -9,6 +9,7 @@
 #include <stdexcept>
 #include <string_view>
 #include <unordered_map>
+#include <unordered_set>
 
 #include "interface.hpp"
 
@@ -190,6 +191,15 @@ struct PreviousProjectionCache
     std::vector<Pose3D> core_poses;
 };
 
+struct FinalAssociationState
+{
+    std::vector<int> candidate_previous_indices;
+    std::vector<int64_t> candidate_previous_track_ids;
+    int resolved_previous_index = -1;
+    int64_t resolved_previous_track_id = -1;
+    AssociationStatus status = AssociationStatus::New;
+};
+
 constexpr std::array<std::string_view, 12> kCoreJoints = {
     "shoulder_left",
     "shoulder_right",
@@ -218,7 +228,7 @@ constexpr std::array<std::pair<std::string_view, std::string_view>, 8> kCoreLimb
 
 std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses);
 PreviousProjectionCache project_previous_poses(
-    const PoseBatch3DView &previous_poses_3d,
+    const TrackedPoseBatch3DView &previous_poses_3d,
     const std::vector<Camera> &internal_cameras,
     const std::vector<size_t> &core_joint_idx);
 PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
@@ -227,7 +237,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
     const std::vector<size_t> &core_joint_idx,
     const std::vector<PairCandidate> &pairs,
     const TriangulationOptions &options,
-    const PoseBatch3DView &previous_poses_3d);
+    const TrackedPoseBatch3DView &previous_poses_3d);
 float calc_pose_score(
     const Pose2D &pose,
     const Pose2D &reference_pose,
@@ -279,7 +289,7 @@ TriangulationTrace triangulate_debug_impl(
     const std::vector<Camera> &cameras,
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
-    const PoseBatch3DView *previous_poses_3d,
+    const TrackedPoseBatch3DView *previous_poses_3d,
     const TriangulationOptions &options);
 
 PreparedInputs prepare_inputs(
@@ -346,6 +356,39 @@ PreparedInputs prepare_inputs(
         std::move(packed_poses));
 }
 
+void validate_previous_tracks(
+    const TrackedPoseBatch3DView &previous_poses_3d,
+    const std::vector<std::string> &joint_names)
+{
+    if (previous_poses_3d.num_persons == 0)
+    {
+        return;
+    }
+    if (previous_poses_3d.track_ids == nullptr)
+    {
+        throw std::invalid_argument("previous_track_ids must not be null.");
+    }
+    if (previous_poses_3d.data == nullptr)
+    {
+        throw std::invalid_argument("previous_poses_3d data must not be null.");
+    }
+    if (previous_poses_3d.num_joints != joint_names.size())
+    {
+        throw std::invalid_argument("previous_poses_3d must use the same joint count as joint_names.");
+    }
+
+    std::unordered_set<int64_t> seen_track_ids;
+    seen_track_ids.reserve(previous_poses_3d.num_persons);
+    for (size_t person = 0; person < previous_poses_3d.num_persons; ++person)
+    {
+        const int64_t track_id = previous_poses_3d.track_id(person);
+        if (!seen_track_ids.insert(track_id).second)
+        {
+            throw std::invalid_argument("previous_track_ids must be unique.");
+        }
+    }
+}
+
 std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses)
 {
     std::vector<PairCandidate> pairs;
@@ -373,7 +416,7 @@ std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseSto
 }
 
 PreviousProjectionCache project_previous_poses(
-    const PoseBatch3DView &previous_poses_3d,
+    const TrackedPoseBatch3DView &previous_poses_3d,
     const std::vector<Camera> &internal_cameras,
     const std::vector<size_t> &core_joint_idx)
 {
@@ -449,7 +492,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
     const std::vector<size_t> &core_joint_idx,
     const std::vector<PairCandidate> &pairs,
     const TriangulationOptions &options,
-    const PoseBatch3DView &previous_poses_3d)
+    const TrackedPoseBatch3DView &previous_poses_3d)
 {
     PreviousPoseFilterDebug debug;
     debug.used_previous_poses = true;
@@ -500,6 +543,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
             {
                 best_match = PreviousPoseMatch {
                     static_cast<int>(previous_index),
+                    previous_poses_3d.track_id(previous_index),
                     score_view1,
                     score_view2,
                     true,
@@ -515,6 +559,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
             {
                 best_match = PreviousPoseMatch {
                     static_cast<int>(previous_index),
+                    previous_poses_3d.track_id(previous_index),
                     score_view1,
                     score_view2,
                     matched_view1,
@@ -549,16 +594,15 @@ TriangulationTrace triangulate_debug_impl(
     const std::vector<Camera> &cameras,
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
-    const PoseBatch3DView *previous_poses_3d,
+    const TrackedPoseBatch3DView *previous_poses_3d,
     const TriangulationOptions &options)
 {
     TriangulationTrace trace;
     trace.final_poses.num_joints = joint_names.size();
 
-    if (previous_poses_3d != nullptr && previous_poses_3d->num_persons > 0 &&
-        previous_poses_3d->num_joints != joint_names.size())
+    if (previous_poses_3d != nullptr)
     {
-        throw std::invalid_argument("previous_poses_3d must use the same joint count as joint_names.");
+        validate_previous_tracks(*previous_poses_3d, joint_names);
     }
 
     PreparedInputs prepared = prepare_inputs(poses_2d, cameras, joint_names);
@@ -831,6 +875,63 @@ TriangulationTrace triangulate_debug_impl(
         trace.merge.merged_poses.push_back(final_poses_3d[i]);
     }
 
+    std::vector<FinalAssociationState> group_associations(groups.size());
+    if (previous_poses_3d != nullptr)
+    {
+        for (size_t group_index = 0; group_index < trace.merge.group_proposal_indices.size(); ++group_index)
+        {
+            FinalAssociationState association;
+            std::set<int> candidate_previous_indices;
+            std::set<int64_t> candidate_previous_track_ids;
+
+            for (const int core_index : trace.merge.group_proposal_indices[group_index])
+            {
+                if (core_index < 0 || static_cast<size_t>(core_index) >= trace.core_proposals.size())
+                {
+                    continue;
+                }
+
+                const int pair_index = trace.core_proposals[static_cast<size_t>(core_index)].pair_index;
+                if (pair_index < 0 || static_cast<size_t>(pair_index) >= trace.previous_filter.matches.size())
+                {
+                    continue;
+                }
+
+                const PreviousPoseMatch &match = trace.previous_filter.matches[static_cast<size_t>(pair_index)];
+                if (!match.kept || match.previous_pose_index < 0 || match.previous_track_id < 0)
+                {
+                    continue;
+                }
+
+                candidate_previous_indices.insert(match.previous_pose_index);
+                candidate_previous_track_ids.insert(match.previous_track_id);
+            }
+
+            association.candidate_previous_indices.assign(
+                candidate_previous_indices.begin(), candidate_previous_indices.end());
+            association.candidate_previous_track_ids.assign(
+                candidate_previous_track_ids.begin(), candidate_previous_track_ids.end());
+
+            if (association.candidate_previous_track_ids.empty())
+            {
+                association.status = AssociationStatus::New;
+            }
+            else if (association.candidate_previous_track_ids.size() == 1 &&
+                     association.candidate_previous_indices.size() == 1)
+            {
+                association.status = AssociationStatus::Matched;
+                association.resolved_previous_index = association.candidate_previous_indices.front();
+                association.resolved_previous_track_id = association.candidate_previous_track_ids.front();
+            }
+            else
+            {
+                association.status = AssociationStatus::Ambiguous;
+            }
+
+            group_associations[group_index] = std::move(association);
+        }
+    }
+
     add_extra_joints(final_poses_3d, joint_names);
     filter_poses(final_poses_3d, roomparams, prepared.core_joint_idx, prepared.core_limbs_idx);
     add_missing_joints(final_poses_3d, joint_names, options.min_match_score);
@@ -854,10 +955,20 @@ TriangulationTrace triangulate_debug_impl(
 
     trace.final_poses.num_persons = valid_persons;
     trace.final_poses.data.resize(valid_persons * trace.final_poses.num_joints * 4);
+    trace.association.pose_previous_indices.reserve(valid_persons);
+    trace.association.pose_previous_track_ids.reserve(valid_persons);
+    trace.association.pose_status.reserve(valid_persons);
+    trace.association.pose_candidate_previous_indices.reserve(valid_persons);
+    trace.association.pose_candidate_previous_track_ids.reserve(valid_persons);
+    trace.final_pose_associations.reserve(valid_persons);
+
+    std::set<int> resolved_previous_indices;
+    std::set<int64_t> resolved_previous_track_ids;
 
     size_t person_index = 0;
-    for (const auto &pose : final_poses_3d)
+    for (size_t group_index = 0; group_index < final_poses_3d.size(); ++group_index)
     {
+        const auto &pose = final_poses_3d[group_index];
         const bool is_valid = std::any_of(
             pose.begin(),
             pose.end(),
@@ -877,9 +988,70 @@ TriangulationTrace triangulate_debug_impl(
                 trace.final_poses.at(person_index, joint, coord) = pose[joint][coord];
             }
         }
+
+        if (previous_poses_3d != nullptr)
+        {
+            const FinalAssociationState &association = group_associations[group_index];
+            trace.association.pose_previous_indices.push_back(association.resolved_previous_index);
+            trace.association.pose_previous_track_ids.push_back(association.resolved_previous_track_id);
+            trace.association.pose_status.push_back(association.status);
+            trace.association.pose_candidate_previous_indices.push_back(association.candidate_previous_indices);
+            trace.association.pose_candidate_previous_track_ids.push_back(
+                association.candidate_previous_track_ids);
+
+            if (association.status == AssociationStatus::Matched)
+            {
+                resolved_previous_indices.insert(association.resolved_previous_index);
+                resolved_previous_track_ids.insert(association.resolved_previous_track_id);
+            }
+            else if (association.status == AssociationStatus::New)
+            {
+                trace.association.new_pose_indices.push_back(static_cast<int>(person_index));
+            }
+            else if (association.status == AssociationStatus::Ambiguous)
+            {
+                trace.association.ambiguous_pose_indices.push_back(static_cast<int>(person_index));
+            }
+
+            FinalPoseAssociationDebug debug_association;
+            debug_association.final_pose_index = static_cast<int>(person_index);
+            debug_association.source_core_proposal_indices =
+                trace.merge.group_proposal_indices[group_index];
+            debug_association.candidate_previous_indices = association.candidate_previous_indices;
+            debug_association.candidate_previous_track_ids = association.candidate_previous_track_ids;
+            debug_association.resolved_previous_index = association.resolved_previous_index;
+            debug_association.resolved_previous_track_id = association.resolved_previous_track_id;
+            debug_association.status = association.status;
+            debug_association.source_pair_indices.reserve(debug_association.source_core_proposal_indices.size());
+            for (const int core_index : debug_association.source_core_proposal_indices)
+            {
+                if (core_index >= 0 && static_cast<size_t>(core_index) < trace.core_proposals.size())
+                {
+                    debug_association.source_pair_indices.push_back(
+                        trace.core_proposals[static_cast<size_t>(core_index)].pair_index);
+                }
+            }
+            trace.final_pose_associations.push_back(std::move(debug_association));
+        }
+
         ++person_index;
     }
 
+    if (previous_poses_3d != nullptr)
+    {
+        for (size_t previous_index = 0; previous_index < previous_poses_3d->num_persons; ++previous_index)
+        {
+            const int previous_index_int = static_cast<int>(previous_index);
+            const int64_t track_id = previous_poses_3d->track_id(previous_index);
+            if (!resolved_previous_indices.contains(previous_index_int) &&
+                !resolved_previous_track_ids.contains(track_id))
+            {
+                trace.association.unmatched_previous_indices.push_back(previous_index_int);
+                trace.association.unmatched_previous_track_ids.push_back(track_id);
+            }
+        }
+    }
+
     return trace;
 }
 
@@ -887,13 +1059,10 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
     const PoseBatch2DView &poses_2d,
     const std::vector<Camera> &cameras,
     const std::vector<std::string> &joint_names,
-    const PoseBatch3DView &previous_poses_3d,
+    const TrackedPoseBatch3DView &previous_poses_3d,
     const TriangulationOptions &options)
 {
-    if (previous_poses_3d.num_persons > 0 && previous_poses_3d.num_joints != joint_names.size())
-    {
-        throw std::invalid_argument("previous_poses_3d must use the same joint count as joint_names.");
-    }
+    validate_previous_tracks(previous_poses_3d, joint_names);
 
     PreparedInputs prepared = prepare_inputs(poses_2d, cameras, joint_names);
     const std::vector<PairCandidate> pairs = build_pair_candidates_from_packed(prepared.packed_poses);
@@ -2350,7 +2519,7 @@ std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d
 PreviousPoseFilterDebug filter_pairs_with_previous_poses(
     const PoseBatch2DView &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3DView &previous_poses_3d,
+    const TrackedPoseBatch3DView &previous_poses_3d,
     const TriangulationOptions *options_override)
 {
     const TriangulationOptions &options =
@@ -2362,7 +2531,7 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses(
 TriangulationTrace triangulate_debug(
     const PoseBatch2DView &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3DView *previous_poses_3d,
+    const TrackedPoseBatch3DView *previous_poses_3d,
     const TriangulationOptions *options_override)
 {
     const TriangulationOptions &options =
@@ -2374,8 +2543,20 @@ TriangulationTrace triangulate_debug(
 PoseBatch3D triangulate_poses(
     const PoseBatch2DView &poses_2d,
     const TriangulationConfig &config,
-    const PoseBatch3DView *previous_poses_3d,
     const TriangulationOptions *options_override)
 {
-    return triangulate_debug(poses_2d, config, previous_poses_3d, options_override).final_poses;
+    return triangulate_debug(poses_2d, config, nullptr, options_override).final_poses;
+}
+
+TriangulationResult triangulate_with_report(
+    const PoseBatch2DView &poses_2d,
+    const TriangulationConfig &config,
+    const TrackedPoseBatch3DView &previous_poses_3d,
+    const TriangulationOptions *options_override)
+{
+    TriangulationTrace trace = triangulate_debug(poses_2d, config, &previous_poses_3d, options_override);
+    return TriangulationResult {
+        std::move(trace.final_poses),
+        std::move(trace.association),
+    };
 }