From 560a60068f2dd1efeb11082ed7f58c142e1931b9 Mon Sep 17 00:00:00 2001 From: hjy <1178065793@qq.com> Date: Sun, 19 Apr 2026 18:11:22 +0800 Subject: [PATCH] fix upstaris problem MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit primary track一直都有，只是有时候会错误 --- config/control_command.yaml | 17 ++- include/cloud_reprojection_ros_node.hpp | 2 + include/target_observation_processing.hpp | 16 +++ script/record_dataset.sh | 7 +- src/cloud_reprojection_ros.cpp | 44 ++++++- src/target_observation_processing.cpp | 138 ++++++++++++++++++---- 6 files changed, 196 insertions(+), 28 deletions(-) diff --git a/config/control_command.yaml b/config/control_command.yaml index 6e37354..8b148fb 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -75,8 +75,13 @@ register_keys: # cloud_reprojection 第4路图像输入；raw / compressed 二选一，由 sync_camera_compressed 控制 # sync_camera_compressed = 0: sync_camera_topic 应该是 sensor_msgs/Image (bgr8)，例如 /odin1/image # sync_camera_compressed = 1: sync_camera_topic 应该是 sensor_msgs/CompressedImage，例如 /odin1/image/compressed + # real run: 0, 0 0 ; dataset: 1 1 1 ; special: 0 1 1 sync_camera_topic: "/odin1/image" - sync_camera_compressed: 1 # sync_camera_topic would be default added /compressed + sync_camera_compressed: 0 # sync_camera_topic would be default added /compressed + # 无论输入是 raw 还是 compressed，都额外发布 {sync_topic_prefix}/image/compressed + always_send_sync_compressed: 1 + # 控制是否发布 {sync_topic_prefix}/cloud_slam + always_send_sync_cloud_slam: 1 sync_topic_prefix: "/odin1/sync" combined_compressed_topic: "/odin1/combined_image/compressed" combined_jpeg_quality: 85 @@ -111,6 +116,16 @@ register_keys: target_reid_input_width: 128 target_reid_feature_dim: 512 target_reid_max_batch_size: 8 + # Edge margin (px) the ReID crop must keep from each image border. 0 lets + # close-range crops (head out of frame) still feed ReID, which prevents + # stable_id inflation when the primary target walks up to the camera. + target_reid_crop_edge_margin_px: 0 + # 3D-proximity fallback binding for the follow target when the ReID crop + # is unavailable. Rescues the stable_id by pure spatial consistency with + # a tighter distance envelope than the regular 3D gate. + target_reid_3d_fallback_enabled: 1 + target_reid_3d_fallback_base_m: 0.75 + target_reid_3d_fallback_per_frame_m: 0.05 # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software. # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/ diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index db5d425..2638725 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -70,6 +70,8 @@ private: std::string wiwc_topic_; std::string camera_image_topic_; bool camera_image_compressed_ = false; + bool always_send_sync_compressed_ = false; + bool always_send_sync_cloud_slam_ = false; std::string sync_topic_prefix_; std::string combined_compressed_topic_; int combined_jpeg_quality_; diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp index 0a00d59..4b637b4 100644 --- a/include/target_observation_processing.hpp +++ b/include/target_observation_processing.hpp @@ -44,12 +44,26 @@ struct TargetObservationConfig { int reid_feature_dim = 512; int reid_max_batch_size = 8; int max_concurrent_tracks = 16; + // 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 + // within a strict (tighter than reid_3d_gate) distance of the target's + // last known world pos. Guards against ID inflation during close-range + // partial views that block ReID extraction. + bool reid_3d_fallback_enabled = true; + float reid_3d_fallback_base_m = 0.75f; + float reid_3d_fallback_per_frame_m = 0.05f; bool debug = false; }; @@ -83,6 +97,7 @@ struct TargetObservationDebugInfo { int lost_frames = 0; int reid_3d_gate_rejections = 0; // hard-mode rejections this frame int reid_3d_gate_soft_warnings = 0; // soft-mode over-threshold this frame + int reid_3d_fallback_rebinds = 0; // follow-target rebinds by 3D fallback bool found_existing_target = false; bool target_selected = false; bool selected_from_center_bootstrap = false; @@ -154,6 +169,7 @@ private: bool has_last_bbox = false; 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; // Per-entry 3D state; refreshed whenever compute_entry_3d succeeds. float last_depth = -1.0f; diff --git a/script/record_dataset.sh b/script/record_dataset.sh index 39fb979..90ff917 100755 --- a/script/record_dataset.sh +++ b/script/record_dataset.sh @@ -1,8 +1,13 @@ -ros2 bag record \ +ros2 bag record --use-sim-time \ + /odin1/sync/cloud_slam \ /odin1/sync/cloud_in_cam \ /odin1/sync/image/compressed \ /odin1/sync/odometry \ /odin1/sync/wiwc \ + /odin1/sync/detection_pos_cam \ + /odin1/sync/detection_pos_world \ + /odin1/sync/track_observations \ + /odin1/sync/target_observation \ /lowstate \ # /odin1/combined_image/compressed diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index e1248e8..ae33504 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -147,9 +147,11 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op << "\n wiwc_topic: " << wiwc_topic_ << "\n camera_image_topic: " << camera_image_topic_ << "\n camera_image_transport: " << (camera_image_compressed_ ? "compressed" : "raw") + << "\n always_send_sync_compressed: " << (always_send_sync_compressed_ ? "on" : "off") + << "\n always_send_sync_cloud_slam: " << (always_send_sync_cloud_slam_ ? "on" : "off") << "\n sync_image_topic: " << sync_image_topic_ << "\n sync_image_compressed_topic: " - << (camera_image_compressed_ ? sync_image_compressed_topic_ : "") + << ((camera_image_compressed_ || always_send_sync_compressed_) ? sync_image_compressed_topic_ : "") << "\n sync_* topics under: " << sync_topic_prefix_ << "\n overlay_compressed_topic: " << sync_overlay_image_topic_ << "\n detection_debug_topic: " << sync_detection_debug_image_topic_ @@ -194,11 +196,13 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op } sync_cloud_pub_ = this->create_publisher(sync_cloud_topic_, 10); - sync_cloud_slam_pub_ = this->create_publisher(sync_cloud_slam_topic_, 10); + if (always_send_sync_cloud_slam_) { + sync_cloud_slam_pub_ = this->create_publisher(sync_cloud_slam_topic_, 10); + } sync_odom_pub_ = this->create_publisher(sync_odom_topic_, 10); sync_wiwc_pub_ = this->create_publisher(sync_wiwc_topic_, 10); sync_image_pub_ = this->create_publisher(sync_image_topic_, 10); - if (camera_image_compressed_) { + if (camera_image_compressed_ || always_send_sync_compressed_) { sync_image_compressed_pub_ = this->create_publisher(sync_image_compressed_topic_, 10); } @@ -258,6 +262,8 @@ void CloudReprojectionRosNode::loadParameters() this->declare_parameter("wiwc_topic", "/odin1/wiwc"); this->declare_parameter("register_keys.sync_camera_topic", "/odin1/image"); this->declare_parameter("register_keys.sync_camera_compressed", 0); + this->declare_parameter("register_keys.always_send_sync_compressed", 0); + this->declare_parameter("register_keys.always_send_sync_cloud_slam", 1); this->declare_parameter("register_keys.sync_topic_prefix", "/odin1/sync"); this->declare_parameter("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed"); this->declare_parameter("register_keys.combined_jpeg_quality", 85); @@ -290,6 +296,10 @@ void CloudReprojectionRosNode::loadParameters() this->declare_parameter("register_keys.target_reid_input_width", 128); this->declare_parameter("register_keys.target_reid_feature_dim", 512); this->declare_parameter("register_keys.target_reid_max_batch_size", 8); + this->declare_parameter("register_keys.target_reid_crop_edge_margin_px", 0); + this->declare_parameter("register_keys.target_reid_3d_fallback_enabled", 1); + this->declare_parameter("register_keys.target_reid_3d_fallback_base_m", 0.75); + this->declare_parameter("register_keys.target_reid_3d_fallback_per_frame_m", 0.05); #endif cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string(); @@ -297,6 +307,10 @@ void CloudReprojectionRosNode::loadParameters() wiwc_topic_ = this->get_parameter("wiwc_topic").as_string(); camera_image_compressed_ = (this->get_parameter("register_keys.sync_camera_compressed").as_int() != 0); + always_send_sync_compressed_ = + (this->get_parameter("register_keys.always_send_sync_compressed").as_int() != 0); + always_send_sync_cloud_slam_ = + (this->get_parameter("register_keys.always_send_sync_cloud_slam").as_int() != 0); camera_image_topic_ = resolve_camera_sync_topic( this->get_parameter("register_keys.sync_camera_topic").as_string(), camera_image_compressed_); @@ -452,6 +466,14 @@ void CloudReprojectionRosNode::loadParameters() this->get_parameter("register_keys.target_reid_feature_dim").as_int(); target_config.reid_max_batch_size = this->get_parameter("register_keys.target_reid_max_batch_size").as_int(); + target_config.reid_crop_edge_margin_px = + this->get_parameter("register_keys.target_reid_crop_edge_margin_px").as_int(); + target_config.reid_3d_fallback_enabled = + (this->get_parameter("register_keys.target_reid_3d_fallback_enabled").as_int() != 0); + target_config.reid_3d_fallback_base_m = static_cast( + this->get_parameter("register_keys.target_reid_3d_fallback_base_m").as_double()); + target_config.reid_3d_fallback_per_frame_m = static_cast( + this->get_parameter("register_keys.target_reid_3d_fallback_per_frame_m").as_double()); target_config.debug = debug_target_observation_; target_observation_processor_ = std::make_unique(); @@ -594,7 +616,9 @@ void CloudReprojectionRosNode::processSyncedData( return; } - sync_cloud_slam_pub_->publish(sync_cloud_slam_msg); + if (sync_cloud_slam_pub_) { + sync_cloud_slam_pub_->publish(sync_cloud_slam_msg); + } sync_odom_pub_->publish(sync_odom_msg); sync_wiwc_pub_->publish(sync_wiwc_msg); sync_cloud_pub_->publish(cloud_cam_msg); @@ -994,6 +1018,18 @@ void CloudReprojectionRosNode::syncCallbackRaw( return; } } + if (always_send_sync_compressed_ && sync_image_compressed_pub_) { + std::vector buf; + if (!cv::imencode(".jpg", cam_bgr, buf)) { + RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (sync compressed image)"); + return; + } + CompressedImage sync_compressed_msg; + sync_compressed_msg.header = image_msg->header; + sync_compressed_msg.format = "jpeg"; + sync_compressed_msg.data.assign(buf.begin(), buf.end()); + sync_image_compressed_pub_->publish(sync_compressed_msg); + } processSyncedData(cloud_msg, odom_msg, wiwc_msg, *image_msg, cam_bgr); } diff --git a/src/target_observation_processing.cpp b/src/target_observation_processing.cpp index b262507..15fb216 100644 --- a/src/target_observation_processing.cpp +++ b/src/target_observation_processing.cpp @@ -413,6 +413,7 @@ std::vector TargetObservationProcessor::bind_and_update_tracks( // debug state never leaks forward. for (auto& kv : tracks_) { kv.second.just_rebound_by_reid = false; + kv.second.just_rebound_by_3d_fallback = false; kv.second.last_rebind_similarity = -1.0f; } @@ -593,6 +594,53 @@ std::vector TargetObservationProcessor::bind_and_update_tracks( } } + // Phase 3.5: 3D-proximity rescue for the follow target. When an + // unmatched row has a fresh 3D estimate but no ReID feature (the crop + // failed is_good_reid_crop, typically because the person is close and + // the head is out of frame), ReID-rebind path is blocked. Fall back to + // pure spatial proximity against the follow target's last known world + // position, with a stricter threshold than the regular 3D gate since + // there's no appearance corroboration. + int fallback_rebinds = 0; + if (config_.reid_3d_fallback_enabled && + current_target_stable_id_ >= 0 && + !taken_sids.count(current_target_stable_id_)) { + auto target_it = tracks_.find(current_target_stable_id_); + if (target_it != tracks_.end() && target_it->second.has_last_pos) { + const int64_t frames_since = std::max( + 0, frame_index_ - target_it->second.last_seen_frame); + const float dmax = config_.reid_3d_fallback_base_m + + config_.reid_3d_fallback_per_frame_m * + static_cast(frames_since); + int best_row = -1; + float best_dist = std::numeric_limits::max(); + for (int i = 0; i < row_count; ++i) { + if (stable_ids[static_cast(i)] >= 0) { + continue; + } + const EntryCloudEstimate& row_est = + row_estimates[static_cast(i)]; + if (!row_est.valid) { + continue; + } + const float dist = + (target_it->second.last_pos_world - row_est.pos_world).norm(); + if (dist < dmax && dist < best_dist) { + best_dist = dist; + best_row = i; + } + } + if (best_row >= 0) { + stable_ids[static_cast(best_row)] = + current_target_stable_id_; + taken_sids.insert(current_target_stable_id_); + target_it->second.just_rebound_by_3d_fallback = true; + target_it->second.last_rebind_similarity = -best_dist; + fallback_rebinds = 1; + } + } + } + // Phase 4: allocate a fresh stable_id for rows that still have no match. for (int i = 0; i < row_count; ++i) { if (stable_ids[static_cast(i)] >= 0) { @@ -661,6 +709,7 @@ std::vector TargetObservationProcessor::bind_and_update_tracks( debug_info->reid_attempted = reid_recovery_attempted; debug_info->reid_3d_gate_rejections = gate_hard_rejections; debug_info->reid_3d_gate_soft_warnings = gate_soft_warnings; + debug_info->reid_3d_fallback_rebinds = fallback_rebinds; // depth_ms = per-entry 3D compute (Phase 2A); reid_extract_ms = TRT // inference (Phase 2B). The outer bind_ms (measured by process()) // also covers Phase 3/4/5 bookkeeping. @@ -803,14 +852,18 @@ TargetObservation TargetObservationProcessor::select_follow_target( observation.keypoints_xyc[i * 3 + 2] = pose.keypoints[i].confidence; } - // Consume the one-shot rebind flag on the follow target's entry. + // Consume the one-shot rebind flags on the follow target's entry. Either + // a ReID match or a 3D-proximity fallback (when ReID crop was rejected) + // counts as "recovered" for downstream logging. bool recovered_by_reid = false; float rebind_sim = -1.0f; const auto it = tracks_.find(selected_sid); if (it != tracks_.end()) { - recovered_by_reid = it->second.just_rebound_by_reid; + recovered_by_reid = it->second.just_rebound_by_reid || + it->second.just_rebound_by_3d_fallback; rebind_sim = it->second.last_rebind_similarity; it->second.just_rebound_by_reid = false; + it->second.just_rebound_by_3d_fallback = false; it->second.last_rebind_similarity = -1.0f; } if (debug_info) { @@ -842,7 +895,7 @@ bool TargetObservationProcessor::is_good_reid_crop( return false; } - const int margin = 3; + const int margin = std::max(0, config_.reid_crop_edge_margin_px); if (bbox.x < margin || bbox.y < margin) { return false; } @@ -897,35 +950,76 @@ TargetObservationProcessor::compute_entry_3d( std::vector per_keypoint_depths; std::vector per_keypoint_weights; - per_keypoint_depths.reserve(kUpperTorsoKeypointIndices.size()); - per_keypoint_weights.reserve(kUpperTorsoKeypointIndices.size()); + per_keypoint_depths.reserve(8); + per_keypoint_weights.reserve(8); est.valid_projected_pixels.reserve(128); - for (const int kp_index : kUpperTorsoKeypointIndices) { - if (kp_index >= static_cast(pose.keypoints.size())) { - continue; + // Collect per-keypoint depth samples from one keypoint tier. Each tier's + // samples can contribute with a scaled weight to downweight anatomical + // fallbacks (knees/ankles less trustworthy than torso for body centroid). + auto try_tier = [&](const std::vector& kp_list, float weight_scale) { + for (const int kp_index : kp_list) { + if (kp_index >= static_cast(pose.keypoints.size())) { + continue; + } + const auto& kp = pose.keypoints[static_cast(kp_index)]; + if (kp.confidence < 0.5f) { + continue; + } + std::vector nearby_depths; + nearby_depths.reserve(32); + for (size_t i = 0; i < projected.uv.size(); ++i) { + const float du = projected.uv[i].x - kp.x; + const float dv = projected.uv[i].y - kp.y; + if (std::hypot(du, dv) < config_.search_radius_px) { + nearby_depths.push_back(projected.cam[i].z()); + est.valid_projected_pixels.emplace_back( + static_cast(std::lround(projected.uv[i].x)), + static_cast(std::lround(projected.uv[i].y))); + } + } + if (nearby_depths.size() < 3) { + continue; + } + per_keypoint_depths.push_back(median_in_place(nearby_depths)); + per_keypoint_weights.push_back(kp.confidence * weight_scale); } - const auto& kp = pose.keypoints[static_cast(kp_index)]; - if (kp.confidence < 0.5f) { - continue; - } - std::vector nearby_depths; - nearby_depths.reserve(32); + }; + + // Tier 1: upper torso (shoulders + hips). Preferred body-centroid anchor. + try_tier({5, 6, 11, 12}, 1.0f); + + // Tier 2: knees — activated when torso is mostly out of frame / occluded. + if (per_keypoint_depths.size() < 2) { + try_tier({13, 14}, 0.8f); + } + // Tier 3: ankles — deeper fallback for heavy upper-body truncation. + if (per_keypoint_depths.size() < 2) { + try_tier({15, 16}, 0.6f); + } + + // Last-ditch fallback: wide radius search around the bbox center. Low + // weight because it may incorporate background cloud points. + if (per_keypoint_depths.empty()) { + const float center_u = 0.5f * (bbox_xyxy[0] + bbox_xyxy[2]); + const float center_v = 0.5f * (bbox_xyxy[1] + bbox_xyxy[3]); + const float fallback_radius = config_.search_radius_px * 2.0f; + std::vector center_depths; + center_depths.reserve(64); for (size_t i = 0; i < projected.uv.size(); ++i) { - const float du = projected.uv[i].x - kp.x; - const float dv = projected.uv[i].y - kp.y; - if (std::hypot(du, dv) < config_.search_radius_px) { - nearby_depths.push_back(projected.cam[i].z()); + const float du = projected.uv[i].x - center_u; + const float dv = projected.uv[i].y - center_v; + if (std::hypot(du, dv) < fallback_radius) { + center_depths.push_back(projected.cam[i].z()); est.valid_projected_pixels.emplace_back( static_cast(std::lround(projected.uv[i].x)), static_cast(std::lround(projected.uv[i].y))); } } - if (nearby_depths.size() < 3) { - continue; + if (center_depths.size() >= 5) { + per_keypoint_depths.push_back(median_in_place(center_depths)); + per_keypoint_weights.push_back(0.4f); } - per_keypoint_depths.push_back(median_in_place(nearby_depths)); - per_keypoint_weights.push_back(kp.confidence); } est.depth_sample_count = static_cast(per_keypoint_depths.size());