add gallery visualization, confirm no bug in gallery

可视化非常优秀非常直观

include/cloud_reprojection_ros_node.hpp

@@ -100,6 +100,7 @@ private:
     std::string sync_image_compressed_topic_;
     std::string sync_overlay_image_topic_;
     std::string sync_detection_debug_image_topic_;
+    std::string sync_gallery_debug_topic_;
     std::string sync_target_observation_topic_;
     std::string sync_track_observations_topic_;
     std::string sync_detection_pos_cam_topic_;
@@ -114,6 +115,7 @@ private:
 
     rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_;  // optional if send_overlay_
     rclcpp::Publisher<CompressedImage>::SharedPtr detection_debug_compressed_pub_;  // debug detections/tracks
+    rclcpp::Publisher<Image>::SharedPtr gallery_debug_pub_;
     rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;  // optional if publish_combined_compressed_
     rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
     rclcpp::Publisher<odin_ros_driver::msg::TargetObservationArray>::SharedPtr track_observations_pub_;

include/reid_trt_extractor.hpp

@@ -25,16 +25,21 @@ public:
     explicit TrackGallery(size_t max_features = 10);
 
     void clear();
-    void add(const ReIDFeature& feature, int64_t frame_index);
+    void add(
+        const ReIDFeature& feature,
+        int64_t frame_index,
+        const cv::Mat& crop_bgr = cv::Mat());
 
     bool empty() const { return features_.empty(); }
     size_t size() const { return features_.size(); }
     int64_t last_update_frame() const { return last_update_frame_; }
+    const std::deque<cv::Mat>& crops_bgr() const { return crops_bgr_; }
 
     float best_similarity(const ReIDFeature& query) const;
 
 private:
     std::deque<ReIDFeature> features_;
+    std::deque<cv::Mat> crops_bgr_;
     size_t max_features_ = 10;
     int64_t last_update_frame_ = -1;
 };

include/target_observation_processing.hpp

@@ -164,6 +164,10 @@ public:
     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_; }
+    cv::Mat render_gallery_debug_image(
+        int max_tracks = 8,
+        int gallery_cols = 10,
+        cv::Size cell_size = cv::Size(96, 192)) const;
 
     bool initialized() const { return initialized_; }
 

src/cloud_reprojection_ros.cpp

@@ -276,6 +276,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
         << "\n  sync_* topics under: " << sync_topic_prefix_
         << "\n  overlay_compressed_topic: " << sync_overlay_image_topic_
         << "\n  detection_debug_topic: " << sync_detection_debug_image_topic_
+        << "\n  gallery_debug_topic: " << sync_gallery_debug_topic_
         << "\n  combined_compressed_topic: " << combined_compressed_topic_
         << "\n  publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
         << "\n  send_overlay: " << (send_overlay_ ? "on" : "off")
@@ -336,6 +337,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
         detection_debug_compressed_pub_ =
             this->create_publisher<CompressedImage>(sync_detection_debug_image_topic_, 10);
     }
+    if (enable_target_observation_ && debug_reid_) {
+        gallery_debug_pub_ =
+            this->create_publisher<Image>(sync_gallery_debug_topic_, 10);
+    }
     if (publish_combined_compressed_) {
         combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
     }
@@ -459,6 +464,7 @@ void CloudReprojectionRosNode::loadParameters()
     sync_image_compressed_topic_ = sync_topic_prefix_ + "/image/compressed";
     sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
     sync_detection_debug_image_topic_ = sync_topic_prefix_ + "/detection_img_debug/compressed";
+    sync_gallery_debug_topic_ = sync_topic_prefix_ + "/gallery_debug";
     sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
     sync_track_observations_topic_ = sync_topic_prefix_ + "/track_observations";
     sync_detection_pos_cam_topic_ = sync_topic_prefix_ + "/detection_pos_cam";
@@ -847,6 +853,19 @@ void CloudReprojectionRosNode::processSyncedData(
                 sims.c_str());
         }
 
