visualization is better now

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;

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,

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);