Simplify triangulation core and remove integrations

README.md

@@ -63,7 +63,6 @@ A general overview can be found in the paper [RapidPoseTriangulation: Multi-view
 
 ## Extras
 
-- For usage in combination with ROS2 see [ros](extras/ros/README.md) directory.
 
 <br>
 

bindings/rpt_module.cpp

@@ -1,6 +1,8 @@
 #include <algorithm>
+#include <array>
 #include <cstdint>
 #include <stdexcept>
+#include <vector>
 
 #include <nanobind/nanobind.h>
 #include <nanobind/ndarray.h>
@@ -19,7 +21,10 @@ 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 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,6 +50,15 @@ PoseBatch2DView pose_batch_view_from_numpy(const PoseArray2D &poses_2d, const Co
     };
 }
 
+PoseBatch3DView pose_batch3d_view_from_numpy(const PoseArray3DConst &poses_3d)
+{
+    return PoseBatch3DView {
+        poses_3d.data(),
+        static_cast<size_t>(poses_3d.shape(0)),
+        static_cast<size_t>(poses_3d.shape(1)),
+    };
+}
+
 std::array<std::array<float, 3>, 2> roomparams_from_numpy(const RoomArray &roomparams)
 {
     std::array<std::array<float, 3>, 2> result {};
@@ -69,10 +83,75 @@ 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)
@@ -82,12 +161,195 @@ NB_MODULE(_core, m)
         .def_rw("T", &Camera::T)
         .def_rw("width", &Camera::width)
         .def_rw("height", &Camera::height)
-        .def_rw("type", &Camera::type)
+        .def_rw("model", &Camera::model)
         .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_<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("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::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_prop_ro("final_poses", [](const TriangulationTrace &trace)
+        {
+            return pose_batch_to_numpy_copy(trace.final_poses);
+        }, nb::rv_policy::move);
+
+    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 std::vector<Camera> &cameras,
+           const std::vector<std::string> &joint_names,
+           const PoseArray3DConst &previous_poses_3d,
+           float min_match_score)
+        {
+            TriangulationOptions options;
+            options.min_match_score = min_match_score;
+            return filter_pairs_with_previous_poses(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                cameras,
+                joint_names,
+                pose_batch3d_view_from_numpy(previous_poses_3d),
+                options);
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "cameras"_a,
+        "joint_names"_a,
+        "previous_poses_3d"_a,
+        "min_match_score"_a = 0.95f);
+
+    m.def(
+        "triangulate_debug",
+        [](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)
+        {
+            TriangulationOptions options;
+            options.min_match_score = min_match_score;
+            options.min_group_size = min_group_size;
+            return triangulate_debug(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                cameras,
+                roomparams_from_numpy(roomparams),
+                joint_names,
+                nullptr,
+                options);
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "cameras"_a,
+        "roomparams"_a,
+        "joint_names"_a,
+        "min_match_score"_a = 0.95f,
+        "min_group_size"_a = 1);
+
+    m.def(
+        "triangulate_debug",
+        [](const PoseArray2D &poses_2d,
+           const CountArray &person_counts,
+           const std::vector<Camera> &cameras,
+           const RoomArray &roomparams,
+           const std::vector<std::string> &joint_names,
+           const PoseArray3DConst &previous_poses_3d,
+           float min_match_score,
+           size_t min_group_size)
+        {
+            const PoseBatch3DView previous_view = pose_batch3d_view_from_numpy(previous_poses_3d);
+            TriangulationOptions options;
+            options.min_match_score = min_match_score;
+            options.min_group_size = min_group_size;
+            return triangulate_debug(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                cameras,
+                roomparams_from_numpy(roomparams),
+                joint_names,
+                &previous_view,
+                options);
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "cameras"_a,
+        "roomparams"_a,
+        "joint_names"_a,
+        "previous_poses_3d"_a,
+        "min_match_score"_a = 0.95f,
+        "min_group_size"_a = 1);
+
     m.def(
         "triangulate_poses",
         [](const PoseArray2D &poses_2d,
@@ -98,11 +360,17 @@ NB_MODULE(_core, m)
            float min_match_score,
            size_t min_group_size)
         {
-            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));
+            TriangulationOptions options;
+            options.min_match_score = min_match_score;
+            options.min_group_size = min_group_size;
+            const PoseBatch3D poses_3d = triangulate_poses(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                cameras,
+                roomparams_from_numpy(roomparams),
+                joint_names,
+                nullptr,
+                options);
+            return pose_batch_to_numpy(poses_3d);
         },
         "poses_2d"_a,
         "person_counts"_a,
@@ -111,4 +379,37 @@ NB_MODULE(_core, m)
         "joint_names"_a,
         "min_match_score"_a = 0.95f,
         "min_group_size"_a = 1);
