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

@@ -167,6 +167,11 @@ NB_MODULE(_core, m)
             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)
@@ -268,12 +273,14 @@ NB_MODULE(_core, m)
            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),
-                min_match_score);
+                options);
         },
         "poses_2d"_a,
         "person_counts"_a,
@@ -292,14 +299,16 @@ NB_MODULE(_core, m)
            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,
-                min_match_score,
+                options);
-                min_group_size);
         },
         "poses_2d"_a,
         "person_counts"_a,
@@ -321,14 +330,16 @@ NB_MODULE(_core, m)
            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,
-                min_match_score,
+                options);
-                min_group_size);
         },
         "poses_2d"_a,
         "person_counts"_a,
@@ -349,14 +360,16 @@ NB_MODULE(_core, m)
            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;
             const PoseBatch3D poses_3d = triangulate_poses(
                 pose_batch_view_from_numpy(poses_2d, person_counts),
                 cameras,
                 roomparams_from_numpy(roomparams),
                 joint_names,
                 nullptr,
-                min_match_score,
+                options);
-                min_group_size);
             return pose_batch_to_numpy(poses_3d);
         },
         "poses_2d"_a,
@@ -379,14 +392,16 @@ NB_MODULE(_core, m)
            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,
-                min_match_score,
+                options);
-                min_group_size);
             return pose_batch_to_numpy(poses_3d);
         },
         "poses_2d"_a,

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/interface.hpp

@@ -144,6 +144,14 @@ struct TriangulationTrace
 
 // =================================================================================================
 
+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(
@@ -151,17 +159,17 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses(
     const std::vector<Camera> &cameras,
     const std::vector<std::string> &joint_names,
     const PoseBatch3DView &previous_poses_3d,
-    float min_match_score = 0.95f);
+    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,
-    float min_match_score = 0.95f)
+    const TriangulationOptions &options = {})
 {
     return filter_pairs_with_previous_poses(
-        poses_2d.view(), cameras, joint_names, previous_poses_3d.view(), min_match_score);
+        poses_2d.view(), cameras, joint_names, previous_poses_3d.view(), options);
 }
 
 TriangulationTrace triangulate_debug(
@@ -170,8 +178,7 @@ TriangulationTrace triangulate_debug(
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
     const PoseBatch3DView *previous_poses_3d = nullptr,
-    float min_match_score = 0.95f,
+    const TriangulationOptions &options = {});
-    size_t min_group_size = 1);
 
 inline TriangulationTrace triangulate_debug(
     const PoseBatch2D &poses_2d,
@@ -179,8 +186,7 @@ inline TriangulationTrace triangulate_debug(
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
     const PoseBatch3D *previous_poses_3d = nullptr,
-    float min_match_score = 0.95f,
+    const TriangulationOptions &options = {})
-    size_t min_group_size = 1)
 {
     PoseBatch3DView previous_view_storage;
     const PoseBatch3DView *previous_view = nullptr;
@@ -190,7 +196,7 @@ inline TriangulationTrace triangulate_debug(
         previous_view = &previous_view_storage;
     }
     return triangulate_debug(
-        poses_2d.view(), cameras, roomparams, joint_names, previous_view, min_match_score, min_group_size);
+        poses_2d.view(), cameras, roomparams, joint_names, previous_view, options);
 }
 
 // =================================================================================================
