Remove unused extras directory

Document Python API and ship package stubs
Add tracked triangulation association reports
2026-03-26 11:22:44 +08:00 · 2026-03-12 14:53:42 +08:00 · 2026-03-12 14:16:31 +08:00 · 2026-03-12 00:24:02 +08:00 · 2026-03-12 00:08:56 +08:00 · 2026-03-11 23:55:04 +08:00
29 changed files with 2105 additions and 26947 deletions
@@ -61,12 +61,6 @@ A general overview can be found in the paper [RapidPoseTriangulation: Multi-view

 <br>

-## Extras
-
- For usage in combination with ROS2 see [ros](extras/ros/README.md) directory.
-
-<br>
-
 ## Citation

 Please cite [RapidPoseTriangulation](https://arxiv.org/pdf/2503.21692) if you found it helpful for your research or business.
@@ -17,6 +17,7 @@ find_package(nanobind CONFIG REQUIRED)
 set(RPT_PYTHON_PACKAGE_DIR "${CMAKE_CURRENT_BINARY_DIR}/rpt")
 file(MAKE_DIRECTORY "${RPT_PYTHON_PACKAGE_DIR}")
 configure_file("${PROJECT_SOURCE_DIR}/src/rpt/__init__.py" "${RPT_PYTHON_PACKAGE_DIR}/__init__.py" COPYONLY)
+configure_file("${PROJECT_SOURCE_DIR}/src/rpt/__init__.pyi" "${RPT_PYTHON_PACKAGE_DIR}/__init__.pyi" COPYONLY)
 configure_file("${PROJECT_SOURCE_DIR}/src/rpt/_helpers.py" "${RPT_PYTHON_PACKAGE_DIR}/_helpers.py" COPYONLY)
 configure_file("${PROJECT_SOURCE_DIR}/src/rpt/py.typed" "${RPT_PYTHON_PACKAGE_DIR}/py.typed" COPYONLY)

@@ -40,4 +41,5 @@ nanobind_add_stub(rpt_core_stub
 )

 install(TARGETS rpt_core_ext LIBRARY DESTINATION rpt)
+install(FILES "${RPT_PYTHON_PACKAGE_DIR}/__init__.pyi" DESTINATION rpt)
 install(FILES "${RPT_PYTHON_PACKAGE_DIR}/_core.pyi" DESTINATION rpt)
@@ -1,6 +1,8 @@
 #include <algorithm>
+#include <array>
 #include <cstdint>
 #include <stdexcept>
+#include <vector>

 #include <nanobind/nanobind.h>
 #include <nanobind/ndarray.h>
@@ -18,8 +20,11 @@ namespace
 using PoseArray2D =
    nb::ndarray<nb::numpy, const float, nb::shape<-1, -1, -1, 3>, nb::c_contig>;
 using CountArray = nb::ndarray<nb::numpy, const uint32_t, nb::shape<-1>, nb::c_contig>;
-using RoomArray = nb::ndarray<nb::numpy, const float, nb::shape<2, 3>, nb::c_contig>;
+using TrackIdArray = nb::ndarray<nb::numpy, const int64_t, nb::shape<-1>, nb::c_contig>;
+using PoseArray3DConst =
+    nb::ndarray<nb::numpy, const float, nb::shape<-1, -1, 4>, nb::c_contig>;
 using PoseArray3D = nb::ndarray<nb::numpy, float, nb::shape<-1, -1, 4>, nb::c_contig>;
+using PoseArray2DOut = nb::ndarray<nb::numpy, float, nb::shape<-1, 4>, nb::c_contig>;

 PoseBatch2DView pose_batch_view_from_numpy(const PoseArray2D &poses_2d, const CountArray &person_counts)
 {
@@ -45,17 +50,31 @@ PoseBatch2DView pose_batch_view_from_numpy(const PoseArray2D &poses_2d, const Co
    };
 }

-std::array<std::array<float, 3>, 2> roomparams_from_numpy(const RoomArray &roomparams)
+PoseBatch3DView pose_batch3d_view_from_numpy(const PoseArray3DConst &poses_3d)
 {
-    std::array<std::array<float, 3>, 2> result {};
-    for (size_t i = 0; i < 2; ++i)
+    return PoseBatch3DView {
+        poses_3d.data(),
+        static_cast<size_t>(poses_3d.shape(0)),
+        static_cast<size_t>(poses_3d.shape(1)),
+    };
+}
+
+TrackedPoseBatch3DView tracked_pose_batch_view_from_numpy(
+    const PoseArray3DConst &poses_3d,
+    const TrackIdArray &track_ids)
+{
+    if (poses_3d.shape(0) != track_ids.shape(0))
    {
-        for (size_t j = 0; j < 3; ++j)
-        {
-            result[i][j] = roomparams(i, j);
-        }
+        throw std::invalid_argument(
+            "previous_poses_3d and previous_track_ids must have the same number of tracks.");
    }
-    return result;
+
+    return TrackedPoseBatch3DView {
+        track_ids.data(),
+        poses_3d.data(),
+        static_cast<size_t>(poses_3d.shape(0)),
+        static_cast<size_t>(poses_3d.shape(1)),
+    };
 }

 PoseArray3D pose_batch_to_numpy(PoseBatch3D batch)
@@ -69,46 +88,367 @@ PoseArray3D pose_batch_to_numpy(PoseBatch3D batch)
    const size_t shape[3] = {batch.num_persons, batch.num_joints, 4};
    return PoseArray3D(storage->data(), 3, shape, owner);
 }
+
+PoseArray3D pose_batch_to_numpy_copy(const PoseBatch3D &batch)
+{
+    PoseBatch3D copy = batch;
+    return pose_batch_to_numpy(std::move(copy));
+}
+
+PoseArray2DOut pose_rows_to_numpy_copy(const std::vector<std::array<float, 4>> &rows)
+{
+    auto *storage = new std::vector<float>(rows.size() * 4, 0.0f);
+    for (size_t row = 0; row < rows.size(); ++row)
+    {
+        for (size_t coord = 0; coord < 4; ++coord)
+        {
+            (*storage)[row * 4 + coord] = rows[row][coord];
+        }
+    }
+
+    nb::capsule owner(storage, [](void *value) noexcept
+    {
+        delete static_cast<std::vector<float> *>(value);
+    });
+
+    const size_t shape[2] = {rows.size(), 4};
+    return PoseArray2DOut(storage->data(), 2, shape, owner);
+}
+
+PoseArray3D merged_poses_to_numpy_copy(const std::vector<std::vector<std::array<float, 4>>> &poses)
+{
+    size_t num_poses = poses.size();
+    size_t num_joints = 0;
+    if (!poses.empty())
+    {
+        num_joints = poses[0].size();
+    }
+
+    auto *storage = new std::vector<float>(num_poses * num_joints * 4, 0.0f);
+    for (size_t pose = 0; pose < num_poses; ++pose)
+    {
+        if (poses[pose].size() != num_joints)
+        {
+            delete storage;
+            throw std::invalid_argument("Merged poses must use a consistent joint count.");
+        }
+        for (size_t joint = 0; joint < num_joints; ++joint)
+        {
+            for (size_t coord = 0; coord < 4; ++coord)
+            {
+                (*storage)[((pose * num_joints) + joint) * 4 + coord] = poses[pose][joint][coord];
+            }
+        }
+    }
+
+    nb::capsule owner(storage, [](void *value) noexcept
+    {
+        delete static_cast<std::vector<float> *>(value);
+    });
+
+    const size_t shape[3] = {num_poses, num_joints, 4};
+    return PoseArray3D(storage->data(), 3, shape, owner);
+}
 } // namespace

 NB_MODULE(_core, m)
 {
+    nb::enum_<CameraModel>(m, "CameraModel")
+        .value("PINHOLE", CameraModel::Pinhole)
+        .value("FISHEYE", CameraModel::Fisheye);
+
    nb::class_<Camera>(m, "Camera")
-        .def(nb::init<>())
-        .def_rw("name", &Camera::name)
-        .def_rw("K", &Camera::K)
-        .def_rw("DC", &Camera::DC)
-        .def_rw("R", &Camera::R)
-        .def_rw("T", &Camera::T)
-        .def_rw("width", &Camera::width)
-        .def_rw("height", &Camera::height)
-        .def_rw("type", &Camera::type)
+        .def_prop_ro("name", [](const Camera &camera)
+        {
+            return camera.name;
+        })
+        .def_prop_ro("K", [](const Camera &camera)
+        {
+            return camera.K;
+        })
+        .def_prop_ro("DC", [](const Camera &camera)
+        {
+            return camera.DC;
+        })
+        .def_prop_ro("R", [](const Camera &camera)
+        {
+            return camera.R;
+        })
+        .def_prop_ro("T", [](const Camera &camera)
+        {
+            return camera.T;
+        })
+        .def_prop_ro("width", [](const Camera &camera)
+        {
+            return camera.width;
+        })
+        .def_prop_ro("height", [](const Camera &camera)
+        {
+            return camera.height;
+        })
+        .def_prop_ro("model", [](const Camera &camera)
+        {
+            return camera.model;
+        })
+        .def_prop_ro("invR", [](const Camera &camera)
+        {
+            return camera.invR;
+        })
+        .def_prop_ro("center", [](const Camera &camera)
+        {
+            return camera.center;
+        })
+        .def_prop_ro("newK", [](const Camera &camera)
+        {
+            return camera.newK;
+        })
+        .def_prop_ro("invK", [](const Camera &camera)
+        {
+            return camera.invK;
+        })
        .def("__repr__", [](const Camera &camera)
        {
            return camera.to_string();
        });

+    nb::class_<TriangulationOptions>(m, "TriangulationOptions")
+        .def(nb::init<>())
+        .def_rw("min_match_score", &TriangulationOptions::min_match_score)
+        .def_rw("min_group_size", &TriangulationOptions::min_group_size);
+
+    nb::class_<TriangulationConfig>(m, "TriangulationConfig")
+        .def(nb::init<>())
+        .def_rw("cameras", &TriangulationConfig::cameras)
+        .def_rw("roomparams", &TriangulationConfig::roomparams)
+        .def_rw("joint_names", &TriangulationConfig::joint_names)
+        .def_rw("options", &TriangulationConfig::options);
+
+    nb::class_<PairCandidate>(m, "PairCandidate")
+        .def(nb::init<>())
+        .def_rw("view1", &PairCandidate::view1)
+        .def_rw("view2", &PairCandidate::view2)
+        .def_rw("person1", &PairCandidate::person1)
+        .def_rw("person2", &PairCandidate::person2)
+        .def_rw("global_person1", &PairCandidate::global_person1)
+        .def_rw("global_person2", &PairCandidate::global_person2);
+
+    nb::class_<PreviousPoseMatch>(m, "PreviousPoseMatch")
+        .def(nb::init<>())
+        .def_rw("previous_pose_index", &PreviousPoseMatch::previous_pose_index)
+        .def_rw("previous_track_id", &PreviousPoseMatch::previous_track_id)
+        .def_rw("score_view1", &PreviousPoseMatch::score_view1)
+        .def_rw("score_view2", &PreviousPoseMatch::score_view2)
+        .def_rw("matched_view1", &PreviousPoseMatch::matched_view1)
+        .def_rw("matched_view2", &PreviousPoseMatch::matched_view2)
+        .def_rw("kept", &PreviousPoseMatch::kept)
+        .def_rw("decision", &PreviousPoseMatch::decision);
+
+    nb::class_<PreviousPoseFilterDebug>(m, "PreviousPoseFilterDebug")
+        .def(nb::init<>())
+        .def_rw("used_previous_poses", &PreviousPoseFilterDebug::used_previous_poses)
+        .def_rw("matches", &PreviousPoseFilterDebug::matches)
+        .def_rw("kept_pair_indices", &PreviousPoseFilterDebug::kept_pair_indices)
+        .def_rw("kept_pairs", &PreviousPoseFilterDebug::kept_pairs);
+
+    nb::class_<CoreProposalDebug>(m, "CoreProposalDebug")
+        .def(nb::init<>())
+        .def_rw("pair_index", &CoreProposalDebug::pair_index)
+        .def_rw("pair", &CoreProposalDebug::pair)
+        .def_rw("score", &CoreProposalDebug::score)
+        .def_rw("kept", &CoreProposalDebug::kept)
+        .def_rw("drop_reason", &CoreProposalDebug::drop_reason)
+        .def_prop_ro("pose_3d", [](const CoreProposalDebug &proposal)
+        {
+            return pose_rows_to_numpy_copy(proposal.pose_3d);
+        }, nb::rv_policy::move);
+
+    nb::class_<ProposalGroupDebug>(m, "ProposalGroupDebug")
+        .def(nb::init<>())
+        .def_rw("center", &ProposalGroupDebug::center)
+        .def_rw("proposal_indices", &ProposalGroupDebug::proposal_indices)
+        .def_prop_ro("pose_3d", [](const ProposalGroupDebug &group)
+        {
+            return pose_rows_to_numpy_copy(group.pose_3d);
+        }, nb::rv_policy::move);
+
+    nb::class_<GroupingDebug>(m, "GroupingDebug")
+        .def(nb::init<>())
+        .def_rw("initial_groups", &GroupingDebug::initial_groups)
+        .def_rw("duplicate_pair_drops", &GroupingDebug::duplicate_pair_drops)
+        .def_rw("groups", &GroupingDebug::groups);
+
+    nb::class_<FullProposalDebug>(m, "FullProposalDebug")
+        .def(nb::init<>())
+        .def_rw("source_core_proposal_index", &FullProposalDebug::source_core_proposal_index)
+        .def_rw("pair", &FullProposalDebug::pair)
+        .def_prop_ro("pose_3d", [](const FullProposalDebug &proposal)
+        {
+            return pose_rows_to_numpy_copy(proposal.pose_3d);
+        }, nb::rv_policy::move);
+
+    nb::class_<MergeDebug>(m, "MergeDebug")
+        .def(nb::init<>())
+        .def_rw("group_proposal_indices", &MergeDebug::group_proposal_indices)
+        .def_prop_ro("merged_poses", [](const MergeDebug &merge)
+        {
+            return merged_poses_to_numpy_copy(merge.merged_poses);
+        }, nb::rv_policy::move);
+
+    nb::enum_<AssociationStatus>(m, "AssociationStatus")
+        .value("MATCHED", AssociationStatus::Matched)
+        .value("NEW", AssociationStatus::New)
+        .value("AMBIGUOUS", AssociationStatus::Ambiguous);
+
+    nb::class_<AssociationReport>(m, "AssociationReport")
+        .def(nb::init<>())
+        .def_rw("pose_previous_indices", &AssociationReport::pose_previous_indices)
+        .def_rw("pose_previous_track_ids", &AssociationReport::pose_previous_track_ids)
+        .def_rw("pose_status", &AssociationReport::pose_status)
+        .def_rw("pose_candidate_previous_indices", &AssociationReport::pose_candidate_previous_indices)
+        .def_rw("pose_candidate_previous_track_ids", &AssociationReport::pose_candidate_previous_track_ids)
+        .def_rw("unmatched_previous_indices", &AssociationReport::unmatched_previous_indices)
+        .def_rw("unmatched_previous_track_ids", &AssociationReport::unmatched_previous_track_ids)
+        .def_rw("new_pose_indices", &AssociationReport::new_pose_indices)
+        .def_rw("ambiguous_pose_indices", &AssociationReport::ambiguous_pose_indices);
+
+    nb::class_<FinalPoseAssociationDebug>(m, "FinalPoseAssociationDebug")
+        .def(nb::init<>())
+        .def_rw("final_pose_index", &FinalPoseAssociationDebug::final_pose_index)
+        .def_rw("source_core_proposal_indices", &FinalPoseAssociationDebug::source_core_proposal_indices)
+        .def_rw("source_pair_indices", &FinalPoseAssociationDebug::source_pair_indices)
+        .def_rw("candidate_previous_indices", &FinalPoseAssociationDebug::candidate_previous_indices)
+        .def_rw("candidate_previous_track_ids", &FinalPoseAssociationDebug::candidate_previous_track_ids)
+        .def_rw("resolved_previous_index", &FinalPoseAssociationDebug::resolved_previous_index)
+        .def_rw("resolved_previous_track_id", &FinalPoseAssociationDebug::resolved_previous_track_id)
+        .def_rw("status", &FinalPoseAssociationDebug::status);
+
+    nb::class_<TriangulationTrace>(m, "TriangulationTrace")
+        .def(nb::init<>())
+        .def_rw("pairs", &TriangulationTrace::pairs)
+        .def_rw("previous_filter", &TriangulationTrace::previous_filter)
+        .def_rw("core_proposals", &TriangulationTrace::core_proposals)
+        .def_rw("grouping", &TriangulationTrace::grouping)
+        .def_rw("full_proposals", &TriangulationTrace::full_proposals)
+        .def_rw("merge", &TriangulationTrace::merge)
+        .def_rw("association", &TriangulationTrace::association)
+        .def_rw("final_pose_associations", &TriangulationTrace::final_pose_associations)
+        .def_prop_ro("final_poses", [](const TriangulationTrace &trace)
+        {
+            return pose_batch_to_numpy_copy(trace.final_poses);
+        }, nb::rv_policy::move);
+
+    nb::class_<TriangulationResult>(m, "TriangulationResult")
+        .def(nb::init<>())
+        .def_rw("association", &TriangulationResult::association)
+        .def_prop_ro("poses_3d", [](const TriangulationResult &result)
+        {
+            return pose_batch_to_numpy_copy(result.poses);
+        }, nb::rv_policy::move);
+
+    m.def(
+        "make_camera",
+        &make_camera,
+        "name"_a,
+        "K"_a,
+        "DC"_a,
+        "R"_a,
+        "T"_a,
+        "width"_a,
+        "height"_a,
+        "model"_a);
+
+    m.def(
+        "build_pair_candidates",
+        [](const PoseArray2D &poses_2d, const CountArray &person_counts)
+        {
+            return build_pair_candidates(pose_batch_view_from_numpy(poses_2d, person_counts));
+        },
+        "poses_2d"_a,
+        "person_counts"_a);
+
+    m.def(
+        "filter_pairs_with_previous_poses",
+        [](const PoseArray2D &poses_2d,
+           const CountArray &person_counts,
+           const TriangulationConfig &config,
+           const PoseArray3DConst &previous_poses_3d,
+           const TrackIdArray &previous_track_ids)
+        {
+            return filter_pairs_with_previous_poses(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                config,
+                tracked_pose_batch_view_from_numpy(previous_poses_3d, previous_track_ids));
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "config"_a,
+        "previous_poses_3d"_a,
+        "previous_track_ids"_a);
+
+    m.def(
+        "triangulate_debug",
+        [](const PoseArray2D &poses_2d,
+           const CountArray &person_counts,
+           const TriangulationConfig &config)
+        {
+            return triangulate_debug(pose_batch_view_from_numpy(poses_2d, person_counts), config);
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "config"_a);
+
+    m.def(
+        "triangulate_debug",
+        [](const PoseArray2D &poses_2d,
+           const CountArray &person_counts,
+           const TriangulationConfig &config,
+           const PoseArray3DConst &previous_poses_3d,
+           const TrackIdArray &previous_track_ids)
+        {
+            const TrackedPoseBatch3DView previous_view =
+                tracked_pose_batch_view_from_numpy(previous_poses_3d, previous_track_ids);
+            return triangulate_debug(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                config,
+                &previous_view);
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "config"_a,
+        "previous_poses_3d"_a,
+        "previous_track_ids"_a);
+
    m.def(
        "triangulate_poses",
        [](const PoseArray2D &poses_2d,
           const CountArray &person_counts,
-           const std::vector<Camera> &cameras,
-           const RoomArray &roomparams,
-           const std::vector<std::string> &joint_names,
-           float min_match_score,
-           size_t min_group_size)
+           const TriangulationConfig &config)
        {
-            const PoseBatch2DView pose_batch = pose_batch_view_from_numpy(poses_2d, person_counts);
-            const auto room = roomparams_from_numpy(roomparams);
-            PoseBatch3D poses_3d = triangulate_poses(
-                pose_batch, cameras, room, joint_names, min_match_score, min_group_size);
-            return pose_batch_to_numpy(std::move(poses_3d));
+            const PoseBatch3D poses_3d =
+                triangulate_poses(pose_batch_view_from_numpy(poses_2d, person_counts), config);
+            return pose_batch_to_numpy(poses_3d);
        },
        "poses_2d"_a,
        "person_counts"_a,
-        "cameras"_a,
-        "roomparams"_a,
-        "joint_names"_a,
-        "min_match_score"_a = 0.95f,
-        "min_group_size"_a = 1);
+        "config"_a);
+
+    m.def(
+        "triangulate_with_report",
+        [](const PoseArray2D &poses_2d,
+           const CountArray &person_counts,
+           const TriangulationConfig &config,
+           const PoseArray3DConst &previous_poses_3d,
+           const TrackIdArray &previous_track_ids)
+        {
+            const TriangulationResult result = triangulate_with_report(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                config,
+                tracked_pose_batch_view_from_numpy(previous_poses_3d, previous_track_ids));
+            return result;
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "config"_a,
+        "previous_poses_3d"_a,
+        "previous_track_ids"_a);
 }
@@ -1,24 +0,0 @@
-FROM nvcr.io/nvidia/tensorrt:24.10-py3
-
-ARG DEBIAN_FRONTEND=noninteractive
-ENV LANG=C.UTF-8
-ENV LC_ALL=C.UTF-8
-WORKDIR /
-
-RUN apt-get update && apt-get install -y --no-install-recommends feh
-RUN apt-get update && apt-get install -y --no-install-recommends python3-opencv
-RUN pip uninstall -y opencv-python && pip install --no-cache-dir "opencv-python<4.3"
-
-# Show matplotlib images
-RUN apt-get update && apt-get install -y --no-install-recommends python3-tk
-
-# Python build frontend
-RUN pip3 install --upgrade --no-cache-dir pip
-
-# Install build dependencies
-RUN apt-get update && apt-get install -y --no-install-recommends build-essential
-RUN apt-get update && apt-get install -y --no-install-recommends libopencv-dev
-RUN pip3 install --no-cache-dir uv
-
-WORKDIR /RapidPoseTriangulation/
-CMD ["/bin/bash"]
@@ -1,22 +0,0 @@
-# ROS Wrapper
-
-Run the 3D triangulator with ROS topics as inputs and publish detected poses.
-
-<br>
-
- Build container:
-
-  ```bash
-  docker build --progress=plain -t rapidposetriangulation_ros3d -f extras/ros/dockerfile_3d .
-  ```
-
- Update or remove the `ROS_DOMAIN_ID` in the `docker-compose.yml` files to fit your environment
-
- Run and test:
-
-  ```bash
-  xhost + && docker compose -f extras/ros/docker-compose-3d.yml up
-
-  docker exec -it ros-test_node-1 bash
-  export ROS_DOMAIN_ID=18
-  ```
@@ -1,70 +0,0 @@
-services:
-
-    test_node:
-        image: rapidposetriangulation_ros3d
-        network_mode: "host"
-        ipc: "host"
-        privileged: true
-        volumes:
-            - ../../:/RapidPoseTriangulation/
-            - /tmp/.X11-unix:/tmp/.X11-unix
-            - /dev/shm:/dev/shm
-        environment:
-            - DISPLAY
-            - QT_X11_NO_MITSHM=1
-            - "PYTHONUNBUFFERED=1"
-        command: /bin/bash -i -c 'sleep infinity'
-
-    triangulator:
-        image: rapidposetriangulation_ros3d
-        network_mode: "host"
-        ipc: "host"
-        privileged: true
-        volumes:
-            - ../../:/RapidPoseTriangulation/
-            - /tmp/.X11-unix:/tmp/.X11-unix
-            - /dev/shm:/dev/shm
-        environment:
-            - DISPLAY
-            - QT_X11_NO_MITSHM=1
-            - "PYTHONUNBUFFERED=1"
-        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt3d_wrapper_cpp rpt3d_wrapper'
-
-    skeleton_markers:
-        image: rapidposetriangulation_ros3d
-        network_mode: "host"
-        ipc: "host"
-        privileged: true
-        volumes:
-            - ../../:/RapidPoseTriangulation/
-            - /tmp/.X11-unix:/tmp/.X11-unix
-            - /dev/shm:/dev/shm
-        environment:
-            - "PYTHONUNBUFFERED=1"
-        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers skeleton_markers'
-
-    cube_markers:
-        image: rapidposetriangulation_ros3d
-        network_mode: "host"
-        ipc: "host"
-        privileged: true
-        volumes:
-            - ../../:/RapidPoseTriangulation/
-            - /tmp/.X11-unix:/tmp/.X11-unix
-            - /dev/shm:/dev/shm
-        environment:
-            - "PYTHONUNBUFFERED=1"
-        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers cube_markers'
-
-    skeleton_tfs:
-        image: rapidposetriangulation_ros3d
-        network_mode: "host"
-        ipc: "host"
-        privileged: true
-        volumes:
-            - ../../:/RapidPoseTriangulation/
-            - /tmp/.X11-unix:/tmp/.X11-unix
-            - /dev/shm:/dev/shm
-        environment:
-            - "PYTHONUNBUFFERED=1"
-        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers skeleton_tfs'
@@ -1,46 +0,0 @@
-FROM ros:humble-ros-base-jammy
-ARG DEBIAN_FRONTEND=noninteractive
-
-# Set the working directory to /project
-WORKDIR /project/
-
-# Update and install basic tools
-RUN apt-get update && apt-get upgrade -y
-RUN apt-get update && apt-get install -y --no-install-recommends git nano wget
-RUN apt-get update && apt-get install -y --no-install-recommends python3-pip
-RUN pip3 install --upgrade pip
-
-# Fix ros package building error
-RUN pip3 install --no-cache-dir "setuptools<=58.2.0"
-
-# Add ROS2 sourcing by default
-RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
-
-# Create ROS2 workspace for project packages
-RUN mkdir -p /project/dev_ws/src/
-RUN cd /project/dev_ws/; colcon build
-RUN echo "source /project/dev_ws/install/setup.bash" >> ~/.bashrc
-
-# Copy modules
-COPY ./extras/include/ /RapidPoseTriangulation/extras/include/
-COPY ./rpt/ /RapidPoseTriangulation/rpt/
-COPY ./extras/ros/marker_publishers/ /RapidPoseTriangulation/extras/ros/marker_publishers/
-COPY ./extras/ros/rpt_msgs/ /RapidPoseTriangulation/extras/ros/rpt_msgs/
-COPY ./extras/ros/rpt3d_wrapper_cpp/ /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/
-
-# Link and build as ros package
-RUN ln -s /RapidPoseTriangulation/extras/ros/marker_publishers/ /project/dev_ws/src/
-RUN ln -s /RapidPoseTriangulation/extras/ros/rpt_msgs/ /project/dev_ws/src/
-RUN ln -s /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/ /project/dev_ws/src/
-RUN /bin/bash -i -c 'cd /project/dev_ws/; colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release'
-
-# Update ros packages -> autocompletion and check
-RUN /bin/bash -i -c 'ros2 pkg list'
-
-# Clear cache to save space, only has an effect if image is squashed
-RUN apt-get autoremove -y \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
-
-WORKDIR /RapidPoseTriangulation/
-CMD ["/bin/bash"]
@@ -1,63 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(marker_publishers)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rpt_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(visualization_msgs REQUIRED)
-
-add_executable(cube_markers src/cube_markers.cpp)
-ament_target_dependencies(cube_markers rclcpp geometry_msgs visualization_msgs)
-target_include_directories(cube_markers PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-add_executable(skeleton_markers src/skeleton_markers.cpp)
-ament_target_dependencies(skeleton_markers rclcpp rpt_msgs geometry_msgs visualization_msgs)
-target_include_directories(skeleton_markers PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-add_executable(skeleton_tfs src/skeleton_tfs.cpp)
-ament_target_dependencies(skeleton_tfs rclcpp rpt_msgs geometry_msgs tf2_ros)
-target_include_directories(skeleton_tfs PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-install(TARGETS cube_markers
-  DESTINATION lib/${PROJECT_NAME})
-
-install(TARGETS skeleton_markers
-  DESTINATION lib/${PROJECT_NAME})
-
-install(TARGETS skeleton_tfs
-  DESTINATION lib/${PROJECT_NAME})
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>marker_publishers</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="root@todo.todo">root</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>rpt_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>tf2_ros</depend>
-  <depend>visualization_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
@@ -1,122 +0,0 @@
-#include <chrono>
-#include <vector>
-
-#include <rclcpp/rclcpp.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-
-// =================================================================================================
-
-const std::string output_topic = "/markers_cube";
-static const std::array<std::array<float, 3>, 2> roomparams = {{
-    {4.0, 5.0, 2.2},
-    {1.0, 0.0, 1.1},
-}};
-
-// =================================================================================================
-// =================================================================================================
-
-class CubePublisher : public rclcpp::Node
-{
-public:
-  CubePublisher() : Node("cube_publisher")
-  {
-    publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(output_topic, 1);
-    timer_ = this->create_wall_timer(
-        std::chrono::seconds(1), std::bind(&CubePublisher::timer_callback, this));
-
-    cube_edges = generate_cube_edges();
-  }
-
-private:
-  visualization_msgs::msg::MarkerArray cube_edges;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher_;
-  rclcpp::TimerBase::SharedPtr timer_;
-
-  void timer_callback()
-  {
-    publisher_->publish(cube_edges);
-  }
-  visualization_msgs::msg::MarkerArray generate_cube_edges();
-};
-
-// =================================================================================================
-
-visualization_msgs::msg::MarkerArray CubePublisher::generate_cube_edges()
-{
-  visualization_msgs::msg::MarkerArray marker_array;
-  visualization_msgs::msg::Marker marker;
-
-  marker.header.frame_id = "world";
-  marker.type = visualization_msgs::msg::Marker::LINE_LIST;
-  marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.scale.x = 0.05;
-  marker.id = 0;
-
-  marker.color.a = 1.0;
-  marker.color.r = 1.0;
-
-  std::vector<std::vector<double>> corners = {
-      {-1, -1, -1},
-      {1, -1, -1},
-      {1, 1, -1},
-      {-1, 1, -1},
-      {-1, -1, 1},
-      {1, -1, 1},
-      {1, 1, 1},
-      {-1, 1, 1},
-  };
-
-  for (auto &corner : corners)
-  {
-    for (size_t i = 0; i < corner.size(); ++i)
-    {
-      corner[i] = 0.5 * roomparams[0][i] * corner[i] + roomparams[1][i];
-    }
-  }
-
-  std::vector<std::pair<int, int>> edge_indices = {
-      {0, 1},
-      {1, 2},
-      {2, 3},
-      {3, 0},
-      {4, 5},
-      {5, 6},
-      {6, 7},
-      {7, 4},
-      {0, 4},
-      {1, 5},
-      {2, 6},
-      {3, 7},
-  };
-
-  for (const auto &edge : edge_indices)
-  {
-    geometry_msgs::msg::Point p1, p2;
-    p1.x = corners[edge.first][0];
-    p1.y = corners[edge.first][1];
-    p1.z = corners[edge.first][2];
-
-    p2.x = corners[edge.second][0];
-    p2.y = corners[edge.second][1];
-    p2.z = corners[edge.second][2];
-
-    marker.points.push_back(p1);
-    marker.points.push_back(p2);
-  }
-
-  marker_array.markers.push_back(marker);
-  return marker_array;
-}
-
-// =================================================================================================
-// =================================================================================================
-
-int main(int argc, char *argv[])
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<CubePublisher>());
-  rclcpp::shutdown();
-  return 0;
-}
@@ -1,255 +0,0 @@
-#include <cstdint>
-#include <string>
-#include <vector>
-
-#include <rclcpp/rclcpp.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-#include <std_msgs/msg/color_rgba.hpp>
-
-#include "rpt_msgs/msg/poses.hpp"
-
-// =================================================================================================
-
-const std::string input_topic = "/poses/humans3d";
-const std::string output_topic = "/markers_skeleton";
-
-// =================================================================================================
-// =================================================================================================
-
-std::array<uint8_t, 3> hue_to_rgb(double hue)
-{
-  double r, g, b;
-  int h = static_cast<int>(hue * 6);
-  double f = hue * 6 - h;
-  double q = 1 - f;
-
-  switch (h % 6)
-  {
-  case 0:
-    r = 1;
-    g = f;
-    b = 0;
-    break;
-  case 1:
-    r = q;
-    g = 1;
-    b = 0;
-    break;
-  case 2:
-    r = 0;
-    g = 1;
-    b = f;
-    break;
-  case 3:
-    r = 0;
-    g = q;
-    b = 1;
-    break;
-  case 4:
-    r = f;
-    g = 0;
-    b = 1;
-    break;
-  case 5:
-    r = 1;
-    g = 0;
-    b = q;
-    break;
-  default:
-    r = g = b = 0;
-    break;
-  }
-
-  std::array<uint8_t, 3> rgb = {
-      static_cast<uint8_t>(r * 255),
-      static_cast<uint8_t>(g * 255),
-      static_cast<uint8_t>(b * 255)};
-
-  return rgb;
-}
-
-// =================================================================================================
-// =================================================================================================
-
-class SkeletonPublisher : public rclcpp::Node
-{
-public:
-  SkeletonPublisher() : Node("skeleton_publisher")
-  {
-    publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(output_topic, 1);
-    subscriber_ = this->create_subscription<rpt_msgs::msg::Poses>(
-        input_topic, 1,
-        std::bind(&SkeletonPublisher::listener_callback, this, std::placeholders::_1));
-
-    connections = {
-        {"shoulder_middle", "head"},
-        {"head", "nose"},
-        {"nose", "eye_left"},
-        {"nose", "eye_right"},
-        {"eye_left", "ear_left"},
-        {"eye_right", "ear_right"},
-        {"shoulder_left", "shoulder_right"},
-        {"hip_middle", "shoulder_left"},
-        {"hip_middle", "shoulder_right"},
-        {"shoulder_left", "elbow_left"},
-        {"elbow_left", "wrist_left"},
-        {"hip_middle", "hip_left"},
-        {"hip_left", "knee_left"},
-        {"knee_left", "ankle_left"},
-        {"shoulder_right", "elbow_right"},
-        {"elbow_right", "wrist_right"},
-        {"hip_middle", "hip_right"},
-        {"hip_right", "knee_right"},
-        {"knee_right", "ankle_right"},
-    };
-  }
-
-private:
-  std::vector<std::array<std::string, 2>> connections;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher_;
-  rclcpp::Subscription<rpt_msgs::msg::Poses>::SharedPtr subscriber_;
-
-  void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg);
-  visualization_msgs::msg::MarkerArray generate_msg(
-      const std::vector<std::vector<std::array<float, 4>>> &poses,
-      const std::vector<std::string> &joint_names);
-};
-
-// =================================================================================================
-
-void SkeletonPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg)
-{
-  std::vector<std::vector<std::array<float, 4>>> poses;
-  const std::vector<std::string> &joint_names = msg->joint_names;
-
-  // Unflatten poses
-  size_t idx = 0;
-  std::vector<int32_t> &bshape = msg->bodies_shape;
-  for (int32_t i = 0; i < bshape[0]; ++i)
-  {
-    std::vector<std::array<float, 4>> body;
-    for (int32_t j = 0; j < bshape[1]; ++j)
-    {
-      std::array<float, 4> joint;
-      for (int32_t k = 0; k < bshape[2]; ++k)
-      {
-        joint[k] = msg->bodies_flat[idx];
-        ++idx;
-      }
-      body.push_back(std::move(joint));
-    }
-    poses.push_back(std::move(body));
-  }
-
-  auto skelmsg = generate_msg(poses, joint_names);
-  publisher_->publish(skelmsg);
-}
-
-// =================================================================================================
-
-visualization_msgs::msg::MarkerArray SkeletonPublisher::generate_msg(
-    const std::vector<std::vector<std::array<float, 4>>> &poses,
-    const std::vector<std::string> &joint_names)
-{
-  visualization_msgs::msg::MarkerArray marker_array;
-  visualization_msgs::msg::Marker marker;
-
-  marker.header.frame_id = "world";
-  marker.type = visualization_msgs::msg::Marker::LINE_LIST;
-  marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.scale.x = 0.02;
-  marker.id = 0;
-
-  size_t num_bodies = poses.size();
-  for (size_t i = 0; i < num_bodies; ++i)
-  {
-    std_msgs::msg::ColorRGBA color;
-    double hue = static_cast<double>(i) / num_bodies;
-    auto rgb = hue_to_rgb(hue);
-    color.r = rgb[0] / 255.0;
-    color.g = rgb[1] / 255.0;
-    color.b = rgb[2] / 255.0;
-    color.a = 1.0;
-
-    for (size_t j = 0; j < connections.size(); ++j)
-    {
-      std::string joint1 = connections[j][0];
-      std::string joint2 = connections[j][1];
-
-      int sidx = -1;
-      int eidx = -1;
-      for (size_t k = 0; k < joint_names.size(); ++k)
-      {
-        if (joint_names[k] == joint1)
-        {
-          sidx = k;
-        }
-        if (joint_names[k] == joint2)
-        {
-          eidx = k;
-        }
-      }
-      if (sidx == -1 || eidx == -1)
-      {
-        continue;
-      }
-
-      if (poses[i][sidx][3] <= 0 || poses[i][eidx][3] <= 0)
-      {
-        continue;
-      }
-
-      geometry_msgs::msg::Point p1, p2;
-      p1.x = poses[i][sidx][0];
-      p1.y = poses[i][sidx][1];
-      p1.z = poses[i][sidx][2];
-      p2.x = poses[i][eidx][0];
-      p2.y = poses[i][eidx][1];
-      p2.z = poses[i][eidx][2];
-
-      marker.points.push_back(p1);
-      marker.points.push_back(p2);
-      marker.colors.push_back(color);
-      marker.colors.push_back(color);
-    }
-  }
-
-  if (num_bodies == 0)
-  {
-    // Create an invisible line to remove any old skeletons
-    std_msgs::msg::ColorRGBA color;
-    color.r = 0.0;
-    color.g = 0.0;
-    color.b = 0.0;
-    color.a = 0.0;
-
-    geometry_msgs::msg::Point p1, p2;
-    p1.x = 0.0;
-    p1.y = 0.0;
-    p1.z = 0.0;
-    p2.x = 0.0;
-    p2.y = 0.0;
-    p2.z = 0.001;
-
-    marker.points.push_back(p1);
-    marker.points.push_back(p2);
-    marker.colors.push_back(color);
-    marker.colors.push_back(color);
-  }
-
-  marker_array.markers.push_back(marker);
-  return marker_array;
-}
-
-// =================================================================================================
-// =================================================================================================
-
-int main(int argc, char *argv[])
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<SkeletonPublisher>());
-  rclcpp::shutdown();
-  return 0;
-}
@@ -1,335 +0,0 @@
-#include <array>
-#include <cstdint>
-#include <map>
-#include <string>
-#include <vector>
-
-#include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/transform_broadcaster.h>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-
-#include "rpt_msgs/msg/poses.hpp"
-
-// =================================================================================================
-
-const std::string input_topic = "/poses/humans3d";
-
-// =================================================================================================
-// =================================================================================================
-
-class SkeletonTFPublisher : public rclcpp::Node
-{
-public:
-  SkeletonTFPublisher() : Node("skeleton_tf_publisher")
-  {
-    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
-    subscriber_ = this->create_subscription<rpt_msgs::msg::Poses>(
-        input_topic, 1,
-        std::bind(&SkeletonTFPublisher::listener_callback, this, std::placeholders::_1));
-
-    pc_connections = {
-        // main joints
-        {"hip_middle", "shoulder_middle"},
-        {"shoulder_middle", "head"},
-        {"head", "nose"},
-        {"head", "eye_left"},
-        {"head", "eye_right"},
-        {"head", "ear_left"},
-        {"head", "ear_right"},
-        {"shoulder_middle", "shoulder_left"},
-        {"shoulder_middle", "shoulder_right"},
-        {"shoulder_left", "elbow_left"},
-        {"elbow_left", "wrist_left"},
-        {"hip_middle", "hip_left"},
-        {"hip_left", "knee_left"},
-        {"knee_left", "ankle_left"},
-        {"shoulder_right", "elbow_right"},
-        {"elbow_right", "wrist_right"},
-        {"hip_middle", "hip_right"},
-        {"hip_right", "knee_right"},
-        {"knee_right", "ankle_right"},
-        // whole-body joints
-        {"ankle_left", "foot_toe_big_left"},
-        {"ankle_left", "foot_toe_small_left"},
-        {"ankle_left", "foot_heel_left"},
-        {"ankle_right", "foot_toe_big_right"},
-        {"ankle_right", "foot_toe_small_right"},
-        {"ankle_right", "foot_heel_right"},
-        {"ear_right", "face_jaw_right_1"},
-        {"ear_right", "face_jaw_right_2"},
-        {"ear_right", "face_jaw_right_3"},
-        {"ear_right", "face_jaw_right_4"},
-        {"ear_right", "face_jaw_right_5"},
-        {"ear_right", "face_jaw_right_6"},
-        {"ear_right", "face_jaw_right_7"},
-        {"ear_right", "face_jaw_right_8"},
-        {"head", "face_jaw_middle"},
-        {"ear_left", "face_jaw_left_1"},
-        {"ear_left", "face_jaw_left_2"},
-        {"ear_left", "face_jaw_left_3"},
-        {"ear_left", "face_jaw_left_4"},
-        {"ear_left", "face_jaw_left_5"},
-        {"ear_left", "face_jaw_left_6"},
-        {"ear_left", "face_jaw_left_7"},
-        {"ear_left", "face_jaw_left_8"},
-        {"eye_right", "face_eyebrow_right_1"},
-        {"eye_right", "face_eyebrow_right_2"},
-        {"eye_right", "face_eyebrow_right_3"},
-        {"eye_right", "face_eyebrow_right_4"},
-        {"eye_right", "face_eyebrow_right_5"},
-        {"eye_left", "face_eyebrow_left_1"},
-        {"eye_left", "face_eyebrow_left_2"},
-        {"eye_left", "face_eyebrow_left_3"},
-        {"eye_left", "face_eyebrow_left_4"},
-        {"eye_left", "face_eyebrow_left_5"},
-        {"nose", "face_nose_1"},
-        {"nose", "face_nose_2"},
-        {"nose", "face_nose_3"},
-        {"nose", "face_nose_4"},
-        {"nose", "face_nose_5"},
-        {"nose", "face_nose_6"},
-        {"nose", "face_nose_7"},
-        {"nose", "face_nose_8"},
-        {"nose", "face_nose_9"},
-        {"eye_right", "face_eye_right_1"},
-        {"eye_right", "face_eye_right_2"},
-        {"eye_right", "face_eye_right_3"},
-        {"eye_right", "face_eye_right_4"},
-        {"eye_right", "face_eye_right_5"},
-        {"eye_right", "face_eye_right_6"},
-        {"eye_left", "face_eye_left_1"},
-        {"eye_left", "face_eye_left_2"},
-        {"eye_left", "face_eye_left_3"},
-        {"eye_left", "face_eye_left_4"},
-        {"eye_left", "face_eye_left_5"},
-        {"eye_left", "face_eye_left_6"},
-        {"head", "face_mouth_1"},
-        {"head", "face_mouth_2"},
-        {"head", "face_mouth_3"},
-        {"head", "face_mouth_4"},
-        {"head", "face_mouth_5"},
-        {"head", "face_mouth_6"},
-        {"head", "face_mouth_7"},
-        {"head", "face_mouth_8"},
-        {"head", "face_mouth_9"},
-        {"head", "face_mouth_10"},
-        {"head", "face_mouth_11"},
-        {"head", "face_mouth_12"},
-        {"head", "face_mouth_13"},
-        {"head", "face_mouth_14"},
-        {"head", "face_mouth_15"},
-        {"head", "face_mouth_16"},
-        {"head", "face_mouth_17"},
-        {"head", "face_mouth_18"},
-        {"head", "face_mouth_19"},
-        {"head", "face_mouth_20"},
-        {"wrist_left", "hand_wrist_left"},
-        {"wrist_left", "hand_finger_thumb_left_1"},
-        {"wrist_left", "hand_finger_thumb_left_2"},
-        {"wrist_left", "hand_finger_thumb_left_3"},
-        {"wrist_left", "hand_finger_thumb_left_4"},
-        {"wrist_left", "hand_finger_index_left_1"},
-        {"wrist_left", "hand_finger_index_left_2"},
-        {"wrist_left", "hand_finger_index_left_3"},
-        {"wrist_left", "hand_finger_index_left_4"},
-        {"wrist_left", "hand_finger_middle_left_1"},
-        {"wrist_left", "hand_finger_middle_left_2"},
-        {"wrist_left", "hand_finger_middle_left_3"},
-        {"wrist_left", "hand_finger_middle_left_4"},
-        {"wrist_left", "hand_finger_ring_left_1"},
-        {"wrist_left", "hand_finger_ring_left_2"},
-        {"wrist_left", "hand_finger_ring_left_3"},
-        {"wrist_left", "hand_finger_ring_left_4"},
-        {"wrist_left", "hand_finger_pinky_left_1"},
-        {"wrist_left", "hand_finger_pinky_left_2"},
-        {"wrist_left", "hand_finger_pinky_left_3"},
-        {"wrist_left", "hand_finger_pinky_left_4"},
-        {"wrist_right", "hand_wrist_right"},
-        {"wrist_right", "hand_finger_thumb_right_1"},
-        {"wrist_right", "hand_finger_thumb_right_2"},
-        {"wrist_right", "hand_finger_thumb_right_3"},
-        {"wrist_right", "hand_finger_thumb_right_4"},
-        {"wrist_right", "hand_finger_index_right_1"},
-        {"wrist_right", "hand_finger_index_right_2"},
-        {"wrist_right", "hand_finger_index_right_3"},
-        {"wrist_right", "hand_finger_index_right_4"},
-        {"wrist_right", "hand_finger_middle_right_1"},
-        {"wrist_right", "hand_finger_middle_right_2"},
-        {"wrist_right", "hand_finger_middle_right_3"},
-        {"wrist_right", "hand_finger_middle_right_4"},
-        {"wrist_right", "hand_finger_ring_right_1"},
-        {"wrist_right", "hand_finger_ring_right_2"},
-        {"wrist_right", "hand_finger_ring_right_3"},
-        {"wrist_right", "hand_finger_ring_right_4"},
-        {"wrist_right", "hand_finger_pinky_right_1"},
-        {"wrist_right", "hand_finger_pinky_right_2"},
-        {"wrist_right", "hand_finger_pinky_right_3"},
-        {"wrist_right", "hand_finger_pinky_right_4"},
-    };
-    for (auto &pair : pc_connections)
-    {
-      cp_map[pair[1]] = pair[0];
-    }
-  }
-
-private:
-  std::vector<std::array<std::string, 2>> pc_connections;
-  std::map<std::string, std::string> cp_map;
-  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
-  rclcpp::Subscription<rpt_msgs::msg::Poses>::SharedPtr subscriber_;
-
-  void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg);
-  std::vector<geometry_msgs::msg::TransformStamped> generate_msg(
-      const std::vector<std::vector<std::array<float, 4>>> &poses,
-      const std::vector<std::string> &joint_names);
-};
-
-// =================================================================================================
-
-void SkeletonTFPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg)
-{
-  std::vector<std::vector<std::array<float, 4>>> poses;
-  const std::vector<std::string> &joint_names = msg->joint_names;
-
-  // Unflatten poses
-  size_t idx = 0;
-  std::vector<int32_t> &bshape = msg->bodies_shape;
-  for (int32_t i = 0; i < bshape[0]; ++i)
-  {
-    std::vector<std::array<float, 4>> body;
-    for (int32_t j = 0; j < bshape[1]; ++j)
-    {
-      std::array<float, 4> joint;
-      for (int32_t k = 0; k < bshape[2]; ++k)
-      {
-        joint[k] = msg->bodies_flat[idx];
-        ++idx;
-      }
-      body.push_back(std::move(joint));
-    }
-    poses.push_back(std::move(body));
-  }
-
-  if (poses.empty())
-  {
-    return;
-  }
-
-  auto tf_msgs = generate_msg(poses, joint_names);
-  if (!tf_msgs.empty())
-  {
-    broadcaster_->sendTransform(tf_msgs);
-  }
-}
-
-// =================================================================================================
-
-std::vector<geometry_msgs::msg::TransformStamped> SkeletonTFPublisher::generate_msg(
-    const std::vector<std::vector<std::array<float, 4>>> &poses,
-    const std::vector<std::string> &joint_names)
-{
-  std::vector<geometry_msgs::msg::TransformStamped> tf_msgs;
-  tf_msgs.reserve(poses.size() * pc_connections.size());
-  rclcpp::Time now = this->get_clock()->now();
-
-  for (size_t i = 0; i < poses.size(); ++i)
-  {
-    const auto &body = poses[i];
-
-    // Find index of "hip_middle" in joint_names
-    int hip_mid_idx = -1;
-    for (size_t k = 0; k < joint_names.size(); ++k)
-    {
-      if (joint_names[k] == "hip_middle")
-      {
-        hip_mid_idx = static_cast<int>(k);
-        break;
-      }
-    }
-
-    // Set "hip_middle" as child of "world"
-    const auto &joint = body[hip_mid_idx];
-    geometry_msgs::msg::TransformStamped tf_stamped;
-    tf_stamped.header.stamp = now;
-    tf_stamped.header.frame_id = "world";
-    tf_stamped.child_frame_id = "s" + std::to_string(i) + "-hip_middle";
-    tf_stamped.transform.translation.x = joint[0];
-    tf_stamped.transform.translation.y = joint[1];
-    tf_stamped.transform.translation.z = joint[2];
-    tf_stamped.transform.rotation.x = 0.0;
-    tf_stamped.transform.rotation.y = 0.0;
-    tf_stamped.transform.rotation.z = 0.0;
-    tf_stamped.transform.rotation.w = 1.0;
-    tf_msgs.push_back(std::move(tf_stamped));
-
-    // Add other joints
-    for (size_t j = 0; j < body.size(); ++j)
-    {
-      // Skip "hip_middle"
-      if (static_cast<int>(j) == hip_mid_idx)
-      {
-        continue;
-      }
-
-      const auto &joint_child = body[j];
-      const std::string &child_name = joint_names[j];
-      float conf = joint_child[3];
-      if (conf <= 0.0f)
-      {
-        continue;
-      }
-
-      // Look up its parent
-      auto it = cp_map.find(child_name);
-      if (it == cp_map.end())
-      {
-        continue;
-      }
-      const std::string &parent_name = it->second;
-
-      // Ensure the parent frame was actually published
-      int parent_idx = -1;
-      for (size_t k = 0; k < joint_names.size(); ++k)
-      {
-        if (joint_names[k] == parent_name)
-        {
-          parent_idx = static_cast<int>(k);
-          break;
-        }
-      }
-      if (parent_name != "hip_middle" && body[parent_idx][3] <= 0.0f)
-      {
-        // Parent not visible, skip this child
-        continue;
-      }
-      const auto &joint_parent = body[parent_idx];
-
-      // Publish child frame
-      geometry_msgs::msg::TransformStamped tf_stamped;
-      tf_stamped.header.stamp = now;
-      tf_stamped.header.frame_id = "s" + std::to_string(i) + "-" + parent_name;
-      tf_stamped.child_frame_id = "s" + std::to_string(i) + "-" + child_name;
-      tf_stamped.transform.translation.x = joint_child[0] - joint_parent[0];
-      tf_stamped.transform.translation.y = joint_child[1] - joint_parent[1];
-      tf_stamped.transform.translation.z = joint_child[2] - joint_parent[2];
-      tf_stamped.transform.rotation.x = 0.0;
-      tf_stamped.transform.rotation.y = 0.0;
-      tf_stamped.transform.rotation.z = 0.0;
-      tf_stamped.transform.rotation.w = 1.0;
-      tf_msgs.push_back(std::move(tf_stamped));
-    }
-  }
-
-  return tf_msgs;
-}
-
-// =================================================================================================
-// =================================================================================================
-
-int main(int argc, char *argv[])
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<SkeletonTFPublisher>());
-  rclcpp::shutdown();
-  return 0;
-}
@@ -1,64 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(rpt3d_wrapper_cpp)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++20
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 20)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# Find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rpt_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-
-# Add RapidPoseTriangulation implementation
-set(RAPID_POSE_TRIANGULATION_DIR "/RapidPoseTriangulation")
-add_library(rapid_pose_triangulation
-${RAPID_POSE_TRIANGULATION_DIR}/rpt/camera.cpp
-  ${RAPID_POSE_TRIANGULATION_DIR}/rpt/interface.cpp
-  ${RAPID_POSE_TRIANGULATION_DIR}/rpt/triangulator.cpp
-)
-target_include_directories(rapid_pose_triangulation PUBLIC
-  ${RAPID_POSE_TRIANGULATION_DIR}/extras/include
-  ${RAPID_POSE_TRIANGULATION_DIR}/rpt
-)
-target_compile_options(rapid_pose_triangulation PUBLIC
-  -fPIC -O3 -march=native -Wall -Werror
-)
-
-# Build the executable
-add_executable(rpt3d_wrapper src/rpt3d_wrapper.cpp)
-ament_target_dependencies(rpt3d_wrapper rclcpp std_msgs rpt_msgs)
-target_include_directories(rpt3d_wrapper PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-target_link_libraries(rpt3d_wrapper
-  rapid_pose_triangulation
-)
-
-install(TARGETS rpt3d_wrapper
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>rpt3d_wrapper_cpp</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="root@todo.todo">root</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>rpt_msgs</depend>
-  <depend>std_msgs</depend>
-
-  <depend>OpenCV</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
@@ -1,323 +0,0 @@
-#include <atomic>
-#include <chrono>
-#include <iostream>
-#include <mutex>
-#include <string>
-#include <vector>
-
-// ROS2
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-
-// JSON library
-#include "/RapidPoseTriangulation/extras/include/nlohmann/json.hpp"
-using json = nlohmann::json;
-
-#include "rpt_msgs/msg/poses.hpp"
-#include "/RapidPoseTriangulation/rpt/camera.hpp"
-#include "/RapidPoseTriangulation/rpt/interface.hpp"
-#include "/RapidPoseTriangulation/rpt/tracker.hpp"
-
-// =================================================================================================
-
-static const std::vector<std::string> cam_ids = {
-    "camera01",
-    "camera02",
-    "camera03",
-    "camera04",
-    "camera05",
-    "camera06",
-    "camera07",
-    "camera08",
-    "camera09",
-    "camera10",
-};
-
-static const std::string pose_in_topic = "/poses/{}";
-static const std::string cam_info_topic = "/{}/calibration";
-static const std::string pose_out_topic = "/poses/humans3d";
-
-static const float min_match_score = 0.94;
-static const size_t min_group_size = 4;
-
-static const bool use_tracking = true;
-static const float max_movement_speed = 2.0 * 1.5;
-static const float cam_fps = 50;
-static const float max_track_distance = 0.3 + max_movement_speed / cam_fps;
-
-static const std::array<std::array<float, 3>, 2> roomparams = {{
-    {4.0, 5.0, 2.2},
-    {1.0, 0.0, 1.1},
-}};
-
-// =================================================================================================
-// =================================================================================================
-
-class Rpt3DWrapperNode : public rclcpp::Node
-{
-public:
-    Rpt3DWrapperNode()
-        : Node("rpt3d_wrapper")
-    {
-        this->all_cameras.resize(cam_ids.size());
-        this->all_poses.resize(cam_ids.size());
-        this->all_timestamps.resize(cam_ids.size());
-        this->joint_names = {};
-        this->all_poses_set.resize(cam_ids.size(), false);
-
-        // Load 3D models
-        pose_tracker = std::make_unique<PoseTracker>(
-            max_movement_speed, max_track_distance);
-
-        // QoS
-        rclcpp::QoS qos_profile(1);
-        qos_profile.reliable();
-        qos_profile.keep_last(1);
-
-        // Parallel executable callbacks
-        auto my_callback_group = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-        rclcpp::SubscriptionOptions options;
-        options.callback_group = my_callback_group;
-
-        // Setup subscribers
-        for (size_t i = 0; i < cam_ids.size(); i++)
-        {
-            std::string cam_id = cam_ids[i];
-            std::string topic_pose = pose_in_topic;
-            std::string topic_cam = cam_info_topic;
-            topic_pose.replace(topic_pose.find("{}"), 2, cam_id);
-            topic_cam.replace(topic_cam.find("{}"), 2, cam_id);
-
-            auto sub_pose = this->create_subscription<rpt_msgs::msg::Poses>(
-                topic_pose, qos_profile,
-                [this, i](const rpt_msgs::msg::Poses::SharedPtr msg)
-                {
-                    this->callback_poses(msg, i);
-                },
-                options);
-            sub_pose_list_.push_back(sub_pose);
-
-            auto sub_cam = this->create_subscription<std_msgs::msg::String>(
-                topic_cam, qos_profile,
-                [this, i](const std_msgs::msg::String::SharedPtr msg)
-                {
-                    this->callback_cam_info(msg, i);
-                },
-                options);
-            sub_cam_list_.push_back(sub_cam);
-        }
-
-        // Setup publisher
-        pose_pub_ = this->create_publisher<rpt_msgs::msg::Poses>(pose_out_topic, qos_profile);
-
-        RCLCPP_INFO(this->get_logger(), "Finished initialization of pose triangulator.");
-    }
-
-private:
-    std::vector<rclcpp::Subscription<rpt_msgs::msg::Poses>::SharedPtr> sub_pose_list_;
-    std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> sub_cam_list_;
-    rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
-
-    std::unique_ptr<PoseTracker> pose_tracker;
-    std::vector<Camera> all_cameras;
-    std::mutex cams_mutex, pose_mutex, model_mutex;
-
-    std::vector<std::vector<std::vector<std::array<float, 3>>>> all_poses;
-    std::vector<double> all_timestamps;
-    std::vector<std::string> joint_names;
-    std::vector<bool> all_poses_set;
-
-    void callback_poses(const rpt_msgs::msg::Poses::SharedPtr msg, size_t cam_idx);
-    void callback_cam_info(const std_msgs::msg::String::SharedPtr msg, size_t cam_idx);
-    void call_model();
-};
-
-// =================================================================================================
-
-void Rpt3DWrapperNode::callback_cam_info(const std_msgs::msg::String::SharedPtr msg, size_t cam_idx)
-{
-    json cam = json::parse(msg->data);
-
-    Camera camera;
-    camera.name = cam["name"].get<std::string>();
-    camera.K = cam["K"].get<std::array<std::array<float, 3>, 3>>();
-    camera.DC = cam["DC"].get<std::vector<float>>();
-    camera.R = cam["R"].get<std::array<std::array<float, 3>, 3>>();
-    camera.T = cam["T"].get<std::array<std::array<float, 1>, 3>>();
-    camera.width = cam["width"].get<int>();
-    camera.height = cam["height"].get<int>();
-    camera.type = cam["type"].get<std::string>();
-
-    cams_mutex.lock();
-    all_cameras[cam_idx] = camera;
-    cams_mutex.unlock();
-}
-
-// =================================================================================================
-
-void Rpt3DWrapperNode::callback_poses(const rpt_msgs::msg::Poses::SharedPtr msg, size_t cam_idx)
-{
-    std::vector<std::vector<std::array<float, 3>>> poses;
-    if (this->joint_names.empty())
-    {
-        this->joint_names = msg->joint_names;
-    }
-
-    // Unflatten poses
-    size_t idx = 0;
-    std::vector<int32_t> &bshape = msg->bodies_shape;
-    for (int32_t i = 0; i < bshape[0]; ++i)
-    {
-        std::vector<std::array<float, 3>> body;
-        for (int32_t j = 0; j < bshape[1]; ++j)
-        {
-            std::array<float, 3> joint;
-            for (int32_t k = 0; k < bshape[2]; ++k)
-            {
-                joint[k] = msg->bodies_flat[idx];
-                ++idx;
-            }
-            body.push_back(std::move(joint));
-        }
-        poses.push_back(std::move(body));
-    }
-
-    // If no pose was detected, create an empty placeholder
-    if (poses.size() == 0)
-    {
-        std::vector<std::array<float, 3>> body(joint_names.size(), {0, 0, 0});
-        poses.push_back(std::move(body));
-    }
-
-    pose_mutex.lock();
-    this->all_poses[cam_idx] = std::move(poses);
-    this->all_poses_set[cam_idx] = true;
-    this->all_timestamps[cam_idx] = msg->header.stamp.sec + msg->header.stamp.nanosec / 1.0e9;
-    pose_mutex.unlock();
-
-    // Trigger model callback
-    model_mutex.lock();
-    call_model();
-    model_mutex.unlock();
-}
-
-// =================================================================================================
-
-void Rpt3DWrapperNode::call_model()
-{
-    auto ts_msg = std::chrono::high_resolution_clock::now();
-
-    // Check if all cameras are set
-    cams_mutex.lock();
-    for (size_t i = 0; i < cam_ids.size(); i++)
-    {
-        if (all_cameras[i].name.empty())
-        {
-            RCLCPP_WARN(this->get_logger(), "Skipping frame, still waiting for cameras...");
-            cams_mutex.unlock();
-            return;
-        }
-    }
-    cams_mutex.unlock();
-
-    // If not all poses are set, return and wait for the others
-    pose_mutex.lock();
-    for (size_t i = 0; i < cam_ids.size(); i++)
-    {
-        if (!this->all_poses_set[i])
-        {
-            pose_mutex.unlock();
-            return;
-        }
-    }
-    pose_mutex.unlock();
-
-    // Call model, and meanwhile lock updates of the inputs
-    // Since the prediction is very fast, parallel callback threads only need to wait a short time
-    cams_mutex.lock();
-    pose_mutex.lock();
-    PoseBatch2D pose_batch_2d = PoseBatch2D::from_nested(all_poses);
-    auto poses_3d = triangulate_poses(
-        pose_batch_2d, all_cameras, roomparams, joint_names, min_match_score, min_group_size)
-                        .to_nested();
-
-    double min_ts = *std::min_element(all_timestamps.begin(), all_timestamps.end());
-    this->all_poses_set = std::vector<bool>(cam_ids.size(), false);
-
-    std::vector<std::vector<std::array<float, 4>>> valid_poses;
-    std::vector<size_t> track_ids;
-    if (use_tracking)
-    {
-        auto pose_tracks = pose_tracker->track_poses(poses_3d, joint_names, min_ts);
-        std::vector<std::vector<std::array<float, 4>>> poses_3d_refined;
-        for (size_t j = 0; j < pose_tracks.size(); j++)
-        {
-            auto &pose = std::get<1>(pose_tracks[j]);
-            poses_3d_refined.push_back(pose);
-            auto &track_id = std::get<0>(pose_tracks[j]);
-            track_ids.push_back(track_id);
-        }
-        valid_poses = std::move(poses_3d_refined);
-    }
-    else
-    {
-        valid_poses = std::move(poses_3d);
-        track_ids = {};
-    }
-    pose_mutex.unlock();
-    cams_mutex.unlock();
-
-    // Calculate timings
-    auto ts_pose = std::chrono::high_resolution_clock::now();
-    double ts_pose_sec = std::chrono::duration<double>(ts_pose.time_since_epoch()).count();
-    double z_trigger_pose3d = ts_pose_sec - min_ts;
-    json jdata;
-    jdata["timestamps"] = {
-        {"trigger", min_ts},
-        {"pose3d", ts_pose_sec},
-        {"z-trigger-pose3d", z_trigger_pose3d}};
-
-    // Publish message
-    auto pose_msg = rpt_msgs::msg::Poses();
-    pose_msg.header.stamp.sec = static_cast<int>(min_ts);
-    pose_msg.header.stamp.nanosec = (min_ts - pose_msg.header.stamp.sec) * 1.0e9;
-    pose_msg.header.frame_id = "world";
-    std::vector<int32_t> pshape = {(int)valid_poses.size(), (int)joint_names.size(), 4};
-    pose_msg.bodies_shape = pshape;
-    pose_msg.bodies_flat.reserve(pshape[0] * pshape[1] * pshape[2]);
-    for (int32_t i = 0; i < pshape[0]; i++)
-    {
-        for (int32_t j = 0; j < pshape[1]; j++)
-        {
-            for (int32_t k = 0; k < pshape[2]; k++)
-            {
-                pose_msg.bodies_flat.push_back(valid_poses[i][j][k]);
-            }
-        }
-    }
-    pose_msg.joint_names = joint_names;
-    jdata["track_ids"] = track_ids;
-    pose_msg.extra_data = jdata.dump();
-    pose_pub_->publish(pose_msg);
-
-    // Print info
-    double elapsed_time = std::chrono::duration<double>(
-                              std::chrono::high_resolution_clock::now() - ts_msg)
-                              .count();
-    std::cout << "Detected persons: " << valid_poses.size()
-              << " - Prediction time: " << elapsed_time << "s" << std::endl;
-}
-
-// =================================================================================================
-// =================================================================================================
-
-int main(int argc, char **argv)
-{
-    rclcpp::init(argc, argv);
-    auto node = std::make_shared<Rpt3DWrapperNode>();
-    rclcpp::executors::MultiThreadedExecutor exec;
-    exec.add_node(node);
-    exec.spin();
-    rclcpp::shutdown();
-    return 0;
-}
@@ -1,33 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(rpt_msgs)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/Poses.msg"
-  DEPENDENCIES std_msgs
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
@@ -1,7 +0,0 @@
-std_msgs/Header header
-
-float32[] bodies_flat
-int32[] bodies_shape
-string[] joint_names
-
-string extra_data
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>rpt_msgs</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="root@todo.todo">root</maintainer>
-  <license>TODO: License declaration</license>
-
-  <depend>std_msgs</depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
@@ -2,11 +2,60 @@
 #include <cmath>
 #include <iomanip>
 #include <sstream>
+#include <stdexcept>
+#include <utility>
 #include <vector>
-#include <cmath>

 #include "camera.hpp"

+namespace
+{
+std::array<std::array<float, 3>, 3> transpose3x3(const std::array<std::array<float, 3>, 3> &M)
+{
+    return {{{M[0][0], M[1][0], M[2][0]},
+             {M[0][1], M[1][1], M[2][1]},
+             {M[0][2], M[1][2], M[2][2]}}};
+}
+
+std::array<std::array<float, 3>, 3> invert3x3(const std::array<std::array<float, 3>, 3> &M)
+{
+    std::array<std::array<float, 3>, 3> adj = {
+        {{
+             M[1][1] * M[2][2] - M[1][2] * M[2][1],
+             M[0][2] * M[2][1] - M[0][1] * M[2][2],
+             M[0][1] * M[1][2] - M[0][2] * M[1][1],
+         },
+         {
+             M[1][2] * M[2][0] - M[1][0] * M[2][2],
+             M[0][0] * M[2][2] - M[0][2] * M[2][0],
+             M[0][2] * M[1][0] - M[0][0] * M[1][2],
+         },
+         {
+             M[1][0] * M[2][1] - M[1][1] * M[2][0],
+             M[0][1] * M[2][0] - M[0][0] * M[2][1],
+             M[0][0] * M[1][1] - M[0][1] * M[1][0],
+         }}};
+
+    float det = M[0][0] * adj[0][0] + M[0][1] * adj[1][0] + M[0][2] * adj[2][0];
+    if (std::fabs(det) < 1e-6f)
+    {
+        return {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}};
+    }
+
+    float idet = 1.0f / det;
+    return {{
+        {{adj[0][0] * idet, adj[0][1] * idet, adj[0][2] * idet}},
+        {{adj[1][0] * idet, adj[1][1] * idet, adj[1][2] * idet}},
+        {{adj[2][0] * idet, adj[2][1] * idet, adj[2][2] * idet}},
+    }};
+}
+
+std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_fisheye(
+    const Camera &cam, float balance, std::pair<int, int> new_size);
+std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_pinhole(
+    const Camera &cam, float alpha, std::pair<int, int> new_size);
+} // namespace
+
 // =================================================================================================
 // =================================================================================================

@@ -63,7 +112,7 @@ std::string Camera::to_string() const

    out << "'width': " << width << ", ";
    out << "'height': " << height << ", ";
-    out << "'type': " << type;
+    out << "'model': '" << camera_model_name(model) << "'";

    out << "}";
    return out.str();
@@ -71,6 +120,33 @@ std::string Camera::to_string() const

 // =================================================================================================

+const char *camera_model_name(CameraModel model)
+{
+    switch (model)
+    {
+    case CameraModel::Pinhole:
+        return "pinhole";
+    case CameraModel::Fisheye:
+        return "fisheye";
+    }
+    return "unknown";
+}
+
+CameraModel parse_camera_model(const std::string &value)
+{
+    if (value == "pinhole")
+    {
+        return CameraModel::Pinhole;
+    }
+    if (value == "fisheye")
+    {
+        return CameraModel::Fisheye;
+    }
+    throw std::invalid_argument("Unsupported camera model: " + value);
+}
+
+// =================================================================================================
+
 std::ostream &operator<<(std::ostream &out, const Camera &cam)
 {
    out << cam.to_string();
@@ -80,93 +156,51 @@ std::ostream &operator<<(std::ostream &out, const Camera &cam)
 // =================================================================================================
 // =================================================================================================

-CameraInternal::CameraInternal(const Camera &cam)
+Camera make_camera(
+    std::string name,
+    std::array<std::array<float, 3>, 3> K,
+    std::array<float, 5> DC,
+    std::array<std::array<float, 3>, 3> R,
+    std::array<std::array<float, 1>, 3> T,
+    int width,
+    int height,
+    CameraModel model)
 {
-    this->cam = cam;
-    this->invR = transpose3x3(cam.R);
+    Camera cam {
+        .name = std::move(name),
+        .K = K,
+        .DC = DC,
+        .R = R,
+        .T = T,
+        .width = width,
+        .height = height,
+        .model = model,
+    };
+
+    cam.invR = transpose3x3(cam.R);

    // Camera center:
    // C = -(Rᵀ * t) = -(Rᵀ * (R * (T * -1))) = -(Rᵀ * (R * -T)) = -(Rᵀ * -R * T) = -(-T) = T
-    this->center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]};
+    cam.center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]};

    // Undistort camera matrix
    // As with the undistortion, the own implementation avoids some overhead compared to OpenCV
