Refactor triangulation stages and camera model

rpt/interface.hpp

@@ -25,6 +25,15 @@ struct PoseBatch2DView
     const float &at(size_t view, size_t person, size_t joint, size_t coord) const;
 };
 
+struct PoseBatch3DView
+{
+    const float *data = nullptr;
+    size_t num_persons = 0;
+    size_t num_joints = 0;
+
+    const float &at(size_t person, size_t joint, size_t coord) const;
+};
+
 struct PoseBatch2D
 {
     std::vector<float> data;
@@ -48,6 +57,7 @@ struct PoseBatch3D
 
     float &at(size_t person, size_t joint, size_t coord);
     const float &at(size_t person, size_t joint, size_t coord) const;
+    PoseBatch3DView view() const;
 
     NestedPoses3D to_nested() const;
     static PoseBatch3D from_nested(const NestedPoses3D &poses_3d);
@@ -55,6 +65,136 @@ struct PoseBatch3D
 
 // =================================================================================================
 
+struct PairCandidate
+{
+    int view1 = -1;
+    int view2 = -1;
+    int person1 = -1;
+    int person2 = -1;
+    int global_person1 = -1;
+    int global_person2 = -1;
+};
+
+struct PreviousPoseMatch
+{
+    int previous_pose_index = -1;
+    float score_view1 = 0.0f;
+    float score_view2 = 0.0f;
+    bool matched_view1 = false;
+    bool matched_view2 = false;
+    bool kept = true;
+    std::string decision = "unfiltered";
+};
+
+struct PreviousPoseFilterDebug
+{
+    bool used_previous_poses = false;
+    std::vector<PreviousPoseMatch> matches;
+    std::vector<int> kept_pair_indices;
+    std::vector<PairCandidate> kept_pairs;
+};
+
+struct CoreProposalDebug
+{
+    int pair_index = -1;
+    PairCandidate pair;
+    std::vector<std::array<float, 4>> pose_3d;
+    float score = 0.0f;
+    bool kept = false;
+    std::string drop_reason = "uninitialized";
+};
+
+struct ProposalGroupDebug
+{
+    std::array<float, 3> center = {0.0f, 0.0f, 0.0f};
+    std::vector<std::array<float, 4>> pose_3d;
+    std::vector<int> proposal_indices;
+};
+
+struct GroupingDebug
+{
+    std::vector<ProposalGroupDebug> initial_groups;
+    std::vector<int> duplicate_pair_drops;
+    std::vector<ProposalGroupDebug> groups;
+};
+
+struct FullProposalDebug
+{
+    int source_core_proposal_index = -1;
+    PairCandidate pair;
+    std::vector<std::array<float, 4>> pose_3d;
+};
+
+struct MergeDebug
+{
+    std::vector<std::vector<std::array<float, 4>>> merged_poses;
+    std::vector<std::vector<int>> group_proposal_indices;
+};
+
+struct TriangulationTrace
+{
+    std::vector<PairCandidate> pairs;
+    PreviousPoseFilterDebug previous_filter;
+    std::vector<CoreProposalDebug> core_proposals;
+    GroupingDebug grouping;
+    std::vector<FullProposalDebug> full_proposals;
+    MergeDebug merge;
+    PoseBatch3D final_poses;
+};
+
+// =================================================================================================
+
+std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d);
+
+PreviousPoseFilterDebug filter_pairs_with_previous_poses(
+    const PoseBatch2DView &poses_2d,
+    const std::vector<Camera> &cameras,
+    const std::vector<std::string> &joint_names,
+    const PoseBatch3DView &previous_poses_3d,
+    float min_match_score = 0.95f);
+
+inline PreviousPoseFilterDebug filter_pairs_with_previous_poses(
+    const PoseBatch2D &poses_2d,
+    const std::vector<Camera> &cameras,
+    const std::vector<std::string> &joint_names,
+    const PoseBatch3D &previous_poses_3d,
+    float min_match_score = 0.95f)
+{
+    return filter_pairs_with_previous_poses(
+        poses_2d.view(), cameras, joint_names, previous_poses_3d.view(), min_match_score);
+}
+
+TriangulationTrace triangulate_debug(
+    const PoseBatch2DView &poses_2d,
+    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 = nullptr,
+    float min_match_score = 0.95f,
+    size_t min_group_size = 1);
+
+inline TriangulationTrace triangulate_debug(
+    const PoseBatch2D &poses_2d,
+    const std::vector<Camera> &cameras,
+    const std::array<std::array<float, 3>, 2> &roomparams,
+    const std::vector<std::string> &joint_names,
+    const PoseBatch3D *previous_poses_3d = nullptr,
+    float min_match_score = 0.95f,
+    size_t min_group_size = 1)
+{
+    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(), cameras, roomparams, joint_names, previous_view, min_match_score, min_group_size);
+}
+
+// =================================================================================================
+
 /**
  * Calculate a triangulation using a padded pose tensor.
  *
@@ -72,6 +212,7 @@ PoseBatch3D triangulate_poses(
     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 = nullptr,
     float min_match_score = 0.95f,
     size_t min_group_size = 1);
 
@@ -80,9 +221,17 @@ inline PoseBatch3D triangulate_poses(
     const std::vector<Camera> &cameras,
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
+    const PoseBatch3D *previous_poses_3d = nullptr,
     float min_match_score = 0.95f,
     size_t min_group_size = 1)
 {
+    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(), cameras, roomparams, joint_names, min_match_score, min_group_size);
+        poses_2d.view(), cameras, roomparams, joint_names, previous_view, min_match_score, min_group_size);
 }