+        if (debug_reid_ && gallery_debug_pub_ && target_observation_processor_) {
+            cv::Mat gallery_debug =
+                target_observation_processor_->render_gallery_debug_image(8, 10);
+            if (!gallery_debug.empty()) {
+                auto msg = cv_bridge::CvImage(
+                    image_msg.header,
+                    "bgr8",
+                    gallery_debug).toImageMsg();
+                msg->header.stamp = sync_stamp;
+                gallery_debug_pub_->publish(*msg);
+            }
+        }
+
         if (debug_target_observation_) {
             if (!target_observation.valid) {
                 if (target_debug.poses_count == 0) {

src/reid_trt_extractor.cpp

@@ -65,10 +65,14 @@ TrackGallery::TrackGallery(size_t max_features)
 void TrackGallery::clear()
 {
     features_.clear();
+    crops_bgr_.clear();
     last_update_frame_ = -1;
 }
 
-void TrackGallery::add(const ReIDFeature& feature, int64_t frame_index)
+void TrackGallery::add(
+    const ReIDFeature& feature,
+    int64_t frame_index,
+    const cv::Mat& crop_bgr)
 {
     if (feature.empty()) {
         return;
@@ -76,8 +80,12 @@ void TrackGallery::add(const ReIDFeature& feature, int64_t frame_index)
 
     if (features_.size() >= max_features_) {
         features_.pop_front();
+        if (!crops_bgr_.empty()) {
+            crops_bgr_.pop_front();
+        }
     }
     features_.push_back(feature);
+    crops_bgr_.push_back(crop_bgr.empty() ? cv::Mat() : crop_bgr.clone());
     last_update_frame_ = frame_index;
 }
 

src/target_observation_processing.cpp

@@ -917,11 +917,15 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
         entry.has_last_bbox = (entry.last_bbox.area() > 0);
         entry.last_seen_frame = frame_index_;
         const ReIDFeature& feature = features_by_row[static_cast<size_t>(i)];
+        cv::Mat gallery_crop;
+        if (entry.has_last_bbox && entry.last_bbox.area() > 0) {
+            gallery_crop = camera_bgr(entry.last_bbox).clone();
+        }
         if (!feature.empty() &&
             (row_needs_matching_feature[static_cast<size_t>(i)] ||
              (row_should_update_gallery[static_cast<size_t>(i)] &&
               row_gallery_refresh_allowed[static_cast<size_t>(i)]))) {
-            entry.gallery.add(feature, frame_index_);
+            entry.gallery.add(feature, frame_index_, gallery_crop);
         }
 
         // Cache 2D observation so snapshot_active_tracks() can emit a full
@@ -1447,6 +1451,128 @@ TargetObservationProcessor::snapshot_active_tracks() const
     return out;
 }
 
+cv::Mat TargetObservationProcessor::render_gallery_debug_image(
+    int max_tracks,
+    int gallery_cols,
+    cv::Size cell_size) const
+{
+    const int rows = std::max(1, max_tracks);
+    const int cols = std::max(1, gallery_cols);
+    const int label_w = 84;
+    const int header_h = 28;
+    const int cell_w = std::max(32, cell_size.width);
+    const int cell_h = std::max(64, cell_size.height);
+    cv::Mat canvas(
+        header_h + rows * cell_h,
+        label_w + cols * cell_w,
+        CV_8UC3,
+        cv::Scalar(24, 24, 24));
+
+    for (int c = 0; c < cols; ++c) {
+        const int x = label_w + c * cell_w;
+        cv::rectangle(
+            canvas,
+            cv::Rect(x, 0, cell_w, header_h),
+            cv::Scalar(36, 36, 36),
+            cv::FILLED);
+        cv::putText(
+            canvas,
+            "g" + std::to_string(c),
+            cv::Point(x + 10, 19),
+            cv::FONT_HERSHEY_SIMPLEX,
+            0.5,
+            cv::Scalar(220, 220, 220),
+            1,
+            cv::LINE_AA);
+    }
+
+    std::vector<int> sids;
+    sids.reserve(tracks_.size());
+    for (const auto& kv : tracks_) {
+        sids.push_back(kv.first);
+    }
+    std::sort(sids.begin(), sids.end());
+
+    for (int r = 0; r < rows; ++r) {
+        const int y = header_h + r * cell_h;
+        cv::rectangle(
+            canvas,
+            cv::Rect(0, y, label_w, cell_h),
+            cv::Scalar(30, 30, 30),
+            cv::FILLED);
+        cv::line(
+            canvas,
+            cv::Point(0, y),
+            cv::Point(canvas.cols - 1, y),
+            cv::Scalar(60, 60, 60),
+            1,
+            cv::LINE_AA);
+
+        if (r >= static_cast<int>(sids.size())) {
+            cv::putText(
+                canvas,
+                "sid=-",
+                cv::Point(10, y + 24),
+                cv::FONT_HERSHEY_SIMPLEX,
+                0.55,
+                cv::Scalar(150, 150, 150),
+                1,
+                cv::LINE_AA);
+            continue;
+        }
+
+        const int sid = sids[static_cast<size_t>(r)];
+        const auto it = tracks_.find(sid);
+        if (it == tracks_.end()) {
+            continue;
+        }
+        const cv::Scalar sid_color = stable_track_color_bgr(sid);
+        cv::putText(
+            canvas,
+            "sid=" + std::to_string(sid),
+            cv::Point(8, y + 24),
+            cv::FONT_HERSHEY_SIMPLEX,
+            0.55,
+            sid_color,
+            2,
+            cv::LINE_AA);
+
+        const auto& crops = it->second.gallery.crops_bgr();
+        for (int c = 0; c < cols; ++c) {
+            const int x = label_w + c * cell_w;
+            cv::rectangle(
+                canvas,
+                cv::Rect(x, y, cell_w, cell_h),
+                cv::Scalar(50, 50, 50),
+                1,
+                cv::LINE_AA);
+            if (c >= static_cast<int>(crops.size()) ||
+                crops[static_cast<size_t>(c)].empty()) {
+                continue;
+            }
+            const cv::Mat& crop = crops[static_cast<size_t>(c)];
+            const double scale = std::min(
+                static_cast<double>(cell_w) / static_cast<double>(crop.cols),
+                static_cast<double>(cell_h) / static_cast<double>(crop.rows));
+            const int rw = std::max(1, static_cast<int>(std::lround(crop.cols * scale)));
+            const int rh = std::max(1, static_cast<int>(std::lround(crop.rows * scale)));
+            cv::Mat resized;
+            cv::resize(crop, resized, cv::Size(rw, rh), 0.0, 0.0, cv::INTER_LINEAR);
+            const int ox = x + (cell_w - rw) / 2;
+            const int oy = y + (cell_h - rh) / 2;
+            resized.copyTo(canvas(cv::Rect(ox, oy, rw, rh)));
+        }
+    }
+
+    cv::rectangle(
+        canvas,
+        cv::Rect(0, 0, canvas.cols, canvas.rows),
+        cv::Scalar(80, 80, 80),
+        1,
+        cv::LINE_AA);
+    return canvas;
+}
+
 
 void draw_target_observation_overlay(
     cv::Mat& image_bgr,