From 6d459f08bf1c73c00fa3e44282895ddc380d9c6a Mon Sep 17 00:00:00 2001 From: hjy <1178065793@qq.com> Date: Mon, 20 Apr 2026 11:12:02 +0800 Subject: [PATCH] add gallery visualization, confirm no bug in gallery MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 可视化非常优秀非常直观 --- include/cloud_reprojection_ros_node.hpp | 2 + include/reid_trt_extractor.hpp | 7 +- include/target_observation_processing.hpp | 4 + src/cloud_reprojection_ros.cpp | 19 ++++ src/reid_trt_extractor.cpp | 10 +- src/target_observation_processing.cpp | 128 +++++++++++++++++++++- 6 files changed, 167 insertions(+), 3 deletions(-) diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index d71c3ee..4785e4d 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/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::SharedPtr overlay_compressed_pub_; // optional if send_overlay_ rclcpp::Publisher::SharedPtr detection_debug_compressed_pub_; // debug detections/tracks + rclcpp::Publisher::SharedPtr gallery_debug_pub_; rclcpp::Publisher::SharedPtr combined_pub_; // optional if publish_combined_compressed_ rclcpp::Publisher::SharedPtr target_observation_pub_; rclcpp::Publisher::SharedPtr track_observations_pub_; diff --git a/include/reid_trt_extractor.hpp b/include/reid_trt_extractor.hpp index 06d9a96..7bddc71 100644 --- a/include/reid_trt_extractor.hpp +++ b/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& crops_bgr() const { return crops_bgr_; } float best_similarity(const ReIDFeature& query) const; private: std::deque features_; + std::deque crops_bgr_; size_t max_features_ = 10; int64_t last_update_frame_ = -1; }; diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp index 859af61..6eb6801 100644 --- a/include/target_observation_processing.hpp +++ b/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_; } diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index c11cfb4..a46ed4a 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/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(sync_detection_debug_image_topic_, 10); } + if (enable_target_observation_ && debug_reid_) { + gallery_debug_pub_ = + this->create_publisher(sync_gallery_debug_topic_, 10); + } if (publish_combined_compressed_) { combined_pub_ = this->create_publisher(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) { diff --git a/src/reid_trt_extractor.cpp b/src/reid_trt_extractor.cpp index ffb38e9..2416b15 100644 --- a/src/reid_trt_extractor.cpp +++ b/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; } diff --git a/src/target_observation_processing.cpp b/src/target_observation_processing.cpp index ece69d4..5e5df37 100644 --- a/src/target_observation_processing.cpp +++ b/src/target_observation_processing.cpp @@ -917,11 +917,15 @@ std::vector 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(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(i)] || (row_should_update_gallery[static_cast(i)] && row_gallery_refresh_allowed[static_cast(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 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(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(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(crops.size()) || + crops[static_cast(c)].empty()) { + continue; + } + const cv::Mat& crop = crops[static_cast(c)]; + const double scale = std::min( + static_cast(cell_w) / static_cast(crop.cols), + static_cast(cell_h) / static_cast(crop.rows)); + const int rw = std::max(1, static_cast(std::lround(crop.cols * scale))); + const int rh = std::max(1, static_cast(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,