From 347a6a12e07f4efc43b92cee09f0a87bbaeee205 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=BB=84JY?= <1178065793@qq.com> Date: Fri, 10 Apr 2026 14:37:41 +0800 Subject: [PATCH] add yolo + mot cpp into driver --- CMakeLists.txt | 114 ++++++- config/control_command.yaml | 7 + include/cloud_reprojection_processing.hpp | 26 ++ include/cloud_reprojection_ros_node.hpp | 20 ++ include/cloud_reprojector.hpp | 6 + include/target_observation_processing.hpp | 104 ++++++ src/cloud_reprojection_processing.cpp | 102 ++++++ src/cloud_reprojection_ros.cpp | 398 +++++++++++++++++----- src/cloud_reprojector.cpp | 41 +++ 9 files changed, 721 insertions(+), 97 deletions(-) create mode 100644 include/cloud_reprojection_processing.hpp create mode 100644 include/target_observation_processing.hpp create mode 100644 src/cloud_reprojection_processing.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 16cfec0..05e010a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 "=======================================") - diff --git a/config/control_command.yaml b/config/control_command.yaml index 03d498c..924d4a1 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -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. diff --git a/include/cloud_reprojection_processing.hpp b/include/cloud_reprojection_processing.hpp new file mode 100644 index 0000000..3295eac --- /dev/null +++ b/include/cloud_reprojection_processing.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#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& 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 diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index dbb0863..eeac4c2 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -15,10 +15,13 @@ limitations under the License. #ifdef ROS2 #include + #include #include #include #include #include + #include + #include #include #include #include @@ -42,6 +45,10 @@ limitations under the License. #include #include +#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::SharedPtr sync_cloud_pub_; rclcpp::Publisher::SharedPtr sync_cloud_slam_pub_; @@ -89,8 +100,17 @@ private: rclcpp::Publisher::SharedPtr overlay_compressed_pub_; // optional if send_overlay_ rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ + rclcpp::Publisher::SharedPtr target_track_id_pub_; + rclcpp::Publisher::SharedPtr target_observation_pub_; + rclcpp::Publisher::SharedPtr target_pos_cam_pub_; + rclcpp::Publisher::SharedPtr target_pos_world_pub_; std::unique_ptr reprojector_; +#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION + std::unique_ptr target_observation_processor_; + bool enable_target_observation_ = false; + bool debug_target_observation_ = false; +#endif void loadParameters(); void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg, diff --git a/include/cloud_reprojector.hpp b/include/cloud_reprojector.hpp index 463e799..72dab4c 100644 --- a/include/cloud_reprojector.hpp +++ b/include/cloud_reprojector.hpp @@ -65,6 +65,12 @@ public: cv::Mat reprojectCloudDepth(const pcl::PointCloud& 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_; } diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp new file mode 100644 index 0000000..2d713d5 --- /dev/null +++ b/include/target_observation_processing.hpp @@ -0,0 +1,104 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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 bbox_xyxy{0.0f, 0.0f, 0.0f, 0.0f}; + std::array 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& 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& poses) const; + + TargetObservation select_target( + const std::vector& 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& poses, + const pcl::PointCloud& cloud_in_cam, + const CloudReprojector& reprojector, + const CloudReprojector::OdomPose& odom_pose, + TargetObservationDebugInfo* debug_info) const; + + TargetObservationConfig config_; + std::unique_ptr yolo_; + std::unique_ptr 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 diff --git a/src/cloud_reprojection_processing.cpp b/src/cloud_reprojection_processing.cpp new file mode 100644 index 0000000..e19cc4c --- /dev/null +++ b/src/cloud_reprojection_processing.cpp @@ -0,0 +1,102 @@ +#include "cloud_reprojection_processing.hpp" + +#include +#include +#include + +#include +#include +#include + +#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(target_h) / static_cast(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(len), CV_8UC1, const_cast(data)); + return cv::imdecode(raw, cv::IMREAD_COLOR); +} + +cv::Mat overlay_projected_cloud_on_image( + const cv::Mat& camera_bgr, + const pcl::PointCloud& 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 pixels; + std::vector 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(std::round(uv.x())); + const int v = static_cast(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(pt.z)); + } + + if (pixels.empty()) { + return overlay; + } + + cv::Mat depth_values(static_cast(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(i, 0); + cv::circle(overlay, pixels[static_cast(i)], point_radius, + cv::Scalar(color[0], color[1], color[2]), -1); + } + + return overlay; +} + +} // namespace odin_ros_driver diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index 83f0f18..49c7ed4 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -12,9 +12,14 @@ limitations under the License. */ #include "cloud_reprojection_ros_node.hpp" +#include "cloud_reprojection_processing.hpp" #include +#include +#include #include +#include +#include #include #include #include @@ -23,95 +28,58 @@ limitations under the License. #ifdef ROS2 #include +#include +#include +#include #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(target_h) / static_cast(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(len), CV_8UC1, const_cast(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& 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& 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 pixels; - std::vector 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& 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(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(std::round(uv.x())); - const int v = static_cast(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(pt.z)); + first = false; + oss << i << ":[" << keypoints_xyc[static_cast(i) * 3 + 0] + << ", " << keypoints_xyc[static_cast(i) * 3 + 1] + << ", " << confidence << "]"; } - - if (pixels.empty()) { - return overlay; + if (first) { + return "none"; } - - cv::Mat depth_values(static_cast(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(i, 0); - cv::circle(overlay, pixels[static_cast(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(combined_compressed_topic_, 10); } +#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION + if (enable_target_observation_) { + target_track_id_pub_ = this->create_publisher( + sync_target_track_id_topic_, 10); + target_observation_pub_ = this->create_publisher( + sync_target_observation_topic_, 10); + target_pos_cam_pub_ = this->create_publisher( + sync_target_pos_cam_topic_, 10); + target_pos_world_pub_ = this->create_publisher( + 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("cloud_slam_topic", "/odin1/cloud_slam"); this->declare_parameter("odometry_topic", "/odin1/odometry"); @@ -207,6 +204,17 @@ void CloudReprojectionRosNode::loadParameters() this->declare_parameter("register_keys.send_combined_compressed", 1); this->declare_parameter("register_keys.send_overlay", 1); this->declare_parameter("register_keys.overlay_jpeg_quality", 85); +#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION + this->declare_parameter("register_keys.process_target_observation", 1); + this->declare_parameter("register_keys.debug", 1); + this->declare_parameter("register_keys.target_yolo_engine", default_yolo_engine); + this->declare_parameter("register_keys.target_yolo_labels", default_yolo_labels); + this->declare_parameter("register_keys.target_yolo_conf", 0.45); + this->declare_parameter("register_keys.target_yolo_nms", 0.50); + this->declare_parameter("register_keys.target_min_depth", 0.5); + this->declare_parameter("register_keys.target_max_depth", 12.0); + this->declare_parameter("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( + this->get_parameter("register_keys.target_yolo_conf").as_double()); + target_config.yolo_nms = static_cast( + this->get_parameter("register_keys.target_yolo_nms").as_double()); + target_config.min_depth = static_cast( + this->get_parameter("register_keys.target_min_depth").as_double()); + target_config.max_depth = static_cast( + this->get_parameter("register_keys.target_max_depth").as_double()); + target_config.search_radius_px = static_cast( + this->get_parameter("register_keys.target_search_radius_px").as_double()); + target_config.debug = debug_target_observation_; + target_observation_processor_ = + std::make_unique(); + 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() ? "" : 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() ? "" : 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(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 obuf; const std::vector 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 obuf; const std::vector 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); diff --git a/src/cloud_reprojector.cpp b/src/cloud_reprojector.cpp index 22e029b..b55cd4e 100644 --- a/src/cloud_reprojector.cpp +++ b/src/cloud_reprojector.cpp @@ -110,6 +110,47 @@ cv::Mat CloudReprojector::reprojectCloudDepth(const pcl::PointCloudworld2cam(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& cloud_in_cam) const { cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3);