@@ -202,8 +208,7 @@ inline TriangulationTrace triangulate_debug(
  * @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 options Triangulation options.
- * @param min_group_size Minimum number of camera pairs that need to see a person.
  *
  * @return Pose tensor of shape [persons, joints, 4].
  */
@@ -213,8 +218,7 @@ PoseBatch3D triangulate_poses(
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
     const PoseBatch3DView *previous_poses_3d = nullptr,
-    float min_match_score = 0.95f,
+    const TriangulationOptions &options = {});
-    size_t min_group_size = 1);
 
 inline PoseBatch3D triangulate_poses(
     const PoseBatch2D &poses_2d,
@@ -222,8 +226,7 @@ inline PoseBatch3D triangulate_poses(
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
     const PoseBatch3D *previous_poses_3d = nullptr,
-    float min_match_score = 0.95f,
+    const TriangulationOptions &options = {})
-    size_t min_group_size = 1)
 {
     PoseBatch3DView previous_view_storage;
     const PoseBatch3DView *previous_view = nullptr;
@@ -233,5 +236,5 @@ inline PoseBatch3D triangulate_poses(
         previous_view = &previous_view_storage;
     }
     return triangulate_poses(
-        poses_2d.view(), cameras, roomparams, joint_names, previous_view, 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;
-}

rpt/triangulator.cpp

@@ -7,6 +7,7 @@
 #include <numeric>
 #include <set>
 #include <stdexcept>
+#include <string_view>
 #include <unordered_map>
 
 #include "interface.hpp"
@@ -222,158 +223,126 @@ private:
     std::vector<float> data;
 };
 
-class TriangulatorInternal
+struct PreparedInputs
 {
-public:
+    std::vector<CachedCamera> internal_cameras;
-    TriangulatorInternal(float min_match_score, size_t min_group_size)
+    std::vector<size_t> core_joint_idx;
-        : min_match_score(min_match_score), min_group_size(min_group_size)
+    std::vector<std::array<size_t, 2>> core_limbs_idx;
+    PackedPoseStore2D packed_poses;
+
+    PreparedInputs(
+        std::vector<CachedCamera> cameras_in,
+        std::vector<size_t> core_joint_idx_in,
+        std::vector<std::array<size_t, 2>> core_limbs_idx_in,
+        PackedPoseStore2D packed_poses_in)
+        : internal_cameras(std::move(cameras_in)),
+          core_joint_idx(std::move(core_joint_idx_in)),
+          core_limbs_idx(std::move(core_limbs_idx_in)),
+          packed_poses(std::move(packed_poses_in))
     {
     }
-
-    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);
-
-    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);
-
-private:
-    struct PreparedInputs
-    {
-        std::vector<CachedCamera> internal_cameras;
-        std::vector<size_t> core_joint_idx;
-        std::vector<std::array<size_t, 2>> core_limbs_idx;
-        PackedPoseStore2D packed_poses;
-
-        PreparedInputs(
-            std::vector<CachedCamera> cameras_in,
-            std::vector<size_t> core_joint_idx_in,
-            std::vector<std::array<size_t, 2>> core_limbs_idx_in,
-            PackedPoseStore2D packed_poses_in)
-            : internal_cameras(std::move(cameras_in)),
-              core_joint_idx(std::move(core_joint_idx_in)),
-              core_limbs_idx(std::move(core_limbs_idx_in)),
-              packed_poses(std::move(packed_poses_in))
-        {
-        }
-    };
-
-    struct PreviousProjectionCache
-    {
-        std::vector<std::vector<Pose2D>> projected_poses;
-        std::vector<std::vector<std::vector<float>>> projected_dists;
-        std::vector<Pose3D> core_poses;
-    };
-
-    PreparedInputs prepare_inputs(
-        const PoseBatch2DView &poses_2d,
-        const std::vector<Camera> &cameras,
-        const std::vector<std::string> &joint_names);
-
-    std::vector<PairCandidate> build_pair_candidates(const PackedPoseStore2D &packed_poses) const;
-
-    PreviousProjectionCache project_previous_poses(
-        const PoseBatch3DView &previous_poses_3d,
-        const std::vector<CachedCamera> &internal_cameras,
-        const std::vector<size_t> &core_joint_idx);
-
-    PreviousPoseFilterDebug filter_pairs_with_previous_poses(
-        const PackedPoseStore2D &packed_poses,
-        const std::vector<CachedCamera> &internal_cameras,
-        const std::vector<size_t> &core_joint_idx,
-        const std::vector<PairCandidate> &pairs,
-        const PoseBatch3DView &previous_poses_3d);
-
-    float calc_pose_score(
-        const Pose2D &pose,
-        const Pose2D &reference_pose,
-        const std::vector<float> &dists,
-        const CachedCamera &icam);
-
-    std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
-        const std::vector<Pose3D> &poses_3d,
-        const CachedCamera &icam,
-        bool calc_dists);
-
-    std::vector<float> score_projection(
-        const Pose2D &pose,
-        const Pose2D &repro,
-        const std::vector<float> &dists,
-        const std::vector<bool> &mask,
-        float iscale);
-
-    std::pair<Pose3D, float> triangulate_and_score(
-        const Pose2D &pose1,
-        const Pose2D &pose2,
-        const CachedCamera &cam1,
-        const CachedCamera &cam2,
-        const std::array<std::array<float, 3>, 2> &roomparams,
-        const std::vector<std::array<size_t, 2>> &core_limbs_idx);
-
-    std::vector<GroupedPose> calc_grouping(
-        const std::vector<PosePair> &all_pairs,
-        const std::vector<std::pair<Pose3D, float>> &all_scored_poses,
-        float min_score);
-
-    Pose3D merge_group(
-        const std::vector<Pose3D> &poses_3d,
-        float min_score);
-
-    void add_extra_joints(
-        std::vector<Pose3D> &poses,
-        const std::vector<std::string> &joint_names);
-
-    void filter_poses(
-        std::vector<Pose3D> &poses,
-        const std::array<std::array<float, 3>, 2> &roomparams,
-        const std::vector<size_t> &core_joint_idx,
-        const std::vector<std::array<size_t, 2>> &core_limbs_idx);
-
-    void add_missing_joints(
-        std::vector<Pose3D> &poses,
-        const std::vector<std::string> &joint_names);
-
-    void replace_far_joints(
-        std::vector<Pose3D> &poses,
-        const std::vector<std::string> &joint_names);
-
-    float min_match_score;
-    size_t min_group_size;
-    size_t num_cams = 0;
-
-    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",
-    };
-    const std::vector<std::pair<std::string, std::string>> core_limbs = {
-        {"knee_left", "ankle_left"},
-        {"hip_left", "knee_left"},
-        {"hip_right", "knee_right"},
-        {"knee_right", "ankle_right"},
-        {"elbow_left", "wrist_left"},
-        {"elbow_right", "wrist_right"},
-        {"shoulder_left", "elbow_left"},
-        {"shoulder_right", "elbow_right"},
-    };
 };
 
-TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
+struct PreviousProjectionCache
+{
+    std::vector<std::vector<Pose2D>> projected_poses;
+    std::vector<std::vector<std::vector<float>>> projected_dists;
+    std::vector<Pose3D> core_poses;
+};
+
+constexpr std::array<std::string_view, 12> kCoreJoints = {
+    "shoulder_left",
+    "shoulder_right",
+    "hip_left",
+    "hip_right",
+    "elbow_left",
+    "elbow_right",
+    "knee_left",
+    "knee_right",
+    "wrist_left",
+    "wrist_right",
+    "ankle_left",
+    "ankle_right",
+};
+
+constexpr std::array<std::pair<std::string_view, std::string_view>, 8> kCoreLimbs = {{
+    {"knee_left", "ankle_left"},
+    {"hip_left", "knee_left"},
+    {"hip_right", "knee_right"},
+    {"knee_right", "ankle_right"},
+    {"elbow_left", "wrist_left"},
+    {"elbow_right", "wrist_right"},
+    {"shoulder_left", "elbow_left"},
+    {"shoulder_right", "elbow_right"},
+}};
+
+std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses);
+PreviousProjectionCache project_previous_poses(
+    const PoseBatch3DView &previous_poses_3d,
+    const std::vector<CachedCamera> &internal_cameras,
+    const std::vector<size_t> &core_joint_idx);
+PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
+    const PackedPoseStore2D &packed_poses,
+    const std::vector<CachedCamera> &internal_cameras,
+    const std::vector<size_t> &core_joint_idx,
+    const std::vector<PairCandidate> &pairs,
+    const TriangulationOptions &options,
+    const PoseBatch3DView &previous_poses_3d);
+float calc_pose_score(
+    const Pose2D &pose,
+    const Pose2D &reference_pose,
+    const std::vector<float> &dists,
+    const CachedCamera &icam);
+std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
+    const std::vector<Pose3D> &poses_3d,
+    const CachedCamera &icam,
+    bool calc_dists);
+std::vector<float> score_projection(
+    const Pose2D &pose,
+    const Pose2D &repro,
+    const std::vector<float> &dists,
+    const std::vector<bool> &mask,
+    float iscale);
+std::pair<Pose3D, float> triangulate_and_score(
+    const Pose2D &pose1,
+    const Pose2D &pose2,
+    const CachedCamera &cam1,
+    const CachedCamera &cam2,
+    const std::array<std::array<float, 3>, 2> &roomparams,
+    const std::vector<std::array<size_t, 2>> &core_limbs_idx);
+std::vector<GroupedPose> calc_grouping(
+    const std::vector<PosePair> &all_pairs,
+    const std::vector<std::pair<Pose3D, float>> &all_scored_poses,
+    float min_score);
+Pose3D merge_group(
+    const std::vector<Pose3D> &poses_3d,
+    float min_score,
+    size_t num_cams);
+void add_extra_joints(
+    std::vector<Pose3D> &poses,
+    const std::vector<std::string> &joint_names);
+void filter_poses(
+    std::vector<Pose3D> &poses,
+    const std::array<std::array<float, 3>, 2> &roomparams,
+    const std::vector<size_t> &core_joint_idx,
+    const std::vector<std::array<size_t, 2>> &core_limbs_idx);
+void add_missing_joints(
+    std::vector<Pose3D> &poses,
+    const std::vector<std::string> &joint_names,
+    float min_match_score);
+void replace_far_joints(
+    std::vector<Pose3D> &poses,
+    const std::vector<std::string> &joint_names,
+    float min_match_score);
+TriangulationTrace triangulate_debug_impl(
+    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,
+    const TriangulationOptions &options);
+
+PreparedInputs prepare_inputs(
     const PoseBatch2DView &poses_2d,
     const std::vector<Camera> &cameras,
     const std::vector<std::string> &joint_names)
