Add tracked triangulation association reports

2026-03-12 14:16:31 +08:00
parent 31c4690121
commit 0209986a2c
6 changed files with 435 additions and 71 deletions
@@ -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),
+    };
 }