diff --git a/README_track.md b/README_track.md index feab7c0..9ef81cc 100644 --- a/README_track.md +++ b/README_track.md @@ -1,3 +1,164 @@ -# yolo+ track使用说明 -选取第一帧中心的人物作为target，持续建立gallery并使用bytetrack跟踪 -如果丢了使用lazy reid，基于gallery进行匹配 +# yolo + stable track 说明 + +这套逻辑的目标不是直接相信 ByteTrack 的 `raw id`，而是维护一个更稳定的 `stable id`。 + +## 1. 基本概念 + +- `raw id` + ByteTrack 当前帧给出的轨迹号，只表示短时 2D 跟踪结果，可能抖动、复用、换人。 +- `stable id` + 本节点自己维护的长期身份号，用于： + - `/target_observation` + - `/track_observations` + - RViz marker + - ReID gallery +- `primary target` + 当前跟随目标。它只是某一个 `stable id`，不是单独一套编号。 + +## 2. 当前关联逻辑 + +每一帧大致按下面顺序运行： + +1. `YOLO pose` +2. `ByteTrack` +3. `raw-id fast path` + - 如果某个 row 的 `raw id` 能续上上一帧同一个 `stable id`，先临时绑定 + - 注意：这一步现在只是“初始猜测”，不是最终判决 +4. `ReID` + - 对还没绑定的 row，和所有已有 `stable id` 的 gallery 做相似度比较 + - 若满足 `match_threshold` 且领先第二名至少 `gap_threshold`，直接回绑旧 `stable id` +5. `single-person 3D fallback` + - 只在单人场景下启用 + - 只服务当前 `primary target` + - 用于 ReID crop 不可用时的兜底恢复 +6. `new stable id allocate` + - 如果和所有旧 `stable id` 都无法匹配 + - 且同一个 ByteTrack `raw id` 连续出现满 `target_reid_confirm_hits_before_allocate` + - 才会申请一个新的 `stable id` + +## 3. stable id 的原则 + +- `stable id` 是长期身份，不是当前画面里的顺位号 +- 某个目标暂时丢失时，它原来的 `stable id` 会继续保留，直到 timeout age out +- 不能因为画面里少了一个人，就把别人的 `stable id` 顶上去 + +这也是为什么： +- `raw id` 可以变 +- 但 `stable id` 应该尽量不变 + +## 4. gallery 更新原则 + +`gallery` 现在不会随意更新。 + +### 4.1 什么时候能进 gallery + +只有在下列情况之一时，当前 feature 才允许写进对应 `stable id`： + +- 该 row 通过 ReID 成功回绑到旧 `stable id` +- 该 row 已经拿到正式 `stable id`，并且到了定期 refresh 时机 +- 该 row 是经过连续确认后刚申请到的新 `stable id` + +### 4.2 什么时候不允许更新 + +如果一个 row 是通过 `raw-id fast path` 临时续上的，但 ReID 明显反对这个绑定，则： + +- 该帧不会更新这个 `stable id` 的 gallery +- 必要时会直接撤销 fast-path 绑定，让后续 ReID 重新决定该 row 属于哪个旧 `stable id` + +这样做是为了避免： + +- ByteTrack 一次短时漂移 +- 把错人的 feature 写进旧 `stable id` +- 进而污染整个 gallery + +### 4.3 fallback dist 会不会更新 gallery + +不会。 + +`fallback dist` 只负责在单人场景下帮 `primary target` 暂时保住 `stable id`，不负责更新外观库。 + +## 5. 默认关键参数 + +这些参数都在： + +- [control_command.yaml](./config/control_command.yaml) + +当前常用参数： + +- `target_reid_gallery_size: 8` + 每个 `stable id` 最多缓存 8 个 ReID feature +- `target_reid_feature_update_interval: 5` + 最多每 5 帧尝试刷新一次 gallery +- `target_reid_match_threshold: 0.65` +- `target_reid_gap_threshold: 0.10` +- `target_reid_lost_timeout_frames: 150` +- `target_reid_confirm_hits_before_allocate: 5` + 新人申请新 `stable id` 前，要求同一个 ByteTrack `raw id` 连续确认 5 帧 +- `target_reid_3d_fallback_enabled: 1` + 单人场景启用 3D fallback + +## 6. debug topic + +假设 `sync_topic_prefix=/odin/sync`，则当前会有这些调试输出： + +- `/odin/sync/detection_img_debug/compressed` + 调试图。未分配 `stable id` 的 detection 画白色；已分配 `stable id` 的 detection 画对应彩色。 +- `/odin/sync/gallery_debug` + 原始 `sensor_msgs/Image`，不是 compressed。 + 用于看 ReID gallery 当前缓存了哪些人像 crop。 + 当前布局是： + - 最多 8 行，对应 8 个 `stable id` + - 10 列，对应 gallery 槽位 + +此外还有： + +- `/odin/sync/target_observation` + 当前 primary target +- `/odin/sync/track_observations` + 当前帧所有 fresh 3D 的 stable tracks +- `/odin/sync/detection_pos_cam` +- `/odin/sync/detection_pos_world` + +## 7. debug_reid + +开启： + +- `debug_reid: 1` + +后，会打印每一帧的 ReID 相似度矩阵，格式类似： + +```text +[ReIDMatrix] prev_sid=1 bytetrack_id=[2,1] stable_id=[1,0] sims=[1->[sid0:0.905,sid1:0.508],2->[sid1:0.938,sid0:0.489]] +``` + +解释： + +- `prev_sid` + 上一时刻的 primary stable id +- `bytetrack_id=[...]` + 当前帧 ByteTrack row 的 raw ids +- `stable_id=[...]` + 当前系统最终给每个 row 绑定到的 stable ids +- `sims=[...]` + 每个 ByteTrack row 和所有已有 `stable id` gallery 的相似度 + +这个日志主要用来排查： + +- 为什么某个 row 没能回绑到旧 `stable id` +- 是 ByteTrack 漂了，还是 gallery 被污染了 +- 当前绑定结果和 ReID 结果是否一致 + +## 8. 当前已知设计取向 + +这版实现明确偏向下面的目标： + +- `stable id` 尽量稳定 +- 允许 ByteTrack 暂时失败 +- 不允许一次错误 fast-path 直接污染 gallery +- 在多人场景下，优先相信 ReID，不用 3D fallback 去硬改多人 stable id + +如果后面还要继续调，最值得重点观察的是： + +- `debug_reid` 的矩阵输出 +- `/odin/sync/gallery_debug` +- 某个 `stable id` 的 gallery 是否被混入了别人的 crop diff --git a/config/control_command.yaml b/config/control_command.yaml index 464685b..5cba715 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -117,6 +117,7 @@ register_keys: target_lost_detection_compensation_enable: 1 target_lost_detection_roi_scale: 2.0 target_lost_detection_roi_min_size_px: 192 + debug_reid: 1 target_reid_enable: 1 target_reid_engine: "/home/hjy/HJY/Codes/AMP_ws/TargetPrediction/deploy/model/target_reid_osnet_x0_25_dukemtmcreid.trt" target_reid_match_threshold: 0.65 @@ -144,7 +145,7 @@ register_keys: # fast-path and ReID rebind) before a fresh stable_id is allocated for it. # Filters out short-lived ByteTrack ID flicker. Set 1 to disable. target_reid_confirm_hits_before_allocate: 5 - debug_reid: 1 + diff --git a/include/target_observation_processing.hpp b/include/target_observation_processing.hpp index 6eb6801..1798a39 100644 --- a/include/target_observation_processing.hpp +++ b/include/target_observation_processing.hpp @@ -157,9 +157,9 @@ public: const CloudReprojector::OdomPose& odom_pose, TargetObservationDebugInfo* debug_info = nullptr); - // All active tracks with a fresh 3D estimate from the most recent - // process() call. One TargetObservation per entry; is_primary_target - // marks the follow target. Empty if process() hasn't run yet. + // All active tracks from the most recent process() call. Every retained + // stable track is emitted every frame; valid=true only when a fresh 3D + // estimate exists for the current frame. std::vector 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_; } diff --git a/msg/TargetObservation.msg b/msg/TargetObservation.msg index 0ce0c34..b58e9b9 100644 --- a/msg/TargetObservation.msg +++ b/msg/TargetObservation.msg @@ -1,7 +1,8 @@ std_msgs/Header header nav_msgs/Odometry odometry bool valid -int32 track_id +int32 stable_id +int32 bytetrack_id int32 detection_index bool is_primary_target float32 confidence diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index a46ed4a..f742452 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -993,10 +993,11 @@ void CloudReprojectionRosNode::processSyncedData( odin_ros_driver::msg::TargetObservation& msg, const odin_ros_driver::TargetObservation& obs) { msg.valid = obs.valid; + msg.stable_id = obs.track_id; + msg.bytetrack_id = obs.raw_track_id; + msg.detection_index = obs.detection_index; + msg.is_primary_target = obs.is_primary_target; if (obs.valid) { - msg.track_id = obs.track_id; - msg.detection_index = obs.detection_index; - msg.is_primary_target = obs.is_primary_target; msg.confidence = obs.confidence; msg.depth = obs.depth; msg.depth_confidence = obs.depth_confidence; @@ -1009,9 +1010,6 @@ void CloudReprojectionRosNode::processSyncedData( msg.detection_pos_world.y = obs.detection_pos_world.y(); msg.detection_pos_world.z = obs.detection_pos_world.z(); } else { - msg.track_id = -1; - msg.detection_index = -1; - msg.is_primary_target = false; msg.confidence = -1.0f; msg.depth = -1.0f; msg.depth_confidence = -1.0f; diff --git a/src/reid_trt_extractor.cpp b/src/reid_trt_extractor.cpp index 2416b15..f6159bb 100644 --- a/src/reid_trt_extractor.cpp +++ b/src/reid_trt_extractor.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include diff --git a/src/target_observation_processing.cpp b/src/target_observation_processing.cpp index 5e5df37..fb4908c 100644 --- a/src/target_observation_processing.cpp +++ b/src/target_observation_processing.cpp @@ -581,12 +581,14 @@ std::vector TargetObservationProcessor::bind_and_update_tracks( const auto it = tracks_.find(stable_ids[static_cast(i)]); if (it != tracks_.end()) { const TrackEntry& entry = it->second; - const bool empty_gallery = entry.gallery.empty(); + const bool gallery_filling = + entry.gallery.size() < + static_cast(std::max(1, config_.reid_gallery_size)); const bool interval_due = config_.reid_feature_update_interval <= 0 || (frame_index_ - entry.gallery.last_update_frame()) >= static_cast(config_.reid_feature_update_interval); - if (empty_gallery || interval_due) { + if (gallery_filling || interval_due) { need_extract = true; row_should_update_gallery[static_cast(i)] = true; } @@ -921,6 +923,7 @@ std::vector TargetObservationProcessor::bind_and_update_tracks( if (entry.has_last_bbox && entry.last_bbox.area() > 0) { gallery_crop = camera_bgr(entry.last_bbox).clone(); } + const int detection_index = static_cast(tracks(i, 7)); if (!feature.empty() && (row_needs_matching_feature[static_cast(i)] || (row_should_update_gallery[static_cast(i)] && @@ -930,7 +933,6 @@ std::vector TargetObservationProcessor::bind_and_update_tracks( // Cache 2D observation so snapshot_active_tracks() can emit a full // TargetObservation for every active entry without re-indexing poses. - const int detection_index = static_cast(tracks(i, 7)); entry.last_detection_index = detection_index; if (detection_index >= 0 && detection_index < static_cast(poses.size())) { @@ -1422,30 +1424,35 @@ TargetObservationProcessor::snapshot_active_tracks() const out.reserve(tracks_.size()); for (const auto& kv : tracks_) { const TrackEntry& entry = kv.second; - // Only emit tracks with a 3D estimate computed in the current frame. - // Transient-lost entries (not seen this frame) are skipped so stale - // positions don't leak into the multi-target topic. - if (!entry.has_last_pos || entry.last_pos_frame != frame_index_) { - continue; - } TargetObservation obs; - obs.valid = true; obs.track_id = entry.stable_id; obs.raw_track_id = entry.last_raw_track_id; - obs.detection_index = entry.last_detection_index; obs.is_primary_target = (entry.stable_id == last_selected_primary_stable_id_); - obs.confidence = entry.last_confidence; - obs.depth = entry.last_depth; - obs.depth_confidence = entry.last_depth_confidence; - obs.bbox_xyxy = { - static_cast(entry.last_bbox.x), - static_cast(entry.last_bbox.y), - static_cast(entry.last_bbox.x + entry.last_bbox.width), - static_cast(entry.last_bbox.y + entry.last_bbox.height), - }; - obs.keypoints_xyc = entry.last_keypoints_xyc; - obs.detection_pos_cam = entry.last_pos_cam; - obs.detection_pos_world = entry.last_pos_world; + obs.valid = entry.has_last_pos && entry.last_pos_frame == frame_index_; + if (obs.valid) { + obs.detection_index = entry.last_detection_index; + obs.confidence = entry.last_confidence; + obs.depth = entry.last_depth; + obs.depth_confidence = entry.last_depth_confidence; + obs.bbox_xyxy = { + static_cast(entry.last_bbox.x), + static_cast(entry.last_bbox.y), + static_cast(entry.last_bbox.x + entry.last_bbox.width), + static_cast(entry.last_bbox.y + entry.last_bbox.height), + }; + obs.keypoints_xyc = entry.last_keypoints_xyc; + obs.detection_pos_cam = entry.last_pos_cam; + obs.detection_pos_world = entry.last_pos_world; + } else { + obs.detection_index = -1; + obs.confidence = -1.0f; + obs.depth = -1.0f; + obs.depth_confidence = -1.0f; + obs.bbox_xyxy.fill(-1.0f); + obs.keypoints_xyc.fill(-1.0f); + obs.detection_pos_cam = Eigen::Vector3f::Constant(-1.0f); + obs.detection_pos_world = Eigen::Vector3f::Constant(-1.0f); + } out.push_back(std::move(obs)); } return out;