-    if (cam.type == "fisheye")
+    if (cam.model == CameraModel::Fisheye)
    {
-        newK = calc_optimal_camera_matrix_fisheye(1.0, {cam.width, cam.height});
+        cam.newK = calc_optimal_camera_matrix_fisheye(cam, 1.0f, {cam.width, cam.height});
    }
    else
    {
-        newK = calc_optimal_camera_matrix_pinhole(1.0, {cam.width, cam.height});
+        cam.newK = calc_optimal_camera_matrix_pinhole(cam, 1.0f, {cam.width, cam.height});
    }
-    this->invK = invert3x3(newK);
+    cam.invK = invert3x3(cam.newK);
+
+    return cam;
 }

 // =================================================================================================

-std::array<std::array<float, 3>, 3> CameraInternal::transpose3x3(
-    const std::array<std::array<float, 3>, 3> &M)
-{
-    return {{{M[0][0], M[1][0], M[2][0]},
-             {M[0][1], M[1][1], M[2][1]},
-             {M[0][2], M[1][2], M[2][2]}}};
-}
-
-// =================================================================================================
-
-std::array<std::array<float, 3>, 3> CameraInternal::invert3x3(
-    const std::array<std::array<float, 3>, 3> &M)
-{
-    // Compute the inverse using the adjugate method
-    // See: https://scicomp.stackexchange.com/a/29206
-
-    std::array<std::array<float, 3>, 3> adj = {
-        {{
-             M[1][1] * M[2][2] - M[1][2] * M[2][1],
-             M[0][2] * M[2][1] - M[0][1] * M[2][2],
-             M[0][1] * M[1][2] - M[0][2] * M[1][1],
-         },
-         {
-             M[1][2] * M[2][0] - M[1][0] * M[2][2],
-             M[0][0] * M[2][2] - M[0][2] * M[2][0],
-             M[0][2] * M[1][0] - M[0][0] * M[1][2],
-         },
-         {
-             M[1][0] * M[2][1] - M[1][1] * M[2][0],
-             M[0][1] * M[2][0] - M[0][0] * M[2][1],
-             M[0][0] * M[1][1] - M[0][1] * M[1][0],
-         }}};
-
-    float det = M[0][0] * adj[0][0] + M[0][1] * adj[1][0] + M[0][2] * adj[2][0];
-    if (std::fabs(det) < 1e-6f)
-    {
-        return {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}};
-    }
-
-    float idet = 1.0f / det;
-    std::array<std::array<float, 3>, 3> inv = {
-        {{
-             adj[0][0] * idet,
-             adj[0][1] * idet,
-             adj[0][2] * idet,
-         },
-         {
-             adj[1][0] * idet,
-             adj[1][1] * idet,
-             adj[1][2] * idet,
-         },
-         {
-             adj[2][0] * idet,
-             adj[2][1] * idet,
-             adj[2][2] * idet,
-         }}};
-
-    return inv;
-}
-
-// =================================================================================================
-
-void CameraInternal::undistort_point_pinhole(std::array<float, 3> &p, const std::vector<float> &k)
+void undistort_point_pinhole(std::array<float, 3> &p, const std::array<float, 5> &k)
 {
    // Following: cv::cvUndistortPointsInternal
    // Uses only the distortion coefficients: [k1, k2, p1, p2, k3]
@@ -202,7 +236,7 @@ void CameraInternal::undistort_point_pinhole(std::array<float, 3> &p, const std:

 // =================================================================================================

-void CameraInternal::undistort_point_fisheye(std::array<float, 3> &p, const std::vector<float> &k)
+void undistort_point_fisheye(std::array<float, 3> &p, const std::array<float, 5> &k)
 {
    // Following: cv::fisheye::undistortPoints
    // Uses only the distortion coefficients: [k1, k2, k3, k4]
@@ -250,8 +284,10 @@ void CameraInternal::undistort_point_fisheye(std::array<float, 3> &p, const std:

 // =================================================================================================

-std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_fisheye(
-    float balance, std::pair<int, int> new_size)
+namespace
+{
+std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_fisheye(
+    const Camera &cam, float balance, std::pair<int, int> new_size)
 {
    // Following: cv::fisheye::estimateNewCameraMatrixForUndistortRectify
    // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/fisheye.cpp#L630
@@ -355,8 +391,8 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_f

 // =================================================================================================

-std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_pinhole(
-    float alpha, std::pair<int, int> new_size)
+std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_pinhole(
+    const Camera &cam, float alpha, std::pair<int, int> new_size)
 {
    // Following: cv::getOptimalNewCameraMatrix
    // https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/calibration_base.cpp#L1565
@@ -479,3 +515,4 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_p
         {0.0, 0.0, 1.0}}};
    return newK;
 }
+} // namespace
@@ -7,46 +7,45 @@

 // =================================================================================================

+enum class CameraModel
+{
+    Pinhole,
+    Fisheye,
+};
+
+const char *camera_model_name(CameraModel model);
+CameraModel parse_camera_model(const std::string &value);
+
+// =================================================================================================
+
 struct Camera
 {
    std::string name;
    std::array<std::array<float, 3>, 3> K;
-    std::vector<float> DC;
+    std::array<float, 5> DC = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    std::array<std::array<float, 3>, 3> R;
    std::array<std::array<float, 1>, 3> T;
-    int width;
-    int height;
-    std::string type;
+    int width = 0;
+    int height = 0;
+    CameraModel model = CameraModel::Pinhole;
+    std::array<std::array<float, 3>, 3> invR {};
+    std::array<float, 3> center {};
+    std::array<std::array<float, 3>, 3> newK {};
+    std::array<std::array<float, 3>, 3> invK {};

    friend std::ostream &operator<<(std::ostream &out, Camera const &camera);
    std::string to_string() const;
 };

-// =================================================================================================
+Camera make_camera(
+    std::string name,
+    std::array<std::array<float, 3>, 3> K,
+    std::array<float, 5> DC,
+    std::array<std::array<float, 3>, 3> R,
+    std::array<std::array<float, 1>, 3> T,
+    int width,
+    int height,
+    CameraModel model);

-class CameraInternal
-{
-public:
-    CameraInternal(const Camera &cam);
-
-    Camera cam;
-
-    std::array<std::array<float, 3>, 3> invR;
-    std::array<float, 3> center;
-    std::array<std::array<float, 3>, 3> newK;
-    std::array<std::array<float, 3>, 3> invK;
-
-    static std::array<std::array<float, 3>, 3> transpose3x3(
-        const std::array<std::array<float, 3>, 3> &M);
-
-    static std::array<std::array<float, 3>, 3> invert3x3(
-        const std::array<std::array<float, 3>, 3> &M);
-
-    static void undistort_point_pinhole(std::array<float, 3> &p, const std::vector<float> &k);
-    static void undistort_point_fisheye(std::array<float, 3> &p, const std::vector<float> &k);
-
-    std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_fisheye(
-        float balance, std::pair<int, int> new_size);
-    std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_pinhole(
-        float alpha, std::pair<int, int> new_size);
-};
+void undistort_point_pinhole(std::array<float, 3> &point, const std::array<float, 5> &distortion);
+void undistort_point_fisheye(std::array<float, 3> &point, const std::array<float, 5> &distortion);
@@ -38,6 +38,21 @@ const float &PoseBatch2DView::at(size_t view, size_t person, size_t joint, size_
    return data[pose2d_offset(view, person, joint, coord, max_persons, num_joints)];
 }

+const float &PoseBatch3DView::at(size_t person, size_t joint, size_t coord) const
+{
+    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)];
@@ -109,6 +124,11 @@ const float &PoseBatch3D::at(size_t person, size_t joint, size_t coord) const
    return data[pose3d_offset(person, joint, coord, num_joints)];
 }

+PoseBatch3DView PoseBatch3D::view() const
+{
+    return PoseBatch3DView {data.data(), num_persons, num_joints};
+}
+
 NestedPoses3D PoseBatch3D::to_nested() const
 {
    NestedPoses3D poses_3d(num_persons);
@@ -25,6 +25,26 @@ 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 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;
@@ -48,6 +68,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,34 +76,199 @@ 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;
+    int64_t previous_track_id = -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;
+};
+
+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;
+    PreviousPoseFilterDebug previous_filter;
+    std::vector<CoreProposalDebug> core_proposals;
+    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
+{
+    float min_match_score = 0.95f;
+    size_t min_group_size = 1;
+};
+
+struct TriangulationConfig
+{
+    std::vector<Camera> cameras;
+    std::array<std::array<float, 3>, 2> roomparams {{{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}};
+    std::vector<std::string> joint_names;
+    TriangulationOptions options;
+};
+
+// =================================================================================================
+
+std::vector<PairCandidate> build_pair_candidates(const PoseBatch2DView &poses_2d);
+
+PreviousPoseFilterDebug filter_pairs_with_previous_poses(
+    const PoseBatch2DView &poses_2d,
+    const TriangulationConfig &config,
+    const TrackedPoseBatch3DView &previous_poses_3d,
+    const TriangulationOptions *options_override = nullptr);
+
+TriangulationTrace triangulate_debug(
+    const PoseBatch2DView &poses_2d,
+    const TriangulationConfig &config,
+    const TrackedPoseBatch3DView *previous_poses_3d = nullptr,
+    const TriangulationOptions *options_override = nullptr);
+
+// =================================================================================================
+
 /**
 * Calculate a triangulation using a padded pose tensor.
 *
 * @param poses_2d Padded poses of shape [views, max_persons, joints, 3].
- * @param cameras List of cameras.
- * @param roomparams Room parameters (room size, room center).
- * @param joint_names List of 2D joint names.
- * @param min_match_score Minimum score to consider a triangulated joint as valid.
- * @param min_group_size Minimum number of camera pairs that need to see a person.
+ * @param config Triangulation configuration (cameras, room parameters, joint names, options).
+ * @param options_override Optional per-call options override. Defaults to config.options.
 *
 * @return Pose tensor of shape [persons, joints, 4].
 */
 PoseBatch3D triangulate_poses(
    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,
-    float min_match_score = 0.95f,
-    size_t min_group_size = 1);
+    const TriangulationConfig &config,
+    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 std::vector<Camera> &cameras,
-    const std::array<std::array<float, 3>, 2> &roomparams,
-    const std::vector<std::string> &joint_names,
-    float min_match_score = 0.95f,
-    size_t min_group_size = 1)
+    const TriangulationConfig &config,
+    const TriangulationOptions *options_override = nullptr)
 {
-    return triangulate_poses(
-        poses_2d.view(), cameras, roomparams, joint_names, min_match_score, min_group_size);
+    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);
 }
