add multi-target support (TargetObservation + rviz publish);

现在可以发布包含多个目标的消息了，而且自带多目标管理系统，可以支持发布多目标的info，debug和后续推理都可以

CMakeLists.txt

@@ -356,6 +356,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
 
     rosidl_generate_interfaces(${PROJECT_NAME}
         "msg/TargetObservation.msg"
+        "msg/TargetObservationArray.msg"
         DEPENDENCIES std_msgs geometry_msgs nav_msgs
     )
 
@@ -466,6 +467,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         nav_msgs
         geometry_msgs
         std_msgs
+        visualization_msgs
         cv_bridge
         pcl_conversions
         message_filters

config/odin_ros2.rviz

@@ -13,7 +13,8 @@ Panels:
         - /slam1
         - /dense_depth_demo1
         - /Prediction1
-        - /Prediction1/Path1
+        - /Prediction1/detection_pos_world1
+        - /Prediction1/detection_pos_cam1
         - /Prediction1/Marker1
         - /Planning1
         - /Planning1/GridMap1
@@ -424,35 +425,30 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: /target/pred_image
           Value: false
-        - Alpha: 1
+        - Class: rviz_default_plugins/MarkerArray
-          Class: rviz_default_plugins/PointStamped
+          Enabled: true
-          Color: 224; 27; 36
+          Name: detection_pos_world
-          Enabled: false
+          Namespaces:
-          History Length: 10
+            detection_label: true
-          Name: target_pos_world
+            detection_sphere: true
-          Radius: 0.30000001192092896
           Topic:
             Depth: 5
             Durability Policy: Volatile
-            Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /odin1/sync/target_pos_world
+            Value: /odin1/sync/detection_pos_world
-          Value: false
+          Value: true
-        - Alpha: 1
+        - Class: rviz_default_plugins/MarkerArray
-          Class: rviz_default_plugins/PointStamped
-          Color: 204; 41; 204
           Enabled: false
-          History Length: 1
+          Name: detection_pos_cam
-          Name: target_pos_cam
+          Namespaces:
-          Radius: 0.20000000298023224
+            {}
           Topic:
             Depth: 5
             Durability Policy: Volatile
-            Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /odin1/sync/target_pos_cam
+            Value: /odin1/sync/detection_pos_cam
           Value: false
         - Alpha: 1
           Buffer Length: 1
@@ -542,8 +538,7 @@ Visualization Manager:
           Enabled: true
           Name: path
           Namespaces:
-            future_path: true
+            {}
-            future_yaw: true
           Topic:
             Depth: 5
             Durability Policy: Volatile

include/cloud_reprojection_ros_node.hpp

@@ -22,10 +22,13 @@ limitations under the License.
     #include <nav_msgs/msg/odometry.hpp>
     #include <std_msgs/msg/float32_multi_array.hpp>
     #include <std_msgs/msg/int32.hpp>
+    #include <visualization_msgs/msg/marker.hpp>
+    #include <visualization_msgs/msg/marker_array.hpp>
     #include <cv_bridge/cv_bridge.h>
     #include <message_filters/subscriber.h>
     #include <message_filters/sync_policies/approximate_time.h>
     #include "odin_ros_driver/msg/target_observation.hpp"
+    #include "odin_ros_driver/msg/target_observation_array.hpp"
 #else
     #include <ros/ros.h>
     #include <sensor_msgs/PointCloud2.h>
@@ -96,8 +99,9 @@ private:
     std::string sync_overlay_image_topic_;
     std::string sync_detection_debug_image_topic_;
     std::string sync_target_observation_topic_;
-    std::string sync_target_pos_cam_topic_;
+    std::string sync_track_observations_topic_;
-    std::string sync_target_pos_world_topic_;
+    std::string sync_detection_pos_cam_topic_;
+    std::string sync_detection_pos_world_topic_;
 
     rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_pub_;
     rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_slam_pub_;
@@ -110,8 +114,9 @@ private:
     rclcpp::Publisher<CompressedImage>::SharedPtr detection_debug_compressed_pub_;  // debug detections/tracks
     rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;  // optional if publish_combined_compressed_
     rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
-    rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_cam_pub_;
+    rclcpp::Publisher<odin_ros_driver::msg::TargetObservationArray>::SharedPtr track_observations_pub_;
-    rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_world_pub_;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr detection_pos_cam_pub_;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr detection_pos_world_pub_;
 
     std::unique_ptr<CloudReprojector> reprojector_;
 #ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION

include/target_observation_processing.hpp

@@ -4,6 +4,7 @@
 #include <cstdint>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include <Eigen/Dense>
@@ -42,6 +43,13 @@ struct TargetObservationConfig {
     int reid_input_width = 128;
     int reid_feature_dim = 512;
     int reid_max_batch_size = 8;
+    int max_concurrent_tracks = 16;
+    // 3D position gate for ReID rebind. dmax = base_m + per_frame_m *
+    // frames_since_last_seen. soft mode (hard=false) logs via debug counters
+    // but still allows the similarity match; hard mode rejects the pair.
+    float reid_3d_gate_base_m = 1.5f;
+    float reid_3d_gate_per_frame_m = 0.1f;
+    bool reid_3d_gate_hard = false;
     bool debug = false;
 };
 
@@ -50,13 +58,14 @@ struct TargetObservation {
     int track_id = -1;
     int raw_track_id = -1;
     int detection_index = -1;
+    bool is_primary_target = false;
     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 detection_pos_cam = Eigen::Vector3f::Zero();
-    Eigen::Vector3f target_pos_world = Eigen::Vector3f::Zero();
+    Eigen::Vector3f detection_pos_world = Eigen::Vector3f::Zero();
 };
 
 struct TargetObservationDebugInfo {
@@ -64,6 +73,7 @@ struct TargetObservationDebugInfo {
     int current_raw_track_id_before = -1;
     int poses_count = 0;
     int tracks_count = 0;
+    int active_tracks_count = 0;
     int selected_track_id = -1;
     int selected_raw_track_id = -1;
     int detection_index = -1;
@@ -71,6 +81,8 @@ struct TargetObservationDebugInfo {
     int depth_sample_count = 0;
     int gallery_size = 0;
     int lost_frames = 0;
+    int reid_3d_gate_rejections = 0;     // hard-mode rejections this frame
+    int reid_3d_gate_soft_warnings = 0;  // soft-mode over-threshold this frame
     bool found_existing_target = false;
     bool target_selected = false;
     bool selected_from_center_bootstrap = false;
@@ -81,7 +93,13 @@ struct TargetObservationDebugInfo {
     float reid_similarity = -1.0f;
     float yolo_ms = 0.0f;
     float mot_ms = 0.0f;
-    float depth_ms = 0.0f;
+    float cloud_project_ms = 0.0f;
+    float bind_ms = 0.0f;        // entire bind_and_update_tracks (covers depth_ms)
+    float depth_ms = 0.0f;       // Phase 2A per-entry 3D compute (subset of bind_ms)
+    float reid_extract_ms = 0.0f; // Phase 2B TRT feature extraction (subset of bind_ms)
+    float age_out_ms = 0.0f;
+    float select_ms = 0.0f;
+    float enrich_ms = 0.0f;
     float total_ms = 0.0f;
     std::vector<yolos::pose::PoseResult> poses;
     std::vector<cv::Point> valid_projected_pixels;
@@ -100,6 +118,12 @@ public:
         const CloudReprojector::OdomPose& odom_pose,
         TargetObservationDebugInfo* debug_info = nullptr);
 
+    // All active tracks with a fresh 3D estimate from the most recent
+    // process() call. One TargetObservation per entry; is_primary_target
+    // marks the follow target. Empty if process() hasn't run yet.
+    std::vector<TargetObservation> snapshot_active_tracks() const;
+    int current_target_stable_id() const { return current_target_stable_id_; }
+
     bool initialized() const { return initialized_; }
 
     void draw_detected_poses(
@@ -107,55 +131,106 @@ public:
         const std::vector<yolos::pose::PoseResult>& poses) const;
 
 private:
+    struct ProjectedCloud {
+        std::vector<cv::Point2f> uv;
+        std::vector<Eigen::Vector3f> cam;
+    };
+
+    struct EntryCloudEstimate {
+        bool valid = false;
+        float depth = -1.0f;
+        float depth_confidence = 0.0f;
+        Eigen::Vector3f pos_cam = Eigen::Vector3f::Zero();
+        Eigen::Vector3f pos_world = Eigen::Vector3f::Zero();
+        int depth_sample_count = 0;
+        std::vector<cv::Point> valid_projected_pixels;
+    };
+
+    struct TrackEntry {
+        int stable_id = -1;
+        int last_raw_track_id = -1;
+        TrackGallery gallery;
+        cv::Rect last_bbox;
+        bool has_last_bbox = false;
+        int64_t last_seen_frame = -1;
+        bool just_rebound_by_reid = false;
+        float last_rebind_similarity = -1.0f;
+        // Per-entry 3D state; refreshed whenever compute_entry_3d succeeds.
+        float last_depth = -1.0f;
+        float last_depth_confidence = 0.0f;
+        Eigen::Vector3f last_pos_cam = Eigen::Vector3f::Zero();
+        Eigen::Vector3f last_pos_world = Eigen::Vector3f::Zero();
+        bool has_last_pos = false;
+        int64_t last_pos_frame = -1;
+        // 2D observation cache — refreshed every frame in Phase 5 while the
+        // entry is seen. Consumed by snapshot_active_tracks() to populate
+        // TargetObservation messages for all tracked people.
+        std::array<float, 17 * 3> last_keypoints_xyc{};
+        int last_detection_index = -1;
+        float last_confidence = 0.0f;
+        // Debug snapshot from the most recent successful estimate — only the
+        // follow target's values are copied into TargetObservationDebugInfo.
+        int last_depth_sample_count = 0;
+        std::vector<cv::Point> last_valid_projected_pixels;
+    };
+
     Eigen::MatrixXf format_detections(
         const std::vector<yolos::pose::PoseResult>& poses) const;
     std::vector<yolos::pose::PoseResult> detect_poses_with_lost_compensation(
         const cv::Mat& camera_bgr,
         TargetObservationDebugInfo* debug_info) const;
-    cv::Rect build_lost_detection_roi(const cv::Size& image_size) const;
+    cv::Rect build_lost_detection_roi(
+        const cv::Size& image_size,
+        const cv::Rect& last_bbox) const;
     static void offset_poses(
         std::vector<yolos::pose::PoseResult>& poses,
         const cv::Point& offset);
 
-    TargetObservation select_target(
+    ProjectedCloud project_cloud_to_image(
+        const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
+        const CloudReprojector& reprojector) const;
+
+    EntryCloudEstimate compute_entry_3d(
+        const yolos::pose::PoseResult& pose,
+        const std::array<float, 4>& bbox_xyxy,
+        const ProjectedCloud& projected,
+        const CloudReprojector& reprojector,
+        const CloudReprojector::OdomPose& odom_pose) const;
+
+    std::vector<int> bind_and_update_tracks(
         const cv::Mat& camera_bgr,
+        const Eigen::MatrixXf& tracks,
+        const std::vector<yolos::pose::PoseResult>& poses,
+        const ProjectedCloud& projected,
+        const CloudReprojector& reprojector,
+        const CloudReprojector::OdomPose& odom_pose,
+        TargetObservationDebugInfo* debug_info);
+
+    void age_out_tracks();
+
+    TargetObservation select_follow_target(
         const std::vector<yolos::pose::PoseResult>& poses,
         const Eigen::MatrixXf& tracks,
+        const std::vector<int>& stable_ids,
         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;
 
-    int recover_target_with_reid(
-        const cv::Mat& camera_bgr,
-        const std::vector<yolos::pose::PoseResult>& poses,
-        float* best_similarity) const;
-    void update_target_gallery(
-        const cv::Mat& camera_bgr,
-        const TargetObservation& target,
-        bool force_update);
-
-    bool should_update_gallery(bool force_update) const;
     bool is_good_reid_crop(const cv::Rect& bbox, const cv::Size& image_size) const;
 
     TargetObservationConfig config_;
     std::unique_ptr<yolos::pose::YOLOPoseDetector> yolo_;
     std::unique_ptr<motcpp::trackers::ByteTrack> tracker_;
     std::unique_ptr<ReIDTensorRTExtractor> reid_extractor_;
-    TrackGallery target_gallery_;
+
-    int current_target_id_ = -1;
+    std::unordered_map<int, TrackEntry> tracks_;
-    int current_raw_track_id_ = -1;
+    int next_stable_id_ = 0;
-    cv::Rect last_target_bbox_;
+    int current_target_stable_id_ = -1;
-    bool has_last_target_bbox_ = false;
     int64_t frame_index_ = 0;
-    int lost_track_frames_ = 0;
     bool initialized_ = false;
 };
 

msg/TargetObservation.msg

@@ -3,10 +3,11 @@ nav_msgs/Odometry odometry
 bool valid
 int32 track_id
 int32 detection_index
+bool is_primary_target
 float32 confidence
 float32 depth
 float32 depth_confidence
 float32[4] bbox_xyxy
 float32[51] keypoints_xyc
-geometry_msgs/Point target_pos_cam
+geometry_msgs/Point detection_pos_cam
-geometry_msgs/Point target_pos_world
+geometry_msgs/Point detection_pos_world

msg/TargetObservationArray.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+int32 primary_target_id
+TargetObservation[] observations

package.xml

@@ -15,6 +15,7 @@
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>pcl_conversions</depend>

package_ros2.xml

@@ -15,6 +15,7 @@
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>pcl_conversions</depend>

script/record_vis.sh

@@ -4,7 +4,8 @@ ros2 bag record \
     /odin1/odometry \
     /odin1/wiwc \
     /tf \
-    /odin1/sync/target_pos_world \
+    /odin1/sync/detection_pos_world \
-    /odin1/sync/target_pos_cam \
+    /odin1/sync/detection_pos_cam \
     /odin1/sync/target_observation \
+    /odin1/sync/track_observations \
 

src/cloud_reprojection_ros.cpp

@@ -160,8 +160,9 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
         << "\n  process_target_observation: " << (enable_target_observation_ ? "on" : "off")
         << "\n  debug: " << (debug_target_observation_ ? "on" : "off")
         << "\n  target_observation_topic: " << sync_target_observation_topic_
-        << "\n  target_pos_cam_topic: " << sync_target_pos_cam_topic_
+        << "\n  track_observations_topic: " << sync_track_observations_topic_
-        << "\n  target_pos_world_topic: " << sync_target_pos_world_topic_
+        << "\n  detection_pos_cam_topic: " << sync_detection_pos_cam_topic_
+        << "\n  detection_pos_world_topic: " << sync_detection_pos_world_topic_
 #endif
     );
 
@@ -217,16 +218,23 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
     if (enable_target_observation_) {
         target_observation_pub_ = this->create_publisher<odin_ros_driver::msg::TargetObservation>(
             sync_target_observation_topic_, 10);
-        target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
+        track_observations_pub_ =
-            sync_target_pos_cam_topic_, 10);
+            this->create_publisher<odin_ros_driver::msg::TargetObservationArray>(
-        target_pos_world_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
+                sync_track_observations_topic_, 10);
-            sync_target_pos_world_topic_, 10);
+        detection_pos_cam_pub_ =
+            this->create_publisher<visualization_msgs::msg::MarkerArray>(
+                sync_detection_pos_cam_topic_, 10);
+        detection_pos_world_pub_ =
+            this->create_publisher<visualization_msgs::msg::MarkerArray>(
+                sync_detection_pos_world_topic_, 10);
         RCLCPP_INFO(
             this->get_logger(),
-            "Target observation publishers created successfully | observation=%s | pos_cam=%s | pos_world=%s",
+            "Target observation publishers created | observation=%s | tracks=%s | "
+            "detection_pos_cam=%s | detection_pos_world=%s",
             sync_target_observation_topic_.c_str(),
-            sync_target_pos_cam_topic_.c_str(),
+            sync_track_observations_topic_.c_str(),
-            sync_target_pos_world_topic_.c_str());
+            sync_detection_pos_cam_topic_.c_str(),
+            sync_detection_pos_world_topic_.c_str());
     }
 #endif
 
