确定是TrackGallery的问题,fix

主要是两个问题： 1. trackgallery只有在有确定sid是才更新新的feature；2. sid应该永远按身份保留，不是顺序顺延

include/cloud_reprojection_ros_node.hpp

@@ -125,6 +125,7 @@ private:
     std::unique_ptr<odin_ros_driver::TargetObservationProcessor> target_observation_processor_;
     bool enable_target_observation_ = false;
     bool debug_target_observation_ = false;
+    bool debug_reid_ = false;
 #endif
 
     void loadParameters();

include/target_observation_processing.hpp

@@ -24,6 +24,16 @@ struct TargetObservationConfig {
     std::string yolo_labels_path;
     float yolo_conf = 0.45f;
     float yolo_nms = 0.50f;
+    // Optional detection ROI applied to every frame before the YOLO call.
+    // Trades coverage (edge strips are ignored) for effective resolution on
+    // the subjects inside the ROI. Width/height are in image pixels; the
+    // ROI is clamped to the image and silently disabled if width*height
+    // covers the full image. Horizontal placement is always centered. The
+    // vertical offset is pixels from the image top; set to -1 to auto-center.
+    bool detection_roi_enabled = false;
+    int detection_roi_width_px = 1440;
+    int detection_roi_height_px = 1080;
+    int detection_roi_y_offset_px = 0;
     float min_depth = 0.5f;
     float max_depth = 12.0f;
     float search_radius_px = 25.0f;
@@ -38,23 +48,17 @@ struct TargetObservationConfig {
     float reid_max_crop_aspect_ratio = 0.9f;
     int reid_feature_update_interval = 5;
     int reid_lost_timeout_frames = 150;
-    int reid_gallery_size = 10;
+    int reid_gallery_size = 8;
     int reid_input_height = 256;
     int reid_input_width = 128;
     int reid_feature_dim = 512;
     int reid_max_batch_size = 8;
-    int max_concurrent_tracks = 16;
+    int max_concurrent_tracks = 8;
     // Edge margin (in pixels) that is_good_reid_crop requires between the
     // bbox and every image border. Set to 0 to allow crops touching the edge
     // (common when the person stands close to the camera and the head gets
     // cut off). Legacy value was 3.
     int reid_crop_edge_margin_px = 0;
-    // 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;
     // 3D-proximity fallback: when a row lacks a ReID feature (e.g. bbox was
     // too close to the edge for is_good_reid_crop to pass) but has a fresh
     // 3D estimate, rebind it to the follow target's stable_id if it lies
@@ -64,7 +68,9 @@ struct TargetObservationConfig {
     bool reid_3d_fallback_enabled = true;
     float reid_3d_fallback_base_m = 0.75f;
     float reid_3d_fallback_per_frame_m = 0.05f;
+    int reid_confirm_hits_before_allocate = 5;
     bool debug = false;
+    bool debug_reid = false;
 };
 
 struct TargetObservation {
@@ -105,7 +111,8 @@ struct TargetObservationDebugInfo {
     bool recovered_by_reid = false;
     bool lost_detection_compensation_attempted = false;
     bool recovered_by_lost_detection_compensation = false;
-    float reid_similarity = -1.0f;
+    float reid_similarity = -1.0f;          // cosine sim from Phase 3 ReID (≥0), or -1 if N/A
+    float reid_3d_fallback_distance = -1.0f; // metres from Phase 3.5, or -1 if N/A
     float yolo_ms = 0.0f;
     float mot_ms = 0.0f;
     float cloud_project_ms = 0.0f;
@@ -118,6 +125,23 @@ struct TargetObservationDebugInfo {
     float total_ms = 0.0f;
     std::vector<yolos::pose::PoseResult> poses;
     std::vector<cv::Point> valid_projected_pixels;
+    // Per-track-row snapshot from bind_and_update_tracks (aligned, one entry
+    // per row of the tracks matrix from ByteTrack). raw_track_ids are what
+    // ByteTrack emitted for this frame; row_stable_ids are what our
+    // TrackManager ultimately bound each row to. Both empty when the
+    // pipeline bailed before binding.
+    std::vector<int> raw_track_ids;
+    std::vector<int> row_stable_ids;
+    std::vector<int> row_detection_indices;
+    // Center-cropped ROI actually used for the primary YOLO pass this frame.
+    // Empty (area()==0) when config_.detection_roi_enabled is false or when
+    // the ROI was clamped away to nothing.
+    cv::Rect detection_roi;
+    // Flat record of every Phase 3 ReID similarity comparison that actually
+    // ran this frame. Tuple is (row_raw_track_id, candidate_stable_id, sim).
+    // Useful for debugging "why didn't this rebind" cases — read the log
+    // and you see each candidate's sim against each active gallery.
+    std::vector<std::tuple<int, int, float>> reid_comparisons;
 };
 
 class TargetObservationProcessor {
@@ -138,6 +162,8 @@ public:
     // marks the follow target. Empty if process() hasn't run yet.
     std::vector<TargetObservation> snapshot_active_tracks() const;
     int current_target_stable_id() const { return current_target_stable_id_; }
+    int current_frame_primary_stable_id() const { return last_selected_primary_stable_id_; }
+    cv::Rect last_detection_roi() const { return last_detection_roi_; }
 
     bool initialized() const { return initialized_; }
 
@@ -170,7 +196,8 @@ private:
         int64_t last_seen_frame = -1;
         bool just_rebound_by_reid = false;
         bool just_rebound_by_3d_fallback = false;
-        float last_rebind_similarity = -1.0f;
+        float last_rebind_similarity = -1.0f;   // cosine sim from ReID (≥0), -1 = none
+        float last_rebind_3d_distance = -1.0f;  // metres from 3D fallback, -1 = none
         // Per-entry 3D state; refreshed whenever compute_entry_3d succeeds.
         float last_depth = -1.0f;
         float last_depth_confidence = 0.0f;
@@ -198,6 +225,7 @@ private:
     cv::Rect build_lost_detection_roi(
         const cv::Size& image_size,
         const cv::Rect& last_bbox) const;
+    cv::Rect build_detection_roi(const cv::Size& image_size) const;
     static void offset_poses(
         std::vector<yolos::pose::PoseResult>& poses,
         const cv::Point& offset);
@@ -243,10 +271,18 @@ private:
     std::unique_ptr<motcpp::trackers::ByteTrack> tracker_;
     std::unique_ptr<ReIDTensorRTExtractor> reid_extractor_;
 
+    struct PendingRaw {
+        int consecutive_hits = 0;
+        int64_t last_hit_frame = -1;
+    };
+
     std::unordered_map<int, TrackEntry> tracks_;
+    std::unordered_map<int, PendingRaw> pending_raw_ids_;
     int next_stable_id_ = 0;
     int current_target_stable_id_ = -1;
+    int last_selected_primary_stable_id_ = -1;
     int64_t frame_index_ = 0;
+    cv::Rect last_detection_roi_;
     bool initialized_ = false;
 };