crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Simplify triangulation core and remove integrations

Browse Source
This commit is contained in:
crosstyan
2026-03-11 23:55:04 +08:00
parent 103aeb5887
commit 7df34b18c3
20 changed files with 222 additions and 1969 deletions
Download Patch File Download Diff File Expand all files Collapse all files
extras/ros/README.md
-22
View File
@@ -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
-70
View File
@@ -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
-46
View File
@@ -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
-63
View File
@@ -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
-24
View File
@@ -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
-122
View File
@@ -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
-255
View File
@@ -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
-335
View File
@@ -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
-64
View File
@@ -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
-24
View File
@@ -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
-323
View File
@@ -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
-33
View File
@@ -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
-7
View File
@@ -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
-23
View File
@@ -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>