fix minor bugs in REID re-track; add detection lost then detect mechanism;

lost了会在附近的pixel截取放大再做yolo，还不行就算了

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

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<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;
+    static void offset_poses(
+        std::vector<yolos::pose::PoseResult>& 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<yolos::pose::PoseResult>& 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;

src/cloud_reprojection_ros.cpp

@@ -221,6 +221,9 @@ void CloudReprojectionRosNode::loadParameters()
     this->declare_parameter<double>("register_keys.target_min_depth", 0.5);
     this->declare_parameter<double>("register_keys.target_max_depth", 12.0);
     this->declare_parameter<double>("register_keys.target_search_radius_px", 25.0);
+    this->declare_parameter<int>("register_keys.target_lost_detection_compensation_enable", 1);
+    this->declare_parameter<double>("register_keys.target_lost_detection_roi_scale", 2.0);
+    this->declare_parameter<int>("register_keys.target_lost_detection_roi_min_size_px", 192);
     this->declare_parameter<int>("register_keys.target_reid_enable", 0);
     this->declare_parameter<std::string>("register_keys.target_reid_engine", "");
     this->declare_parameter<double>("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<float>(
                 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<float>(
+                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() ? "<default-person>" : 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() ? "<disabled>" : 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
 

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<size_t>(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<yolos::pose::PoseResult> 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<int>(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<int>(std::lround(target.bbox_xyxy[0])),
+            static_cast<int>(std::lround(target.bbox_xyxy[1])),
+            std::max(0, static_cast<int>(std::lround(target.bbox_xyxy[2] - target.bbox_xyxy[0]))),
+            std::max(0, static_cast<int>(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<yolos::pose::PoseResult> 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<float>(last_target_bbox_.x) + 0.5f * static_cast<float>(last_target_bbox_.width);
+    const float cy = static_cast<float>(last_target_bbox_.y) + 0.5f * static_cast<float>(last_target_bbox_.height);
+    const float roi_w = std::max(
+        static_cast<float>(config_.lost_detection_roi_min_size_px),
+        static_cast<float>(last_target_bbox_.width) * scale);
+    const float roi_h = std::max(
+        static_cast<float>(config_.lost_detection_roi_min_size_px),
+        static_cast<float>(last_target_bbox_.height) * scale);
+
+    const int x = static_cast<int>(std::floor(cx - 0.5f * roi_w));
+    const int y = static_cast<int>(std::floor(cy - 0.5f * roi_h));
+    const int w = static_cast<int>(std::ceil(roi_w));
+    const int h = static_cast<int>(std::ceil(roi_h));
+    return clamp_rect_to_image(cv::Rect(x, y, w, h), image_size);
+}
+
+void TargetObservationProcessor::offset_poses(
+    std::vector<yolos::pose::PoseResult>& 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<float>(offset.x);
+            keypoint.y += static_cast<float>(offset.y);
+        }
+    }
+}
+
 void TargetObservationProcessor::draw_detected_poses(
     cv::Mat& image_bgr,
     const std::vector<yolos::pose::PoseResult>& 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<float>(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<int>(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<int>(tracks(best_row, 7));
+    int detection_index = -1;
+    int selected_raw_track_id = -1;
+    if (best_row >= 0) {
+        detection_index = static_cast<int>(tracks(best_row, 7));
+        selected_raw_track_id = static_cast<int>(tracks(best_row, 4));
+    } else {
+        detection_index = best_detection_index;
+    }
     if (detection_index < 0 ||
         detection_index >= static_cast<int>(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<int>(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<yolos::pose::PoseResult>& 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<int> candidate_rows;
+    std::vector<int> candidate_indices;
     std::vector<cv::Rect> candidate_boxes;
-    candidate_rows.reserve(static_cast<size_t>(tracks.rows()));
-    candidate_boxes.reserve(static_cast<size_t>(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<int>(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;
         }