@@ -311,8 +319,9 @@ void CloudReprojectionRosNode::loadParameters()
     sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
     sync_detection_debug_image_topic_ = sync_topic_prefix_ + "/detection_img_debug/compressed";
     sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
-    sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
+    sync_track_observations_topic_ = sync_topic_prefix_ + "/track_observations";
-    sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world";
+    sync_detection_pos_cam_topic_ = sync_topic_prefix_ + "/detection_pos_cam";
+    sync_detection_pos_world_topic_ = sync_topic_prefix_ + "/detection_pos_world";
 
     // Load camera parameters from calib.yaml file directly
     std::string calib_file = (package_path / "config" / "calib.yaml").string();
@@ -662,9 +671,10 @@ void CloudReprojectionRosNode::processSyncedData(
                         this->get_logger(),
                         *this->get_clock(),
                         1000,
-                        "Target observation | detections=%d tracked=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms",
+                        "Target observation | detections=%d tracked=%d active=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms",
                         target_debug.poses_count,
                         target_debug.tracks_count,
+                        target_debug.active_tracks_count,
                         target_debug.selected_track_id,
                         target_debug.selected_raw_track_id,
                         target_debug.detection_index,
@@ -680,7 +690,13 @@ void CloudReprojectionRosNode::processSyncedData(
                         target_debug.depth_sample_count,
                         target_debug.yolo_ms,
                         target_debug.mot_ms,
+                        target_debug.cloud_project_ms,
+                        target_debug.bind_ms,
                         target_debug.depth_ms,
+                        target_debug.reid_extract_ms,
+                        target_debug.age_out_ms,
+                        target_debug.select_ms,
+                        target_debug.enrich_ms,
                         target_debug.total_ms);
                 }
             } else {
@@ -688,7 +704,7 @@ void CloudReprojectionRosNode::processSyncedData(
                     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 current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d 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",
+                    "Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d active=%d current_target_id=%d current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gate_rej=%d gate_soft=%d gallery=%d lost=%d bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms | node_total=%.2f ms",
                     image_msg.header.stamp.sec,
                     image_msg.header.stamp.nanosec,
                     cam_bgr.cols,
@@ -696,6 +712,7 @@ void CloudReprojectionRosNode::processSyncedData(
                     cloud_cam.size(),
                     target_debug.poses_count,
                     target_debug.tracks_count,
+                    target_debug.active_tracks_count,
                     target_debug.current_target_id_before,
                     target_debug.current_raw_track_id_before,
                     target_observation.track_id,
@@ -708,19 +725,27 @@ void CloudReprojectionRosNode::processSyncedData(
                     target_debug.reid_attempted ? "yes" : "no",
                     target_debug.recovered_by_reid ? "yes" : "no",
                     target_debug.reid_similarity,
+                    target_debug.reid_3d_gate_rejections,
+                    target_debug.reid_3d_gate_soft_warnings,
                     target_debug.gallery_size,
                     target_debug.lost_frames,
                     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.detection_pos_cam).c_str(),
-                    format_vector3f(target_observation.target_pos_world).c_str(),
+                    format_vector3f(target_observation.detection_pos_world).c_str(),
                     target_debug.projected_cloud_points,
                     target_debug.depth_sample_count,
                     target_debug.yolo_ms,
                     target_debug.mot_ms,
+                    target_debug.cloud_project_ms,
+                    target_debug.bind_ms,
                     target_debug.depth_ms,
+                    target_debug.reid_extract_ms,
+                    target_debug.age_out_ms,
+                    target_debug.select_ms,
+                    target_debug.enrich_ms,
                     target_debug.total_ms,
                     std::chrono::duration<double, std::milli>(target_end - target_start).count());
                 RCLCPP_INFO_THROTTLE(
@@ -732,75 +757,152 @@ void CloudReprojectionRosNode::processSyncedData(
             }
         }
 
+        // Helper: fill a TargetObservation ROS msg from the C++ struct.
+        auto fill_observation_msg = [](
+            odin_ros_driver::msg::TargetObservation& msg,
+            const odin_ros_driver::TargetObservation& obs) {
+            msg.valid = obs.valid;
+            if (obs.valid) {
+                msg.track_id = obs.track_id;
+                msg.detection_index = obs.detection_index;
+                msg.is_primary_target = obs.is_primary_target;
+                msg.confidence = obs.confidence;
+                msg.depth = obs.depth;
+                msg.depth_confidence = obs.depth_confidence;
+                msg.bbox_xyxy = obs.bbox_xyxy;
+                msg.keypoints_xyc = obs.keypoints_xyc;
+                msg.detection_pos_cam.x = obs.detection_pos_cam.x();
+                msg.detection_pos_cam.y = obs.detection_pos_cam.y();
+                msg.detection_pos_cam.z = obs.detection_pos_cam.z();
+                msg.detection_pos_world.x = obs.detection_pos_world.x();
+                msg.detection_pos_world.y = obs.detection_pos_world.y();
+                msg.detection_pos_world.z = obs.detection_pos_world.z();
+            } else {
+                msg.track_id = -1;
+                msg.detection_index = -1;
+                msg.is_primary_target = false;
+                msg.confidence = -1.0f;
+                msg.depth = -1.0f;
+                msg.depth_confidence = -1.0f;
+                msg.bbox_xyxy.fill(-1.0f);
+                msg.keypoints_xyc.fill(-1.0f);
+                msg.detection_pos_cam.x = -1.0;
+                msg.detection_pos_cam.y = -1.0;
+                msg.detection_pos_cam.z = -1.0;
+                msg.detection_pos_world.x = -1.0;
+                msg.detection_pos_world.y = -1.0;
+                msg.detection_pos_world.z = -1.0;
+            }
+        };
+
+        // Single-target topic — still published for backwards-compatible
+        // downstream that only cares about the follow target.
         odin_ros_driver::msg::TargetObservation observation_msg;
         observation_msg.header = image_msg.header;
         observation_msg.header.stamp = sync_stamp;
         observation_msg.odometry = sync_odom_msg;
-        observation_msg.valid = target_observation.valid;
+        fill_observation_msg(observation_msg, target_observation);
-        if (target_observation.valid) {
-            observation_msg.track_id = target_observation.track_id;
-            observation_msg.detection_index = target_observation.detection_index;
-            observation_msg.confidence = target_observation.confidence;
-            observation_msg.depth = target_observation.depth;
-            observation_msg.depth_confidence = target_observation.depth_confidence;
-            observation_msg.bbox_xyxy = target_observation.bbox_xyxy;
-            observation_msg.keypoints_xyc = target_observation.keypoints_xyc;
-            observation_msg.target_pos_cam.x = target_observation.target_pos_cam.x();
-            observation_msg.target_pos_cam.y = target_observation.target_pos_cam.y();
-            observation_msg.target_pos_cam.z = target_observation.target_pos_cam.z();
-            observation_msg.target_pos_world.x = target_observation.target_pos_world.x();
-            observation_msg.target_pos_world.y = target_observation.target_pos_world.y();
-            observation_msg.target_pos_world.z = target_observation.target_pos_world.z();
-        } else {
-            observation_msg.track_id = -1;
-            observation_msg.detection_index = -1;
-            observation_msg.confidence = -1.0f;
-            observation_msg.depth = -1.0f;
-            observation_msg.depth_confidence = -1.0f;
-            observation_msg.bbox_xyxy.fill(-1.0f);
-            observation_msg.keypoints_xyc.fill(-1.0f);
-            observation_msg.target_pos_cam.x = -1.0;
-            observation_msg.target_pos_cam.y = -1.0;
-            observation_msg.target_pos_cam.z = -1.0;
-            observation_msg.target_pos_world.x = -1.0;
-            observation_msg.target_pos_world.y = -1.0;
-            observation_msg.target_pos_world.z = -1.0;
-        }
         target_observation_pub_->publish(observation_msg);
 
-        geometry_msgs::msg::PointStamped pos_cam_msg;
+        // Multi-target topic — every active track with a fresh 3D estimate,
-        pos_cam_msg.header = image_msg.header;
+        // including the follow target (marked via is_primary_target).
-        pos_cam_msg.header.stamp = sync_stamp;
+        const std::vector<odin_ros_driver::TargetObservation> active_tracks =
-        pos_cam_msg.header.frame_id = cloud_cam_msg.header.frame_id.empty()
+            target_observation_processor_->snapshot_active_tracks();
-            ? "camera"
+        odin_ros_driver::msg::TargetObservationArray track_array_msg;
-            : cloud_cam_msg.header.frame_id;
+        track_array_msg.header = image_msg.header;
-        if (target_observation.valid) {
+        track_array_msg.header.stamp = sync_stamp;
-            pos_cam_msg.point.x = target_observation.target_pos_cam.x();
+        track_array_msg.primary_target_id =
-            pos_cam_msg.point.y = target_observation.target_pos_cam.y();
+            target_observation_processor_->current_target_stable_id();
-            pos_cam_msg.point.z = target_observation.target_pos_cam.z();
+        track_array_msg.observations.reserve(active_tracks.size());
-        } else {
+        for (const auto& obs : active_tracks) {
-            pos_cam_msg.point.x = -1.0;
+            odin_ros_driver::msg::TargetObservation one;
-            pos_cam_msg.point.y = -1.0;
+            one.header = image_msg.header;
-            pos_cam_msg.point.z = -1.0;
+            one.header.stamp = sync_stamp;
+            one.odometry = sync_odom_msg;
+            fill_observation_msg(one, obs);
+            track_array_msg.observations.push_back(std::move(one));
         }
-        target_pos_cam_pub_->publish(pos_cam_msg);
+        track_observations_pub_->publish(track_array_msg);
 
-        geometry_msgs::msg::PointStamped pos_world_msg;
+        // Debug Marker topics — rendered directly in RViz. Primary target in
-        pos_world_msg.header = image_msg.header;
+        // red, others in cyan; short lifetime so stale markers auto-clear.
-        pos_world_msg.header.stamp = sync_stamp;
+        std_msgs::msg::Header cam_header = image_msg.header;
-        pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty()
+        cam_header.stamp = sync_stamp;
-            ? "odom"
+        cam_header.frame_id = cloud_cam_msg.header.frame_id.empty()
+            ? std::string("camera")
+            : cloud_cam_msg.header.frame_id;
+        std_msgs::msg::Header world_header = image_msg.header;
+        world_header.stamp = sync_stamp;
+        world_header.frame_id = odom_msg->header.frame_id.empty()
+            ? std::string("odom")
             : odom_msg->header.frame_id;
-        if (target_observation.valid) {
+
-            pos_world_msg.point.x = target_observation.target_pos_world.x();
+        auto build_marker_array = [&active_tracks](
-            pos_world_msg.point.y = target_observation.target_pos_world.y();
+            const std_msgs::msg::Header& header, bool world_frame) {
-            pos_world_msg.point.z = target_observation.target_pos_world.z();
+            visualization_msgs::msg::MarkerArray out;
-        } else {
+            out.markers.reserve(1 + active_tracks.size() * 2);
-            pos_world_msg.point.x = -1.0;
+            // DELETEALL first clears any leftover markers from prior frames.
-            pos_world_msg.point.y = -1.0;
+            visualization_msgs::msg::Marker clear_all;
-            pos_world_msg.point.z = -1.0;
+            clear_all.header = header;
-        }
+            clear_all.action = visualization_msgs::msg::Marker::DELETEALL;
-        target_pos_world_pub_->publish(pos_world_msg);
+            out.markers.push_back(clear_all);
+            for (const auto& obs : active_tracks) {
+                if (!obs.valid) continue;
+                const auto& pos = world_frame ? obs.detection_pos_world
+                                              : obs.detection_pos_cam;
+                visualization_msgs::msg::Marker sphere;
+                sphere.header = header;
+                sphere.ns = "detection_sphere";
+                sphere.id = obs.track_id;
+                sphere.type = visualization_msgs::msg::Marker::SPHERE;
+                sphere.action = visualization_msgs::msg::Marker::ADD;
+                sphere.pose.position.x = pos.x();
+                sphere.pose.position.y = pos.y();
+                sphere.pose.position.z = pos.z();
+                sphere.pose.orientation.w = 1.0;
+                sphere.scale.x = 0.3;
+                sphere.scale.y = 0.3;
+                sphere.scale.z = 0.3;
+                if (obs.is_primary_target) {
+                    sphere.color.r = 1.0f;
+                    sphere.color.g = 0.2f;
+                    sphere.color.b = 0.2f;
+                } else {
+                    sphere.color.r = 1.0f;
+                    sphere.color.g = 0.95f;
+                    sphere.color.b = 0.2f;
+                }
+                sphere.color.a = 0.9f;
+                sphere.lifetime.sec = 0;
+                sphere.lifetime.nanosec = 300000000;  // 0.3 s
+                out.markers.push_back(sphere);
+
+                visualization_msgs::msg::Marker label;
+                label.header = header;
+                label.ns = "detection_label";
+                label.id = obs.track_id;
+                label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+                label.action = visualization_msgs::msg::Marker::ADD;
+                label.pose.position.x = pos.x();
+                label.pose.position.y = pos.y();
+                label.pose.position.z = pos.z() + 0.5;
+                label.pose.orientation.w = 1.0;
+                label.scale.z = 0.25;
+                label.color.r = 1.0f;
+                label.color.g = 1.0f;
+                label.color.b = 1.0f;
+                label.color.a = 1.0f;
+                label.lifetime.sec = 0;
+                label.lifetime.nanosec = 300000000;
+                label.text = (obs.is_primary_target ? "*id=" : "id=") +
+                             std::to_string(obs.track_id);
+                out.markers.push_back(label);
+            }
+            return out;
+        };
+
+        detection_pos_cam_pub_->publish(build_marker_array(cam_header, false));
+        detection_pos_world_pub_->publish(build_marker_array(world_header, true));
     }
 #endif