@@ -1,325 +0,0 @@
-#pragma once
-
-#include <array>
-#include <string>
-#include <vector>
-#include <algorithm>
-#include <limits>
-#include <cmath>
-#include <iostream>
-
-// =================================================================================================
-
-struct Track
-{
-    std::vector<std::vector<std::array<float, 4>>> core_poses;
-    std::vector<std::vector<std::array<float, 4>>> full_poses;
-
-    std::vector<double> timestamps;
-    size_t id;
-};
-
-// =================================================================================================
-
-class PoseTracker
-{
-public:
-    PoseTracker(float max_movement_speed, float max_distance);
-
-    std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> track_poses(
-        const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
-        const std::vector<std::string> &joint_names,
-        const double timestamp);
-
-    void reset();
-
-private:
-    float max_distance;
-    float max_movement_speed;
-    size_t history_size = 3;
-
-    std::vector<double> timestamps;
-    std::vector<Track> pose_tracks;
-
-    const std::vector<std::string> core_joints = {
-        "shoulder_left",
-        "shoulder_right",
-        "hip_left",
-        "hip_right",
-        "elbow_left",
-        "elbow_right",
-        "knee_left",
-        "knee_right",
-        "wrist_left",
-        "wrist_right",
-        "ankle_left",
-        "ankle_right",
-    };
-
-    std::tuple<int, float> match_to_track(const std::vector<std::array<float, 4>> &core_pose_3d);
-
-    std::vector<std::array<float, 4>> refine_pose(const Track &track);
-};
-
-// =================================================================================================
-// =================================================================================================
-
-PoseTracker::PoseTracker(float max_movement_speed, float max_distance)
-{
-    this->max_movement_speed = max_movement_speed;
-    this->max_distance = max_distance;
-}
-
-// =================================================================================================
-
-void PoseTracker::reset()
-{
-    pose_tracks.clear();
-    timestamps.clear();
-}
-
-// =================================================================================================
-
-std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> PoseTracker::track_poses(
-    const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
-    const std::vector<std::string> &joint_names,
-    const double timestamp)
-{
-    // Extract core joints
-    std::vector<size_t> core_joint_idx;
-    for (const auto &joint : core_joints)
-    {
-        auto it = std::find(joint_names.begin(), joint_names.end(), joint);
-        core_joint_idx.push_back(std::distance(joint_names.begin(), it));
-    }
-    std::vector<std::vector<std::array<float, 4>>> core_poses;
-    core_poses.resize(poses_3d.size());
-    for (size_t i = 0; i < poses_3d.size(); ++i)
-    {
-        core_poses[i].resize(core_joint_idx.size());
-        for (size_t j = 0; j < core_joint_idx.size(); ++j)
-        {
-            for (size_t k = 0; k < 4; ++k)
-            {
-                core_poses[i][j][k] = poses_3d[i][core_joint_idx[j]][k];
-            }
-        }
-    }
-
-    // Match core poses to tracks
-    std::vector<std::tuple<size_t, int, float>> matches;
-    for (size_t i = 0; i < core_poses.size(); ++i)
-    {
-        auto [track_idx, distance_sq] = match_to_track(core_poses[i]);
-        matches.emplace_back(i, track_idx, distance_sq);
-    }
-    std::sort(matches.begin(), matches.end(),
-              [](const auto &a, const auto &b)
-              { return std::get<2>(a) < std::get<2>(b); });
-
-    // If track is matched multiple times, only add the best and create new tracks for the rest
-    std::vector<bool> used(pose_tracks.size(), false);
-    for (size_t i = 0; i < matches.size(); ++i)
-    {
-        auto [pose_idx, track_idx, distance_sq] = matches[i];
-        if (track_idx != -1 && !used[track_idx])
-        {
-            used[track_idx] = true;
-        }
-        else
-        {
-            std::get<1>(matches[i]) = -1;
-        }
-    }
-
-    // Update tracks
-    for (size_t i = 0; i < matches.size(); ++i)
-    {
-        auto [pose_idx, track_idx, distance_sq] = matches[i];
-        if (track_idx == -1)
-        {
-            // Create a new track
-            Track new_track;
-            new_track.core_poses.push_back(core_poses[pose_idx]);
-            new_track.full_poses.push_back(poses_3d[pose_idx]);
-            new_track.timestamps.push_back(timestamp);
-            new_track.id = pose_tracks.size();
-            pose_tracks.push_back(new_track);
-        }
-        else
-        {
-            // Update existing track
-            auto &track = pose_tracks[track_idx];
-            track.core_poses.push_back(core_poses[pose_idx]);
-            track.full_poses.push_back(poses_3d[pose_idx]);
-            track.timestamps.push_back(timestamp);
-        }
-    }
-
-    // Remove old track entries
-    timestamps.push_back(timestamp);
-    if (timestamps.size() > history_size)
-    {
-        timestamps.erase(timestamps.begin());
-    }
-    double max_age = timestamps.front();
-    for (size_t i = 0; i < pose_tracks.size();)
-    {
-        auto &track = pose_tracks[i];
-        for (size_t j = 0; j < track.timestamps.size();)
-        {
-            double ts = track.timestamps[j];
-            if (ts < max_age)
-            {
-                track.core_poses.erase(track.core_poses.begin() + j);
-                track.full_poses.erase(track.full_poses.begin() + j);
-                track.timestamps.erase(track.timestamps.begin() + j);
-            }
-            else
-            {
-                j++;
-            }
-        }
-        if (track.timestamps.size() == 0)
-        {
-            pose_tracks.erase(pose_tracks.begin() + i);
-        }
-        else
-        {
-            ++i;
-        }
-    }
-
-    // Refine poses
-    std::vector<std::tuple<size_t, std::vector<std::array<float, 4>>>> tracked_poses;
-    for (size_t i = 0; i < pose_tracks.size(); ++i)
-    {
-        auto &track = pose_tracks[i];
-        // Create a refined pose for current tracks, or old tracks with a bit history,
-        // to avoid continuing tracks of flickering persons
-        if (track.core_poses.size() >= std::ceil(history_size / 2.0) ||
-            track.timestamps.back() == timestamps.back())
-        {
-            std::vector<std::array<float, 4>> refined_pose = refine_pose(track);
-            tracked_poses.emplace_back(track.id, refined_pose);
-        }
-    }
-
-    return tracked_poses;
-}
-
-// =================================================================================================
-
-std::tuple<int, float> PoseTracker::match_to_track(
-    const std::vector<std::array<float, 4>> &core_pose_3d)
-{
-    int best_track = -1;
-    float best_distance_sq = max_distance * max_distance;
-
-    for (size_t i = 0; i < pose_tracks.size(); ++i)
-    {
-        const auto &track = pose_tracks[i];
-        if (track.core_poses.size() == 0)
-        {
-            continue;
-        }
-
-        // Calculate distance to the last pose in the track
-        const auto &last_pose = track.core_poses.back();
-        float distance_sq = 0.0;
-        for (size_t j = 0; j < core_pose_3d.size(); ++j)
-        {
-            float dx = core_pose_3d[j][0] - last_pose[j][0];
-            float dy = core_pose_3d[j][1] - last_pose[j][1];
-            float dz = core_pose_3d[j][2] - last_pose[j][2];
-            distance_sq += dx * dx + dy * dy + dz * dz;
-        }
-        distance_sq /= core_pose_3d.size();
-
-        if (distance_sq < best_distance_sq)
-        {
-            best_distance_sq = distance_sq;
-            best_track = static_cast<int>(i);
-        }
-    }
-
-    return {best_track, best_distance_sq};
-}
-
-// =================================================================================================
-
-std::vector<std::array<float, 4>> PoseTracker::refine_pose(const Track &track)
-{
-    // Restrict maximum movement by physical constraints, by clipping the pose to the maximum
-    // movement distance from one of the track's history poses
-    //
-    // While advanced sensor filtering techniques, like using a Kalman-Filter, might improve the
-    // average accuracy of the pose, they introduce update delays on fast movement changes. For
-    // example, if a person stands still for a while and then suddenly moves, the first few frames
-    // will likely be treated as outliers, since the filter will not be able to adapt fast enough.
-    // This behaviour is not desired in a real-time critical applications, where the pose needs to
-    // be updated to the real physical position of the person as fast as possible. Therefore, only
-    // the movement speed is limited here.
-
-    if (track.core_poses.size() < 2)
-    {
-        return track.full_poses.back();
-    }
-
-    // Calculate maximum possible movement distance from each history pose
-    std::vector<float> max_movement_dists_sq;
-    max_movement_dists_sq.resize(track.core_poses.size() - 1);
-    double last_timestamp = track.timestamps.back();
-    for (size_t i = 0; i < track.core_poses.size() - 1; ++i)
-    {
-        double ts = track.timestamps[i];
-        float max_movement = max_movement_speed * (last_timestamp - ts);
-        max_movement_dists_sq[i] = max_movement * max_movement;
-    }
-
-    // Clip joint if required
-    std::vector<std::array<float, 4>> refined_pose = track.full_poses.back();
-    for (size_t i = 0; i < refined_pose.size(); ++i)
-    {
-        float min_dist_sq = std::numeric_limits<float>::infinity();
-        size_t closest_idx = 0;
-        bool clip = true;
-
-        for (size_t j = 0; j < max_movement_dists_sq.size(); ++j)
-        {
-
-            float dx = refined_pose[i][0] - track.full_poses[j][i][0];
-            float dy = refined_pose[i][1] - track.full_poses[j][i][1];
-            float dz = refined_pose[i][2] - track.full_poses[j][i][2];
-            float dist_sq = dx * dx + dy * dy + dz * dz;
-            if (dist_sq < min_dist_sq)
-            {
-                min_dist_sq = dist_sq;
-                closest_idx = j;
-            }
-            if (dist_sq <= max_movement_dists_sq[j])
-            {
-                clip = false;
-                break;
-            }
-        }
-
-        if (clip)
-        {
-            float dist_sq = min_dist_sq;
-            float scale = max_movement_dists_sq[closest_idx] / dist_sq;
-
-            float dx = refined_pose[i][0] - track.full_poses[closest_idx][i][0];
-            float dy = refined_pose[i][1] - track.full_poses[closest_idx][i][1];
-            float dz = refined_pose[i][2] - track.full_poses[closest_idx][i][2];
-            refined_pose[i][0] = track.full_poses[closest_idx][i][0] + dx * scale;
-            refined_pose[i][1] = track.full_poses[closest_idx][i][1] + dy * scale;
-            refined_pose[i][2] = track.full_poses[closest_idx][i][2] + dz * scale;
-
-            // Set confidence to a low value if the joint is clipped
-            refined_pose[i][3] = 0.1;
-        }
-    }
-
-    return refined_pose;
-}
@@ -3,32 +3,223 @@ from __future__ import annotations
 from collections.abc import Sequence
 from typing import TYPE_CHECKING

