diff --git a/config/control_command.yaml b/config/control_command.yaml index bfb7a28..7030c45 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -91,6 +91,10 @@ register_keys: debug: 1 # lazy ReID only runs after the current ByteTrack target is lost # first valid frame still bootstraps by choosing the target closest to image center + # if a frame misses all detections, retry YOLO once inside a local ROI around the previous target bbox + target_lost_detection_compensation_enable: 1 + target_lost_detection_roi_scale: 2.0 + target_lost_detection_roi_min_size_px: 192 target_reid_enable: 1 target_reid_engine: "/home/hjy/HJY/Codes/AMP_ws/TargetPrediction/models/target_reid_osnet_x0_25_dukemtmcreid.trt" target_reid_match_threshold: 0.65 diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp index 96f6611..92cb02e 100644 --- a/include/target_observation_processing.hpp +++ b/include/target_observation_processing.hpp @@ -26,6 +26,9 @@ struct TargetObservationConfig { float min_depth = 0.5f; float max_depth = 12.0f; float search_radius_px = 25.0f; + bool lost_detection_compensation_enabled = true; + float lost_detection_roi_scale = 2.0f; + int lost_detection_roi_min_size_px = 192; bool reid_enabled = false; std::string reid_engine_path; float reid_match_threshold = 0.65f; @@ -73,6 +76,8 @@ struct TargetObservationDebugInfo { bool selected_from_center_bootstrap = false; bool reid_attempted = false; bool recovered_by_reid = false; + bool lost_detection_compensation_attempted = false; + bool recovered_by_lost_detection_compensation = false; float reid_similarity = -1.0f; float yolo_ms = 0.0f; float mot_ms = 0.0f; @@ -104,6 +109,13 @@ public: private: 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; + static void offset_poses( + std::vector& poses, + const cv::Point& offset); TargetObservation select_target( const cv::Mat& camera_bgr, @@ -123,7 +135,7 @@ private: int recover_target_with_reid( const cv::Mat& camera_bgr, - const Eigen::MatrixXf& tracks, + const std::vector& poses, float* best_similarity) const; void update_target_gallery( const cv::Mat& camera_bgr, @@ -140,6 +152,8 @@ private: 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; int64_t frame_index_ = 0; int lost_track_frames_ = 0; bool initialized_ = false; diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index 511a3b9..8d22de5 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -221,6 +221,9 @@ void CloudReprojectionRosNode::loadParameters() this->declare_parameter("register_keys.target_min_depth", 0.5); this->declare_parameter("register_keys.target_max_depth", 12.0); this->declare_parameter("register_keys.target_search_radius_px", 25.0); + this->declare_parameter("register_keys.target_lost_detection_compensation_enable", 1); + this->declare_parameter("register_keys.target_lost_detection_roi_scale", 2.0); + this->declare_parameter("register_keys.target_lost_detection_roi_min_size_px", 192); this->declare_parameter("register_keys.target_reid_enable", 0); this->declare_parameter("register_keys.target_reid_engine", ""); this->declare_parameter("register_keys.target_reid_match_threshold", 0.65); @@ -358,6 +361,12 @@ void CloudReprojectionRosNode::loadParameters() this->get_parameter("register_keys.target_max_depth").as_double()); target_config.search_radius_px = static_cast( this->get_parameter("register_keys.target_search_radius_px").as_double()); + target_config.lost_detection_compensation_enabled = + (this->get_parameter("register_keys.target_lost_detection_compensation_enable").as_int() != 0); + target_config.lost_detection_roi_scale = static_cast( + this->get_parameter("register_keys.target_lost_detection_roi_scale").as_double()); + target_config.lost_detection_roi_min_size_px = + this->get_parameter("register_keys.target_lost_detection_roi_min_size_px").as_int(); target_config.reid_enabled = (this->get_parameter("register_keys.target_reid_enable").as_int() != 0); target_config.reid_engine_path = @@ -401,7 +410,7 @@ void CloudReprojectionRosNode::loadParameters() if (debug_target_observation_) { RCLCPP_INFO( this->get_logger(), - "Target observation debug enabled | yolo_engine=%s | yolo_labels=%s | yolo_conf=%.3f | yolo_nms=%.3f | min_depth=%.2f | max_depth=%.2f | search_radius_px=%.1f | reid=%s | reid_engine=%s | reid_match=%.3f | reid_gap=%.3f | reid_gallery=%d | reid_update_interval=%d | reid_timeout=%d", + "Target observation debug enabled | yolo_engine=%s | yolo_labels=%s | yolo_conf=%.3f | yolo_nms=%.3f | min_depth=%.2f | max_depth=%.2f | search_radius_px=%.1f | lost_comp=%s | lost_comp_scale=%.2f | lost_comp_min=%d | reid=%s | reid_engine=%s | reid_match=%.3f | reid_gap=%.3f | reid_gallery=%d | reid_update_interval=%d | reid_timeout=%d", target_config.yolo_engine_path.c_str(), target_config.yolo_labels_path.empty() ? "" : target_config.yolo_labels_path.c_str(), target_config.yolo_conf, @@ -409,6 +418,9 @@ void CloudReprojectionRosNode::loadParameters() target_config.min_depth, target_config.max_depth, target_config.search_radius_px, + target_config.lost_detection_compensation_enabled ? "on" : "off", + target_config.lost_detection_roi_scale, + target_config.lost_detection_roi_min_size_px, target_config.reid_enabled ? "on" : "off", target_config.reid_engine_path.empty() ? "" : target_config.reid_engine_path.c_str(), target_config.reid_match_threshold, @@ -595,13 +607,15 @@ void CloudReprojectionRosNode::syncCallback( 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 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 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_debug.poses_count, target_debug.tracks_count, target_debug.selected_track_id, target_debug.selected_raw_track_id, target_debug.detection_index, target_debug.selected_from_center_bootstrap ? "yes" : "no", + target_debug.lost_detection_compensation_attempted ? "yes" : "no", + target_debug.recovered_by_lost_detection_compensation ? "yes" : "no", target_debug.reid_attempted ? "yes" : "no", target_debug.recovered_by_reid ? "yes" : "no", target_debug.reid_similarity, @@ -619,7 +633,7 @@ void CloudReprojectionRosNode::syncCallback( 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 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 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", image_msg->header.stamp.sec, image_msg->header.stamp.nanosec, cam_bgr.cols, @@ -634,6 +648,8 @@ void CloudReprojectionRosNode::syncCallback( target_observation.detection_index, target_debug.found_existing_target ? "yes" : "no", target_debug.selected_from_center_bootstrap ? "yes" : "no", + target_debug.lost_detection_compensation_attempted ? "yes" : "no", + target_debug.recovered_by_lost_detection_compensation ? "yes" : "no", target_debug.reid_attempted ? "yes" : "no", target_debug.recovered_by_reid ? "yes" : "no", target_debug.reid_similarity, @@ -661,12 +677,12 @@ void CloudReprojectionRosNode::syncCallback( } } + 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) { - 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; observation_msg.track_id = target_observation.track_id; observation_msg.detection_index = target_observation.detection_index; observation_msg.confidence = target_observation.confidence; @@ -680,30 +696,56 @@ void CloudReprojectionRosNode::syncCallback( 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(); - target_observation_pub_->publish(observation_msg); + } 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; - 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; + 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(); - target_pos_cam_pub_->publish(pos_cam_msg); + } else { + pos_cam_msg.point.x = -1.0; + pos_cam_msg.point.y = -1.0; + pos_cam_msg.point.z = -1.0; + } + target_pos_cam_pub_->publish(pos_cam_msg); - geometry_msgs::msg::PointStamped pos_world_msg; - pos_world_msg.header = image_msg->header; - pos_world_msg.header.stamp = sync_stamp; - pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty() - ? "odom" - : odom_msg->header.frame_id; + geometry_msgs::msg::PointStamped pos_world_msg; + pos_world_msg.header = image_msg->header; + pos_world_msg.header.stamp = sync_stamp; + pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty() + ? "odom" + : odom_msg->header.frame_id; + 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(); - target_pos_world_pub_->publish(pos_world_msg); + } 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); } #endif diff --git a/src/target_observation_processing.cpp b/src/target_observation_processing.cpp index d75b008..7dd1bc9 100644 --- a/src/target_observation_processing.cpp +++ b/src/target_observation_processing.cpp @@ -58,6 +58,11 @@ std::string format_target_text(const TargetObservation& observation) return oss.str(); } +cv::Rect clamp_rect_to_image(const cv::Rect& rect, const cv::Size& image_size) +{ + return rect & cv::Rect(0, 0, image_size.width, image_size.height); +} + } // namespace void TargetObservationProcessor::initialize(const TargetObservationConfig& config) @@ -79,6 +84,8 @@ void TargetObservationProcessor::initialize(const TargetObservationConfig& confi 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; frame_index_ = 0; lost_track_frames_ = 0; reid_extractor_.reset(); @@ -128,6 +135,8 @@ TargetObservation TargetObservationProcessor::process( 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(); } @@ -142,7 +151,11 @@ TargetObservation TargetObservationProcessor::process( } const auto yolo_start = std::chrono::steady_clock::now(); - const auto poses = yolo_->detect(camera_bgr, config_.yolo_conf, config_.yolo_nms); + std::vector poses = + yolo_->detect(camera_bgr, config_.yolo_conf, config_.yolo_nms); + if (poses.empty()) { + poses = detect_poses_with_lost_compensation(camera_bgr, debug_info); + } const auto yolo_end = std::chrono::steady_clock::now(); if (debug_info) { debug_info->poses_count = static_cast(poses.size()); @@ -181,6 +194,14 @@ TargetObservation TargetObservationProcessor::process( 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 && @@ -202,6 +223,73 @@ TargetObservation TargetObservationProcessor::process( return target; } +std::vector TargetObservationProcessor::detect_poses_with_lost_compensation( + const cv::Mat& camera_bgr, + TargetObservationDebugInfo* debug_info) const +{ + if (!config_.lost_detection_compensation_enabled || !has_last_target_bbox_ || camera_bgr.empty()) { + return {}; + } + + const cv::Rect roi = build_lost_detection_roi(camera_bgr.size()); + if (roi.width <= 1 || roi.height <= 1) { + return {}; + } + + if (debug_info) { + debug_info->lost_detection_compensation_attempted = true; + } + + const cv::Mat roi_bgr = camera_bgr(roi).clone(); + auto poses = yolo_->detect(roi_bgr, config_.yolo_conf, config_.yolo_nms); + if (poses.empty()) { + return {}; + } + + offset_poses(poses, roi.tl()); + if (debug_info) { + debug_info->recovered_by_lost_detection_compensation = true; + } + return poses; +} + +cv::Rect TargetObservationProcessor::build_lost_detection_roi(const cv::Size& image_size) const +{ + if (!has_last_target_bbox_ || 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 roi_w = std::max( + static_cast(config_.lost_detection_roi_min_size_px), + static_cast(last_target_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); + + const int x = static_cast(std::floor(cx - 0.5f * roi_w)); + const int y = static_cast(std::floor(cy - 0.5f * roi_h)); + const int w = static_cast(std::ceil(roi_w)); + const int h = static_cast(std::ceil(roi_h)); + return clamp_rect_to_image(cv::Rect(x, y, w, h), image_size); +} + +void TargetObservationProcessor::offset_poses( + std::vector& poses, + const cv::Point& offset) +{ + for (auto& pose : poses) { + pose.box.x += offset.x; + pose.box.y += offset.y; + for (auto& keypoint : pose.keypoints) { + keypoint.x += static_cast(offset.x); + keypoint.y += static_cast(offset.y); + } + } +} + void TargetObservationProcessor::draw_detected_poses( cv::Mat& image_bgr, const std::vector& poses) const @@ -250,6 +338,7 @@ TargetObservation TargetObservationProcessor::select_target( { 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; @@ -274,11 +363,20 @@ TargetObservation TargetObservationProcessor::select_target( if (!found_existing && has_tracking_history && can_attempt_reid) { float best_similarity = -1.0f; attempted_reid = true; - best_row = recover_target_with_reid(camera_bgr, tracks, &best_similarity); + 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; + } + } + } if (debug_info) { debug_info->reid_attempted = true; debug_info->reid_similarity = best_similarity; - debug_info->recovered_by_reid = (best_row >= 0); + debug_info->recovered_by_reid = (best_detection_index >= 0); } } @@ -298,7 +396,7 @@ TargetObservation TargetObservationProcessor::select_target( } } - if (best_row < 0) { + if (best_row < 0 && best_detection_index < 0) { if (debug_info) { debug_info->found_existing_target = found_existing; debug_info->reid_attempted = attempted_reid; @@ -306,7 +404,14 @@ TargetObservation TargetObservationProcessor::select_target( return observation; } - const int detection_index = static_cast(tracks(best_row, 7)); + 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; + } if (detection_index < 0 || detection_index >= static_cast(poses.size())) { return observation; @@ -319,9 +424,14 @@ TargetObservation TargetObservationProcessor::select_target( debug_info->reid_attempted = attempted_reid; debug_info->target_selected = true; } - const int selected_raw_track_id = static_cast(tracks(best_row, 4)); observation.raw_track_id = selected_raw_track_id; - observation.track_id = (current_target_id_ >= 0) ? current_target_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; @@ -346,7 +456,7 @@ TargetObservation TargetObservationProcessor::select_target( int TargetObservationProcessor::recover_target_with_reid( const cv::Mat& camera_bgr, - const Eigen::MatrixXf& tracks, + const std::vector& poses, float* best_similarity) const { if (best_similarity) { @@ -357,20 +467,25 @@ int TargetObservationProcessor::recover_target_with_reid( !reid_extractor_->initialized() || target_gallery_.empty() || camera_bgr.empty() || - tracks.rows() == 0) { + poses.empty()) { return -1; } - std::vector candidate_rows; + std::vector candidate_indices; std::vector candidate_boxes; - candidate_rows.reserve(static_cast(tracks.rows())); - candidate_boxes.reserve(static_cast(tracks.rows())); - for (int row = 0; row < tracks.rows(); ++row) { - const cv::Rect bbox = track_row_to_rect(tracks, row); + 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_rows.push_back(row); + candidate_indices.push_back(static_cast(i)); candidate_boxes.push_back(bbox); } if (candidate_boxes.empty()) { @@ -389,7 +504,7 @@ int TargetObservationProcessor::recover_target_with_reid( if (similarity > top1) { top2 = top1; top1 = similarity; - best_row = candidate_rows[i]; + best_row = candidate_indices[i]; } else if (similarity > top2) { top2 = similarity; }