diff --git a/README.md b/README.md index c60436a..02df642 100644 --- a/README.md +++ b/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.
diff --git a/bindings/rpt_module.cpp b/bindings/rpt_module.cpp index 8f94f47..a83934c 100644 --- a/bindings/rpt_module.cpp +++ b/bindings/rpt_module.cpp @@ -167,6 +167,11 @@ NB_MODULE(_core, m) return camera.to_string(); }); + nb::class_(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_(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, - min_group_size); + options); }, "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, - min_group_size); + options); }, "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, - min_group_size); + options); 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, - min_group_size); + options); return pose_batch_to_numpy(poses_3d); }, "poses_2d"_a, diff --git a/extras/ros/README.md b/extras/ros/README.md deleted file mode 100644 index e1b3d11..0000000 --- a/extras/ros/README.md +++ /dev/null @@ -1,22 +0,0 @@ -# ROS Wrapper - -Run the 3D triangulator with ROS topics as inputs and publish detected poses. - -
- -- 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 - ``` diff --git a/extras/ros/docker-compose-3d.yml b/extras/ros/docker-compose-3d.yml deleted file mode 100644 index 7989b27..0000000 --- a/extras/ros/docker-compose-3d.yml +++ /dev/null @@ -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' diff --git a/extras/ros/dockerfile_3d b/extras/ros/dockerfile_3d deleted file mode 100644 index 1196e69..0000000 --- a/extras/ros/dockerfile_3d +++ /dev/null @@ -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"] diff --git a/extras/ros/marker_publishers/CMakeLists.txt b/extras/ros/marker_publishers/CMakeLists.txt deleted file mode 100644 index bd51c83..0000000 --- a/extras/ros/marker_publishers/CMakeLists.txt +++ /dev/null @@ -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 - $ - $) - -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 - $ - $) - -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 - $ - $) - -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() diff --git a/extras/ros/marker_publishers/package.xml b/extras/ros/marker_publishers/package.xml deleted file mode 100644 index 2ca1087..0000000 --- a/extras/ros/marker_publishers/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - marker_publishers - 0.0.0 - TODO: Package description - root - TODO: License declaration - - ament_cmake - - rclcpp - rpt_msgs - geometry_msgs - tf2_ros - visualization_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/extras/ros/marker_publishers/src/cube_markers.cpp b/extras/ros/marker_publishers/src/cube_markers.cpp deleted file mode 100644 index 403f2fd..0000000 --- a/extras/ros/marker_publishers/src/cube_markers.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -// ================================================================================================= - -const std::string output_topic = "/markers_cube"; -static const std::array, 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(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::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> 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> 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()); - rclcpp::shutdown(); - return 0; -} diff --git a/extras/ros/marker_publishers/src/skeleton_markers.cpp b/extras/ros/marker_publishers/src/skeleton_markers.cpp deleted file mode 100644 index c4c3448..0000000 --- a/extras/ros/marker_publishers/src/skeleton_markers.cpp +++ /dev/null @@ -1,255 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "rpt_msgs/msg/poses.hpp" - -// ================================================================================================= - -const std::string input_topic = "/poses/humans3d"; -const std::string output_topic = "/markers_skeleton"; - -// ================================================================================================= -// ================================================================================================= - -std::array hue_to_rgb(double hue) -{ - double r, g, b; - int h = static_cast(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 rgb = { - static_cast(r * 255), - static_cast(g * 255), - static_cast(b * 255)}; - - return rgb; -} - -// ================================================================================================= -// ================================================================================================= - -class SkeletonPublisher : public rclcpp::Node -{ -public: - SkeletonPublisher() : Node("skeleton_publisher") - { - publisher_ = this->create_publisher(output_topic, 1); - subscriber_ = this->create_subscription( - 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> connections; - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Subscription::SharedPtr subscriber_; - - void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg); - visualization_msgs::msg::MarkerArray generate_msg( - const std::vector>> &poses, - const std::vector &joint_names); -}; - -// ================================================================================================= - -void SkeletonPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg) -{ - std::vector>> poses; - const std::vector &joint_names = msg->joint_names; - - // Unflatten poses - size_t idx = 0; - std::vector &bshape = msg->bodies_shape; - for (int32_t i = 0; i < bshape[0]; ++i) - { - std::vector> body; - for (int32_t j = 0; j < bshape[1]; ++j) - { - std::array 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>> &poses, - const std::vector &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(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()); - rclcpp::shutdown(); - return 0; -} diff --git a/extras/ros/marker_publishers/src/skeleton_tfs.cpp b/extras/ros/marker_publishers/src/skeleton_tfs.cpp deleted file mode 100644 index 2444b56..0000000 --- a/extras/ros/marker_publishers/src/skeleton_tfs.cpp +++ /dev/null @@ -1,335 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include -#include - -#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(this); - subscriber_ = this->create_subscription( - 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> pc_connections; - std::map cp_map; - std::shared_ptr broadcaster_; - rclcpp::Subscription::SharedPtr subscriber_; - - void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg); - std::vector generate_msg( - const std::vector>> &poses, - const std::vector &joint_names); -}; - -// ================================================================================================= - -void SkeletonTFPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg) -{ - std::vector>> poses; - const std::vector &joint_names = msg->joint_names; - - // Unflatten poses - size_t idx = 0; - std::vector &bshape = msg->bodies_shape; - for (int32_t i = 0; i < bshape[0]; ++i) - { - std::vector> body; - for (int32_t j = 0; j < bshape[1]; ++j) - { - std::array 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 SkeletonTFPublisher::generate_msg( - const std::vector>> &poses, - const std::vector &joint_names) -{ - std::vector 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(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(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(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()); - rclcpp::shutdown(); - return 0; -} diff --git a/extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt b/extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt deleted file mode 100644 index df7d33d..0000000 --- a/extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt +++ /dev/null @@ -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 - $ - $) - -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() diff --git a/extras/ros/rpt3d_wrapper_cpp/package.xml b/extras/ros/rpt3d_wrapper_cpp/package.xml deleted file mode 100644 index 953455d..0000000 --- a/extras/ros/rpt3d_wrapper_cpp/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - rpt3d_wrapper_cpp - 0.0.0 - TODO: Package description - root - TODO: License declaration - - ament_cmake - - rclcpp - rpt_msgs - std_msgs - - OpenCV - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp b/extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp deleted file mode 100644 index be9104d..0000000 --- a/extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp +++ /dev/null @@ -1,323 +0,0 @@ -#include -#include -#include -#include -#include -#include - -// ROS2 -#include -#include - -// 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 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, 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( - 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( - 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( - 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(pose_out_topic, qos_profile); - - RCLCPP_INFO(this->get_logger(), "Finished initialization of pose triangulator."); - } - -private: - std::vector::SharedPtr> sub_pose_list_; - std::vector::SharedPtr> sub_cam_list_; - rclcpp::Publisher::SharedPtr pose_pub_; - - std::unique_ptr pose_tracker; - std::vector all_cameras; - std::mutex cams_mutex, pose_mutex, model_mutex; - - std::vector>>> all_poses; - std::vector all_timestamps; - std::vector joint_names; - std::vector 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(); - camera.K = cam["K"].get, 3>>(); - camera.DC = cam["DC"].get>(); - camera.R = cam["R"].get, 3>>(); - camera.T = cam["T"].get, 3>>(); - camera.width = cam["width"].get(); - camera.height = cam["height"].get(); - camera.type = cam["type"].get(); - - 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>> poses; - if (this->joint_names.empty()) - { - this->joint_names = msg->joint_names; - } - - // Unflatten poses - size_t idx = 0; - std::vector &bshape = msg->bodies_shape; - for (int32_t i = 0; i < bshape[0]; ++i) - { - std::vector> body; - for (int32_t j = 0; j < bshape[1]; ++j) - { - std::array 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> 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(cam_ids.size(), false); - - std::vector>> valid_poses; - std::vector track_ids; - if (use_tracking) - { - auto pose_tracks = pose_tracker->track_poses(poses_3d, joint_names, min_ts); - std::vector>> 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(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(min_ts); - pose_msg.header.stamp.nanosec = (min_ts - pose_msg.header.stamp.sec) * 1.0e9; - pose_msg.header.frame_id = "world"; - std::vector 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( - 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(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(node); - exec.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/extras/ros/rpt_msgs/CMakeLists.txt b/extras/ros/rpt_msgs/CMakeLists.txt deleted file mode 100644 index b326d03..0000000 --- a/extras/ros/rpt_msgs/CMakeLists.txt +++ /dev/null @@ -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( 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() diff --git a/extras/ros/rpt_msgs/msg/Poses.msg b/extras/ros/rpt_msgs/msg/Poses.msg deleted file mode 100644 index 96ad74b..0000000 --- a/extras/ros/rpt_msgs/msg/Poses.msg +++ /dev/null @@ -1,7 +0,0 @@ -std_msgs/Header header - -float32[] bodies_flat -int32[] bodies_shape -string[] joint_names - -string extra_data diff --git a/extras/ros/rpt_msgs/package.xml b/extras/ros/rpt_msgs/package.xml deleted file mode 100644 index 7c1fda5..0000000 --- a/extras/ros/rpt_msgs/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - rpt_msgs - 0.0.0 - TODO: Package description - root - TODO: License declaration - - std_msgs - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/rpt/interface.hpp b/rpt/interface.hpp index 1d13bbf..02cbe6d 100644 --- a/rpt/interface.hpp +++ b/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 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 &cameras, const std::vector &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 &cameras, const std::vector &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, 2> &roomparams, const std::vector &joint_names, const PoseBatch3DView *previous_poses_3d = nullptr, - float min_match_score = 0.95f, - size_t min_group_size = 1); + const TriangulationOptions &options = {}); inline TriangulationTrace triangulate_debug( const PoseBatch2D &poses_2d, @@ -179,8 +186,7 @@ inline TriangulationTrace triangulate_debug( const std::array, 2> &roomparams, const std::vector &joint_names, const PoseBatch3D *previous_poses_3d = nullptr, - float min_match_score = 0.95f, - size_t min_group_size = 1) + const TriangulationOptions &options = {}) { 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 min_group_size Minimum number of camera pairs that need to see a person. + * @param options Triangulation options. * * @return Pose tensor of shape [persons, joints, 4]. */ @@ -213,8 +218,7 @@ PoseBatch3D triangulate_poses( const std::array, 2> &roomparams, const std::vector &joint_names, const PoseBatch3DView *previous_poses_3d = nullptr, - float min_match_score = 0.95f, - size_t min_group_size = 1); + const TriangulationOptions &options = {}); inline PoseBatch3D triangulate_poses( const PoseBatch2D &poses_2d, @@ -222,8 +226,7 @@ inline PoseBatch3D triangulate_poses( const std::array, 2> &roomparams, const std::vector &joint_names, const PoseBatch3D *previous_poses_3d = nullptr, - float min_match_score = 0.95f, - size_t min_group_size = 1) + const TriangulationOptions &options = {}) { 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); } diff --git a/rpt/tracker.hpp b/rpt/tracker.hpp deleted file mode 100644 index 53be2b4..0000000 --- a/rpt/tracker.hpp +++ /dev/null @@ -1,325 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -// ================================================================================================= - -struct Track -{ - std::vector>> core_poses; - std::vector>> full_poses; - - std::vector timestamps; - size_t id; -}; - -// ================================================================================================= - -class PoseTracker -{ -public: - PoseTracker(float max_movement_speed, float max_distance); - - std::vector>>> track_poses( - const std::vector>> &poses_3d, - const std::vector &joint_names, - const double timestamp); - - void reset(); - -private: - float max_distance; - float max_movement_speed; - size_t history_size = 3; - - std::vector timestamps; - std::vector pose_tracks; - - const std::vector 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 match_to_track(const std::vector> &core_pose_3d); - - std::vector> 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>>> PoseTracker::track_poses( - const std::vector>> &poses_3d, - const std::vector &joint_names, - const double timestamp) -{ - // Extract core joints - std::vector 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>> 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> 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 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>>> 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> refined_pose = refine_pose(track); - tracked_poses.emplace_back(track.id, refined_pose); - } - } - - return tracked_poses; -} - -// ================================================================================================= - -std::tuple PoseTracker::match_to_track( - const std::vector> &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(i); - } - } - - return {best_track, best_distance_sq}; -} - -// ================================================================================================= - -std::vector> 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 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> refined_pose = track.full_poses.back(); - for (size_t i = 0; i < refined_pose.size(); ++i) - { - float min_dist_sq = std::numeric_limits::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; -} diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index 24ae9a7..c40e3c1 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include "interface.hpp" @@ -222,158 +223,126 @@ private: std::vector data; }; -class TriangulatorInternal +struct PreparedInputs { -public: - TriangulatorInternal(float min_match_score, size_t min_group_size) - : min_match_score(min_match_score), min_group_size(min_group_size) + std::vector internal_cameras; + std::vector core_joint_idx; + std::vector> core_limbs_idx; + PackedPoseStore2D packed_poses; + + PreparedInputs( + std::vector cameras_in, + std::vector core_joint_idx_in, + std::vector> 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 &cameras, - const std::array, 2> &roomparams, - const std::vector &joint_names, - const PoseBatch3DView *previous_poses_3d); - - PreviousPoseFilterDebug filter_pairs_with_previous_poses( - const PoseBatch2DView &poses_2d, - const std::vector &cameras, - const std::vector &joint_names, - const PoseBatch3DView &previous_poses_3d); - -private: - struct PreparedInputs - { - std::vector internal_cameras; - std::vector core_joint_idx; - std::vector> core_limbs_idx; - PackedPoseStore2D packed_poses; - - PreparedInputs( - std::vector cameras_in, - std::vector core_joint_idx_in, - std::vector> 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> projected_poses; - std::vector>> projected_dists; - std::vector core_poses; - }; - - PreparedInputs prepare_inputs( - const PoseBatch2DView &poses_2d, - const std::vector &cameras, - const std::vector &joint_names); - - std::vector build_pair_candidates(const PackedPoseStore2D &packed_poses) const; - - PreviousProjectionCache project_previous_poses( - const PoseBatch3DView &previous_poses_3d, - const std::vector &internal_cameras, - const std::vector &core_joint_idx); - - PreviousPoseFilterDebug filter_pairs_with_previous_poses( - const PackedPoseStore2D &packed_poses, - const std::vector &internal_cameras, - const std::vector &core_joint_idx, - const std::vector &pairs, - const PoseBatch3DView &previous_poses_3d); - - float calc_pose_score( - const Pose2D &pose, - const Pose2D &reference_pose, - const std::vector &dists, - const CachedCamera &icam); - - std::tuple, std::vector>> project_poses( - const std::vector &poses_3d, - const CachedCamera &icam, - bool calc_dists); - - std::vector score_projection( - const Pose2D &pose, - const Pose2D &repro, - const std::vector &dists, - const std::vector &mask, - float iscale); - - std::pair triangulate_and_score( - const Pose2D &pose1, - const Pose2D &pose2, - const CachedCamera &cam1, - const CachedCamera &cam2, - const std::array, 2> &roomparams, - const std::vector> &core_limbs_idx); - - std::vector calc_grouping( - const std::vector &all_pairs, - const std::vector> &all_scored_poses, - float min_score); - - Pose3D merge_group( - const std::vector &poses_3d, - float min_score); - - void add_extra_joints( - std::vector &poses, - const std::vector &joint_names); - - void filter_poses( - std::vector &poses, - const std::array, 2> &roomparams, - const std::vector &core_joint_idx, - const std::vector> &core_limbs_idx); - - void add_missing_joints( - std::vector &poses, - const std::vector &joint_names); - - void replace_far_joints( - std::vector &poses, - const std::vector &joint_names); - - float min_match_score; - size_t min_group_size; - size_t num_cams = 0; - - const std::vector 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> 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> projected_poses; + std::vector>> projected_dists; + std::vector core_poses; +}; + +constexpr std::array 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, 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 build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses); +PreviousProjectionCache project_previous_poses( + const PoseBatch3DView &previous_poses_3d, + const std::vector &internal_cameras, + const std::vector &core_joint_idx); +PreviousPoseFilterDebug filter_pairs_with_previous_poses_impl( + const PackedPoseStore2D &packed_poses, + const std::vector &internal_cameras, + const std::vector &core_joint_idx, + const std::vector &pairs, + const TriangulationOptions &options, + const PoseBatch3DView &previous_poses_3d); +float calc_pose_score( + const Pose2D &pose, + const Pose2D &reference_pose, + const std::vector &dists, + const CachedCamera &icam); +std::tuple, std::vector>> project_poses( + const std::vector &poses_3d, + const CachedCamera &icam, + bool calc_dists); +std::vector score_projection( + const Pose2D &pose, + const Pose2D &repro, + const std::vector &dists, + const std::vector &mask, + float iscale); +std::pair triangulate_and_score( + const Pose2D &pose1, + const Pose2D &pose2, + const CachedCamera &cam1, + const CachedCamera &cam2, + const std::array, 2> &roomparams, + const std::vector> &core_limbs_idx); +std::vector calc_grouping( + const std::vector &all_pairs, + const std::vector> &all_scored_poses, + float min_score); +Pose3D merge_group( + const std::vector &poses_3d, + float min_score, + size_t num_cams); +void add_extra_joints( + std::vector &poses, + const std::vector &joint_names); +void filter_poses( + std::vector &poses, + const std::array, 2> &roomparams, + const std::vector &core_joint_idx, + const std::vector> &core_limbs_idx); +void add_missing_joints( + std::vector &poses, + const std::vector &joint_names, + float min_match_score); +void replace_far_joints( + std::vector &poses, + const std::vector &joint_names, + float min_match_score); +TriangulationTrace triangulate_debug_impl( + const PoseBatch2DView &poses_2d, + const std::vector &cameras, + const std::array, 2> &roomparams, + const std::vector &joint_names, + const PoseBatch3DView *previous_poses_3d, + const TriangulationOptions &options); + +PreparedInputs prepare_inputs( const PoseBatch2DView &poses_2d, const std::vector &cameras, const std::vector &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 internal_cameras; internal_cameras.reserve(cameras.size()); for (const auto &camera : cameras) @@ -420,22 +387,22 @@ TriangulatorInternal::PreparedInputs TriangulatorInternal::prepare_inputs( } std::vector core_joint_idx; - core_joint_idx.reserve(core_joints.size()); - for (const auto &joint : core_joints) + core_joint_idx.reserve(kCoreJoints.size()); + for (const auto &joint : kCoreJoints) { auto it = std::find(joint_names.begin(), joint_names.end(), joint); core_joint_idx.push_back(static_cast(std::distance(joint_names.begin(), it))); } std::vector> core_limbs_idx; - core_limbs_idx.reserve(core_limbs.size()); - for (const auto &limb : core_limbs) + core_limbs_idx.reserve(kCoreLimbs.size()); + for (const auto &limb : kCoreLimbs) { - auto it1 = std::find(core_joints.begin(), core_joints.end(), limb.first); - auto it2 = std::find(core_joints.begin(), core_joints.end(), limb.second); + auto it1 = std::find(kCoreJoints.begin(), kCoreJoints.end(), limb.first); + auto it2 = std::find(kCoreJoints.begin(), kCoreJoints.end(), limb.second); core_limbs_idx.push_back( - {static_cast(std::distance(core_joints.begin(), it1)), - static_cast(std::distance(core_joints.begin(), it2))}); + {static_cast(std::distance(kCoreJoints.begin(), it1)), + static_cast(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 TriangulatorInternal::build_pair_candidates(const PackedPoseStore2D &packed_poses) const +std::vector build_pair_candidates_from_packed(const PackedPoseStore2D &packed_poses) { std::vector pairs; for (size_t view1 = 0; view1 < packed_poses.num_views; ++view1) @@ -472,7 +439,7 @@ std::vector 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 &internal_cameras, const std::vector &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 &dists, @@ -543,11 +510,12 @@ float TriangulatorInternal::calc_pose_score( return total / static_cast(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 &internal_cameras, const std::vector &core_joint_idx, const std::vector &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 &cameras, const std::array, 2> &roomparams, const std::vector &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(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(i - 1)); } @@ -923,15 +893,15 @@ TriangulationTrace TriangulatorInternal::triangulate_debug( poses.push_back(full_pose_buffer[static_cast(index)]); source_core_indices.push_back(kept_core_indices[static_cast(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); - replace_far_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, 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 &joint) + [&options](const std::array &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 &joint) + [&options](const std::array &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 &cameras, const std::vector &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 pairs = build_pair_candidates(prepared.packed_poses); + const std::vector 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>> TriangulatorInternal::project_poses( +std::tuple, std::vector>> project_poses( const std::vector &poses_3d, const CachedCamera &icam, bool calc_dists) @@ -1082,7 +1054,7 @@ std::tuple, std::vector>> TriangulatorInt return std::make_tuple(poses_2d, all_dists); } -std::vector TriangulatorInternal::score_projection( +std::vector score_projection( const Pose2D &pose, const Pose2D &repro, const std::vector &dists, @@ -1231,7 +1203,7 @@ std::array triangulate_midpoint( // ================================================================================================= -std::pair>, float> TriangulatorInternal::triangulate_and_score( +std::pair>, float> triangulate_and_score( const std::vector> &pose1, const std::vector> &pose2, const CachedCamera &cam1, @@ -1424,7 +1396,7 @@ std::pair>, float> TriangulatorInternal::triang // ================================================================================================= std::vector, std::vector>, std::vector>> -TriangulatorInternal::calc_grouping( +calc_grouping( const std::vector, std::pair>> &all_pairs, const std::vector>, float>> &all_scored_poses, float min_score) @@ -1692,9 +1664,10 @@ TriangulatorInternal::calc_grouping( // ================================================================================================= -std::vector> TriangulatorInternal::merge_group( +std::vector> merge_group( const std::vector>> &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> 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> best_k_mask(num_poses, std::vector(num_joints, false)); for (size_t j = 0; j < num_joints; ++j) { @@ -1869,7 +1842,7 @@ std::vector> TriangulatorInternal::merge_group( // ================================================================================================= -void TriangulatorInternal::add_extra_joints( +void add_extra_joints( std::vector>> &poses, const std::vector &joint_names) { @@ -1900,7 +1873,7 @@ void TriangulatorInternal::add_extra_joints( // ================================================================================================= -void TriangulatorInternal::filter_poses( +void filter_poses( std::vector>> &poses, const std::array, 2> &roomparams, const std::vector &core_joint_idx, @@ -2091,9 +2064,10 @@ void TriangulatorInternal::filter_poses( // ================================================================================================= -void TriangulatorInternal::add_missing_joints( +void add_missing_joints( std::vector>> &poses, - const std::vector &joint_names) + const std::vector &joint_names, + float min_match_score) { // Map joint names to their indices for quick lookup std::unordered_map joint_name_to_idx; @@ -2229,9 +2203,10 @@ void TriangulatorInternal::add_missing_joints( // ================================================================================================= -void TriangulatorInternal::replace_far_joints( +void replace_far_joints( std::vector>> &poses, - const std::vector &joint_names) + const std::vector &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 &cameras, const std::vector &joint_names, const PoseBatch3DView &previous_poses_3d, - float min_match_score) + const TriangulationOptions &options) { - TriangulatorInternal triangulator(min_match_score, 1); - return triangulator.filter_pairs_with_previous_poses(poses_2d, cameras, joint_names, previous_poses_3d); + return filter_pairs_with_previous_poses_impl(poses_2d, cameras, joint_names, previous_poses_3d, options); } TriangulationTrace triangulate_debug( @@ -2456,11 +2430,9 @@ TriangulationTrace triangulate_debug( const std::array, 2> &roomparams, const std::vector &joint_names, const PoseBatch3DView *previous_poses_3d, - float min_match_score, - size_t min_group_size) + const TriangulationOptions &options) { - TriangulatorInternal triangulator(min_match_score, min_group_size); - return triangulator.triangulate_debug(poses_2d, cameras, roomparams, joint_names, previous_poses_3d); + return triangulate_debug_impl(poses_2d, cameras, roomparams, joint_names, previous_poses_3d, options); } PoseBatch3D triangulate_poses( @@ -2469,8 +2441,7 @@ PoseBatch3D triangulate_poses( const std::array, 2> &roomparams, const std::vector &joint_names, const PoseBatch3DView *previous_poses_3d, - float min_match_score, - size_t min_group_size) + const TriangulationOptions &options) { return triangulate_debug( poses_2d, @@ -2478,7 +2449,6 @@ PoseBatch3D triangulate_poses( roomparams, joint_names, previous_poses_3d, - min_match_score, - min_group_size) + options) .final_poses; } diff --git a/src/rpt/__init__.py b/src/rpt/__init__.py index e8be5ed..95a4587 100644 --- a/src/rpt/__init__.py +++ b/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",