-from ._core import Camera, triangulate_poses
+from ._core import (
+    AssociationReport,
+    AssociationStatus,
+    Camera,
+    CameraModel,
+    FinalPoseAssociationDebug,
+    TriangulationConfig,
+    TriangulationOptions,
+    TriangulationResult,
+    CoreProposalDebug,
+    FullProposalDebug,
+    GroupingDebug,
+    MergeDebug,
+    PairCandidate,
+    PreviousPoseFilterDebug,
+    PreviousPoseMatch,
+    ProposalGroupDebug,
+    TriangulationTrace,
+    build_pair_candidates as _build_pair_candidates,
+    filter_pairs_with_previous_poses as _filter_pairs_with_previous_poses,
+    make_camera as _make_camera,
+    triangulate_debug as _triangulate_debug,
+    triangulate_poses as _triangulate_poses,
+    triangulate_with_report as _triangulate_with_report,
+)

 if TYPE_CHECKING:
    import numpy as np
    import numpy.typing as npt

-    from ._helpers import CameraLike, PoseViewLike
+    from ._helpers import CameraLike, CameraModelLike, Matrix3x3Like, PoseViewLike, VectorLike
+
+    PoseArray2D = npt.NDArray[np.float32]
+    PoseArray3D = npt.NDArray[np.float32]
+    PersonCountArray = npt.NDArray[np.uint32]
+    TrackIdArray = npt.NDArray[np.int64]
+
+
+Camera.__doc__ = """Immutable camera calibration with precomputed projection cache fields."""
+TriangulationConfig.__doc__ = """Stable scene configuration used for triangulation."""
+TriangulationOptions.__doc__ = """Score and grouping thresholds used by triangulation."""
+TriangulationResult.__doc__ = """Tracked triangulation output containing poses and association metadata."""
+AssociationReport.__doc__ = """Track-association summary for a tracked triangulation call."""
+TriangulationTrace.__doc__ = """Full debug trace for triangulation, including pair, grouping, and association stages."""


 def convert_cameras(cameras: "Sequence[CameraLike]") -> list[Camera]:
+    """Normalize mapping-like camera inputs into immutable bound `Camera` instances."""
+
    from ._helpers import convert_cameras as _convert_cameras

    return _convert_cameras(cameras)


+def make_camera(
+    name: str,
+    K: "Matrix3x3Like",
+    DC: "VectorLike",
+    R: "Matrix3x3Like",
+    T: "Sequence[Sequence[float]]",
+    width: int,
+    height: int,
+    model: "CameraModel | CameraModelLike",
+) -> Camera:
+    """Create an immutable camera and precompute its cached projection fields."""
+
+    from ._helpers import _coerce_camera_model, _coerce_distortion
+
+    camera_model = _coerce_camera_model(model)
+    return _make_camera(
+        name,
+        K,
+        _coerce_distortion(DC, camera_model),
+        R,
+        T,
+        width,
+        height,
+        camera_model,
+    )
+
+
+def build_pair_candidates(
+    poses_2d: "PoseArray2D",
+    person_counts: "PersonCountArray",
+) -> list[PairCandidate]:
+    """Enumerate all cross-view person pairs implied by the padded 2D pose batch."""
+
+    return _build_pair_candidates(poses_2d, person_counts)
+
+
 def pack_poses_2d(
    views: "Sequence[PoseViewLike]", *, joint_count: int | None = None
 ) -> "tuple[npt.NDArray[np.float32], npt.NDArray[np.uint32]]":
