wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add yolo + mot cpp into driver

Browse Source
This commit is contained in:
黄JY
2026-04-10 14:37:41 +08:00
parent 3659ffd110
commit 347a6a12e0
9 changed files with 721 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+106 -8
View File
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 3.5)
project(odin_ros_driver)
option(ODIN_DRIVER_ENABLE_TARGET_OBSERVATION "Enable YOLO+ByteTrack target observation in odin_ros_driver" ON)
if(DEFINED BUILD_SYSTEM)
set(ROS_VERSION ${BUILD_SYSTEM})
@@ -99,6 +101,60 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Set optimization flags
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2")
set(TARGET_PREDICTION_DEPLOY_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../../TargetPrediction/deploy")
set(ODIN_TARGET_OBSERVATION_AVAILABLE OFF)
set(ODIN_TARGET_OBSERVATION_READY OFF)
if(ODIN_DRIVER_ENABLE_TARGET_OBSERVATION AND
EXISTS "${TARGET_PREDICTION_DEPLOY_DIR}/YOLOs-CPP-TensorRT/include/yolos/tasks/pose.hpp" AND
EXISTS "${TARGET_PREDICTION_DEPLOY_DIR}/motcpp/CMakeLists.txt")
enable_language(CUDA)
find_package(CUDAToolkit REQUIRED)
find_path(TENSORRT_INCLUDE_DIR NvInfer.h
HINTS
${TENSORRT_DIR}/include
$ENV{TENSORRT_DIR}/include
/home/hjy/Library/TensorRT-10.3.0.26/include
/usr/include
/usr/include/x86_64-linux-gnu
/usr/include/aarch64-linux-gnu
/usr/local/include
/usr/local/TensorRT/include
)
find_library(NVINFER_LIB nvinfer
HINTS
${TENSORRT_DIR}/lib
$ENV{TENSORRT_DIR}/lib
/home/hjy/Library/TensorRT-10.3.0.26/lib
/usr/lib
/usr/lib/x86_64-linux-gnu
/usr/lib/aarch64-linux-gnu
/usr/local/lib
/usr/local/TensorRT/lib
)
if(TENSORRT_INCLUDE_DIR AND NVINFER_LIB)
set(MOTCPP_BUILD_TESTS OFF CACHE BOOL "" FORCE)
set(MOTCPP_BUILD_BENCHMARKS OFF CACHE BOOL "" FORCE)
set(MOTCPP_BUILD_EXAMPLES OFF CACHE BOOL "" FORCE)
set(MOTCPP_BUILD_TOOLS OFF CACHE BOOL "" FORCE)
set(MOTCPP_ENABLE_ONNX OFF CACHE BOOL "" FORCE)
set(MOTCPP_INSTALL OFF CACHE BOOL "" FORCE)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
add_subdirectory(
${TARGET_PREDICTION_DEPLOY_DIR}/motcpp
${CMAKE_CURRENT_BINARY_DIR}/motcpp
EXCLUDE_FROM_ALL
)
set(ODIN_TARGET_OBSERVATION_READY ON)
else()
message(WARNING "TensorRT not found. Target observation support disabled.")
endif()
elseif(ODIN_DRIVER_ENABLE_TARGET_OBSERVATION)
message(WARNING "TargetPrediction deploy dependencies not found. Target observation support disabled.")
endif()
# Find common dependencies
find_package(PkgConfig REQUIRED)
find_package(OpenCV REQUIRED)
@@ -132,6 +188,33 @@ set(COMMON_LIBS
${LYD_HOST_API_LIB}
)
if(ODIN_TARGET_OBSERVATION_READY)
add_library(target_observation_processing STATIC
src/target_observation_processing.cpp
${TARGET_PREDICTION_DEPLOY_DIR}/YOLOs-CPP-TensorRT/include/yolos/core/cuda_preprocessing.cu
)
target_include_directories(target_observation_processing PUBLIC
include
${TARGET_PREDICTION_DEPLOY_DIR}/YOLOs-CPP-TensorRT/include
${TENSORRT_INCLUDE_DIR}
)
target_link_libraries(target_observation_processing
Eigen3::Eigen
${OpenCV_LIBS}
${PCL_LIBRARIES}
motcpp::motcpp
${NVINFER_LIB}
CUDA::cudart
)
target_compile_definitions(target_observation_processing PUBLIC ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
set_target_properties(target_observation_processing PROPERTIES
CUDA_SEPARABLE_COMPILATION ON
POSITION_INDEPENDENT_CODE ON
)
set(ODIN_TARGET_OBSERVATION_AVAILABLE ON)
message(STATUS "Target observation support enabled in odin_ros_driver")
endif()
# ===== ROS1 Configuration =====
if(ROS_VERSION STREQUAL "ROS1")
message(STATUS "Configuring for ROS1 build")
@@ -201,13 +284,20 @@ if(ROS_VERSION STREQUAL "ROS1")
${PCL_LIBRARIES}
)
add_executable(cloud_reprojection_node src/cloud_reprojection_ros.cpp)
target_link_libraries(cloud_reprojection_node
cloud_reprojector
${catkin_LIBRARIES}
${OpenCV_LIBS}
${PCL_LIBRARIES}
add_executable(cloud_reprojection_node
src/cloud_reprojection_ros.cpp
src/cloud_reprojection_processing.cpp
)
target_link_libraries(cloud_reprojection_node
cloud_reprojector
${catkin_LIBRARIES}
${OpenCV_LIBS}
${PCL_LIBRARIES}
)
if(ODIN_TARGET_OBSERVATION_AVAILABLE)
target_link_libraries(cloud_reprojection_node target_observation_processing)
target_compile_definitions(cloud_reprojection_node PRIVATE ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
endif()
add_executable(image_overlay_node src/image_overlay_node.cpp)
target_link_libraries(image_overlay_node
@@ -328,7 +418,10 @@ elseif(ROS_VERSION STREQUAL "ROS2")
${PCL_LIBRARIES}
)
add_executable(cloud_reprojection_ros2_node src/cloud_reprojection_ros.cpp)
add_executable(cloud_reprojection_ros2_node
src/cloud_reprojection_ros.cpp
src/cloud_reprojection_processing.cpp
)
target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
target_link_libraries(cloud_reprojection_ros2_node
cloud_reprojector_ros2
@@ -336,10 +429,16 @@ elseif(ROS_VERSION STREQUAL "ROS2")
${PCL_LIBRARIES}
yaml-cpp
)
if(ODIN_TARGET_OBSERVATION_AVAILABLE)
target_link_libraries(cloud_reprojection_ros2_node target_observation_processing)
target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
endif()
ament_target_dependencies(cloud_reprojection_ros2_node
rclcpp
sensor_msgs
nav_msgs
geometry_msgs
std_msgs
cv_bridge
pcl_conversions
message_filters
@@ -443,4 +542,3 @@ message(STATUS "Target platform: ${TARGET_PLATFORM}")
message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
message(STATUS "=======================================")
config/control_command.yaml
+7
View File
@@ -83,6 +83,13 @@ register_keys:
send_overlay: 1
overlay_jpeg_quality: 85
# target observation processing on synced image + cloud_in_cam
# 0: off; 1: on
process_target_observation: 1
# draw target observation result on overlay image and print debug info
# 0: off; 1: on
debug: 1
# record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software.
# save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
# ATTENTION: please copy the full folder for post-processing.
include/cloud_reprojection_processing.hpp
+26
View File
@@ -0,0 +1,26 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <opencv2/core.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "cloud_reprojector.hpp"
namespace odin_ros_driver {
cv::Mat resize_to_height(const cv::Mat& src, int target_h);
cv::Mat decode_compressed_to_bgr(const uint8_t* data, size_t len);
cv::Mat overlay_projected_cloud_on_image(
const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector::CameraParams& cam_params,
int point_radius = 2,
float min_depth = 0.5f,
float max_depth = 30.0f);
} // namespace odin_ros_driver
include/cloud_reprojection_ros_node.hpp
+20
View File
@@ -15,10 +15,13 @@ limitations under the License.
#ifdef ROS2
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
@@ -42,6 +45,10 @@ limitations under the License.
#include <string>
#include <memory>
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
#include "target_observation_processing.hpp"
#endif
#ifdef ROS2
class CloudReprojectionRosNode : public rclcpp::Node
{
@@ -80,6 +87,10 @@ private:
std::string sync_wiwc_topic_;
std::string sync_image_topic_;
std::string sync_overlay_image_topic_;
std::string sync_target_track_id_topic_;
std::string sync_target_observation_topic_;
std::string sync_target_pos_cam_topic_;
std::string sync_target_pos_world_topic_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_pub_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_slam_pub_;
@@ -89,8 +100,17 @@ private:
rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_; // optional if send_overlay_
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_; // optional if publish_combined_compressed_
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr target_track_id_pub_;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr target_observation_pub_;
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_cam_pub_;
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_world_pub_;
std::unique_ptr<CloudReprojector> reprojector_;
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
std::unique_ptr<odin_ros_driver::TargetObservationProcessor> target_observation_processor_;
bool enable_target_observation_ = false;
bool debug_target_observation_ = false;
#endif
void loadParameters();
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
include/cloud_reprojector.hpp
+6
View File
@@ -65,6 +65,12 @@ public:
cv::Mat reprojectCloudDepth(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
const OdomPose& odom_pose);
Eigen::Vector2d projectCameraPointToPixel(const Eigen::Vector3d& point_cam) const;
Eigen::Vector3d pixelToCameraRay(double u, double v) const;
Eigen::Vector3d pixelToCameraPoint(double u, double v, double depth_z) const;
Eigen::Vector3d cameraPointToWorld(const Eigen::Vector3d& point_cam,
const OdomPose& odom_pose) const;
void setPointRadius(int radius) { point_radius_ = radius; }
int getPointRadius() const { return point_radius_; }
include/target_observation_processing.hpp
+104
View File
@@ -0,0 +1,104 @@
#pragma once
#include <array>
#include <memory>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "cloud_reprojector.hpp"
#include "motcpp/trackers/bytetrack.hpp"
#include "yolos/tasks/pose.hpp"
namespace odin_ros_driver {
struct TargetObservationConfig {
std::string yolo_engine_path;
std::string yolo_labels_path;
float yolo_conf = 0.45f;
float yolo_nms = 0.50f;
float min_depth = 0.5f;
float max_depth = 12.0f;
float search_radius_px = 25.0f;
bool debug = false;
};
struct TargetObservation {
bool valid = false;
int track_id = -1;
int detection_index = -1;
float confidence = 0.0f;
float depth = -1.0f;
float depth_confidence = 0.0f;
std::array<float, 4> bbox_xyxy{0.0f, 0.0f, 0.0f, 0.0f};
std::array<float, 17 * 3> keypoints_xyc{};
Eigen::Vector3f target_pos_cam = Eigen::Vector3f::Zero();
Eigen::Vector3f target_pos_world = Eigen::Vector3f::Zero();
};
struct TargetObservationDebugInfo {
int current_target_id_before = -1;
int poses_count = 0;
int tracks_count = 0;
int selected_track_id = -1;
int detection_index = -1;
int projected_cloud_points = 0;
int depth_sample_count = 0;
bool found_existing_target = false;
bool target_selected = false;
float yolo_ms = 0.0f;
float mot_ms = 0.0f;
float depth_ms = 0.0f;
float total_ms = 0.0f;
};
class TargetObservationProcessor {
public:
TargetObservationProcessor() = default;
void initialize(const TargetObservationConfig& config);
TargetObservation process(
const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector& reprojector,
const CloudReprojector::OdomPose& odom_pose,
TargetObservationDebugInfo* debug_info = nullptr);
bool initialized() const { return initialized_; }
private:
Eigen::MatrixXf format_detections(
const std::vector<yolos::pose::PoseResult>& poses) const;
TargetObservation select_target(
const std::vector<yolos::pose::PoseResult>& poses,
const Eigen::MatrixXf& tracks,
int image_width,
int image_height,
TargetObservationDebugInfo* debug_info);
TargetObservation enrich_target_with_cloud(
const TargetObservation& target,
const std::vector<yolos::pose::PoseResult>& poses,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector& reprojector,
const CloudReprojector::OdomPose& odom_pose,
TargetObservationDebugInfo* debug_info) const;
TargetObservationConfig config_;
std::unique_ptr<yolos::pose::YOLOPoseDetector> yolo_;
std::unique_ptr<motcpp::trackers::ByteTrack> tracker_;
int current_target_id_ = -1;
bool initialized_ = false;
};
void draw_target_observation_overlay(
cv::Mat& image_bgr,
const TargetObservation& observation);
} // namespace odin_ros_driver
src/cloud_reprojection_processing.cpp
+102
View File
@@ -0,0 +1,102 @@
#include "cloud_reprojection_processing.hpp"
#include <algorithm>
#include <cmath>
#include <vector>
#include <Eigen/Core>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include "polynomial_camera.hpp"
namespace odin_ros_driver {
cv::Mat resize_to_height(const cv::Mat& src, int target_h)
{
if (src.empty() || target_h <= 0) {
return src.clone();
}
if (src.rows == target_h) {
return src.clone();
}
const double scale = static_cast<double>(target_h) / static_cast<double>(src.rows);
cv::Mat out;
cv::resize(src, out, cv::Size(), scale, scale, cv::INTER_LINEAR);
return out;
}
cv::Mat decode_compressed_to_bgr(const uint8_t* data, size_t len)
{
if (!data || len == 0) {
return cv::Mat();
}
cv::Mat raw(1, static_cast<int>(len), CV_8UC1, const_cast<uint8_t*>(data));
return cv::imdecode(raw, cv::IMREAD_COLOR);
}
cv::Mat overlay_projected_cloud_on_image(
const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector::CameraParams& cam_params,
int point_radius,
float min_depth,
float max_depth)
{
if (camera_bgr.empty()) {
return cv::Mat();
}
cv::Mat overlay = camera_bgr.clone();
mini_vikit::PolynomialCamera camera_model(
cam_params.image_width, cam_params.image_height,
cam_params.A11, cam_params.A22,
cam_params.u0, cam_params.v0,
cam_params.A12,
cam_params.k2, cam_params.k3, cam_params.k4,
cam_params.k5, cam_params.k6, cam_params.k7);
std::vector<cv::Point> pixels;
std::vector<float> depths;
pixels.reserve(cloud_in_cam.size());
depths.reserve(cloud_in_cam.size());
for (const auto& pt : cloud_in_cam) {
if (pt.z <= min_depth || pt.z >= max_depth) {
continue;
}
const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z));
const int u = static_cast<int>(std::round(uv.x()));
const int v = static_cast<int>(std::round(uv.y()));
if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) {
continue;
}
pixels.emplace_back(u, v);
depths.emplace_back(static_cast<float>(pt.z));
}
if (pixels.empty()) {
return overlay;
}
cv::Mat depth_values(static_cast<int>(depths.size()), 1, CV_32F, depths.data());
cv::Mat clipped;
cv::Mat norm_u8;
cv::Mat colors;
cv::min(cv::max(depth_values, min_depth), max_depth, clipped);
clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f));
clipped.convertTo(norm_u8, CV_8U);
cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET);
for (int i = 0; i < colors.rows; ++i) {
const cv::Vec3b color = colors.at<cv::Vec3b>(i, 0);
cv::circle(overlay, pixels[static_cast<size_t>(i)], point_radius,
cv::Scalar(color[0], color[1], color[2]), -1);
}
return overlay;
}
} // namespace odin_ros_driver
src/cloud_reprojection_ros.cpp
+309 -89
View File
@@ -12,9 +12,14 @@ limitations under the License.
*/
#include "cloud_reprojection_ros_node.hpp"
#include "cloud_reprojection_processing.hpp"
#include <algorithm>
#include <chrono>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <sys/stat.h>
#include <thread>
#include <chrono>
@@ -23,95 +28,58 @@ limitations under the License.
#ifdef ROS2
#include <sensor_msgs/msg/compressed_image.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#endif
namespace {
cv::Mat resizeToHeight(const cv::Mat& src, int target_h)
std::filesystem::path get_package_source_directory_from_file()
{
if (src.empty() || target_h <= 0) {
return src.clone();
}
if (src.rows == target_h) {
return src.clone();
}
const double scale = static_cast<double>(target_h) / static_cast<double>(src.rows);
cv::Mat out;
cv::resize(src, out, cv::Size(), scale, scale, cv::INTER_LINEAR);
return out;
return std::filesystem::path(__FILE__).parent_path().parent_path();
}
/** Decode JPEG/PNG payload from sensor_msgs/CompressedImage to BGR. */
cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len)
std::string format_vector3f(const Eigen::Vector3f& value)
{
if (!data || len == 0) {
return cv::Mat();
}
cv::Mat raw(1, static_cast<int>(len), CV_8UC1, const_cast<uint8_t*>(data));
return cv::imdecode(raw, cv::IMREAD_COLOR);
std::ostringstream oss;
oss << std::fixed << std::setprecision(2)
<< "[" << value.x() << ", " << value.y() << ", " << value.z() << "]";
return oss.str();
}
cv::Mat overlayProjectedCloudOnImage(const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector::CameraParams& cam_params,
int point_radius = 2,
float min_depth = 0.5f,
float max_depth = 30.0f)
std::string format_bbox_xyxy(const std::array<float, 4>& bbox)
{
if (camera_bgr.empty()) {
return cv::Mat();
}
std::ostringstream oss;
oss << std::fixed << std::setprecision(1)
<< "[" << bbox[0] << ", " << bbox[1] << ", " << bbox[2] << ", " << bbox[3] << "]";
return oss.str();
}
cv::Mat overlay = camera_bgr.clone();
mini_vikit::PolynomialCamera camera_model(
cam_params.image_width, cam_params.image_height,
cam_params.A11, cam_params.A22,
cam_params.u0, cam_params.v0,
cam_params.A12,
cam_params.k2, cam_params.k3, cam_params.k4,
cam_params.k5, cam_params.k6, cam_params.k7);
std::vector<cv::Point> pixels;
std::vector<float> depths;
pixels.reserve(cloud_in_cam.size());
depths.reserve(cloud_in_cam.size());
for (const auto& pt : cloud_in_cam) {
if (pt.z <= min_depth || pt.z >= max_depth) {
std::string format_keypoints_xyc(
const std::array<float, 17 * 3>& keypoints_xyc,
float min_confidence = 0.5f)
{
std::ostringstream oss;
oss << std::fixed << std::setprecision(2);
bool first = true;
for (int i = 0; i < 17; ++i) {
const float confidence = keypoints_xyc[static_cast<size_t>(i) * 3 + 2];
if (confidence < min_confidence) {
continue;
}
const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z));
const int u = static_cast<int>(std::round(uv.x()));
const int v = static_cast<int>(std::round(uv.y()));
if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) {
continue;
if (!first) {
oss << " ";
}
pixels.emplace_back(u, v);
depths.emplace_back(static_cast<float>(pt.z));
first = false;
oss << i << ":[" << keypoints_xyc[static_cast<size_t>(i) * 3 + 0]
<< ", " << keypoints_xyc[static_cast<size_t>(i) * 3 + 1]
<< ", " << confidence << "]";
}
if (pixels.empty()) {
return overlay;
if (first) {
return "none";
}
cv::Mat depth_values(static_cast<int>(depths.size()), 1, CV_32F, depths.data());
cv::Mat clipped, norm_u8, colors;
cv::min(cv::max(depth_values, min_depth), max_depth, clipped);
clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f));
clipped.convertTo(norm_u8, CV_8U);
cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET);
for (int i = 0; i < colors.rows; ++i) {
const cv::Vec3b color = colors.at<cv::Vec3b>(i, 0);
cv::circle(overlay, pixels[static_cast<size_t>(i)], point_radius,
cv::Scalar(color[0], color[1], color[2]), -1);
}
return overlay;
return oss.str();
}
} // namespace
#ifdef ROS2
@@ -139,14 +107,8 @@ static bool fileExists(const std::string& filename) {
}
#ifdef ROS2
// Helper function to get package source directory for ROS2
static std::string get_package_source_directory() {
std::string current_file = __FILE__;
size_t pos = current_file.find("/src/cloud_reprojection_ros.cpp");
if (pos != std::string::npos) {
return current_file.substr(0, pos);
}
return "";
return get_package_source_directory_from_file().string();
}
// ==================== ROS2 Implementation ====================
@@ -165,7 +127,16 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off")
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
<< "\n process_target_observation: " << (enable_target_observation_ ? "on" : "off")
<< "\n debug: " << (debug_target_observation_ ? "on" : "off")
<< "\n target_track_id_topic: " << sync_target_track_id_topic_
<< "\n target_observation_topic: " << sync_target_observation_topic_
<< "\n target_pos_cam_topic: " << sync_target_pos_cam_topic_
<< "\n target_pos_world_topic: " << sync_target_pos_world_topic_
#endif
);
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
@@ -190,12 +161,38 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
if (publish_combined_compressed_) {
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
}
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
if (enable_target_observation_) {
target_track_id_pub_ = this->create_publisher<std_msgs::msg::Int32>(
sync_target_track_id_topic_, 10);
target_observation_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
sync_target_observation_topic_, 10);
target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
sync_target_pos_cam_topic_, 10);
target_pos_world_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
sync_target_pos_world_topic_, 10);
RCLCPP_INFO(
this->get_logger(),
"Target observation publishers created successfully | track_id=%s | observation=%s | pos_cam=%s | pos_world=%s",
sync_target_track_id_topic_.c_str(),
sync_target_observation_topic_.c_str(),
sync_target_pos_cam_topic_.c_str(),
sync_target_pos_world_topic_.c_str());
}
#endif
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
}
void CloudReprojectionRosNode::loadParameters()
{
const auto package_path = get_package_source_directory_from_file();
const auto workspace_src_path = package_path.parent_path().parent_path().parent_path();
const auto default_yolo_engine =
(workspace_src_path / "TargetPrediction" / "deploy" / "model" / "yolo26s-pose.trt").string();
const auto default_yolo_labels =
(workspace_src_path / "TargetPrediction" / "deploy" / "YOLOs-CPP-TensorRT" / "models" / "coco.names").string();
// Declare and get parameters
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
@@ -207,6 +204,17 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
this->declare_parameter<int>("register_keys.send_overlay", 1);
this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
this->declare_parameter<int>("register_keys.process_target_observation", 1);
this->declare_parameter<int>("register_keys.debug", 1);
this->declare_parameter<std::string>("register_keys.target_yolo_engine", default_yolo_engine);
this->declare_parameter<std::string>("register_keys.target_yolo_labels", default_yolo_labels);
this->declare_parameter<double>("register_keys.target_yolo_conf", 0.45);
this->declare_parameter<double>("register_keys.target_yolo_nms", 0.50);
this->declare_parameter<double>("register_keys.target_min_depth", 0.5);
this->declare_parameter<double>("register_keys.target_max_depth", 12.0);
this->declare_parameter<double>("register_keys.target_search_radius_px", 25.0);
#endif
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
@@ -228,10 +236,13 @@ void CloudReprojectionRosNode::loadParameters()
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
sync_target_track_id_topic_ = sync_topic_prefix_ + "/target_track_id";
sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world";
// Load camera parameters from calib.yaml file directly
std::string package_path = get_package_source_directory();
std::string calib_file = package_path + "/config/calib.yaml";
std::string calib_file = (package_path / "config" / "calib.yaml").string();
YAML::Node calib_config;
try {
@@ -304,6 +315,63 @@ void CloudReprojectionRosNode::loadParameters()
RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
rclcpp::shutdown();
}
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
enable_target_observation_ =
(this->get_parameter("register_keys.process_target_observation").as_int() != 0);
debug_target_observation_ =
(this->get_parameter("register_keys.debug").as_int() != 0);
if (enable_target_observation_) {
try {
odin_ros_driver::TargetObservationConfig target_config;
target_config.yolo_engine_path =
this->get_parameter("register_keys.target_yolo_engine").as_string();
target_config.yolo_labels_path =
this->get_parameter("register_keys.target_yolo_labels").as_string();
target_config.yolo_conf = static_cast<float>(
this->get_parameter("register_keys.target_yolo_conf").as_double());
target_config.yolo_nms = static_cast<float>(
this->get_parameter("register_keys.target_yolo_nms").as_double());
target_config.min_depth = static_cast<float>(
this->get_parameter("register_keys.target_min_depth").as_double());
target_config.max_depth = static_cast<float>(
this->get_parameter("register_keys.target_max_depth").as_double());
target_config.search_radius_px = static_cast<float>(
this->get_parameter("register_keys.target_search_radius_px").as_double());
target_config.debug = debug_target_observation_;
target_observation_processor_ =
std::make_unique<odin_ros_driver::TargetObservationProcessor>();
target_observation_processor_->initialize(target_config);
RCLCPP_INFO(
this->get_logger(),
"Target observation enabled | engine=%s",
target_config.yolo_engine_path.c_str());
RCLCPP_INFO(
this->get_logger(),
"Target observation processor initialized successfully | labels=%s",
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str());
if (debug_target_observation_) {
RCLCPP_INFO(
this->get_logger(),
"Target observation debug enabled | yolo_engine=%s | yolo_labels=%s | yolo_conf=%.3f | yolo_nms=%.3f | min_depth=%.2f | max_depth=%.2f | search_radius_px=%.1f",
target_config.yolo_engine_path.c_str(),
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str(),
target_config.yolo_conf,
target_config.yolo_nms,
target_config.min_depth,
target_config.max_depth,
target_config.search_radius_px);
}
} catch (const std::exception& e) {
enable_target_observation_ = false;
target_observation_processor_.reset();
RCLCPP_ERROR(
this->get_logger(),
"Failed to initialize target observation processor: %s",
e.what());
}
}
#endif
}
void CloudReprojectionRosNode::syncCallback(
@@ -388,7 +456,11 @@ void CloudReprojectionRosNode::syncCallback(
}
cv::Mat cam_bgr;
if (send_overlay_ || need_combined) {
if (send_overlay_ || need_combined
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|| enable_target_observation_
#endif
) {
try {
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
cam_bgr = cv_ptr->image;
@@ -404,9 +476,157 @@ void CloudReprojectionRosNode::syncCallback(
sync_cloud_pub_->publish(cloud_cam_msg);
sync_image_pub_->publish(sync_image_msg);
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
odin_ros_driver::TargetObservation target_observation;
odin_ros_driver::TargetObservationDebugInfo target_debug;
if (enable_target_observation_ && target_observation_processor_) {
const auto target_start = std::chrono::steady_clock::now();
target_observation = target_observation_processor_->process(
cam_bgr,
cloud_cam,
*reprojector_,
odom_pose,
debug_target_observation_ ? &target_debug : nullptr);
const auto target_end = std::chrono::steady_clock::now();
if (debug_target_observation_) {
if (!target_observation.valid) {
if (target_debug.poses_count == 0) {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
2000,
"Target observation | no detections | yolo=%.2f ms | total=%.2f ms",
target_debug.yolo_ms,
target_debug.total_ms);
} else if (target_debug.tracks_count == 0) {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
2000,
"Target observation | detections=%d tracked=0 current_target_id=%d | yolo=%.2f ms | mot=%.2f ms | total=%.2f ms",
target_debug.poses_count,
target_debug.current_target_id_before,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.total_ms);
} else {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
1000,
"Target observation | detections=%d tracked=%d selected_id=%d det_ind=%d cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms",
target_debug.poses_count,
target_debug.tracks_count,
target_debug.selected_track_id,
target_debug.detection_index,
target_debug.projected_cloud_points,
target_debug.depth_sample_count,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.depth_ms,
target_debug.total_ms);
}
} else {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
500,
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d current_target_id=%d selected_id=%d det_ind=%d reused=%s bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms | node_total=%.2f ms",
image_msg->header.stamp.sec,
image_msg->header.stamp.nanosec,
cam_bgr.cols,
cam_bgr.rows,
cloud_cam.size(),
target_debug.poses_count,
target_debug.tracks_count,
target_debug.current_target_id_before,
target_observation.track_id,
target_observation.detection_index,
target_debug.found_existing_target ? "yes" : "no",
format_bbox_xyxy(target_observation.bbox_xyxy).c_str(),
target_observation.depth,
target_observation.confidence,
target_observation.depth_confidence,
format_vector3f(target_observation.target_pos_cam).c_str(),
format_vector3f(target_observation.target_pos_world).c_str(),
target_debug.projected_cloud_points,
target_debug.depth_sample_count,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.depth_ms,
target_debug.total_ms,
std::chrono::duration<double, std::milli>(target_end - target_start).count());
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
500,
"Target observation keypoints: %s",
format_keypoints_xyc(target_observation.keypoints_xyc).c_str());
}
}
if (target_observation.valid) {
std_msgs::msg::Int32 track_id_msg;
track_id_msg.data = target_observation.track_id;
target_track_id_pub_->publish(track_id_msg);
std_msgs::msg::Float32MultiArray observation_msg;
observation_msg.data.reserve(4 + 17 * 3 + 3 + 3 + 3);
observation_msg.data.insert(
observation_msg.data.end(),
target_observation.bbox_xyxy.begin(),
target_observation.bbox_xyxy.end());
observation_msg.data.insert(
observation_msg.data.end(),
target_observation.keypoints_xyc.begin(),
target_observation.keypoints_xyc.end());
observation_msg.data.push_back(target_observation.target_pos_cam.x());
observation_msg.data.push_back(target_observation.target_pos_cam.y());
observation_msg.data.push_back(target_observation.target_pos_cam.z());
observation_msg.data.push_back(target_observation.target_pos_world.x());
observation_msg.data.push_back(target_observation.target_pos_world.y());
observation_msg.data.push_back(target_observation.target_pos_world.z());
observation_msg.data.push_back(target_observation.confidence);
observation_msg.data.push_back(target_observation.depth);
observation_msg.data.push_back(target_observation.depth_confidence);
target_observation_pub_->publish(observation_msg);
geometry_msgs::msg::PointStamped pos_cam_msg;
pos_cam_msg.header = image_msg->header;
pos_cam_msg.header.stamp = sync_stamp;
pos_cam_msg.header.frame_id = cloud_cam_msg.header.frame_id.empty()
? "camera"
: cloud_cam_msg.header.frame_id;
pos_cam_msg.point.x = target_observation.target_pos_cam.x();
pos_cam_msg.point.y = target_observation.target_pos_cam.y();
pos_cam_msg.point.z = target_observation.target_pos_cam.z();
target_pos_cam_pub_->publish(pos_cam_msg);
geometry_msgs::msg::PointStamped pos_world_msg;
pos_world_msg.header = image_msg->header;
pos_world_msg.header.stamp = sync_stamp;
pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty()
? "odom"
: odom_msg->header.frame_id;
pos_world_msg.point.x = target_observation.target_pos_world.x();
pos_world_msg.point.y = target_observation.target_pos_world.y();
pos_world_msg.point.z = target_observation.target_pos_world.z();
target_pos_world_pub_->publish(pos_world_msg);
}
}
#endif
if (send_overlay_ && overlay_compressed_pub_) {
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
cv::Mat overlay_vis = odin_ros_driver::overlay_projected_cloud_on_image(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
if (target_observation.valid && debug_target_observation_) {
odin_ros_driver::draw_target_observation_overlay(
overlay_vis,
target_observation);
}
#endif
if (!overlay_vis.empty()) {
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
@@ -424,8 +644,8 @@ void CloudReprojectionRosNode::syncCallback(
if (combined_pub_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
cv::Mat combined;
cv::hconcat(left, right, combined);
@@ -720,7 +940,7 @@ void CloudReprojectionRosNode::syncCallback(
if (send_overlay_) {
cv::Mat overlay_vis =
overlayProjectedCloudOnImage(cam_bgr, cloud_cam, reprojector_->getCameraParams());
odin_ros_driver::overlay_projected_cloud_on_image(cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (!overlay_vis.empty()) {
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
@@ -738,8 +958,8 @@ void CloudReprojectionRosNode::syncCallback(
if (publish_combined_compressed_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
cv::Mat combined;
cv::hconcat(left, right, combined);
src/cloud_reprojector.cpp
+41
View File
@@ -110,6 +110,47 @@ cv::Mat CloudReprojector::reprojectCloudDepth(const pcl::PointCloud<pcl::PointXY
return projectCloudToImageDepth(cloud_in_cam);
}
Eigen::Vector2d CloudReprojector::projectCameraPointToPixel(const Eigen::Vector3d& point_cam) const
{
if (!initialized_ || !camera_model_) {
return Eigen::Vector2d::Constant(-1.0);
}
return camera_model_->world2cam(point_cam);
}
Eigen::Vector3d CloudReprojector::pixelToCameraRay(double u, double v) const
{
if (!initialized_ || !camera_model_) {
return Eigen::Vector3d::Zero();
}
return camera_model_->cam2world(Eigen::Vector2d(u, v));
}
Eigen::Vector3d CloudReprojector::pixelToCameraPoint(double u, double v, double depth_z) const
{
const Eigen::Vector3d ray = pixelToCameraRay(u, v);
if (depth_z <= 0.0 || std::abs(ray.z()) < 1e-8) {
return Eigen::Vector3d::Zero();
}
return ray * (depth_z / ray.z());
}
Eigen::Vector3d CloudReprojector::cameraPointToWorld(
const Eigen::Vector3d& point_cam,
const OdomPose& odom_pose) const
{
if (!initialized_) {
return Eigen::Vector3d::Zero();
}
Eigen::Vector4d point_cam_h;
point_cam_h << point_cam, 1.0;
const Eigen::Vector4d point_imu_h = extrinsic_params_.Tic * point_cam_h;
const Eigen::Matrix4d T_odom_imu = odomPoseToMatrix(odom_pose);
const Eigen::Vector4d point_world_h = T_odom_imu * point_imu_h;
return point_world_h.head<3>();
}
cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const
{
cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3);