@@ -402,16 +371,14 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
     {
         throw std::invalid_argument("poses_2d data must not be null.");
     }
-    for (const auto &joint : core_joints)
+    for (const auto &joint : kCoreJoints)
     {
         if (std::find(joint_names.begin(), joint_names.end(), joint) == joint_names.end())
         {
-            throw std::invalid_argument("Core joint '" + joint + "' not found in joint names.");
+            throw std::invalid_argument("Core joint '" + std::string(joint) + "' not found in joint names.");
         }
     }
 
-    num_cams = cameras.size();
-
     std::vector<CachedCamera> internal_cameras;
     internal_cameras.reserve(cameras.size());
     for (const auto &camera : cameras)
@@ -420,22 +387,22 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
     }
 
     std::vector<size_t> core_joint_idx;
-    core_joint_idx.reserve(core_joints.size());
+    core_joint_idx.reserve(kCoreJoints.size());
-    for (const auto &joint : core_joints)
+    for (const auto &joint : kCoreJoints)
     {
         auto it = std::find(joint_names.begin(), joint_names.end(), joint);
         core_joint_idx.push_back(static_cast<size_t>(std::distance(joint_names.begin(), it)));
     }
 
     std::vector<std::array<size_t, 2>> core_limbs_idx;
-    core_limbs_idx.reserve(core_limbs.size());
+    core_limbs_idx.reserve(kCoreLimbs.size());
-    for (const auto &limb : core_limbs)
+    for (const auto &limb : kCoreLimbs)
     {
-        auto it1 = std::find(core_joints.begin(), core_joints.end(), limb.first);
+        auto it1 = std::find(kCoreJoints.begin(), kCoreJoints.end(), limb.first);
-        auto it2 = std::find(core_joints.begin(), core_joints.end(), limb.second);
+        auto it2 = std::find(kCoreJoints.begin(), kCoreJoints.end(), limb.second);
         core_limbs_idx.push_back(
-            {static_cast<size_t>(std::distance(core_joints.begin(), it1)),
+            {static_cast<size_t>(std::distance(kCoreJoints.begin(), it1)),
-             static_cast<size_t>(std::distance(core_joints.begin(), it2))});
+             static_cast<size_t>(std::distance(kCoreJoints.begin(), it2))});
     }
 
     PackedPoseStore2D packed_poses(poses_2d, internal_cameras);