+    """Pack ragged per-view pose detections into the padded tensor expected by the core API."""
+
    from ._helpers import pack_poses_2d as _pack_poses_2d

    return _pack_poses_2d(views, joint_count=joint_count)


+def make_triangulation_config(
+    cameras: "Sequence[CameraLike]",
+    roomparams: "npt.NDArray[np.generic] | Sequence[Sequence[float]]",
+    joint_names: "Sequence[str]",
+    *,
+    min_match_score: float = 0.95,
+    min_group_size: int = 1,
+) -> TriangulationConfig:
+    """Build a triangulation config from cameras, room parameters, and joint names."""
+
+    from ._helpers import make_triangulation_config as _make_triangulation_config
+
+    return _make_triangulation_config(
+        cameras,
+        roomparams,
+        joint_names,
+        min_match_score=min_match_score,
+        min_group_size=min_group_size,
+    )
+
+
+def filter_pairs_with_previous_poses(
+    poses_2d: "PoseArray2D",
+    person_counts: "PersonCountArray",
+    config: TriangulationConfig,
+    previous_poses_3d: "PoseArray3D",
+    previous_track_ids: "TrackIdArray",
+) -> PreviousPoseFilterDebug:
+    """Filter raw cross-view pairs against caller-owned previous 3D tracks."""
+
+    return _filter_pairs_with_previous_poses(
+        poses_2d,
+        person_counts,
+        config,
+        previous_poses_3d,
+        previous_track_ids,
+    )
+
+
+def triangulate_debug(
+    poses_2d: "PoseArray2D",
+    person_counts: "PersonCountArray",
+    config: TriangulationConfig,
+    previous_poses_3d: "PoseArray3D | None" = None,
+    previous_track_ids: "TrackIdArray | None" = None,
+) -> TriangulationTrace:
+    """Run triangulation and return the full debug trace.
+
+    If previous-frame 3D tracks are supplied, `previous_track_ids` must be supplied as an
+    aligned `int64` array of the same length.
+    """
+
+    if previous_poses_3d is None:
+        return _triangulate_debug(poses_2d, person_counts, config)
+    if previous_track_ids is None:
+        raise ValueError("previous_track_ids is required when previous_poses_3d is provided.")
+    return _triangulate_debug(poses_2d, person_counts, config, previous_poses_3d, previous_track_ids)
+
+
+def triangulate_poses(
+    poses_2d: "PoseArray2D",
+    person_counts: "PersonCountArray",
+    config: TriangulationConfig,
+) -> "PoseArray3D":
+    """Triangulate a frame into anonymous 3D poses without track association."""
+
+    return _triangulate_poses(poses_2d, person_counts, config)
+
+
+def triangulate_with_report(
+    poses_2d: "PoseArray2D",
+    person_counts: "PersonCountArray",
+    config: TriangulationConfig,
+    previous_poses_3d: "PoseArray3D",
+    previous_track_ids: "TrackIdArray",
+) -> TriangulationResult:
+    """Triangulate a frame and return caller-owned track association results.
+
+    The previous-frame poses and IDs are treated as immutable caller state. The returned
+    association report classifies each final pose as matched, new, or ambiguous, and lists
+    unmatched previous tracks as deletion candidates.
+    """
+
+    return _triangulate_with_report(
+        poses_2d,
+        person_counts,
+        config,
+        previous_poses_3d,
+        previous_track_ids,
+    )
+
+
 __all__ = [
    "Camera",
+    "CameraModel",
+    "AssociationReport",
+    "AssociationStatus",
+    "FinalPoseAssociationDebug",
+    "TriangulationConfig",
+    "TriangulationOptions",
+    "TriangulationResult",
+    "CoreProposalDebug",
+    "FullProposalDebug",
+    "GroupingDebug",
+    "MergeDebug",
+    "PairCandidate",
+    "PreviousPoseFilterDebug",
+    "PreviousPoseMatch",
+    "ProposalGroupDebug",
+    "TriangulationTrace",
+    "build_pair_candidates",
    "convert_cameras",
+    "filter_pairs_with_previous_poses",
+    "make_camera",
+    "make_triangulation_config",
    "pack_poses_2d",
+    "triangulate_debug",
    "triangulate_poses",
+    "triangulate_with_report",
 ]
