From 5b177c90f4d160191029a6e4d76e64179f192a66 Mon Sep 17 00:00:00 2001 From: hjy <1178065793@qq.com> Date: Sun, 19 Apr 2026 16:48:54 +0800 Subject: [PATCH] add multi-target support (TargetObservation + rviz publish); MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 现在可以发布包含多个目标的消息了，而且自带多目标管理系统，可以支持发布多目标的info，debug和后续推理都可以 --- CMakeLists.txt | 2 + config/odin_ros2.rviz | 37 +- include/cloud_reprojection_ros_node.hpp | 13 +- include/target_observation_processing.hpp | 125 ++- msg/TargetObservation.msg | 5 +- msg/TargetObservationArray.msg | 3 + package.xml | 1 + package_ros2.xml | 1 + script/record_vis.sh | 5 +- src/cloud_reprojection_ros.cpp | 252 ++++-- src/target_observation_processing.cpp | 927 ++++++++++++++-------- 11 files changed, 910 insertions(+), 461 deletions(-) create mode 100644 msg/TargetObservationArray.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 20afd17..2fe9b99 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz index 2bbd903..ec59bc0 100644 --- a/config/odin_ros2.rviz +++ b/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/PointStamped - Color: 224; 27; 36 - Enabled: false - History Length: 10 - Name: target_pos_world - Radius: 0.30000001192092896 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: detection_pos_world + Namespaces: + detection_label: true + detection_sphere: true Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /odin1/sync/target_pos_world - Value: false - - Alpha: 1 - Class: rviz_default_plugins/PointStamped - Color: 204; 41; 204 + Value: /odin1/sync/detection_pos_world + Value: true + - Class: rviz_default_plugins/MarkerArray Enabled: false - History Length: 1 - Name: target_pos_cam - Radius: 0.20000000298023224 + Name: detection_pos_cam + Namespaces: + {} 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 diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index 7e3b40e..db5d425 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -22,10 +22,13 @@ limitations under the License. #include #include #include + #include + #include #include #include #include #include "odin_ros_driver/msg/target_observation.hpp" + #include "odin_ros_driver/msg/target_observation_array.hpp" #else #include #include @@ -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_target_pos_world_topic_; + std::string sync_track_observations_topic_; + std::string sync_detection_pos_cam_topic_; + std::string sync_detection_pos_world_topic_; rclcpp::Publisher::SharedPtr sync_cloud_pub_; rclcpp::Publisher::SharedPtr sync_cloud_slam_pub_; @@ -110,8 +114,9 @@ private: rclcpp::Publisher::SharedPtr detection_debug_compressed_pub_; // debug detections/tracks rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ rclcpp::Publisher::SharedPtr target_observation_pub_; - rclcpp::Publisher::SharedPtr target_pos_cam_pub_; - rclcpp::Publisher::SharedPtr target_pos_world_pub_; + rclcpp::Publisher::SharedPtr track_observations_pub_; + rclcpp::Publisher::SharedPtr detection_pos_cam_pub_; + rclcpp::Publisher::SharedPtr detection_pos_world_pub_; std::unique_ptr reprojector_; #ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp index 92cb02e..0a00d59 100644 --- a/include/target_observation_processing.hpp +++ b/include/target_observation_processing.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -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 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(); + Eigen::Vector3f detection_pos_cam = 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 poses; std::vector 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 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& poses) const; private: + struct ProjectedCloud { + std::vector uv; + std::vector 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 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 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 last_valid_projected_pixels; + }; + Eigen::MatrixXf format_detections( const std::vector& poses) const; std::vector 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& poses, const cv::Point& offset); - TargetObservation select_target( + ProjectedCloud project_cloud_to_image( + const pcl::PointCloud& cloud_in_cam, + const CloudReprojector& reprojector) const; + + EntryCloudEstimate compute_entry_3d( + const yolos::pose::PoseResult& pose, + const std::array& bbox_xyxy, + const ProjectedCloud& projected, + const CloudReprojector& reprojector, + const CloudReprojector::OdomPose& odom_pose) const; + + std::vector bind_and_update_tracks( const cv::Mat& camera_bgr, + const Eigen::MatrixXf& tracks, + const std::vector& 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& poses, const Eigen::MatrixXf& tracks, + const std::vector& stable_ids, 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; - int recover_target_with_reid( - const cv::Mat& camera_bgr, - const std::vector& 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 yolo_; std::unique_ptr tracker_; std::unique_ptr reid_extractor_; - TrackGallery target_gallery_; - int current_target_id_ = -1; - int current_raw_track_id_ = -1; - cv::Rect last_target_bbox_; - bool has_last_target_bbox_ = false; + + std::unordered_map tracks_; + int next_stable_id_ = 0; + int current_target_stable_id_ = -1; int64_t frame_index_ = 0; - int lost_track_frames_ = 0; bool initialized_ = false; }; diff --git a/msg/TargetObservation.msg b/msg/TargetObservation.msg index 12350ef..0ce0c34 100644 --- a/msg/TargetObservation.msg +++ b/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 target_pos_world +geometry_msgs/Point detection_pos_cam +geometry_msgs/Point detection_pos_world diff --git a/msg/TargetObservationArray.msg b/msg/TargetObservationArray.msg new file mode 100644 index 0000000..770b106 --- /dev/null +++ b/msg/TargetObservationArray.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +int32 primary_target_id +TargetObservation[] observations diff --git a/package.xml b/package.xml index 19b00f0..bded50f 100755 --- a/package.xml +++ b/package.xml @@ -15,6 +15,7 @@ sensor_msgs nav_msgs geometry_msgs + visualization_msgs cv_bridge image_transport pcl_conversions diff --git a/package_ros2.xml b/package_ros2.xml index 19b00f0..bded50f 100644 --- a/package_ros2.xml +++ b/package_ros2.xml @@ -15,6 +15,7 @@ sensor_msgs nav_msgs geometry_msgs + visualization_msgs cv_bridge image_transport pcl_conversions diff --git a/script/record_vis.sh b/script/record_vis.sh index e4993d2..16c320b 100755 --- a/script/record_vis.sh +++ b/script/record_vis.sh @@ -4,7 +4,8 @@ ros2 bag record \ /odin1/odometry \ /odin1/wiwc \ /tf \ - /odin1/sync/target_pos_world \ - /odin1/sync/target_pos_cam \ + /odin1/sync/detection_pos_world \ + /odin1/sync/detection_pos_cam \ /odin1/sync/target_observation \ + /odin1/sync/track_observations \ diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index 208cac6..e1248e8 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/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 target_pos_world_topic: " << sync_target_pos_world_topic_ + << "\n track_observations_topic: " << sync_track_observations_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( 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); + track_observations_pub_ = + this->create_publisher( + sync_track_observations_topic_, 10); + detection_pos_cam_pub_ = + this->create_publisher( + sync_detection_pos_cam_topic_, 10); + detection_pos_world_pub_ = + this->create_publisher( + 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_target_pos_world_topic_.c_str()); + sync_track_observations_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_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world"; + sync_track_observations_topic_ = sync_topic_prefix_ + "/track_observations"; + 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.target_pos_world).c_str(), + format_vector3f(target_observation.detection_pos_cam).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(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; - 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; - } + fill_observation_msg(observation_msg, target_observation); 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; - if (target_observation.valid) { - 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(); - } else { - pos_cam_msg.point.x = -1.0; - pos_cam_msg.point.y = -1.0; - pos_cam_msg.point.z = -1.0; + // Multi-target topic — every active track with a fresh 3D estimate, + // including the follow target (marked via is_primary_target). + const std::vector active_tracks = + target_observation_processor_->snapshot_active_tracks(); + odin_ros_driver::msg::TargetObservationArray track_array_msg; + track_array_msg.header = image_msg.header; + track_array_msg.header.stamp = sync_stamp; + track_array_msg.primary_target_id = + target_observation_processor_->current_target_stable_id(); + track_array_msg.observations.reserve(active_tracks.size()); + for (const auto& obs : active_tracks) { + odin_ros_driver::msg::TargetObservation one; + one.header = image_msg.header; + 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; - 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" + // Debug Marker topics — rendered directly in RViz. Primary target in + // red, others in cyan; short lifetime so stale markers auto-clear. + std_msgs::msg::Header cam_header = image_msg.header; + cam_header.stamp = sync_stamp; + 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(); - pos_world_msg.point.y = target_observation.target_pos_world.y(); - pos_world_msg.point.z = target_observation.target_pos_world.z(); - } else { - pos_world_msg.point.x = -1.0; - pos_world_msg.point.y = -1.0; - pos_world_msg.point.z = -1.0; - } - target_pos_world_pub_->publish(pos_world_msg); + + auto build_marker_array = [&active_tracks]( + const std_msgs::msg::Header& header, bool world_frame) { + visualization_msgs::msg::MarkerArray out; + out.markers.reserve(1 + active_tracks.size() * 2); + // DELETEALL first clears any leftover markers from prior frames. + visualization_msgs::msg::Marker clear_all; + clear_all.header = header; + clear_all.action = visualization_msgs::msg::Marker::DELETEALL; + 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 diff --git a/src/target_observation_processing.cpp b/src/target_observation_processing.cpp index 444cc49..b262507 100644 --- a/src/target_observation_processing.cpp +++ b/src/target_observation_processing.cpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include @@ -115,13 +117,10 @@ void TargetObservationProcessor::initialize(const TargetObservationConfig& confi config_.yolo_labels_path); tracker_ = std::make_unique( 0.3f, 30, 50, 3, 0.3f, false, 1); - target_gallery_ = TrackGallery(static_cast(std::max(1, config_.reid_gallery_size))); - current_target_id_ = -1; - current_raw_track_id_ = -1; - last_target_bbox_ = cv::Rect(); - has_last_target_bbox_ = false; + tracks_.clear(); + next_stable_id_ = 0; + current_target_stable_id_ = -1; frame_index_ = 0; - lost_track_frames_ = 0; reid_extractor_.reset(); if (config_.reid_enabled && !config_.reid_engine_path.empty()) { try { @@ -154,31 +153,39 @@ TargetObservation TargetObservationProcessor::process( const auto total_start = std::chrono::steady_clock::now(); TargetObservation empty; ++frame_index_; + + auto populate_follow_debug = [&](TargetObservationDebugInfo* info) { + if (!info) { + return; + } + info->active_tracks_count = static_cast(tracks_.size()); + if (current_target_stable_id_ >= 0) { + auto it = tracks_.find(current_target_stable_id_); + if (it != tracks_.end()) { + info->gallery_size = static_cast(it->second.gallery.size()); + info->lost_frames = static_cast(std::max( + 0, frame_index_ - it->second.last_seen_frame)); + } + } + }; auto finalize_empty = [&](TargetObservationDebugInfo* info) { + populate_follow_debug(info); if (info) { - info->gallery_size = static_cast(target_gallery_.size()); - info->lost_frames = lost_track_frames_; info->total_ms = std::chrono::duration(std::chrono::steady_clock::now() - total_start).count(); } return empty; }; - auto mark_lost = [&]() { - ++lost_track_frames_; - if (config_.reid_lost_timeout_frames > 0 && - lost_track_frames_ >= config_.reid_lost_timeout_frames) { - current_target_id_ = -1; - current_raw_track_id_ = -1; - last_target_bbox_ = cv::Rect(); - has_last_target_bbox_ = false; - lost_track_frames_ = 0; - target_gallery_.clear(); - } - }; + if (debug_info) { *debug_info = TargetObservationDebugInfo{}; - debug_info->current_target_id_before = current_target_id_; - debug_info->current_raw_track_id_before = current_raw_track_id_; + debug_info->current_target_id_before = current_target_stable_id_; + if (current_target_stable_id_ >= 0) { + auto it = tracks_.find(current_target_stable_id_); + if (it != tracks_.end()) { + debug_info->current_raw_track_id_before = it->second.last_raw_track_id; + } + } } if (!initialized_ || camera_bgr.empty()) { return finalize_empty(debug_info); @@ -198,7 +205,7 @@ TargetObservation TargetObservationProcessor::process( std::chrono::duration(yolo_end - yolo_start).count(); } if (poses.empty()) { - mark_lost(); + age_out_tracks(); return finalize_empty(debug_info); } @@ -212,45 +219,63 @@ TargetObservation TargetObservationProcessor::process( std::chrono::duration(mot_end - mot_start).count(); } if (tracks.rows() == 0) { - mark_lost(); - return finalize_empty(debug_info); - } - - TargetObservation target = select_target( - camera_bgr, poses, tracks, camera_bgr.cols, camera_bgr.rows, debug_info); - if (!target.valid) { - mark_lost(); - return finalize_empty(debug_info); - } - - current_raw_track_id_ = target.raw_track_id; - lost_track_frames_ = 0; - if (current_target_id_ < 0) { - current_target_id_ = target.raw_track_id; - } - last_target_bbox_ = clamp_rect_to_image( - cv::Rect( - static_cast(std::lround(target.bbox_xyxy[0])), - static_cast(std::lround(target.bbox_xyxy[1])), - std::max(0, static_cast(std::lround(target.bbox_xyxy[2] - target.bbox_xyxy[0]))), - std::max(0, static_cast(std::lround(target.bbox_xyxy[3] - target.bbox_xyxy[1])))), - camera_bgr.size()); - has_last_target_bbox_ = (last_target_bbox_.area() > 0); - const bool force_gallery_update = - target_gallery_.empty() || - (debug_info != nullptr && - (debug_info->selected_from_center_bootstrap || debug_info->recovered_by_reid)); - update_target_gallery(camera_bgr, target, force_gallery_update); - - target = enrich_target_with_cloud( - target, poses, cloud_in_cam, reprojector, odom_pose, debug_info); - if (!target.valid) { + age_out_tracks(); return finalize_empty(debug_info); } + const auto project_start = std::chrono::steady_clock::now(); + const ProjectedCloud projected = + project_cloud_to_image(cloud_in_cam, reprojector); + if (debug_info) { + debug_info->projected_cloud_points = + static_cast(projected.cam.size()); + debug_info->cloud_project_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - project_start).count(); + } + + const auto bind_start = std::chrono::steady_clock::now(); + const std::vector stable_ids = bind_and_update_tracks( + camera_bgr, tracks, poses, projected, reprojector, odom_pose, debug_info); + if (debug_info) { + debug_info->bind_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - bind_start).count(); + } + + const auto age_start = std::chrono::steady_clock::now(); + age_out_tracks(); + if (debug_info) { + debug_info->age_out_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - age_start).count(); + } + + const auto select_start = std::chrono::steady_clock::now(); + TargetObservation target = select_follow_target( + poses, tracks, stable_ids, camera_bgr.cols, camera_bgr.rows, debug_info); + if (debug_info) { + debug_info->select_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - select_start).count(); + } + if (!target.valid) { + return finalize_empty(debug_info); + } + + const auto enrich_start = std::chrono::steady_clock::now(); + target = enrich_target_with_cloud(target, debug_info); + if (debug_info) { + debug_info->enrich_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - enrich_start).count(); + } + if (!target.valid) { + return finalize_empty(debug_info); + } + + populate_follow_debug(debug_info); if (debug_info) { - debug_info->gallery_size = static_cast(target_gallery_.size()); - debug_info->lost_frames = lost_track_frames_; debug_info->total_ms = std::chrono::duration(std::chrono::steady_clock::now() - total_start).count(); } @@ -261,11 +286,16 @@ std::vector TargetObservationProcessor::detect_poses_wi const cv::Mat& camera_bgr, TargetObservationDebugInfo* debug_info) const { - if (!config_.lost_detection_compensation_enabled || !has_last_target_bbox_ || camera_bgr.empty()) { + if (!config_.lost_detection_compensation_enabled || camera_bgr.empty() || + current_target_stable_id_ < 0) { + return {}; + } + const auto it = tracks_.find(current_target_stable_id_); + if (it == tracks_.end() || !it->second.has_last_bbox) { return {}; } - const cv::Rect roi = build_lost_detection_roi(camera_bgr.size()); + const cv::Rect roi = build_lost_detection_roi(camera_bgr.size(), it->second.last_bbox); if (roi.width <= 1 || roi.height <= 1) { return {}; } @@ -287,21 +317,23 @@ std::vector TargetObservationProcessor::detect_poses_wi return poses; } -cv::Rect TargetObservationProcessor::build_lost_detection_roi(const cv::Size& image_size) const +cv::Rect TargetObservationProcessor::build_lost_detection_roi( + const cv::Size& image_size, + const cv::Rect& last_bbox) const { - if (!has_last_target_bbox_ || image_size.width <= 0 || image_size.height <= 0) { + if (last_bbox.area() <= 0 || image_size.width <= 0 || image_size.height <= 0) { return {}; } const float scale = std::max(config_.lost_detection_roi_scale, 1.0f); - const float cx = static_cast(last_target_bbox_.x) + 0.5f * static_cast(last_target_bbox_.width); - const float cy = static_cast(last_target_bbox_.y) + 0.5f * static_cast(last_target_bbox_.height); + const float cx = static_cast(last_bbox.x) + 0.5f * static_cast(last_bbox.width); + const float cy = static_cast(last_bbox.y) + 0.5f * static_cast(last_bbox.height); const float roi_w = std::max( static_cast(config_.lost_detection_roi_min_size_px), - static_cast(last_target_bbox_.width) * scale); + static_cast(last_bbox.width) * scale); const float roi_h = std::max( static_cast(config_.lost_detection_roi_min_size_px), - static_cast(last_target_bbox_.height) * scale); + static_cast(last_bbox.height) * scale); const int x = static_cast(std::floor(cx - 0.5f * roi_w)); const int y = static_cast(std::floor(cy - 0.5f * roi_h)); @@ -362,116 +394,402 @@ Eigen::MatrixXf TargetObservationProcessor::format_detections( return dets; } -TargetObservation TargetObservationProcessor::select_target( +std::vector TargetObservationProcessor::bind_and_update_tracks( const cv::Mat& camera_bgr, + const Eigen::MatrixXf& tracks, + const std::vector& poses, + const ProjectedCloud& projected, + const CloudReprojector& reprojector, + const CloudReprojector::OdomPose& odom_pose, + TargetObservationDebugInfo* debug_info) +{ + const int row_count = static_cast(tracks.rows()); + std::vector stable_ids(static_cast(row_count), -1); + if (row_count == 0) { + return stable_ids; + } + + // Clear one-shot ReID-rebind flags from the previous frame so stale + // debug state never leaks forward. + for (auto& kv : tracks_) { + kv.second.just_rebound_by_reid = false; + kv.second.last_rebind_similarity = -1.0f; + } + + // Phase 1: fast-path binding by raw_track_id. We only trust raw IDs that + // are still "hot" (the entry was seen in the immediately previous frame); + // ByteTrack can reuse retired raw IDs for a different person. + std::unordered_set taken_sids; + taken_sids.reserve(tracks_.size()); + for (int i = 0; i < row_count; ++i) { + const int raw_id = static_cast(tracks(i, 4)); + for (auto& kv : tracks_) { + if (taken_sids.count(kv.first)) { + continue; + } + if (kv.second.last_raw_track_id != raw_id) { + continue; + } + if (kv.second.last_seen_frame < frame_index_ - 1) { + continue; + } + stable_ids[static_cast(i)] = kv.first; + taken_sids.insert(kv.first); + break; + } + } + + // Phase 2A: per-row 3D estimate. We compute depth / pos_cam / pos_world + // for every bound row up-front so downstream consumers can use it as a + // dataset observation for non-follow targets, and Phase 3 can enforce a + // spatial-consistency gate against each entry's last known world pos. + const auto depth_compute_start = std::chrono::steady_clock::now(); + std::vector row_estimates(static_cast(row_count)); + for (int i = 0; i < row_count; ++i) { + const int detection_index = static_cast(tracks(i, 7)); + if (detection_index < 0 || + detection_index >= static_cast(poses.size())) { + continue; + } + const auto& pose = poses[static_cast(detection_index)]; + const std::array bbox_xyxy{ + static_cast(pose.box.x), + static_cast(pose.box.y), + static_cast(pose.box.x + pose.box.width), + static_cast(pose.box.y + pose.box.height), + }; + row_estimates[static_cast(i)] = + compute_entry_3d(pose, bbox_xyxy, projected, reprojector, odom_pose); + } + const float depth_compute_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - depth_compute_start).count(); + + // Phase 2B: batch-extract ReID features for rows that (a) still need a + // stable_id (slow-path candidates) or (b) are bound but due for a gallery + // refresh. Poor-quality crops are skipped. + const bool reid_ready = config_.reid_enabled && + reid_extractor_ != nullptr && + reid_extractor_->initialized() && + !camera_bgr.empty(); + std::vector rows_to_reid; + std::vector reid_boxes; + if (reid_ready) { + rows_to_reid.reserve(row_count); + reid_boxes.reserve(row_count); + for (int i = 0; i < row_count; ++i) { + const cv::Rect bbox = track_row_to_rect(tracks, i); + if (!is_good_reid_crop(bbox, camera_bgr.size())) { + continue; + } + if (stable_ids[static_cast(i)] < 0) { + rows_to_reid.push_back(i); + reid_boxes.push_back(bbox); + continue; + } + const auto it = tracks_.find(stable_ids[static_cast(i)]); + if (it == tracks_.end()) { + continue; + } + const TrackEntry& entry = it->second; + const bool empty_gallery = entry.gallery.empty(); + const bool interval_due = + config_.reid_feature_update_interval <= 0 || + (frame_index_ - entry.gallery.last_update_frame()) >= + static_cast(config_.reid_feature_update_interval); + if (empty_gallery || interval_due) { + rows_to_reid.push_back(i); + reid_boxes.push_back(bbox); + } + } + } + + std::vector features_by_row(static_cast(row_count)); + float reid_extract_ms = 0.0f; + if (!reid_boxes.empty()) { + const auto reid_start = std::chrono::steady_clock::now(); + auto features = reid_extractor_->extract(camera_bgr, reid_boxes); + reid_extract_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - reid_start).count(); + for (size_t k = 0; k < features.size(); ++k) { + const int row = rows_to_reid[k]; + if (feature_has_signal(features[k])) { + features_by_row[static_cast(row)] = std::move(features[k]); + } + } + } + + // Phase 3: ReID recovery. For rows that still lack a stable_id, compare + // their feature against every unmatched entry's gallery and rebind if + // top1 passes the match threshold and the top1-top2 gap is wide enough. + // A 3D position gate rejects (soft-log / hard-reject) candidates whose + // current world pos is implausibly far from the entry's last pos. + bool reid_recovery_attempted = false; + int gate_hard_rejections = 0; + int gate_soft_warnings = 0; + for (int i = 0; i < row_count; ++i) { + if (stable_ids[static_cast(i)] >= 0) { + continue; + } + const ReIDFeature& feature = features_by_row[static_cast(i)]; + if (feature.empty()) { + continue; + } + const EntryCloudEstimate& row_est = row_estimates[static_cast(i)]; + + int best_sid = -1; + float top1 = -1.0f; + float top2 = -1.0f; + for (const auto& kv : tracks_) { + if (taken_sids.count(kv.first) || kv.second.gallery.empty()) { + continue; + } + + // 3D gate: only apply when both sides have valid world positions. + bool gate_pass = true; + if (row_est.valid && kv.second.has_last_pos) { + const float dist = + (kv.second.last_pos_world - row_est.pos_world).norm(); + const int64_t frames_since = std::max( + 0, frame_index_ - kv.second.last_seen_frame); + const float dmax = config_.reid_3d_gate_base_m + + config_.reid_3d_gate_per_frame_m * + static_cast(frames_since); + if (dist > dmax) { + if (config_.reid_3d_gate_hard) { + ++gate_hard_rejections; + continue; // skip this (row, entry) pair + } else { + ++gate_soft_warnings; + gate_pass = false; // (soft mode: still compare) + } + } + } + (void)gate_pass; + + reid_recovery_attempted = true; + const float sim = kv.second.gallery.best_similarity(feature); + if (sim > top1) { + top2 = top1; + top1 = sim; + best_sid = kv.first; + } else if (sim > top2) { + top2 = sim; + } + } + if (best_sid < 0 || top1 < config_.reid_match_threshold) { + continue; + } + if (top2 >= 0.0f && (top1 - top2) < config_.reid_gap_threshold) { + continue; + } + stable_ids[static_cast(i)] = best_sid; + taken_sids.insert(best_sid); + auto it = tracks_.find(best_sid); + if (it != tracks_.end()) { + it->second.just_rebound_by_reid = true; + it->second.last_rebind_similarity = top1; + } + } + + // Phase 4: allocate a fresh stable_id for rows that still have no match. + for (int i = 0; i < row_count; ++i) { + if (stable_ids[static_cast(i)] >= 0) { + continue; + } + const int new_sid = next_stable_id_++; + TrackEntry entry; + entry.stable_id = new_sid; + entry.gallery = TrackGallery( + static_cast(std::max(1, config_.reid_gallery_size))); + tracks_.emplace(new_sid, std::move(entry)); + stable_ids[static_cast(i)] = new_sid; + taken_sids.insert(new_sid); + } + + // Phase 5: per-row bookkeeping + gallery update + 2D/3D cache writeback. + for (int i = 0; i < row_count; ++i) { + const int sid = stable_ids[static_cast(i)]; + const auto it = tracks_.find(sid); + if (it == tracks_.end()) { + continue; + } + TrackEntry& entry = it->second; + entry.last_raw_track_id = static_cast(tracks(i, 4)); + entry.last_bbox = clamp_rect_to_image( + track_row_to_rect(tracks, i), camera_bgr.size()); + entry.has_last_bbox = (entry.last_bbox.area() > 0); + entry.last_seen_frame = frame_index_; + const ReIDFeature& feature = features_by_row[static_cast(i)]; + if (!feature.empty()) { + entry.gallery.add(feature, frame_index_); + } + + // Cache 2D observation so snapshot_active_tracks() can emit a full + // TargetObservation for every active entry without re-indexing poses. + const int detection_index = static_cast(tracks(i, 7)); + entry.last_detection_index = detection_index; + if (detection_index >= 0 && + detection_index < static_cast(poses.size())) { + const auto& pose = poses[static_cast(detection_index)]; + entry.last_confidence = pose.conf; + entry.last_keypoints_xyc.fill(0.0f); + const size_t kp_count = std::min(pose.keypoints.size(), 17); + for (size_t k = 0; k < kp_count; ++k) { + entry.last_keypoints_xyc[k * 3 + 0] = pose.keypoints[k].x; + entry.last_keypoints_xyc[k * 3 + 1] = pose.keypoints[k].y; + entry.last_keypoints_xyc[k * 3 + 2] = pose.keypoints[k].confidence; + } + } + + EntryCloudEstimate& est = row_estimates[static_cast(i)]; + if (est.valid) { + entry.last_depth = est.depth; + entry.last_depth_confidence = est.depth_confidence; + entry.last_pos_cam = est.pos_cam; + entry.last_pos_world = est.pos_world; + entry.has_last_pos = true; + entry.last_pos_frame = frame_index_; + entry.last_depth_sample_count = est.depth_sample_count; + entry.last_valid_projected_pixels = + std::move(est.valid_projected_pixels); + } + } + + if (debug_info) { + debug_info->reid_attempted = reid_recovery_attempted; + debug_info->reid_3d_gate_rejections = gate_hard_rejections; + debug_info->reid_3d_gate_soft_warnings = gate_soft_warnings; + // depth_ms = per-entry 3D compute (Phase 2A); reid_extract_ms = TRT + // inference (Phase 2B). The outer bind_ms (measured by process()) + // also covers Phase 3/4/5 bookkeeping. + debug_info->depth_ms = depth_compute_ms; + debug_info->reid_extract_ms = reid_extract_ms; + } + return stable_ids; +} + +void TargetObservationProcessor::age_out_tracks() +{ + const int64_t timeout = + static_cast(std::max(1, config_.reid_lost_timeout_frames)); + + // Drop entries unseen for longer than the timeout. If the follow target + // itself ages out, clear current_target_stable_id_ so the next frame can + // bootstrap. + for (auto it = tracks_.begin(); it != tracks_.end();) { + if (frame_index_ - it->second.last_seen_frame > timeout) { + if (it->first == current_target_stable_id_) { + current_target_stable_id_ = -1; + } + it = tracks_.erase(it); + } else { + ++it; + } + } + + // Soft cap: evict least-recently-seen entries (never the follow target). + const int cap = std::max(1, config_.max_concurrent_tracks); + while (static_cast(tracks_.size()) > cap) { + int lru_sid = -1; + int64_t oldest = std::numeric_limits::max(); + for (const auto& kv : tracks_) { + if (kv.first == current_target_stable_id_) { + continue; + } + if (kv.second.last_seen_frame < oldest) { + oldest = kv.second.last_seen_frame; + lru_sid = kv.first; + } + } + if (lru_sid < 0) { + break; + } + tracks_.erase(lru_sid); + } +} + +TargetObservation TargetObservationProcessor::select_follow_target( const std::vector& poses, const Eigen::MatrixXf& tracks, + const std::vector& stable_ids, int image_width, int image_height, TargetObservationDebugInfo* debug_info) { TargetObservation observation; - int best_row = -1; - int best_detection_index = -1; - bool found_existing = false; - bool attempted_reid = false; - const float img_cx = static_cast(image_width) * 0.5f; - const float img_cy = static_cast(image_height) * 0.5f; - const bool has_tracking_history = - current_target_id_ >= 0 || current_raw_track_id_ >= 0 || !target_gallery_.empty(); - const bool can_attempt_reid = - config_.reid_enabled && - reid_extractor_ != nullptr && - reid_extractor_->initialized() && - !target_gallery_.empty(); - - for (int i = 0; i < tracks.rows(); ++i) { - const int track_id = static_cast(tracks(i, 4)); - if (track_id == current_raw_track_id_) { - best_row = i; - found_existing = true; - break; - } + const int row_count = static_cast(tracks.rows()); + if (row_count == 0) { + return observation; } - if (!found_existing && has_tracking_history && can_attempt_reid) { - float best_similarity = -1.0f; - attempted_reid = true; - best_detection_index = recover_target_with_reid(camera_bgr, poses, &best_similarity); - if (best_detection_index >= 0) { - for (int row = 0; row < tracks.rows(); ++row) { - const int detection_index = static_cast(tracks(row, 7)); - if (detection_index == best_detection_index) { - best_row = row; - break; - } + int best_row = -1; + bool found_existing = false; + + // Fast path: follow target is present in this frame's stable_ids. + if (current_target_stable_id_ >= 0) { + for (int i = 0; i < row_count; ++i) { + if (stable_ids[static_cast(i)] == current_target_stable_id_) { + best_row = i; + found_existing = true; + break; } } - if (debug_info) { - debug_info->reid_attempted = true; - debug_info->reid_similarity = best_similarity; - debug_info->recovered_by_reid = (best_detection_index >= 0); - } } - if (best_row < 0 && !has_tracking_history) { + // If the follow target entry is still alive but wasn't seen this frame, + // wait for it to reappear (via raw-ID fast path or ReID) instead of + // hijacking to a different person. age_out_tracks() already cleared + // current_target_stable_id_ when the entry is truly gone. + if (best_row < 0 && current_target_stable_id_ >= 0 && + tracks_.count(current_target_stable_id_) > 0) { + if (debug_info) { + debug_info->found_existing_target = false; + } + return observation; + } + + // Bootstrap: no follow target (never set or just aged out) — pick the + // track closest to image center. + bool bootstrap_selected = false; + if (best_row < 0) { + const float img_cx = static_cast(image_width) * 0.5f; + const float img_cy = static_cast(image_height) * 0.5f; float best_dist = std::numeric_limits::max(); - for (int i = 0; i < tracks.rows(); ++i) { + for (int i = 0; i < row_count; ++i) { const float cx = 0.5f * (tracks(i, 0) + tracks(i, 2)); const float cy = 0.5f * (tracks(i, 1) + tracks(i, 3)); - const float dist = (cx - img_cx) * (cx - img_cx) + (cy - img_cy) * (cy - img_cy); + const float dist = + (cx - img_cx) * (cx - img_cx) + (cy - img_cy) * (cy - img_cy); if (dist < best_dist) { best_dist = dist; best_row = i; } } - if (debug_info) { - debug_info->selected_from_center_bootstrap = (best_row >= 0); + if (best_row >= 0) { + current_target_stable_id_ = stable_ids[static_cast(best_row)]; + bootstrap_selected = true; } } - - if (best_row < 0 && best_detection_index < 0) { - if (debug_info) { - debug_info->found_existing_target = found_existing; - debug_info->reid_attempted = attempted_reid; - } + if (best_row < 0) { return observation; } - int detection_index = -1; - int selected_raw_track_id = -1; - if (best_row >= 0) { - detection_index = static_cast(tracks(best_row, 7)); - selected_raw_track_id = static_cast(tracks(best_row, 4)); - } else { - detection_index = best_detection_index; - } + const int detection_index = static_cast(tracks(best_row, 7)); + const int selected_raw_track_id = static_cast(tracks(best_row, 4)); if (detection_index < 0 || detection_index >= static_cast(poses.size())) { return observation; } + const int selected_sid = stable_ids[static_cast(best_row)]; const auto& pose = poses[static_cast(detection_index)]; observation.valid = true; - if (debug_info) { - debug_info->found_existing_target = found_existing; - debug_info->reid_attempted = attempted_reid; - debug_info->target_selected = true; - } + observation.is_primary_target = true; + observation.track_id = selected_sid; observation.raw_track_id = selected_raw_track_id; - observation.track_id = - (current_target_id_ >= 0) - ? current_target_id_ - : (selected_raw_track_id >= 0 ? selected_raw_track_id : current_target_id_); - if (observation.track_id < 0) { - observation.track_id = selected_raw_track_id; - } observation.detection_index = detection_index; - if (debug_info) { - debug_info->selected_track_id = observation.track_id; - debug_info->selected_raw_track_id = selected_raw_track_id; - debug_info->detection_index = detection_index; - } observation.confidence = pose.conf; observation.bbox_xyxy = { static_cast(pose.box.x), @@ -479,128 +797,35 @@ TargetObservation TargetObservationProcessor::select_target( static_cast(pose.box.x + pose.box.width), static_cast(pose.box.y + pose.box.height), }; - for (size_t i = 0; i < std::min(pose.keypoints.size(), 17); ++i) { observation.keypoints_xyc[i * 3 + 0] = pose.keypoints[i].x; observation.keypoints_xyc[i * 3 + 1] = pose.keypoints[i].y; observation.keypoints_xyc[i * 3 + 2] = pose.keypoints[i].confidence; } + + // Consume the one-shot rebind flag on the follow target's entry. + bool recovered_by_reid = false; + float rebind_sim = -1.0f; + const auto it = tracks_.find(selected_sid); + if (it != tracks_.end()) { + recovered_by_reid = it->second.just_rebound_by_reid; + rebind_sim = it->second.last_rebind_similarity; + it->second.just_rebound_by_reid = false; + it->second.last_rebind_similarity = -1.0f; + } + if (debug_info) { + debug_info->found_existing_target = found_existing; + debug_info->selected_from_center_bootstrap = bootstrap_selected; + debug_info->target_selected = true; + debug_info->selected_track_id = selected_sid; + debug_info->selected_raw_track_id = selected_raw_track_id; + debug_info->detection_index = detection_index; + debug_info->recovered_by_reid = recovered_by_reid; + debug_info->reid_similarity = rebind_sim; + } return observation; } -int TargetObservationProcessor::recover_target_with_reid( - const cv::Mat& camera_bgr, - const std::vector& poses, - float* best_similarity) const -{ - if (best_similarity) { - *best_similarity = -1.0f; - } - if (!config_.reid_enabled || - reid_extractor_ == nullptr || - !reid_extractor_->initialized() || - target_gallery_.empty() || - camera_bgr.empty() || - poses.empty()) { - return -1; - } - - std::vector candidate_indices; - std::vector candidate_boxes; - candidate_indices.reserve(poses.size()); - candidate_boxes.reserve(poses.size()); - for (size_t i = 0; i < poses.size(); ++i) { - const auto& pose = poses[i]; - const cv::Rect bbox( - pose.box.x, - pose.box.y, - std::max(0, pose.box.width), - std::max(0, pose.box.height)); - if (!is_good_reid_crop(bbox, camera_bgr.size())) { - continue; - } - candidate_indices.push_back(static_cast(i)); - candidate_boxes.push_back(bbox); - } - if (candidate_boxes.empty()) { - return -1; - } - - const auto features = reid_extractor_->extract(camera_bgr, candidate_boxes); - int best_row = -1; - float top1 = -1.0f; - float top2 = -1.0f; - for (size_t i = 0; i < features.size(); ++i) { - if (!feature_has_signal(features[i])) { - continue; - } - const float similarity = target_gallery_.best_similarity(features[i]); - if (similarity > top1) { - top2 = top1; - top1 = similarity; - best_row = candidate_indices[i]; - } else if (similarity > top2) { - top2 = similarity; - } - } - - if (best_similarity) { - *best_similarity = top1; - } - if (best_row < 0 || top1 < config_.reid_match_threshold) { - return -1; - } - - const float second = (top2 < 0.0f) ? -1.0f : top2; - if (second >= 0.0f && - (top1 - second) < config_.reid_gap_threshold) { - return -1; - } - return best_row; -} - -void TargetObservationProcessor::update_target_gallery( - const cv::Mat& camera_bgr, - const TargetObservation& target, - bool force_update) -{ - if (!config_.reid_enabled || - reid_extractor_ == nullptr || - !reid_extractor_->initialized() || - !target.valid || - !should_update_gallery(force_update) || - camera_bgr.empty()) { - return; - } - - const int x1 = static_cast(std::floor(target.bbox_xyxy[0])); - const int y1 = static_cast(std::floor(target.bbox_xyxy[1])); - const int x2 = static_cast(std::ceil(target.bbox_xyxy[2])); - const int y2 = static_cast(std::ceil(target.bbox_xyxy[3])); - const cv::Rect bbox(x1, y1, std::max(0, x2 - x1), std::max(0, y2 - y1)); - if (!is_good_reid_crop(bbox, camera_bgr.size())) { - return; - } - - const auto features = reid_extractor_->extract(camera_bgr, {bbox}); - if (features.empty() || !feature_has_signal(features.front())) { - return; - } - target_gallery_.add(features.front(), frame_index_); -} - -bool TargetObservationProcessor::should_update_gallery(bool force_update) const -{ - if (force_update || target_gallery_.empty()) { - return true; - } - if (config_.reid_feature_update_interval <= 0) { - return true; - } - return (frame_index_ - target_gallery_.last_update_frame()) >= - static_cast(config_.reid_feature_update_interval); -} - bool TargetObservationProcessor::is_good_reid_crop( const cv::Rect& bbox, const cv::Size& image_size) const @@ -628,69 +853,53 @@ bool TargetObservationProcessor::is_good_reid_crop( return true; } -TargetObservation TargetObservationProcessor::enrich_target_with_cloud( - const TargetObservation& target, - const std::vector& poses, +TargetObservationProcessor::ProjectedCloud +TargetObservationProcessor::project_cloud_to_image( const pcl::PointCloud& cloud_in_cam, - const CloudReprojector& reprojector, - const CloudReprojector::OdomPose& odom_pose, - TargetObservationDebugInfo* debug_info) const + const CloudReprojector& reprojector) const { - const auto depth_start = std::chrono::steady_clock::now(); - if (!target.valid || - target.detection_index < 0 || - target.detection_index >= static_cast(poses.size()) || - cloud_in_cam.empty()) { - if (debug_info) { - debug_info->depth_ms = - std::chrono::duration(std::chrono::steady_clock::now() - depth_start).count(); - } - return TargetObservation{}; + ProjectedCloud projected; + if (cloud_in_cam.empty()) { + return projected; } - + projected.uv.reserve(cloud_in_cam.size()); + projected.cam.reserve(cloud_in_cam.size()); const auto& cam_params = reprojector.getCameraParams(); - const auto& pose = poses[static_cast(target.detection_index)]; - - std::vector uv_points; - std::vector cam_points; - uv_points.reserve(cloud_in_cam.size()); - cam_points.reserve(cloud_in_cam.size()); - for (const auto& pt : cloud_in_cam) { if (pt.z <= config_.min_depth || pt.z >= config_.max_depth) { continue; } - const Eigen::Vector2d uv = reprojector.projectCameraPointToPixel( Eigen::Vector3d(pt.x, pt.y, pt.z)); if (uv.x() < 0.0 || uv.x() >= cam_params.image_width || uv.y() < 0.0 || uv.y() >= cam_params.image_height) { continue; } - - uv_points.emplace_back( - static_cast(uv.x()), - static_cast(uv.y())); - cam_points.emplace_back(pt.x, pt.y, pt.z); - } - if (debug_info) { - debug_info->projected_cloud_points = static_cast(cam_points.size()); + projected.uv.emplace_back( + static_cast(uv.x()), static_cast(uv.y())); + projected.cam.emplace_back(pt.x, pt.y, pt.z); } + return projected; +} - if (uv_points.empty()) { - if (debug_info) { - debug_info->depth_ms = - std::chrono::duration(std::chrono::steady_clock::now() - depth_start).count(); - } - return TargetObservation{}; +TargetObservationProcessor::EntryCloudEstimate +TargetObservationProcessor::compute_entry_3d( + const yolos::pose::PoseResult& pose, + const std::array& bbox_xyxy, + const ProjectedCloud& projected, + const CloudReprojector& reprojector, + const CloudReprojector::OdomPose& odom_pose) const +{ + EntryCloudEstimate est; + if (projected.uv.empty()) { + return est; } std::vector per_keypoint_depths; std::vector per_keypoint_weights; per_keypoint_depths.reserve(kUpperTorsoKeypointIndices.size()); per_keypoint_weights.reserve(kUpperTorsoKeypointIndices.size()); - std::vector valid_projected_pixels; - valid_projected_pixels.reserve(128); + est.valid_projected_pixels.reserve(128); for (const int kp_index : kUpperTorsoKeypointIndices) { if (kp_index >= static_cast(pose.keypoints.size())) { @@ -700,19 +909,16 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud( if (kp.confidence < 0.5f) { continue; } - std::vector nearby_depths; nearby_depths.reserve(32); - for (size_t i = 0; i < uv_points.size(); ++i) { - const float du = uv_points[i].x - kp.x; - const float dv = uv_points[i].y - kp.y; + for (size_t i = 0; i < projected.uv.size(); ++i) { + const float du = projected.uv[i].x - kp.x; + const float dv = projected.uv[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(std::lround(uv_points[i].x)), - static_cast(std::lround(uv_points[i].y))); - } + nearby_depths.push_back(projected.cam[i].z()); + est.valid_projected_pixels.emplace_back( + static_cast(std::lround(projected.uv[i].x)), + static_cast(std::lround(projected.uv[i].y))); } } if (nearby_depths.size() < 3) { @@ -721,17 +927,10 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud( per_keypoint_depths.push_back(median_in_place(nearby_depths)); per_keypoint_weights.push_back(kp.confidence); } - if (debug_info) { - debug_info->depth_sample_count = static_cast(per_keypoint_depths.size()); - debug_info->valid_projected_pixels = std::move(valid_projected_pixels); - } + est.depth_sample_count = static_cast(per_keypoint_depths.size()); if (per_keypoint_depths.empty()) { - if (debug_info) { - debug_info->depth_ms = - std::chrono::duration(std::chrono::steady_clock::now() - depth_start).count(); - } - return TargetObservation{}; + return est; } // MAD rejection uses the unweighted median as the robust center (classic @@ -763,9 +962,8 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud( } } - TargetObservation enriched = target; - enriched.depth = weighted_median(per_keypoint_depths, per_keypoint_weights); - enriched.depth_confidence = std::min( + est.depth = weighted_median(per_keypoint_depths, per_keypoint_weights); + est.depth_confidence = std::min( 1.0f, static_cast(per_keypoint_depths.size()) / static_cast(kUpperTorsoKeypointIndices.size())); @@ -785,10 +983,10 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud( } } if (valid_torso_pixels.empty()) { - const float center_u = 0.5f * (target.bbox_xyxy[0] + target.bbox_xyxy[2]); - const float center_v = 0.5f * (target.bbox_xyxy[1] + target.bbox_xyxy[3]); - enriched.target_pos_cam = reprojector.pixelToCameraPoint( - center_u, center_v, enriched.depth).cast(); + const float center_u = 0.5f * (bbox_xyxy[0] + bbox_xyxy[2]); + const float center_v = 0.5f * (bbox_xyxy[1] + bbox_xyxy[3]); + est.pos_cam = reprojector.pixelToCameraPoint( + center_u, center_v, est.depth).cast(); } else { Eigen::Vector2f weighted_sum = Eigen::Vector2f::Zero(); float weight_total = 0.0f; @@ -798,28 +996,93 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud( } const Eigen::Vector2f mean_pixel = weighted_sum / std::max(weight_total, 1e-6f); - enriched.target_pos_cam = reprojector.pixelToCameraPoint( - mean_pixel.x(), mean_pixel.y(), enriched.depth).cast(); + est.pos_cam = reprojector.pixelToCameraPoint( + mean_pixel.x(), mean_pixel.y(), est.depth).cast(); } - if (enriched.target_pos_cam.z() <= 0.0f) { - if (debug_info) { - debug_info->depth_ms = - std::chrono::duration(std::chrono::steady_clock::now() - depth_start).count(); - } + if (est.pos_cam.z() <= 0.0f) { + est.valid = false; + return est; + } + + est.pos_world = reprojector.cameraPointToWorld( + est.pos_cam.cast(), odom_pose).cast(); + est.valid = true; + return est; +} + +TargetObservation TargetObservationProcessor::enrich_target_with_cloud( + const TargetObservation& target, + TargetObservationDebugInfo* debug_info) const +{ + // Heavy depth / pos estimation is now performed once per frame for every + // active entry inside bind_and_update_tracks; debug_info->depth_ms is set + // there. Here we only copy the follow target's cached 3D state onto the + // outgoing TargetObservation. + if (!target.valid) { + return target; + } + const auto it = tracks_.find(target.track_id); + if (it == tracks_.end() || + !it->second.has_last_pos || + it->second.last_pos_frame != frame_index_) { + // Follow target has no fresh 3D estimate for this frame — report as + // invalid so the caller skips publishing stale data. return TargetObservation{}; } - enriched.target_pos_world = reprojector.cameraPointToWorld( - enriched.target_pos_cam.cast(), odom_pose).cast(); + const TrackEntry& entry = it->second; + TargetObservation enriched = target; + enriched.depth = entry.last_depth; + enriched.depth_confidence = entry.last_depth_confidence; + enriched.detection_pos_cam = entry.last_pos_cam; + enriched.detection_pos_world = entry.last_pos_world; + enriched.is_primary_target = true; enriched.valid = true; if (debug_info) { - debug_info->depth_ms = - std::chrono::duration(std::chrono::steady_clock::now() - depth_start).count(); + debug_info->depth_sample_count = entry.last_depth_sample_count; + debug_info->valid_projected_pixels = entry.last_valid_projected_pixels; } return enriched; } +std::vector +TargetObservationProcessor::snapshot_active_tracks() const +{ + std::vector out; + out.reserve(tracks_.size()); + for (const auto& kv : tracks_) { + const TrackEntry& entry = kv.second; + // Only emit tracks with a 3D estimate computed in the current frame. + // Transient-lost entries (not seen this frame) are skipped so stale + // positions don't leak into the multi-target topic. + if (!entry.has_last_pos || entry.last_pos_frame != frame_index_) { + continue; + } + TargetObservation obs; + obs.valid = true; + obs.track_id = entry.stable_id; + obs.raw_track_id = entry.last_raw_track_id; + obs.detection_index = entry.last_detection_index; + obs.is_primary_target = (entry.stable_id == current_target_stable_id_); + obs.confidence = entry.last_confidence; + obs.depth = entry.last_depth; + obs.depth_confidence = entry.last_depth_confidence; + obs.bbox_xyxy = { + static_cast(entry.last_bbox.x), + static_cast(entry.last_bbox.y), + static_cast(entry.last_bbox.x + entry.last_bbox.width), + static_cast(entry.last_bbox.y + entry.last_bbox.height), + }; + obs.keypoints_xyc = entry.last_keypoints_xyc; + obs.detection_pos_cam = entry.last_pos_cam; + obs.detection_pos_world = entry.last_pos_world; + out.push_back(std::move(obs)); + } + return out; +} + + void draw_target_observation_overlay( cv::Mat& image_bgr, const TargetObservation& observation)