@@ -446,7 +413,7 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs(
         std::move(packed_poses));
 }
 
-std::vector<PairCandidate> TriangulatorInternal::build_pair_candidates(const PackedPoseStore2D &packed_poses) const
+std::vector<PairCandidate> build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses)
 {
     std::vector<PairCandidate> pairs;
     for (size_t view1 = 0; view1 < packed_poses.num_views; ++view1)
@@ -472,7 +439,7 @@ std::vector<PairCandidate> TriangulatorInternal::build_pair_candidates(const Pac
     return pairs;
 }
 
-TriangulatorInternal::PreviousProjectionCache TriangulatorInternal::project_previous_poses(
+PreviousProjectionCache project_previous_poses(
     const PoseBatch3DView &previous_poses_3d,
     const std::vector<CachedCamera> &internal_cameras,
     const std::vector<size_t> &core_joint_idx)
@@ -505,7 +472,7 @@ TriangulatorInternal::PreviousProjectionCache TriangulatorInternal::project_prev
     return cache;
 }
 
-float TriangulatorInternal::calc_pose_score(
+float calc_pose_score(
     const Pose2D &pose,
     const Pose2D &reference_pose,
     const std::vector<float> &dists,
@@ -543,11 +510,12 @@ float TriangulatorInternal::calc_pose_score(
     return total / static_cast<float>(scores.size() - drop_k);
 }
 
-PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
+PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
     const PackedPoseStore2D &packed_poses,
     const std::vector<CachedCamera> &internal_cameras,
     const std::vector<size_t> &core_joint_idx,
     const std::vector<PairCandidate> &pairs,
+    const TriangulationOptions &options,
     const PoseBatch3DView &previous_poses_3d)
 {
     PreviousPoseFilterDebug debug;
@@ -568,7 +536,7 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
 
     const PreviousProjectionCache projected_previous =
         project_previous_poses(previous_poses_3d, internal_cameras, core_joint_idx);
-    const float threshold = min_match_score;
+    const float threshold = options.min_match_score;
 
     for (size_t pair_index = 0; pair_index < pairs.size(); ++pair_index)
     {
@@ -643,12 +611,13 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
     return debug;
 }
 
-TriangulationTrace TriangulatorInternal::triangulate_debug(
+TriangulationTrace triangulate_debug_impl(
     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)
+    const PoseBatch3DView *previous_poses_3d,
+    const TriangulationOptions &options)
 {
     TriangulationTrace trace;
     trace.final_poses.num_joints = joint_names.size();
@@ -660,7 +629,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
     }
 
     PreparedInputs prepared = prepare_inputs(poses_2d, cameras, joint_names);
-    trace.pairs = build_pair_candidates(prepared.packed_poses);
+    trace.pairs = build_pair_candidates_from_packed(prepared.packed_poses);
     if (trace.pairs.empty())
     {
         return trace;
@@ -672,11 +641,12 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
 
     if (previous_poses_3d != nullptr)
     {
-        trace.previous_filter = filter_pairs_with_previous_poses(
+        trace.previous_filter = filter_pairs_with_previous_poses_impl(
             prepared.packed_poses,
             prepared.internal_cameras,
             prepared.core_joint_idx,
             trace.pairs,
+            options,
             *previous_poses_3d);
         active_pair_indices = trace.previous_filter.kept_pair_indices;
         active_pairs = trace.previous_filter.kept_pairs;
@@ -708,7 +678,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
         proposal.pair = pair;
         proposal.pose_3d = pose3d;
         proposal.score = score;
-        proposal.kept = score >= min_match_score;
+        proposal.kept = score >= options.min_match_score;
         proposal.drop_reason = proposal.kept ? "kept" : "below_min_match_score";
         trace.core_proposals.push_back(proposal);
 
@@ -729,7 +699,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
         return trace;
     }
 
-    auto groups = calc_grouping(kept_pose_pairs, kept_scored_poses, min_match_score);
+    auto groups = calc_grouping(kept_pose_pairs, kept_scored_poses, options.min_match_score);
     trace.grouping.initial_groups.reserve(groups.size());
     for (const auto &group : groups)
     {
@@ -745,7 +715,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
 
     for (size_t i = groups.size(); i > 0; --i)
     {
-        if (std::get<2>(groups[i - 1]).size() < min_group_size)
+        if (std::get<2>(groups[i - 1]).size() < options.min_group_size)
         {
             groups.erase(groups.begin() + static_cast<std::ptrdiff_t>(i - 1));
         }
@@ -842,7 +812,7 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
 
     for (size_t i = groups.size(); i > 0; --i)
     {
-        if (std::get<2>(groups[i - 1]).size() < min_group_size)
+        if (std::get<2>(groups[i - 1]).size() < options.min_group_size)
         {
             groups.erase(groups.begin() + static_cast<std::ptrdiff_t>(i - 1));
         }
@@ -923,15 +893,15 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
             poses.push_back(full_pose_buffer[static_cast<size_t>(index)]);
             source_core_indices.push_back(kept_core_indices[static_cast<size_t>(index)]);
         }
-        final_poses_3d[i] = merge_group(poses, min_match_score);
+        final_poses_3d[i] = merge_group(poses, options.min_match_score, prepared.internal_cameras.size());
         trace.merge.group_proposal_indices.push_back(std::move(source_core_indices));
         trace.merge.merged_poses.push_back(final_poses_3d[i]);
     }
 
     add_extra_joints(final_poses_3d, joint_names);
     filter_poses(final_poses_3d, roomparams, prepared.core_joint_idx, prepared.core_limbs_idx);
-    add_missing_joints(final_poses_3d, joint_names);
+    add_missing_joints(final_poses_3d, joint_names, options.min_match_score);
-    replace_far_joints(final_poses_3d, joint_names);
+    replace_far_joints(final_poses_3d, joint_names, options.min_match_score);
 
     size_t valid_persons = 0;
     for (const auto &pose : final_poses_3d)
@@ -939,9 +909,9 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
         const bool is_valid = std::any_of(
             pose.begin(),
             pose.end(),
-            [this](const std::array<float, 4> &joint)
+            [&options](const std::array<float, 4> &joint)
             {
-                return joint[3] > min_match_score;
+                return joint[3] > options.min_match_score;
             });
         if (is_valid)
         {
@@ -958,9 +928,9 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
         const bool is_valid = std::any_of(
             pose.begin(),
             pose.end(),
-            [this](const std::array<float, 4> &joint)
+            [&options](const std::array<float, 4> &joint)
             {
-                return joint[3] > min_match_score;
+                return joint[3] > options.min_match_score;
             });
         if (!is_valid)
         {
@@ -980,11 +950,12 @@ TriangulationTrace TriangulatorInternal::triangulate_debug(
     return trace;
 }
 
-PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
+PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl(
     const PoseBatch2DView &poses_2d,
     const std::vector<Camera> &cameras,
     const std::vector<std::string> &joint_names,
-    const PoseBatch3DView &previous_poses_3d)
+    const PoseBatch3DView &previous_poses_3d,
+    const TriangulationOptions &options)
 {
     if (previous_poses_3d.num_persons > 0 && previous_poses_3d.num_joints != joint_names.size())
     {
@@ -992,7 +963,7 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
     }
 
     PreparedInputs prepared = prepare_inputs(poses_2d, cameras, joint_names);
-    const std::vector<PairCandidate> pairs = build_pair_candidates(prepared.packed_poses);
+    const std::vector<PairCandidate> pairs = build_pair_candidates_from_packed(prepared.packed_poses);
     if (pairs.empty())
     {
         PreviousPoseFilterDebug empty;
@@ -1000,15 +971,16 @@ PreviousPoseFilterDebug TriangulatorInternal::filter_pairs_with_previous_poses(
         return empty;
     }
 
-    return filter_pairs_with_previous_poses(
+    return filter_pairs_with_previous_poses_impl(
         prepared.packed_poses,
         prepared.internal_cameras,
         prepared.core_joint_idx,
         pairs,
+        options,
         previous_poses_3d);
 }
 
-std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> TriangulatorInternal::project_poses(
+std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> project_poses(
     const std::vector<Pose3D> &poses_3d,
     const CachedCamera &icam,
     bool calc_dists)
@@ -1082,7 +1054,7 @@ std::tuple<std::vector<Pose2D>, std::vector<std::vector<float>>> TriangulatorInt
     return std::make_tuple(poses_2d, all_dists);
 }
 
-std::vector<float> TriangulatorInternal::score_projection(
+std::vector<float> score_projection(
     const Pose2D &pose,
     const Pose2D &repro,
     const std::vector<float> &dists,
@@ -1231,7 +1203,7 @@ std::array<float, 4> triangulate_midpoint(
 
 // =================================================================================================
 
-std::pair<std::vector<std::array<float, 4>>, float> TriangulatorInternal::triangulate_and_score(
+std::pair<std::vector<std::array<float, 4>>, float> triangulate_and_score(
     const std::vector<std::array<float, 3>> &pose1,
     const std::vector<std::array<float, 3>> &pose2,
     const CachedCamera &cam1,
@@ -1424,7 +1396,7 @@ std::pair<std::vector<std::array<float, 4>>, float> TriangulatorInternal::triang
 // =================================================================================================
 
 std::vector<std::tuple<std::array<float, 3>, std::vector<std::array<float, 4>>, std::vector<int>>>
-TriangulatorInternal::calc_grouping(
+calc_grouping(
     const std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> &all_pairs,
     const std::vector<std::pair<std::vector<std::array<float, 4>>, float>> &all_scored_poses,
     float min_score)
@@ -1692,9 +1664,10 @@ TriangulatorInternal::calc_grouping(
 
 // =================================================================================================
 
-std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
+std::vector<std::array<float, 4>> merge_group(
     const std::vector<std::vector<std::array<float, 4>>> &poses_3d,
-    float min_score)
+    float min_score,
+    size_t num_cams)
 {
     size_t num_poses = poses_3d.size();
     size_t num_joints = poses_3d[0].size();
@@ -1794,7 +1767,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
     }
 
     // Select the best-k proposals for each joint that are closest to the initial pose
-    int keep_best = std::max((int)std::ceil(this->num_cams / 2.0), (int)std::ceil(num_poses / 3.0));
+    int keep_best = std::max((int)std::ceil(num_cams / 2.0), (int)std::ceil(num_poses / 3.0));
     std::vector<std::vector<bool>> best_k_mask(num_poses, std::vector<bool>(num_joints, false));
     for (size_t j = 0; j < num_joints; ++j)
     {
@@ -1869,7 +1842,7 @@ std::vector<std::array<float, 4>> TriangulatorInternal::merge_group(
 
 // =================================================================================================
 
-void TriangulatorInternal::add_extra_joints(
+void add_extra_joints(
     std::vector<std::vector<std::array<float, 4>>> &poses,
     const std::vector<std::string> &joint_names)
 {
@@ -1900,7 +1873,7 @@ void TriangulatorInternal::add_extra_joints(
 
 // =================================================================================================
 
-void TriangulatorInternal::filter_poses(
+void filter_poses(
     std::vector<std::vector<std::array<float, 4>>> &poses,
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<size_t> &core_joint_idx,
@@ -2091,9 +2064,10 @@ void TriangulatorInternal::filter_poses(
 
 // =================================================================================================
 
-void TriangulatorInternal::add_missing_joints(
+void add_missing_joints(
     std::vector<std::vector<std::array<float, 4>>> &poses,
-    const std::vector<std::string> &joint_names)
+    const std::vector<std::string> &joint_names,
+    float min_match_score)
 {
     // Map joint names to their indices for quick lookup
     std::unordered_map<std::string, size_t> joint_name_to_idx;
@@ -2229,9 +2203,10 @@ void TriangulatorInternal::add_missing_joints(
 
 // =================================================================================================
 
-void TriangulatorInternal::replace_far_joints(
+void replace_far_joints(
     std::vector<std::vector<std::array<float, 4>>> &poses,
-    const std::vector<std::string> &joint_names)
+    const std::vector<std::string> &joint_names,
+    float min_match_score)
 {
     for (size_t i = 0; i < poses.size(); ++i)
     {
@@ -2248,7 +2223,7 @@ void TriangulatorInternal::replace_far_joints(
         {
             const std::string &jname = joint_names[j];
             float offset2 = (1.0 - min_match_score) * 2;
-            float min_score = this->min_match_score - offset2;
+            float min_score = min_match_score - offset2;
 
             if (pose[j][3] > min_score)
             {
@@ -2444,10 +2419,9 @@ PreviousPoseFilterDebug filter_pairs_with_previous_poses(
     const std::vector<Camera> &cameras,
     const std::vector<std::string> &joint_names,
     const PoseBatch3DView &previous_poses_3d,
-    float min_match_score)
+    const TriangulationOptions &options)
 {
-    TriangulatorInternal triangulator(min_match_score, 1);
+    return filter_pairs_with_previous_poses_impl(poses_2d, cameras, joint_names, previous_poses_3d, options);
-    return triangulator.filter_pairs_with_previous_poses(poses_2d, cameras, joint_names, previous_poses_3d);
 }
 
 TriangulationTrace triangulate_debug(
@@ -2456,11 +2430,9 @@ TriangulationTrace triangulate_debug(
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
     const PoseBatch3DView *previous_poses_3d,
-    float min_match_score,
+    const TriangulationOptions &options)
-    size_t min_group_size)
 {
-    TriangulatorInternal triangulator(min_match_score, min_group_size);
+    return triangulate_debug_impl(poses_2d, cameras, roomparams, joint_names, previous_poses_3d, options);
-    return triangulator.triangulate_debug(poses_2d, cameras, roomparams, joint_names, previous_poses_3d);
 }
 
 PoseBatch3D triangulate_poses(
@@ -2469,8 +2441,7 @@ PoseBatch3D triangulate_poses(
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names,
     const PoseBatch3DView *previous_poses_3d,
-    float min_match_score,
+    const TriangulationOptions &options)
-    size_t min_group_size)
 {
     return triangulate_debug(
                poses_2d,
@@ -2478,7 +2449,6 @@ PoseBatch3D triangulate_poses(
                roomparams,
                joint_names,
                previous_poses_3d,
-               min_match_score,
+               options)
-               min_group_size)
         .final_poses;
 }

src/rpt/__init__.py

@@ -6,6 +6,7 @@ from typing import TYPE_CHECKING
 from ._core import (
     Camera,
     CameraModel,
+    TriangulationOptions,
     CoreProposalDebug,
     FullProposalDebug,
     GroupingDebug,
@@ -45,6 +46,7 @@ def pack_poses_2d(
 __all__ = [
     "Camera",
     "CameraModel",
+    "TriangulationOptions",
     "CoreProposalDebug",
     "FullProposalDebug",
     "GroupingDebug",