@@ -0,0 +1,115 @@
+from collections.abc import Sequence
+from typing import TypeAlias, overload
+
+import numpy as np
+import numpy.typing as npt
+
+from ._core import (
+    AssociationReport,
+    AssociationStatus,
+    Camera,
+    CameraModel,
+    CoreProposalDebug,
+    FinalPoseAssociationDebug,
+    FullProposalDebug,
+    GroupingDebug,
+    MergeDebug,
+    PairCandidate,
+    PreviousPoseFilterDebug,
+    PreviousPoseMatch,
+    ProposalGroupDebug,
+    TriangulationConfig,
+    TriangulationOptions,
+    TriangulationResult,
+    TriangulationTrace,
+)
+from ._helpers import CameraLike, CameraModelLike, Matrix3x3Like, PoseViewLike, RoomParamsLike, VectorLike
+
+PoseArray2D: TypeAlias = npt.NDArray[np.float32]
+PoseArray3D: TypeAlias = npt.NDArray[np.float32]
+PersonCountArray: TypeAlias = npt.NDArray[np.uint32]
+TrackIdArray: TypeAlias = npt.NDArray[np.int64]
+
+
+def convert_cameras(cameras: Sequence[CameraLike]) -> list[Camera]: ...
+
+
+def make_camera(
+    name: str,
+    K: Matrix3x3Like,
+    DC: VectorLike,
+    R: Matrix3x3Like,
+    T: Sequence[Sequence[float]],
+    width: int,
+    height: int,
+    model: CameraModel | CameraModelLike,
+) -> Camera: ...
+
+
+def build_pair_candidates(
+    poses_2d: PoseArray2D,
+    person_counts: PersonCountArray,
+) -> list[PairCandidate]: ...
+
+
+def pack_poses_2d(
+    views: Sequence[PoseViewLike],
+    *,
+    joint_count: int | None = None,
+) -> tuple[npt.NDArray[np.float32], npt.NDArray[np.uint32]]: ...
+
+
+def make_triangulation_config(
+    cameras: Sequence[CameraLike],
+    roomparams: RoomParamsLike,
+    joint_names: Sequence[str],
+    *,
+    min_match_score: float = 0.95,
+    min_group_size: int = 1,
+) -> TriangulationConfig: ...
+
+
+def filter_pairs_with_previous_poses(
+    poses_2d: PoseArray2D,
+    person_counts: PersonCountArray,
+    config: TriangulationConfig,
+    previous_poses_3d: PoseArray3D,
+    previous_track_ids: TrackIdArray,
+) -> PreviousPoseFilterDebug: ...
+
+
+@overload
+def triangulate_debug(
+    poses_2d: PoseArray2D,
+    person_counts: PersonCountArray,
+    config: TriangulationConfig,
+) -> TriangulationTrace: ...
+
+
+@overload
+def triangulate_debug(
+    poses_2d: PoseArray2D,
+    person_counts: PersonCountArray,
+    config: TriangulationConfig,
+    previous_poses_3d: PoseArray3D,
+    previous_track_ids: TrackIdArray,
+) -> TriangulationTrace: ...
+
+
+def triangulate_poses(
+    poses_2d: PoseArray2D,
+    person_counts: PersonCountArray,
+    config: TriangulationConfig,
+) -> PoseArray3D: ...
+
+
+def triangulate_with_report(
+    poses_2d: PoseArray2D,
+    person_counts: PersonCountArray,
+    config: TriangulationConfig,
+    previous_poses_3d: PoseArray3D,
+    previous_track_ids: TrackIdArray,
+) -> TriangulationResult: ...
+
+
+__all__: list[str]
@@ -1,15 +1,16 @@
 from __future__ import annotations

 from collections.abc import Sequence
