publish invalid stable tracks as well ; could run with py ; add readme

README_track.md

@@ -1,3 +1,164 @@
-# yolo+ track使用说明
+# yolo + stable track 说明
-选取第一帧中心的人物作为target，持续建立gallery并使用bytetrack跟踪
+
-如果丢了使用lazy reid，基于gallery进行匹配
+这套逻辑的目标不是直接相信 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

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
+
 
 
 

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
+    // All active tracks from the most recent process() call. Every retained
-    // process() call. One TargetObservation per entry; is_primary_target
+    // stable track is emitted every frame; valid=true only when a fresh 3D
-    // marks the follow target. Empty if process() hasn't run yet.
+    // estimate exists for the current frame.
     std::vector<TargetObservation> 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_; }

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

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;

src/reid_trt_extractor.cpp

@@ -3,6 +3,7 @@
 #include <algorithm>
 #include <cmath>
 #include <cstring>
+#include <iterator>
 #include <numeric>
 #include <stdexcept>
 #include <utility>

src/target_observation_processing.cpp

@@ -581,12 +581,14 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
                 const auto it = tracks_.find(stable_ids[static_cast<size_t>(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<size_t>(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<int64_t>(config_.reid_feature_update_interval);
-                    if (empty_gallery || interval_due) {
+                    if (gallery_filling || interval_due) {
                         need_extract = true;
                         row_should_update_gallery[static_cast<size_t>(i)] = true;
                     }
@@ -921,6 +923,7 @@ std::vector<int> 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<int>(tracks(i, 7));
         if (!feature.empty() &&
             (row_needs_matching_feature[static_cast<size_t>(i)] ||
              (row_should_update_gallery[static_cast<size_t>(i)] &&
@@ -930,7 +933,6 @@ std::vector<int> 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<int>(tracks(i, 7));
         entry.last_detection_index = detection_index;
         if (detection_index >= 0 &&
             detection_index < static_cast<int>(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.valid = entry.has_last_pos && entry.last_pos_frame == frame_index_;
-        obs.depth = entry.last_depth;
+        if (obs.valid) {
-        obs.depth_confidence = entry.last_depth_confidence;
+            obs.detection_index = entry.last_detection_index;
-        obs.bbox_xyxy = {
+            obs.confidence = entry.last_confidence;
-            static_cast<float>(entry.last_bbox.x),
+            obs.depth = entry.last_depth;
-            static_cast<float>(entry.last_bbox.y),
+            obs.depth_confidence = entry.last_depth_confidence;
-            static_cast<float>(entry.last_bbox.x + entry.last_bbox.width),
+            obs.bbox_xyxy = {
-            static_cast<float>(entry.last_bbox.y + entry.last_bbox.height),
+                static_cast<float>(entry.last_bbox.x),
-        };
+                static_cast<float>(entry.last_bbox.y),
-        obs.keypoints_xyc = entry.last_keypoints_xyc;
+                static_cast<float>(entry.last_bbox.x + entry.last_bbox.width),
-        obs.detection_pos_cam = entry.last_pos_cam;
+                static_cast<float>(entry.last_bbox.y + entry.last_bbox.height),
-        obs.detection_pos_world = entry.last_pos_world;
+            };
+            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;