add lazy REID in code, better tracking to one target

就是yolo这一帧没检测到目标,因为正常来说前后的detections都是2,他这时候只是1,确实就是没检测到

include/cloud_reprojection_ros_node.hpp

@@ -88,6 +88,7 @@ private:
     std::string sync_wiwc_topic_;
     std::string sync_image_topic_;
     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_;
@@ -99,6 +100,7 @@ private:
     rclcpp::Publisher<Image>::SharedPtr sync_image_pub_;
 
     rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_;  // optional if send_overlay_
+    rclcpp::Publisher<CompressedImage>::SharedPtr detection_debug_compressed_pub_;  // debug detections/tracks
     rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;  // optional if publish_combined_compressed_
     rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_cam_pub_;

include/reid_trt_extractor.hpp

@@ -0,0 +1,65 @@
+#pragma once
+
+#include <cstdint>
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <opencv2/core.hpp>
+
+namespace odin_ros_driver {
+
+using ReIDFeature = std::vector<float>;
+
+struct ReIDTensorRTConfig {
+    std::string engine_path;
+    int input_height = 256;
+    int input_width = 128;
+    int feature_dim = 512;
+    int max_batch_size = 8;
+};
+
+class TrackGallery {
+public:
+    explicit TrackGallery(size_t max_features = 10);
+
+    void clear();
+    void add(const ReIDFeature& feature, int64_t frame_index);
+
+    bool empty() const { return features_.empty(); }
+    size_t size() const { return features_.size(); }
+    int64_t last_update_frame() const { return last_update_frame_; }
+
+    float best_similarity(const ReIDFeature& query) const;
+
+private:
+    std::deque<ReIDFeature> features_;
+    size_t max_features_ = 10;
+    int64_t last_update_frame_ = -1;
+};
+
+class ReIDTensorRTExtractor {
+public:
+    ReIDTensorRTExtractor();
+    ~ReIDTensorRTExtractor();
+
+    ReIDTensorRTExtractor(const ReIDTensorRTExtractor&) = delete;
+    ReIDTensorRTExtractor& operator=(const ReIDTensorRTExtractor&) = delete;
+
+    void initialize(const ReIDTensorRTConfig& config);
+    bool initialized() const { return initialized_; }
+    int feature_dim() const;
+
+    std::vector<ReIDFeature> extract(
+        const cv::Mat& frame_bgr,
+        const std::vector<cv::Rect>& boxes) const;
+
+private:
+    struct Impl;
+    std::unique_ptr<Impl> impl_;
+    ReIDTensorRTConfig config_;
+    bool initialized_ = false;
+};
+
+}  // namespace odin_ros_driver

include/target_observation_processing.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <array>
+#include <cstdint>
 #include <memory>
 #include <string>
 #include <vector>
@@ -11,6 +12,7 @@
 #include <pcl/point_types.h>
 
 #include "cloud_reprojector.hpp"
+#include "reid_trt_extractor.hpp"
 #include "motcpp/trackers/bytetrack.hpp"
 #include "yolos/tasks/pose.hpp"
 
@@ -24,12 +26,26 @@ struct TargetObservationConfig {
     float min_depth = 0.5f;
     float max_depth = 12.0f;
     float search_radius_px = 25.0f;
+    bool reid_enabled = false;
+    std::string reid_engine_path;
+    float reid_match_threshold = 0.65f;
+    float reid_gap_threshold = 0.10f;
+    float reid_min_crop_area = 2000.0f;
+    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_input_height = 256;
+    int reid_input_width = 128;
+    int reid_feature_dim = 512;
+    int reid_max_batch_size = 8;
     bool debug = false;
 };
 
 struct TargetObservation {
     bool valid = false;
     int track_id = -1;
+    int raw_track_id = -1;
     int detection_index = -1;
     float confidence = 0.0f;
     float depth = -1.0f;
@@ -42,14 +58,22 @@ struct TargetObservation {
 
 struct TargetObservationDebugInfo {
     int current_target_id_before = -1;
+    int current_raw_track_id_before = -1;
     int poses_count = 0;
     int tracks_count = 0;
     int selected_track_id = -1;
+    int selected_raw_track_id = -1;
     int detection_index = -1;
     int projected_cloud_points = 0;
     int depth_sample_count = 0;
+    int gallery_size = 0;
+    int lost_frames = 0;
     bool found_existing_target = false;
     bool target_selected = false;
+    bool selected_from_center_bootstrap = false;
+    bool reid_attempted = false;
+    bool recovered_by_reid = false;
+    float reid_similarity = -1.0f;
     float yolo_ms = 0.0f;
     float mot_ms = 0.0f;
     float depth_ms = 0.0f;
@@ -82,6 +106,7 @@ private:
         const std::vector<yolos::pose::PoseResult>& poses) const;
 
     TargetObservation select_target(
+        const cv::Mat& camera_bgr,
         const std::vector<yolos::pose::PoseResult>& poses,
         const Eigen::MatrixXf& tracks,
         int image_width,
@@ -96,10 +121,27 @@ private:
         const CloudReprojector::OdomPose& odom_pose,
         TargetObservationDebugInfo* debug_info) const;
 
+    int recover_target_with_reid(
+        const cv::Mat& camera_bgr,
+        const Eigen::MatrixXf& tracks,
+        float* best_similarity) const;
+    void update_target_gallery(
+        const cv::Mat& camera_bgr,
+        const TargetObservation& target,
+        bool force_update);
+
+    bool should_update_gallery(bool force_update) const;
+    bool is_good_reid_crop(const cv::Rect& bbox, const cv::Size& image_size) const;
+
     TargetObservationConfig config_;
     std::unique_ptr<yolos::pose::YOLOPoseDetector> yolo_;
     std::unique_ptr<motcpp::trackers::ByteTrack> tracker_;
+    std::unique_ptr<ReIDTensorRTExtractor> reid_extractor_;
+    TrackGallery target_gallery_;
     int current_target_id_ = -1;
+    int current_raw_track_id_ = -1;
+    int64_t frame_index_ = 0;
+    int lost_track_frames_ = 0;
     bool initialized_ = false;
 };