From 62d2ce8cc611b39be1fb08470b2ade115d7618e0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E9=BB=84JY?= <1178065793@qq.com>
Date: Fri, 10 Apr 2026 14:59:19 +0800
Subject: [PATCH] visualization is better now

---
 include/target_observation_processing.hpp |  6 ++++++
 src/cloud_reprojection_ros.cpp            | 14 +++++++++++++-
 src/target_observation_processing.cpp     | 21 ++++++++++++++++++++-
 3 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp
index 2d713d5..b79672f 100644
--- a/include/target_observation_processing.hpp
+++ b/include/target_observation_processing.hpp
@@ -54,6 +54,8 @@ struct TargetObservationDebugInfo {
     float mot_ms = 0.0f;
     float depth_ms = 0.0f;
     float total_ms = 0.0f;
+    std::vector<yolos::pose::PoseResult> poses;
+    std::vector<cv::Point> valid_projected_pixels;
 };
 
 class TargetObservationProcessor {
@@ -71,6 +73,10 @@ public:
 
     bool initialized() const { return initialized_; }
 
+    void draw_detected_poses(
+        cv::Mat& image_bgr,
+        const std::vector<yolos::pose::PoseResult>& poses) const;
+
 private:
     Eigen::MatrixXf format_detections(
         const std::vector<yolos::pose::PoseResult>& poses) const;
diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp
index 49c7ed4..f08ac63 100644
--- a/src/cloud_reprojection_ros.cpp
+++ b/src/cloud_reprojection_ros.cpp
@@ -209,7 +209,7 @@ void CloudReprojectionRosNode::loadParameters()
     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_conf", 0.5);
     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);
@@ -621,6 +621,18 @@ void CloudReprojectionRosNode::syncCallback(
         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 (debug_target_observation_ && target_observation_processor_) {
+            target_observation_processor_->draw_detected_poses(
+                overlay_vis,
+                target_debug.poses);
+            for (const auto& pixel : target_debug.valid_projected_pixels) {
+                if (pixel.x < 0 || pixel.x >= overlay_vis.cols ||
+                    pixel.y < 0 || pixel.y >= overlay_vis.rows) {
+                    continue;
+                }
+                cv::circle(overlay_vis, pixel, 3, cv::Scalar(0, 255, 0), -1);
+            }
+        }
         if (target_observation.valid && debug_target_observation_) {
             odin_ros_driver::draw_target_observation_overlay(
                 overlay_vis,
diff --git a/src/target_observation_processing.cpp b/src/target_observation_processing.cpp
index a17c82f..ba2ec32 100644
--- a/src/target_observation_processing.cpp
+++ b/src/target_observation_processing.cpp
@@ -87,6 +87,7 @@ TargetObservation TargetObservationProcessor::process(
     const auto yolo_end = std::chrono::steady_clock::now();
     if (debug_info) {
         debug_info->poses_count = static_cast<int>(poses.size());
+        debug_info->poses = poses;
         debug_info->yolo_ms =
             std::chrono::duration<float, std::milli>(yolo_end - yolo_start).count();
     }
@@ -143,6 +144,16 @@ TargetObservation TargetObservationProcessor::process(
     return target;
 }
 
+void TargetObservationProcessor::draw_detected_poses(
+    cv::Mat& image_bgr,
+    const std::vector<yolos::pose::PoseResult>& poses) const
+{
+    if (!initialized_ || image_bgr.empty() || poses.empty()) {
+        return;
+    }
+    yolo_->drawPoses(image_bgr, poses);
+}
+
 Eigen::MatrixXf TargetObservationProcessor::format_detections(
     const std::vector<yolos::pose::PoseResult>& poses) const
 {
@@ -290,6 +301,8 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
 
     std::vector<float> per_keypoint_depths;
     per_keypoint_depths.reserve(kHipKeypointIndices.size());
+    std::vector<cv::Point> valid_projected_pixels;
+    valid_projected_pixels.reserve(128);
 
     for (const int kp_index : kHipKeypointIndices) {
         if (kp_index >= static_cast<int>(pose.keypoints.size())) {
@@ -307,6 +320,11 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
             const float dv = uv_points[i].y - kp.y;
             if (std::hypot(du, dv) < config_.search_radius_px) {
                 nearby_depths.push_back(cam_points[i].z());
+                if (debug_info) {
+                    valid_projected_pixels.emplace_back(
+                        static_cast<int>(std::lround(uv_points[i].x)),
+                        static_cast<int>(std::lround(uv_points[i].y)));
+                }
             }
         }
         if (nearby_depths.size() < 3) {
@@ -316,6 +334,7 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
     }
     if (debug_info) {
         debug_info->depth_sample_count = static_cast<int>(per_keypoint_depths.size());
+        debug_info->valid_projected_pixels = std::move(valid_projected_pixels);
     }
 
     if (per_keypoint_depths.empty()) {
@@ -422,7 +441,7 @@ void draw_target_observation_overlay(
         }
         const int u = static_cast<int>(std::lround(observation.keypoints_xyc[i * 3 + 0]));
         const int v = static_cast<int>(std::lround(observation.keypoints_xyc[i * 3 + 1]));
-        cv::circle(image_bgr, cv::Point(u, v), 4, cv::Scalar(0, 255, 0), -1);
+        cv::circle(image_bgr, cv::Point(u, v), 3, cv::Scalar(0, 255, 0), -1);
     }
 
     const std::string label = format_target_text(observation);