-from typing import TypeAlias, TypedDict
+from typing import Literal, TypeAlias, TypedDict

 import numpy as np
 import numpy.typing as npt

-from ._core import Camera
+from ._core import Camera, CameraModel, TriangulationConfig, TriangulationOptions, make_camera

 Matrix3x3Like: TypeAlias = Sequence[Sequence[float]]
 VectorLike: TypeAlias = Sequence[float]
+RoomParamsLike: TypeAlias = npt.NDArray[np.generic] | Sequence[Sequence[float]]
 PoseViewLike: TypeAlias = npt.NDArray[np.generic] | Sequence[Sequence[Sequence[float]]] | Sequence[Sequence[float]]


@@ -21,12 +22,39 @@ class CameraDict(TypedDict, total=False):
    T: Sequence[Sequence[float]]
    width: int
    height: int
-    type: str
+    type: Literal["pinhole", "fisheye"]
+    model: Literal["pinhole", "fisheye"] | CameraModel


+CameraModelLike: TypeAlias = CameraModel | Literal["pinhole", "fisheye"]
 CameraLike = Camera | CameraDict


+def _coerce_camera_model(model: CameraModelLike) -> CameraModel:
+    if isinstance(model, CameraModel):
+        return model
+    if model == "pinhole":
+        return CameraModel.PINHOLE
+    if model == "fisheye":
+        return CameraModel.FISHEYE
+    raise ValueError(f"Unsupported camera model: {model}")
+
+
+def _coerce_distortion(distortion: VectorLike, camera_model: CameraModel) -> tuple[float, float, float, float, float]:
+    values = tuple(float(value) for value in distortion)
+    expected = 4 if camera_model is CameraModel.FISHEYE else 5
+    if len(values) not in {expected, 5}:
+        raise ValueError(
+            f"{camera_model.name.lower()} cameras require {expected} distortion coefficients"
+            + (" (or 5 with a trailing zero)." if camera_model is CameraModel.FISHEYE else ".")
+        )
+    if camera_model is CameraModel.FISHEYE and len(values) == 4:
+        values = values + (0.0,)
+    if len(values) != 5:
+        raise ValueError("Distortion coefficients must normalize to exactly 5 values.")
+    return values
+
+
 def convert_cameras(cameras: Sequence[CameraLike]) -> list[Camera]:
    """Normalize mappings or existing Camera objects into bound Camera instances."""

@@ -36,16 +64,19 @@ def convert_cameras(cameras: Sequence[CameraLike]) -> list[Camera]:
            converted.append(cam)
            continue

-        camera = Camera()
-        camera.name = str(cam["name"])
-        camera.K = cam["K"]
-        camera.DC = cam["DC"]
-        camera.R = cam["R"]
-        camera.T = cam["T"]
-        camera.width = int(cam["width"])
-        camera.height = int(cam["height"])
-        camera.type = str(cam.get("type", "pinhole"))
-        converted.append(camera)
+        camera_model = _coerce_camera_model(cam.get("model", cam.get("type", "pinhole")))
+        converted.append(
+            make_camera(
+                str(cam["name"]),
+                cam["K"],
+                _coerce_distortion(cam["DC"], camera_model),
+                cam["R"],
+                cam["T"],
+                int(cam["width"]),
+                int(cam["height"]),
+                camera_model,
+            )
+        )
    return converted


@@ -101,3 +132,28 @@ def pack_poses_2d(
            packed[view_idx, :person_count, :, :] = array

    return packed, counts
+
+
+def make_triangulation_config(
+    cameras: Sequence[CameraLike],
+    roomparams: RoomParamsLike,
+    joint_names: Sequence[str],
+    *,
+    min_match_score: float = 0.95,
+    min_group_size: int = 1,
+) -> TriangulationConfig:
+    config = TriangulationConfig()
+    config.cameras = convert_cameras(cameras)
+
+    roomparams_array = np.asarray(roomparams, dtype=np.float32)
+    if roomparams_array.shape != (2, 3):
+        raise ValueError("roomparams must have shape [2, 3].")
+    config.roomparams = roomparams_array.tolist()
+
+    config.joint_names = [str(joint_name) for joint_name in joint_names]
+
+    options = TriangulationOptions()
+    options.min_match_score = float(min_match_score)
+    options.min_group_size = int(min_group_size)
+    config.options = options
+    return config
@@ -39,20 +39,28 @@ def load_case(camera_path: str, pose_path: str):
        pose_data = json.load(file)

    poses_2d, person_counts = rpt.pack_poses_2d(pose_data["2D"], joint_count=len(JOINT_NAMES))
-    cameras = rpt.convert_cameras(camera_data["cameras"])
-    return poses_2d, person_counts, cameras
+    return poses_2d, person_counts, camera_data["cameras"]
+
+
+def make_config(cameras, roomparams) -> rpt.TriangulationConfig:
+    return rpt.make_triangulation_config(
+        cameras,
+        np.asarray(roomparams, dtype=np.float32),
+        JOINT_NAMES,
+    )


 def test_camera_structure_repr():
-    camera = rpt.Camera()
-    camera.name = "Camera 1"
-    camera.K = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
-    camera.DC = [0, 0, 0, 0, 0]
-    camera.R = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
-    camera.T = [[1], [2], [3]]
-    camera.width = 640
-    camera.height = 480
-    camera.type = "pinhole"
+    camera = rpt.make_camera(
+        "Camera 1",
+        [[1, 0, 0], [0, 1, 0], [0, 0, 1]],
+        [0, 0, 0, 0, 0],
+        [[1, 0, 0], [0, 1, 0], [0, 0, 1]],
+        [[1], [2], [3]],
+        640,
+        480,
+        rpt.CameraModel.PINHOLE,
+    )

    rendered = repr(camera)
    assert "Camera 1" in rendered
@@ -69,14 +77,8 @@ def test_camera_structure_repr():
 )
 def test_triangulate_samples(camera_path: str, pose_path: str, roomparams):
    poses_2d, person_counts, cameras = load_case(camera_path, pose_path)
-    poses_3d = rpt.triangulate_poses(
-        poses_2d,
-        person_counts,
-        cameras,
-        np.asarray(roomparams, dtype=np.float32),
-        JOINT_NAMES,
-        min_match_score=0.95,
-    )
+    config = make_config(cameras, roomparams)
+    poses_3d = rpt.triangulate_poses(poses_2d, person_counts, config)

    assert isinstance(poses_3d, np.ndarray)
    assert poses_3d.dtype == np.float32
@@ -88,26 +90,141 @@ def test_triangulate_samples(camera_path: str, pose_path: str, roomparams):

 def test_triangulate_repeatability():
    poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
-    roomparams = np.asarray([[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]], dtype=np.float32)
+    config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])

-    first = rpt.triangulate_poses(
-        poses_2d, person_counts, cameras, roomparams, JOINT_NAMES, min_match_score=0.95
-    )
-    second = rpt.triangulate_poses(
-        poses_2d, person_counts, cameras, roomparams, JOINT_NAMES, min_match_score=0.95
-    )
+    first = rpt.triangulate_poses(poses_2d, person_counts, config)
+    second = rpt.triangulate_poses(poses_2d, person_counts, config)

    np.testing.assert_allclose(first, second, rtol=1e-5, atol=1e-5)


+def test_build_pair_candidates_exposes_cartesian_view_pairs():
+    poses_2d, person_counts = rpt.pack_poses_2d(
+        [
+            np.zeros((2, 2, 3), dtype=np.float32),
+            np.zeros((1, 2, 3), dtype=np.float32),
+            np.zeros((3, 2, 3), dtype=np.float32),
+        ],
+        joint_count=2,
+    )
+
+    pairs = rpt.build_pair_candidates(poses_2d, person_counts)
+
+    assert len(pairs) == (2 * 1) + (2 * 3) + (1 * 3)
+    assert (pairs[0].view1, pairs[0].view2, pairs[0].person1, pairs[0].person2) == (0, 1, 0, 0)
+    assert pairs[-1].global_person2 == 5
+
+
+def test_triangulate_accepts_empty_previous_poses():
+    poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
+    config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
+    empty_previous = np.zeros((0, len(JOINT_NAMES), 4), dtype=np.float32)
+    empty_previous_ids = np.zeros((0,), dtype=np.int64)
+
+    baseline = rpt.triangulate_poses(poses_2d, person_counts, config)
+    result = rpt.triangulate_with_report(
+        poses_2d,
+        person_counts,
+        config,
+        empty_previous,
+        empty_previous_ids,
+    )
+
+    np.testing.assert_allclose(result.poses_3d, baseline, rtol=1e-5, atol=1e-5)
+    assert result.association.unmatched_previous_track_ids == []
+
+
+def test_triangulate_debug_matches_final_output():
+    poses_2d, person_counts, cameras = load_case("data/h1/sample.json", "tests/poses_h1.json")
+    config = make_config(cameras, [[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]])
+
+    final_poses = rpt.triangulate_poses(poses_2d, person_counts, config)
+    trace = rpt.triangulate_debug(poses_2d, person_counts, config)
+
+    np.testing.assert_allclose(trace.final_poses, final_poses, rtol=1e-5, atol=1e-5)
+    assert len(trace.pairs) >= len(trace.core_proposals)
+    assert trace.association.pose_previous_track_ids == []
+    for group in trace.grouping.groups:
+        assert all(0 <= index < len(trace.core_proposals) for index in group.proposal_indices)
+    for merge_indices in trace.merge.group_proposal_indices:
+        assert all(0 <= index < len(trace.core_proposals) for index in merge_indices)
+
+
+def test_filter_pairs_with_previous_poses_returns_debug_matches():
+    poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
+    config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
+    previous_poses = rpt.triangulate_poses(poses_2d, person_counts, config)
+    previous_track_ids = np.arange(previous_poses.shape[0], dtype=np.int64) + 100
+
+    debug = rpt.filter_pairs_with_previous_poses(
+        poses_2d,
+        person_counts,
+        config,
+        previous_poses,
+        previous_track_ids,
+    )
+
+    assert debug.used_previous_poses is True
+    assert len(debug.matches) == len(rpt.build_pair_candidates(poses_2d, person_counts))
+    assert len(debug.kept_pairs) == len(debug.kept_pair_indices)
+    assert any(match.matched_view1 or match.matched_view2 for match in debug.matches)
+    assert any(match.previous_track_id >= 100 for match in debug.matches if match.previous_pose_index >= 0)
+
+
+def test_triangulate_with_report_resolves_previous_track_ids():
+    poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
+    config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
+    previous_poses = rpt.triangulate_poses(poses_2d, person_counts, config)
+    previous_track_ids = np.arange(previous_poses.shape[0], dtype=np.int64) + 100
+
+    result = rpt.triangulate_with_report(
+        poses_2d,
+        person_counts,
+        config,
+        previous_poses,
+        previous_track_ids,
+    )
+
+    assert result.poses_3d.shape == previous_poses.shape
+    assert len(result.association.pose_previous_track_ids) == result.poses_3d.shape[0]
+    matched_track_ids = sorted(
+        track_id for track_id in result.association.pose_previous_track_ids if track_id >= 0
+    )
+    unmatched_track_ids = sorted(result.association.unmatched_previous_track_ids)
+
+    for pose_index in result.association.new_pose_indices:
+        assert result.association.pose_previous_track_ids[pose_index] == -1
+    for pose_index in result.association.ambiguous_pose_indices:
+        assert result.association.pose_previous_track_ids[pose_index] == -1
+
+    assert matched_track_ids == sorted(set(matched_track_ids))
+    assert sorted(matched_track_ids + unmatched_track_ids) == list(previous_track_ids)
+
+
+def test_triangulate_with_report_rejects_duplicate_previous_track_ids():
+    poses_2d, person_counts, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
+    config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
+    previous_poses = rpt.triangulate_poses(poses_2d, person_counts, config)
+    duplicate_ids = np.zeros((previous_poses.shape[0],), dtype=np.int64)
+
+    with pytest.raises(ValueError, match="unique"):
+        rpt.triangulate_with_report(
+            poses_2d,
+            person_counts,
+            config,
+            previous_poses,
+            duplicate_ids,
+        )
+
+
 def test_triangulate_does_not_mutate_inputs():
    poses_2d, person_counts, cameras = load_case("data/h1/sample.json", "tests/poses_h1.json")
-    roomparams = np.asarray([[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]], dtype=np.float32)
+    config = make_config(cameras, [[4.8, 6.0, 2.0], [0.0, 0.0, 1.0]])

    poses_before = poses_2d.copy()
    counts_before = person_counts.copy()

-    rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES)
+    rpt.triangulate_poses(poses_2d, person_counts, config)

    np.testing.assert_array_equal(poses_2d, poses_before)
    np.testing.assert_array_equal(person_counts, counts_before)
@@ -150,3 +267,15 @@ def test_pack_poses_2d_rejects_inconsistent_joint_count():
 def test_pack_poses_2d_rejects_invalid_last_dimension():
    with pytest.raises(ValueError, match="shape"):
        rpt.pack_poses_2d([np.zeros((1, 2, 2), dtype=np.float32)])
+
+
+def test_make_triangulation_config_builds_bound_struct():
+    _, _, cameras = load_case("data/p1/sample.json", "tests/poses_p1.json")
+    config = make_config(cameras, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
+
+    assert isinstance(config, rpt.TriangulationConfig)
+    assert len(config.cameras) > 0
+    np.testing.assert_allclose(config.roomparams, [[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]])
+    assert config.joint_names == JOINT_NAMES
+    assert config.options.min_match_score == pytest.approx(0.95)
+    assert config.options.min_group_size == 1
Author	SHA1	Message	Date
crosstyan	6c09f7044b	Remove unused extras directory	2026-03-26 11:22:44 +08:00
crosstyan	68b2b32510	Document Python API and ship package stubs	2026-03-12 14:53:42 +08:00
crosstyan	0209986a2c	Add tracked triangulation association reports	2026-03-12 14:16:31 +08:00
crosstyan	31c4690121	Unify camera model into immutable factory-built type	2026-03-12 00:24:02 +08:00
crosstyan	c23f25f871	Simplify triangulation API with config struct	2026-03-12 00:08:56 +08:00
crosstyan	7df34b18c3	Simplify triangulation core and remove integrations	2026-03-11 23:55:04 +08:00
crosstyan	103aeb5887	Refactor triangulation stages and camera model	2026-03-11 23:42:07 +08:00