+
+    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,
+           const PoseArray3DConst &previous_poses_3d,
+           float min_match_score,
+           size_t min_group_size)
+        {
+            const PoseBatch3DView previous_view = pose_batch3d_view_from_numpy(previous_poses_3d);
+            TriangulationOptions options;
+            options.min_match_score = min_match_score;
+            options.min_group_size = min_group_size;
+            const PoseBatch3D poses_3d = triangulate_poses(
+                pose_batch_view_from_numpy(poses_2d, person_counts),
+                cameras,
+                roomparams_from_numpy(roomparams),
+                joint_names,
+                &previous_view,
+                options);
+            return pose_batch_to_numpy(poses_3d);
+        },
+        "poses_2d"_a,
+        "person_counts"_a,
+        "cameras"_a,
+        "roomparams"_a,
+        "joint_names"_a,
+        "previous_poses_3d"_a,
+        "min_match_score"_a = 0.95f,
+        "min_group_size"_a = 1);
 }

extras/ros/README.md

@@ -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
-  ```

extras/ros/docker-compose-3d.yml

@@ -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'

extras/ros/dockerfile_3d

@@ -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"]

extras/ros/marker_publishers/CMakeLists.txt

@@ -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()

extras/ros/marker_publishers/package.xml

@@ -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>

extras/ros/marker_publishers/src/cube_markers.cpp

@@ -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;
-}

extras/ros/marker_publishers/src/skeleton_markers.cpp

@@ -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;
-}

extras/ros/marker_publishers/src/skeleton_tfs.cpp

@@ -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;
-}

extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt

@@ -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()

extras/ros/rpt3d_wrapper_cpp/package.xml

@@ -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>

extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp

@@ -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;
-}

extras/ros/rpt_msgs/CMakeLists.txt

@@ -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()

extras/ros/rpt_msgs/msg/Poses.msg

@@ -1,7 +0,0 @@
-std_msgs/Header header
-
-float32[] bodies_flat
-int32[] bodies_shape
-string[] joint_names
-
-string extra_data

extras/ros/rpt_msgs/package.xml

@@ -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>

rpt/cached_camera.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <array>
+
+#include "camera.hpp"
+
+struct CachedCamera
+{
+    const Camera cam;
+    const std::array<std::array<float, 3>, 3> invR;
+    const std::array<float, 3> center;
+    const std::array<std::array<float, 3>, 3> newK;
+    const std::array<std::array<float, 3>, 3> invK;
+};
+
+CachedCamera cache_camera(const Camera &camera);
+
+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);

rpt/camera.cpp

@@ -1,11 +1,60 @@
 #include <array>
 #include <cmath>
 #include <iomanip>
+#include <stdexcept>
 #include <sstream>
 #include <vector>
 #include <cmath>
 
-#include "camera.hpp"
+#include "cached_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,33 @@ std::ostream &operator<<(std::ostream &out, const Camera &cam)
 // =================================================================================================
 // =================================================================================================
 
-CameraInternal::CameraInternal(const Camera &cam)
+CachedCamera cache_camera(const Camera &cam)
 {
-    this->cam = cam;
-    this->invR = transpose3x3(cam.R);
+    const std::array<std::array<float, 3>, 3> 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]};
+    const std::array<float, 3> 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")
+    std::array<std::array<float, 3>, 3> newK;
+    if (cam.model == CameraModel::Fisheye)
     {
-        newK = calc_optimal_camera_matrix_fisheye(1.0, {cam.width, cam.height});
+        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});
+        newK = calc_optimal_camera_matrix_pinhole(cam, 1.0f, {cam.width, cam.height});
     }
-    this->invK = invert3x3(newK);
+    const std::array<std::array<float, 3>, 3> invK = invert3x3(newK);
+
+    return CachedCamera {cam, invR, center, newK, invK};
 }
 
 // =================================================================================================
 
-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 +218,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 +266,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 +373,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 +497,4 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_p
          {0.0, 0.0, 1.0}}};
     return newK;
 }
+} // namespace

rpt/camera.hpp

@@ -7,46 +7,28 @@
 
 // =================================================================================================
 
+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;
+    CameraModel model = CameraModel::Pinhole;
 
     friend std::ostream &operator<<(std::ostream &out, Camera const &camera);
     std::string to_string() const;
 };
-
-// =================================================================================================
-
-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);
-};

rpt/interface.cpp

@@ -38,6 +38,11 @@ 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)];
+}
+
 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 +114,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);

rpt/interface.hpp

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

rpt/tracker.hpp

@@ -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;
-}

src/rpt/__init__.py

@@ -3,7 +3,24 @@ from __future__ import annotations
 from collections.abc import Sequence
 from typing import TYPE_CHECKING
 
-from ._core import Camera, triangulate_poses
+from ._core import (
+    Camera,
+    CameraModel,
+    TriangulationOptions,
+    CoreProposalDebug,
+    FullProposalDebug,
+    GroupingDebug,
+    MergeDebug,
+    PairCandidate,
+    PreviousPoseFilterDebug,
+    PreviousPoseMatch,
+    ProposalGroupDebug,
+    TriangulationTrace,
+    build_pair_candidates,
+    filter_pairs_with_previous_poses,
+    triangulate_debug,
+    triangulate_poses,
+)
 
 if TYPE_CHECKING:
     import numpy as np
@@ -28,7 +45,21 @@ def pack_poses_2d(
 
 __all__ = [
     "Camera",
+    "CameraModel",
+    "TriangulationOptions",
+    "CoreProposalDebug",
+    "FullProposalDebug",
+    "GroupingDebug",
+    "MergeDebug",
+    "PairCandidate",
+    "PreviousPoseFilterDebug",
+    "PreviousPoseMatch",
+    "ProposalGroupDebug",
+    "TriangulationTrace",
+    "build_pair_candidates",
     "convert_cameras",
+    "filter_pairs_with_previous_poses",
     "pack_poses_2d",
+    "triangulate_debug",
     "triangulate_poses",
 ]

src/rpt/_helpers.py

@@ -1,12 +1,12 @@
 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
 
 Matrix3x3Like: TypeAlias = Sequence[Sequence[float]]
 VectorLike: TypeAlias = Sequence[float]
@@ -21,12 +21,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."""
 
@@ -39,12 +66,13 @@ def convert_cameras(cameras: Sequence[CameraLike]) -> list[Camera]:
         camera = Camera()
         camera.name = str(cam["name"])
         camera.K = cam["K"]
-        camera.DC = cam["DC"]
+        camera_model = _coerce_camera_model(cam.get("model", cam.get("type", "pinhole")))
+        camera.DC = _coerce_distortion(cam["DC"], camera_model)
         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"))
+        camera.model = camera_model
         converted.append(camera)
     return converted
 

tests/test_interface.py

@@ -52,7 +52,7 @@ def test_camera_structure_repr():
     camera.T = [[1], [2], [3]]
     camera.width = 640
     camera.height = 480
-    camera.type = "pinhole"
+    camera.model = rpt.CameraModel.PINHOLE
 
     rendered = repr(camera)
     assert "Camera 1" in rendered
@@ -100,6 +100,75 @@ def test_triangulate_repeatability():
     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")
+    roomparams = np.asarray([[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]], dtype=np.float32)
+    empty_previous = np.zeros((0, len(JOINT_NAMES), 4), dtype=np.float32)
+
+    baseline = rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES)
+    with_previous = rpt.triangulate_poses(
+        poses_2d,
+        person_counts,
+        cameras,
+        roomparams,
+        JOINT_NAMES,
+        empty_previous,
+    )
+
+    np.testing.assert_allclose(with_previous, baseline, rtol=1e-5, atol=1e-5)
+
+
+def test_triangulate_debug_matches_final_output():
+    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)
+
+    final_poses = rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES)
+    trace = rpt.triangulate_debug(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES)
+
+    np.testing.assert_allclose(trace.final_poses, final_poses, rtol=1e-5, atol=1e-5)
+    assert len(trace.pairs) >= len(trace.core_proposals)
+    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")
+    roomparams = np.asarray([[5.6, 6.4, 2.4], [0.0, -0.5, 1.2]], dtype=np.float32)
+    previous_poses = rpt.triangulate_poses(poses_2d, person_counts, cameras, roomparams, JOINT_NAMES)
+
+    debug = rpt.filter_pairs_with_previous_poses(
+        poses_2d,
+        person_counts,
+        cameras,
+        JOINT_NAMES,
+        previous_poses,
+    )
+
+    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)
+
+
 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)