wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

确定是TrackGallery的问题,fix

Browse Source 
主要是两个问题： 1. trackgallery只有在有确定sid是才更新新的feature；2. sid应该永远按身份保留，不是顺序顺延
This commit is contained in:
hjy
2026-04-19 23:56:36 +08:00
parent 560a60068f
commit e1c2e8f970
5 changed files with 704 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/control_command.yaml
+23 -2
View File
@@ -77,7 +77,7 @@ register_keys:
# 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: 0 # sync_camera_topic would be default added /compressed
sync_camera_compressed: 1 # 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
@@ -97,6 +97,20 @@ register_keys:
# draw target observation result on overlay image and print debug info
# 0: off; 1: on
debug: 1
# Center-cropped detection ROI: the primary YOLO pass sees only this
# square region (pixels), improving effective resolution on central
# subjects at the cost of ignoring edge strips. Set 0 to disable.
# Detection ROI applied before the primary YOLO pass. Horizontal placement
# is always centered; y_offset is pixels from image top (set -1 to auto-
# center vertically). For a 1600x1296 image with ROI 1440x1080, centered
# vertically would be y_offset=108 ((1296-1080)/2); y_offset=54 biases the
# ROI slightly upward so heads/torsos stay inside when the camera tilts down.
target_detection_roi_enabled: 1
target_detection_roi_width_px: 1440
target_detection_roi_height_px: 1080
target_detection_roi_y_offset_px: 54
target_detection_roi_size_px: 1080
target_yolo_engine: "/home/hjy/HJY/Codes/AMP_ws/TargetPrediction/deploy/model/yolo26m-pose_960.trt"
# lazy ReID only runs after the current ByteTrack target is lost
# first valid frame still bootstraps by choosing the target closest to image center
# if a frame misses all detections, retry YOLO once inside a local ROI around the previous target bbox
@@ -111,7 +125,7 @@ register_keys:
target_reid_max_crop_aspect_ratio: 0.90
target_reid_feature_update_interval: 5
target_reid_lost_timeout_frames: 150
target_reid_gallery_size: 10
target_reid_gallery_size: 8
target_reid_input_height: 256
target_reid_input_width: 128
target_reid_feature_dim: 512
@@ -126,6 +140,13 @@ register_keys:
target_reid_3d_fallback_enabled: 1
target_reid_3d_fallback_base_m: 0.75
target_reid_3d_fallback_per_frame_m: 0.05
# A ByteTrack raw_id must persist for N consecutive frames (failing both
# 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
# 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}/
include/cloud_reprojection_ros_node.hpp
+1
View File
@@ -125,6 +125,7 @@ private:
std::unique_ptr<odin_ros_driver::TargetObservationProcessor> target_observation_processor_;
bool enable_target_observation_ = false;
bool debug_target_observation_ = false;
bool debug_reid_ = false;
#endif
void loadParameters();
include/target_observation_processing.hpp
+46 -10
View File
@@ -24,6 +24,16 @@ struct TargetObservationConfig {
std::string yolo_labels_path;
float yolo_conf = 0.45f;
float yolo_nms = 0.50f;
// Optional detection ROI applied to every frame before the YOLO call.
// Trades coverage (edge strips are ignored) for effective resolution on
// the subjects inside the ROI. Width/height are in image pixels; the
// ROI is clamped to the image and silently disabled if width*height
// covers the full image. Horizontal placement is always centered. The
// vertical offset is pixels from the image top; set to -1 to auto-center.
bool detection_roi_enabled = false;
int detection_roi_width_px = 1440;
int detection_roi_height_px = 1080;
int detection_roi_y_offset_px = 0;
float min_depth = 0.5f;
float max_depth = 12.0f;
float search_radius_px = 25.0f;
@@ -38,23 +48,17 @@ struct TargetObservationConfig {
float reid_max_crop_aspect_ratio = 0.9f;
int reid_feature_update_interval = 5;
int reid_lost_timeout_frames = 150;
int reid_gallery_size = 10;
int reid_gallery_size = 8;
int reid_input_height = 256;
int reid_input_width = 128;
int reid_feature_dim = 512;
int reid_max_batch_size = 8;
int max_concurrent_tracks = 16;
int max_concurrent_tracks = 8;
// 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
@@ -64,7 +68,9 @@ struct TargetObservationConfig {
bool reid_3d_fallback_enabled = true;
float reid_3d_fallback_base_m = 0.75f;
float reid_3d_fallback_per_frame_m = 0.05f;
int reid_confirm_hits_before_allocate = 5;
bool debug = false;
bool debug_reid = false;
};
struct TargetObservation {
@@ -105,7 +111,8 @@ struct TargetObservationDebugInfo {
bool recovered_by_reid = false;
bool lost_detection_compensation_attempted = false;
bool recovered_by_lost_detection_compensation = false;
float reid_similarity = -1.0f;
float reid_similarity = -1.0f; // cosine sim from Phase 3 ReID (≥0), or -1 if N/A
float reid_3d_fallback_distance = -1.0f; // metres from Phase 3.5, or -1 if N/A
float yolo_ms = 0.0f;
float mot_ms = 0.0f;
float cloud_project_ms = 0.0f;
@@ -118,6 +125,23 @@ struct TargetObservationDebugInfo {
float total_ms = 0.0f;
std::vector<yolos::pose::PoseResult> poses;
std::vector<cv::Point> valid_projected_pixels;
// Per-track-row snapshot from bind_and_update_tracks (aligned, one entry
// per row of the tracks matrix from ByteTrack). raw_track_ids are what
// ByteTrack emitted for this frame; row_stable_ids are what our
// TrackManager ultimately bound each row to. Both empty when the
// pipeline bailed before binding.
std::vector<int> raw_track_ids;
std::vector<int> row_stable_ids;
std::vector<int> row_detection_indices;
// Center-cropped ROI actually used for the primary YOLO pass this frame.
// Empty (area()==0) when config_.detection_roi_enabled is false or when
// the ROI was clamped away to nothing.
cv::Rect detection_roi;
// Flat record of every Phase 3 ReID similarity comparison that actually
// ran this frame. Tuple is (row_raw_track_id, candidate_stable_id, sim).
// Useful for debugging "why didn't this rebind" cases — read the log
// and you see each candidate's sim against each active gallery.
std::vector<std::tuple<int, int, float>> reid_comparisons;
};
class TargetObservationProcessor {
@@ -138,6 +162,8 @@ public:
// marks the follow target. Empty if process() hasn't run yet.
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_; }
cv::Rect last_detection_roi() const { return last_detection_roi_; }
bool initialized() const { return initialized_; }
@@ -170,7 +196,8 @@ private:
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;
float last_rebind_similarity = -1.0f; // cosine sim from ReID (≥0), -1 = none
float last_rebind_3d_distance = -1.0f; // metres from 3D fallback, -1 = none
// Per-entry 3D state; refreshed whenever compute_entry_3d succeeds.
float last_depth = -1.0f;
float last_depth_confidence = 0.0f;
@@ -198,6 +225,7 @@ private:
cv::Rect build_lost_detection_roi(
const cv::Size& image_size,
const cv::Rect& last_bbox) const;
cv::Rect build_detection_roi(const cv::Size& image_size) const;
static void offset_poses(
std::vector<yolos::pose::PoseResult>& poses,
const cv::Point& offset);
@@ -243,10 +271,18 @@ private:
std::unique_ptr<motcpp::trackers::ByteTrack> tracker_;
std::unique_ptr<ReIDTensorRTExtractor> reid_extractor_;
struct PendingRaw {
int consecutive_hits = 0;
int64_t last_hit_frame = -1;
};
std::unordered_map<int, TrackEntry> tracks_;
std::unordered_map<int, PendingRaw> pending_raw_ids_;
int next_stable_id_ = 0;
int current_target_stable_id_ = -1;
int last_selected_primary_stable_id_ = -1;
int64_t frame_index_ = 0;
cv::Rect last_detection_roi_;
bool initialized_ = false;
};
src/cloud_reprojection_ros.cpp
+293 -22
View File
@@ -41,6 +41,69 @@ std::filesystem::path get_package_source_directory_from_file()
return std::filesystem::path(__FILE__).parent_path().parent_path();
}
std_msgs::msg::ColorRGBA stable_track_color_rgba(int track_id, float alpha = 0.9f)
{
static const std::array<std::array<float, 3>, 8> kPalette{{
{1.0f, 0.0f, 0.0f},
{1.0f, 0.5f, 0.0f},
{1.0f, 0.85f, 0.0f},
{0.0f, 0.8f, 0.0f},
{0.0f, 1.0f, 1.0f},
{0.0f, 0.35f, 1.0f},
{0.29f, 0.0f, 0.51f},
{0.58f, 0.0f, 0.83f},
}};
std_msgs::msg::ColorRGBA color;
if (track_id < 0) {
color.r = 1.0f;
color.g = 1.0f;
color.b = 1.0f;
} else {
const auto& c = kPalette[static_cast<size_t>(track_id) % kPalette.size()];
color.r = c[0];
color.g = c[1];
color.b = c[2];
}
color.a = alpha;
return color;
}
cv::Scalar stable_track_color_bgr(int track_id)
{
if (track_id < 0) {
return cv::Scalar(255, 255, 255);
}
const auto color = stable_track_color_rgba(track_id, 1.0f);
return cv::Scalar(
static_cast<int>(std::lround(color.b * 255.0f)),
static_cast<int>(std::lround(color.g * 255.0f)),
static_cast<int>(std::lround(color.r * 255.0f)));
}
void draw_pose_with_yolo_api(
cv::Mat& image,
const yolos::pose::PoseResult& pose,
const cv::Scalar& color,
int kpt_radius = 3,
float kpt_threshold = 0.3f,
int line_thickness = 2)
{
if (image.empty()) {
return;
}
cv::Mat skeleton_vis = cv::Mat::zeros(image.rows, image.cols, CV_8UC3);
yolos::drawing::drawPoseSkeleton(
skeleton_vis,
pose.keypoints,
yolos::pose::YOLOPoseDetector::getPoseSkeleton(),
kpt_radius,
kpt_threshold,
line_thickness);
cv::Mat mask;
cv::cvtColor(skeleton_vis, mask, cv::COLOR_BGR2GRAY);
image.setTo(color, mask > 0);
}
std::filesystem::path get_target_prediction_root_directory()
{
return get_package_source_directory_from_file().parent_path().parent_path().parent_path().parent_path();
@@ -62,6 +125,64 @@ std::string format_bbox_xyxy(const std::array<float, 4>& bbox)
return oss.str();
}
std::string format_int_vec(const std::vector<int>& v)
{
std::ostringstream oss;
oss << "[";
for (size_t i = 0; i < v.size(); ++i) {
if (i > 0) {
oss << ",";
}
oss << v[i];
}
oss << "]";
return oss.str();
}
// Format the per-row ReID comparisons as
// bytetrack_id=[2->[sid0:0.990, sid1:0.566],1->[]]
// One group per bytetrack raw_id, inside are the sim against each
// candidate stable_id's gallery. Rows with no available comparisons emit [].
std::string format_reid_comparisons(
const std::vector<int>& raw_track_ids,
const std::vector<std::tuple<int, int, float>>& comparisons)
{
std::map<int, std::vector<std::pair<int, float>>> grouped;
for (const int raw_id : raw_track_ids) {
grouped.try_emplace(raw_id);
}
for (const auto& c : comparisons) {
grouped[std::get<0>(c)].emplace_back(std::get<1>(c), std::get<2>(c));
}
if (grouped.empty()) {
return "[]";
}
std::ostringstream oss;
oss << std::fixed << std::setprecision(3);
oss << "[";
bool first_raw = true;
for (auto& kv : grouped) {
if (!first_raw) {
oss << ",";
}
first_raw = false;
std::sort(kv.second.begin(), kv.second.end(),
[](const auto& a, const auto& b) {
return a.second > b.second; // sort by sim desc
});
oss << kv.first << "->[";
for (size_t i = 0; i < kv.second.size(); ++i) {
if (i > 0) {
oss << ",";
}
oss << "sid" << kv.second[i].first << ":" << kv.second[i].second;
}
oss << "]";
}
oss << "]";
return oss.str();
}
std::string format_keypoints_xyc(
const std::array<float, 17 * 3>& keypoints_xyc,
float min_confidence = 0.5f)
@@ -291,7 +412,7 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<double>("register_keys.target_reid_max_crop_aspect_ratio", 0.90);
this->declare_parameter<int>("register_keys.target_reid_feature_update_interval", 5);
this->declare_parameter<int>("register_keys.target_reid_lost_timeout_frames", 150);
this->declare_parameter<int>("register_keys.target_reid_gallery_size", 10);
this->declare_parameter<int>("register_keys.target_reid_gallery_size", 8);
this->declare_parameter<int>("register_keys.target_reid_input_height", 256);
this->declare_parameter<int>("register_keys.target_reid_input_width", 128);
this->declare_parameter<int>("register_keys.target_reid_feature_dim", 512);
@@ -300,6 +421,12 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<int>("register_keys.target_reid_3d_fallback_enabled", 1);
this->declare_parameter<double>("register_keys.target_reid_3d_fallback_base_m", 0.75);
this->declare_parameter<double>("register_keys.target_reid_3d_fallback_per_frame_m", 0.05);
this->declare_parameter<int>("register_keys.target_reid_confirm_hits_before_allocate", 5);
this->declare_parameter<int>("register_keys.debug_reid", 0);
this->declare_parameter<int>("register_keys.target_detection_roi_enabled", 0);
this->declare_parameter<int>("register_keys.target_detection_roi_width_px", 1440);
this->declare_parameter<int>("register_keys.target_detection_roi_height_px", 1080);
this->declare_parameter<int>("register_keys.target_detection_roi_y_offset_px", 0);
#endif
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
@@ -417,6 +544,8 @@ void CloudReprojectionRosNode::loadParameters()
(this->get_parameter("register_keys.process_target_observation").as_int() != 0);
debug_target_observation_ =
(this->get_parameter("register_keys.debug").as_int() != 0);
debug_reid_ =
(this->get_parameter("register_keys.debug_reid").as_int() != 0);
if (enable_target_observation_) {
try {
odin_ros_driver::TargetObservationConfig target_config;
@@ -474,6 +603,17 @@ void CloudReprojectionRosNode::loadParameters()
this->get_parameter("register_keys.target_reid_3d_fallback_base_m").as_double());
target_config.reid_3d_fallback_per_frame_m = static_cast<float>(
this->get_parameter("register_keys.target_reid_3d_fallback_per_frame_m").as_double());
target_config.reid_confirm_hits_before_allocate =
this->get_parameter("register_keys.target_reid_confirm_hits_before_allocate").as_int();
target_config.debug_reid = debug_reid_;
target_config.detection_roi_enabled =
(this->get_parameter("register_keys.target_detection_roi_enabled").as_int() != 0);
target_config.detection_roi_width_px =
this->get_parameter("register_keys.target_detection_roi_width_px").as_int();
target_config.detection_roi_height_px =
this->get_parameter("register_keys.target_detection_roi_height_px").as_int();
target_config.detection_roi_y_offset_px =
this->get_parameter("register_keys.target_detection_roi_y_offset_px").as_int();
target_config.debug = debug_target_observation_;
target_observation_processor_ =
std::make_unique<odin_ros_driver::TargetObservationProcessor>();
@@ -637,15 +777,42 @@ void CloudReprojectionRosNode::processSyncedData(
&target_debug);
const auto target_end = std::chrono::steady_clock::now();
if (target_debug.reid_attempted) {
// Exactly one of (sim, fallback_dist) is meaningful per rebind path;
// the other stays at -1 as "N/A". Emit a compact reason string so
// operators see which mechanism fired and how confident it was.
auto format_rebind_reason = [](float sim, float dist) {
char buf[64];
if (sim >= 0.0f) {
std::snprintf(buf, sizeof(buf),
"sim=%.3f fallback_dist=N/A", sim);
} else if (dist >= 0.0f) {
std::snprintf(buf, sizeof(buf),
"sim=N/A fallback_dist=%.2fm", dist);
} else {
std::snprintf(buf, sizeof(buf),
"sim=N/A fallback_dist=N/A");
}
return std::string(buf);
};
if (target_debug.reid_attempted ||
target_debug.reid_3d_fallback_distance >= 0.0f) {
const std::string reason = format_rebind_reason(
target_debug.reid_similarity,
target_debug.reid_3d_fallback_distance);
const std::string sims =
format_reid_comparisons(
target_debug.raw_track_ids,
target_debug.reid_comparisons);
if (target_debug.recovered_by_reid) {
RCLCPP_WARN(
this->get_logger(),
"\033[1;33m[LazyReID]\033[0m \033[1;32mRECOVERED\033[0m stable_id=%d raw_id=%d det_ind=%d sim=%.3f gallery=%d lost=%d lost_comp_attempted=%s lost_comp_recovered=%s",
"\033[1;33m[LazyReID]\033[0m \033[1;32mRECOVERED\033[0m stable_id=%d raw_id=%d det_ind=%d %s sims={%s} gallery=%d lost=%d lost_comp_attempted=%s lost_comp_recovered=%s",
target_debug.current_target_id_before,
target_debug.selected_raw_track_id,
target_debug.detection_index,
target_debug.reid_similarity,
reason.c_str(),
sims.c_str(),
target_debug.gallery_size,
target_debug.lost_frames,
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
@@ -653,9 +820,10 @@ void CloudReprojectionRosNode::processSyncedData(
} else {
RCLCPP_WARN(
this->get_logger(),
"\033[1;33m[LazyReID]\033[0m \033[1;31mNO_MATCH\033[0m prev_id=%d sim=%.3f gallery=%d lost=%d tracked=%d detections=%d lost_comp_attempted=%s lost_comp_recovered=%s",
"\033[1;33m[LazyReID]\033[0m \033[1;31mNO_MATCH\033[0m prev_sid=%d %s sims=%s gallery=%d lost=%d tracked=%d detections=%d lost_comp_attempted=%s lost_comp_recovered=%s",
target_debug.current_target_id_before,
target_debug.reid_similarity,
reason.c_str(),
sims.c_str(),
target_debug.gallery_size,
target_debug.lost_frames,
target_debug.tracks_count,
@@ -665,6 +833,20 @@ void CloudReprojectionRosNode::processSyncedData(
}
}
if (debug_reid_) {
const std::string sims =
format_reid_comparisons(
target_debug.raw_track_ids,
target_debug.reid_comparisons);
RCLCPP_INFO(
this->get_logger(),
"[ReIDMatrix] prev_sid=%d bytetrack_id=%s stable_id=%s sims=%s",
target_debug.current_target_id_before,
format_int_vec(target_debug.raw_track_ids).c_str(),
format_int_vec(target_debug.row_stable_ids).c_str(),
sims.c_str());
}
if (debug_target_observation_) {
if (!target_observation.valid) {
if (target_debug.poses_count == 0) {
@@ -695,9 +877,11 @@ void CloudReprojectionRosNode::processSyncedData(
this->get_logger(),
*this->get_clock(),
1000,
"Target observation | detections=%d tracked=%d active=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms",
"Target observation | detections=%d tracked=%d bytetrack_id=%s stable_id=%s active=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s sim=%.3f fallback_dist=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms",
target_debug.poses_count,
target_debug.tracks_count,
format_int_vec(target_debug.raw_track_ids).c_str(),
format_int_vec(target_debug.row_stable_ids).c_str(),
target_debug.active_tracks_count,
target_debug.selected_track_id,
target_debug.selected_raw_track_id,
@@ -708,6 +892,7 @@ void CloudReprojectionRosNode::processSyncedData(
target_debug.reid_attempted ? "yes" : "no",
target_debug.recovered_by_reid ? "yes" : "no",
target_debug.reid_similarity,
target_debug.reid_3d_fallback_distance,
target_debug.gallery_size,
target_debug.lost_frames,
target_debug.projected_cloud_points,
@@ -728,7 +913,7 @@ void CloudReprojectionRosNode::processSyncedData(
this->get_logger(),
*this->get_clock(),
500,
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d active=%d current_target_id=%d current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gate_rej=%d gate_soft=%d gallery=%d lost=%d bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms | node_total=%.2f ms",
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d bytetrack_id=%s stable_id=%s active=%d current_target_id=%d current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s sim=%.3f fallback_dist=%.3f gate_rej=%d gate_soft=%d gallery=%d lost=%d bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms | node_total=%.2f ms",
image_msg.header.stamp.sec,
image_msg.header.stamp.nanosec,
cam_bgr.cols,
@@ -736,6 +921,8 @@ void CloudReprojectionRosNode::processSyncedData(
cloud_cam.size(),
target_debug.poses_count,
target_debug.tracks_count,
format_int_vec(target_debug.raw_track_ids).c_str(),
format_int_vec(target_debug.row_stable_ids).c_str(),
target_debug.active_tracks_count,
target_debug.current_target_id_before,
target_debug.current_raw_track_id_before,
@@ -749,6 +936,7 @@ void CloudReprojectionRosNode::processSyncedData(
target_debug.reid_attempted ? "yes" : "no",
target_debug.recovered_by_reid ? "yes" : "no",
target_debug.reid_similarity,
target_debug.reid_3d_fallback_distance,
target_debug.reid_3d_gate_rejections,
target_debug.reid_3d_gate_soft_warnings,
target_debug.gallery_size,
@@ -836,7 +1024,7 @@ void CloudReprojectionRosNode::processSyncedData(
track_array_msg.header = image_msg.header;
track_array_msg.header.stamp = sync_stamp;
track_array_msg.primary_target_id =
target_observation_processor_->current_target_stable_id();
target_observation_processor_->current_frame_primary_stable_id();
track_array_msg.observations.reserve(active_tracks.size());
for (const auto& obs : active_tracks) {
odin_ros_driver::msg::TargetObservation one;
@@ -861,7 +1049,10 @@ void CloudReprojectionRosNode::processSyncedData(
? std::string("odom")
: odom_msg->header.frame_id;
auto build_marker_array = [&active_tracks](
const int marker_primary_id =
target_observation.valid ? target_observation.track_id : -1;
auto build_marker_array = [&active_tracks, marker_primary_id](
const std_msgs::msg::Header& header, bool world_frame) {
visualization_msgs::msg::MarkerArray out;
out.markers.reserve(1 + active_tracks.size() * 2);
@@ -887,16 +1078,7 @@ void CloudReprojectionRosNode::processSyncedData(
sphere.scale.x = 0.3;
sphere.scale.y = 0.3;
sphere.scale.z = 0.3;
if (obs.is_primary_target) {
sphere.color.r = 1.0f;
sphere.color.g = 0.2f;
sphere.color.b = 0.2f;
} else {
sphere.color.r = 1.0f;
sphere.color.g = 0.95f;
sphere.color.b = 0.2f;
}
sphere.color.a = 0.9f;
sphere.color = stable_track_color_rgba(obs.track_id, 0.9f);
sphere.lifetime.sec = 0;
sphere.lifetime.nanosec = 300000000; // 0.3 s
out.markers.push_back(sphere);
@@ -918,7 +1100,7 @@ void CloudReprojectionRosNode::processSyncedData(
label.color.a = 1.0f;
label.lifetime.sec = 0;
label.lifetime.nanosec = 300000000;
label.text = (obs.is_primary_target ? "*id=" : "id=") +
label.text = (obs.track_id == marker_primary_id ? "*id=" : "id=") +
std::to_string(obs.track_id);
out.markers.push_back(label);
}
@@ -934,6 +1116,22 @@ void CloudReprojectionRosNode::processSyncedData(
cv::Mat overlay_vis = odin_ros_driver::overlay_projected_cloud_on_image(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (!overlay_vis.empty()) {
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
// Draw the detection ROI (yellow rectangle) when it is active so
// operators can see which portion of the image the primary YOLO
// pass is actually looking at.
if (enable_target_observation_ && target_observation_processor_) {
const cv::Rect roi =
target_observation_processor_->last_detection_roi();
if (roi.area() > 0) {
cv::rectangle(overlay_vis, roi, cv::Scalar(0, 255, 255), 2);
cv::putText(overlay_vis, "detection_roi",
cv::Point(roi.x + 8, roi.y + 22),
cv::FONT_HERSHEY_SIMPLEX, 0.55,
cv::Scalar(0, 255, 255), 2);
}
}
#endif
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
@@ -956,8 +1154,81 @@ void CloudReprojectionRosNode::processSyncedData(
detection_debug_compressed_pub_) {
cv::Mat detection_debug_vis = cam_bgr.clone();
if (!detection_debug_vis.empty()) {
target_observation_processor_->draw_detected_poses(detection_debug_vis, target_debug.poses);
std::vector<int> detection_stable_ids(
target_debug.poses.size(), -1);
for (size_t row = 0; row < target_debug.row_stable_ids.size() &&
row < target_debug.row_detection_indices.size();
++row) {
const int sid = target_debug.row_stable_ids[row];
const int det_idx = target_debug.row_detection_indices[row];
if (det_idx < 0 ||
det_idx >= static_cast<int>(target_debug.poses.size())) {
continue;
}
if (sid >= 0) {
detection_stable_ids[static_cast<size_t>(det_idx)] = sid;
}
}
for (size_t det_idx = 0; det_idx < target_debug.poses.size(); ++det_idx) {
const auto& pose = target_debug.poses[det_idx];
const int sid = detection_stable_ids[det_idx];
const cv::Scalar color = stable_track_color_bgr(sid);
draw_pose_with_yolo_api(detection_debug_vis, pose, color, 3, 0.3f, 2);
const cv::Rect bbox(
static_cast<int>(std::lround(pose.box.x)),
static_cast<int>(std::lround(pose.box.y)),
static_cast<int>(std::lround(pose.box.width)),
static_cast<int>(std::lround(pose.box.height)));
cv::rectangle(detection_debug_vis, bbox, color, 2, cv::LINE_AA);
}
for (size_t row = 0; row < target_debug.row_stable_ids.size() &&
row < target_debug.row_detection_indices.size();
++row) {
const int sid = target_debug.row_stable_ids[row];
const int det_idx = target_debug.row_detection_indices[row];
if (sid < 0 || det_idx < 0 ||
det_idx >= static_cast<int>(target_debug.poses.size())) {
continue;
}
const auto& pose = target_debug.poses[static_cast<size_t>(det_idx)];
const cv::Scalar color = stable_track_color_bgr(sid);
const cv::Rect bbox(
static_cast<int>(std::lround(pose.box.x)),
static_cast<int>(std::lround(pose.box.y)),
static_cast<int>(std::lround(pose.box.width)),
static_cast<int>(std::lround(pose.box.height)));
cv::rectangle(detection_debug_vis, bbox, color, 3, cv::LINE_AA);
const std::string sid_text = "sid=" + std::to_string(sid);
cv::putText(
detection_debug_vis,
sid_text,
cv::Point(bbox.x, std::max(20, bbox.y - 10)),
cv::FONT_HERSHEY_SIMPLEX,
0.6,
cv::Scalar(0, 0, 0),
3,
cv::LINE_AA);
cv::putText(
detection_debug_vis,
sid_text,
cv::Point(bbox.x, std::max(20, bbox.y - 10)),
cv::FONT_HERSHEY_SIMPLEX,
0.6,
color,
2,
cv::LINE_AA);
}
odin_ros_driver::draw_target_observation_overlay(detection_debug_vis, target_observation);
if (target_debug.detection_roi.area() > 0) {
cv::rectangle(detection_debug_vis, target_debug.detection_roi,
cv::Scalar(0, 255, 255), 2);
cv::putText(detection_debug_vis, "detection_roi",
cv::Point(target_debug.detection_roi.x + 8,
target_debug.detection_roi.y + 22),
cv::FONT_HERSHEY_SIMPLEX, 0.55,
cv::Scalar(0, 255, 255), 2);
}
std::vector<uchar> dbuf;
const std::vector<int> denc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
src/target_observation_processing.cpp
+341 -70
View File
@@ -18,6 +18,50 @@ namespace odin_ros_driver {
namespace {
cv::Scalar stable_track_color_bgr(int track_id)
{
static const std::array<cv::Scalar, 8> kPalette{{
cv::Scalar(0, 0, 255), // red
cv::Scalar(0, 128, 255), // orange
cv::Scalar(0, 215, 255), // yellow
cv::Scalar(0, 200, 0), // green
cv::Scalar(255, 255, 0), // cyan
cv::Scalar(255, 0, 0), // blue
cv::Scalar(130, 0, 75), // indigo-like
cv::Scalar(211, 0, 148), // violet
}};
if (track_id < 0) {
return cv::Scalar(255, 255, 255);
}
return kPalette[static_cast<size_t>(track_id) % kPalette.size()];
}
void draw_pose_skeleton_with_yolo_api(
cv::Mat& image_bgr,
const std::vector<yolos::KeyPoint>& keypoints,
const cv::Scalar& color,
int kpt_radius = 3,
float kpt_threshold = 0.3f,
int line_thickness = 2)
{
if (image_bgr.empty() || keypoints.empty()) {
return;
}
cv::Mat skeleton_vis =
cv::Mat::zeros(image_bgr.rows, image_bgr.cols, CV_8UC3);
yolos::drawing::drawPoseSkeleton(
skeleton_vis,
keypoints,
yolos::pose::YOLOPoseDetector::getPoseSkeleton(),
kpt_radius,
kpt_threshold,
line_thickness);
cv::Mat mask;
cv::cvtColor(skeleton_vis, mask, cv::COLOR_BGR2GRAY);
image_bgr.setTo(color, mask > 0);
}
// COCO upper-body keypoints used for depth estimation and the torso position
// anchor. 5/6 = shoulders, 11/12 = hips. Shoulders+hips are stable under
// occlusion, have healthy LiDAR point density, and avoid arms (high pose
@@ -118,8 +162,10 @@ void TargetObservationProcessor::initialize(const TargetObservationConfig& confi
tracker_ = std::make_unique<motcpp::trackers::ByteTrack>(
0.3f, 30, 50, 3, 0.3f, false, 1);
tracks_.clear();
pending_raw_ids_.clear();
next_stable_id_ = 0;
current_target_stable_id_ = -1;
last_selected_primary_stable_id_ = -1;
frame_index_ = 0;
reid_extractor_.reset();
if (config_.reid_enabled && !config_.reid_engine_path.empty()) {
@@ -153,6 +199,7 @@ TargetObservation TargetObservationProcessor::process(
const auto total_start = std::chrono::steady_clock::now();
TargetObservation empty;
++frame_index_;
last_selected_primary_stable_id_ = -1;
auto populate_follow_debug = [&](TargetObservationDebugInfo* info) {
if (!info) {
@@ -192,10 +239,20 @@ TargetObservation TargetObservationProcessor::process(
}
const auto yolo_start = std::chrono::steady_clock::now();
std::vector<yolos::pose::PoseResult> poses =
yolo_->detect(camera_bgr, config_.yolo_conf, config_.yolo_nms);
if (poses.empty()) {
poses = detect_poses_with_lost_compensation(camera_bgr, debug_info);
const cv::Rect detection_roi = build_detection_roi(camera_bgr.size());
last_detection_roi_ = detection_roi;
if (debug_info) {
debug_info->detection_roi = detection_roi;
}
std::vector<yolos::pose::PoseResult> poses;
if (detection_roi.area() > 0) {
const cv::Mat roi_bgr = camera_bgr(detection_roi).clone();
poses = yolo_->detect(roi_bgr, config_.yolo_conf, config_.yolo_nms);
if (!poses.empty()) {
offset_poses(poses, detection_roi.tl());
}
} else {
poses = yolo_->detect(camera_bgr, config_.yolo_conf, config_.yolo_nms);
}
const auto yolo_end = std::chrono::steady_clock::now();
if (debug_info) {
@@ -317,6 +374,35 @@ std::vector<yolos::pose::PoseResult> TargetObservationProcessor::detect_poses_wi
return poses;
}
cv::Rect TargetObservationProcessor::build_detection_roi(
const cv::Size& image_size) const
{
if (!config_.detection_roi_enabled) {
return {};
}
int w = config_.detection_roi_width_px;
int h = config_.detection_roi_height_px;
if (w <= 0 || h <= 0 ||
image_size.width <= 0 || image_size.height <= 0) {
return {};
}
// Disable if the ROI would cover the full image: just let YOLO see
// the full frame directly.
if (w >= image_size.width && h >= image_size.height) {
return {};
}
w = std::min(w, image_size.width);
h = std::min(h, image_size.height);
const int x = (image_size.width - w) / 2;
int y;
if (config_.detection_roi_y_offset_px < 0) {
y = (image_size.height - h) / 2; // auto-center vertically
} else {
y = std::min(config_.detection_roi_y_offset_px, image_size.height - h);
}
return cv::Rect(x, y, w, h);
}
cv::Rect TargetObservationProcessor::build_lost_detection_roi(
const cv::Size& image_size,
const cv::Rect& last_bbox) const
@@ -364,9 +450,8 @@ void TargetObservationProcessor::draw_detected_poses(
return;
}
yolo_->drawSkeletonsOnly(image_bgr, poses);
const cv::Scalar detection_box_color(0, 255, 255);
const cv::Scalar detection_color(255, 255, 255);
yolo_->drawSkeletonsOnly(image_bgr, poses, 3, 0.3f, 2);
for (size_t i = 0; i < poses.size(); ++i) {
const auto& pose = poses[i];
const cv::Point top_left(
@@ -375,7 +460,7 @@ void TargetObservationProcessor::draw_detected_poses(
const cv::Point bottom_right(
static_cast<int>(std::lround(pose.box.x + pose.box.width)),
static_cast<int>(std::lround(pose.box.y + pose.box.height)));
cv::rectangle(image_bgr, top_left, bottom_right, detection_box_color, 3);
cv::rectangle(image_bgr, top_left, bottom_right, detection_color, 2, cv::LINE_AA);
}
}
@@ -415,12 +500,14 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
kv.second.just_rebound_by_reid = false;
kv.second.just_rebound_by_3d_fallback = false;
kv.second.last_rebind_similarity = -1.0f;
kv.second.last_rebind_3d_distance = -1.0f;
}
// Phase 1: fast-path binding by raw_track_id. We only trust raw IDs that
// are still "hot" (the entry was seen in the immediately previous frame);
// ByteTrack can reuse retired raw IDs for a different person.
std::unordered_set<int> taken_sids;
std::vector<bool> row_bound_by_fast_path(static_cast<size_t>(row_count), false);
taken_sids.reserve(tracks_.size());
for (int i = 0; i < row_count; ++i) {
const int raw_id = static_cast<int>(tracks(i, 4));
@@ -435,6 +522,7 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
continue;
}
stable_ids[static_cast<size_t>(i)] = kv.first;
row_bound_by_fast_path[static_cast<size_t>(i)] = true;
taken_sids.insert(kv.first);
break;
}
@@ -475,6 +563,8 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
!camera_bgr.empty();
std::vector<int> rows_to_reid;
std::vector<cv::Rect> reid_boxes;
std::vector<bool> row_should_update_gallery(static_cast<size_t>(row_count), false);
std::vector<bool> row_needs_matching_feature(static_cast<size_t>(row_count), false);
if (reid_ready) {
rows_to_reid.reserve(row_count);
reid_boxes.reserve(row_count);
@@ -483,22 +573,29 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
if (!is_good_reid_crop(bbox, camera_bgr.size())) {
continue;
}
bool need_extract = config_.debug_reid;
if (stable_ids[static_cast<size_t>(i)] < 0) {
rows_to_reid.push_back(i);
reid_boxes.push_back(bbox);
continue;
need_extract = true;
row_needs_matching_feature[static_cast<size_t>(i)] = true;
} else {
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 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) {
need_extract = true;
row_should_update_gallery[static_cast<size_t>(i)] = true;
}
}
}
const auto it = tracks_.find(stable_ids[static_cast<size_t>(i)]);
if (it == tracks_.end()) {
continue;
if (stable_ids[static_cast<size_t>(i)] < 0) {
row_should_update_gallery[static_cast<size_t>(i)] = false;
}
const TrackEntry& entry = it->second;
const bool empty_gallery = entry.gallery.empty();
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 (need_extract) {
rows_to_reid.push_back(i);
reid_boxes.push_back(bbox);
}
@@ -521,14 +618,134 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
}
}
// Phase 3: ReID recovery. For rows that still lack a stable_id, compare
// their feature against every unmatched entry's gallery and rebind if
// top1 passes the match threshold and the top1-top2 gap is wide enough.
// A 3D position gate rejects (soft-log / hard-reject) candidates whose
// current world pos is implausibly far from the entry's last pos.
if (debug_info && config_.debug_reid) {
debug_info->reid_comparisons.clear();
for (int i = 0; i < row_count; ++i) {
const int row_raw_id = static_cast<int>(tracks(i, 4));
const ReIDFeature& feature = features_by_row[static_cast<size_t>(i)];
if (feature.empty()) {
continue;
}
for (const auto& kv : tracks_) {
if (kv.second.gallery.empty()) {
continue;
}
const float sim = kv.second.gallery.best_similarity(feature);
debug_info->reid_comparisons.emplace_back(
row_raw_id, kv.first, sim);
}
}
}
// Phase 2C: raw-id fast-path is only an initial guess. If ReID strongly
// prefers a different *currently free* stable_id, revoke the fast-path
// assignment and let Phase 3 rebind it. This prevents one temporary
// ByteTrack carry-over from occupying the wrong identity slot and forcing
// the true owner into a brand-new sid.
for (int i = 0; i < row_count; ++i) {
if (!row_bound_by_fast_path[static_cast<size_t>(i)]) {
continue;
}
const int bound_sid = stable_ids[static_cast<size_t>(i)];
if (bound_sid < 0) {
continue;
}
const ReIDFeature& feature = features_by_row[static_cast<size_t>(i)];
if (feature.empty()) {
continue;
}
float bound_sim = -1.0f;
float best_other_sim = -1.0f;
int best_other_sid = -1;
for (const auto& kv : tracks_) {
if (kv.second.gallery.empty()) {
continue;
}
const float sim = kv.second.gallery.best_similarity(feature);
if (kv.first == bound_sid) {
bound_sim = sim;
continue;
}
if (sim > best_other_sim) {
best_other_sim = sim;
best_other_sid = kv.first;
}
}
const bool should_veto_fast_path =
best_other_sid >= 0 &&
!taken_sids.count(best_other_sid) &&
best_other_sim >= config_.reid_match_threshold &&
(bound_sim < 0.0f ||
(best_other_sim - bound_sim) >= config_.reid_gap_threshold);
if (!should_veto_fast_path) {
continue;
}
taken_sids.erase(bound_sid);
stable_ids[static_cast<size_t>(i)] = -1;
row_bound_by_fast_path[static_cast<size_t>(i)] = false;
row_needs_matching_feature[static_cast<size_t>(i)] = true;
row_should_update_gallery[static_cast<size_t>(i)] = false;
}
// Guard periodic gallery refresh for raw-id fast-path rows. If ReID
// strongly prefers another stable_id, do not write this frame's feature
// back into the current entry's gallery, otherwise one bad ByteTrack
// carry-over can pollute the identity bank for many frames afterward.
std::vector<bool> row_gallery_refresh_allowed(
static_cast<size_t>(row_count), true);
for (int i = 0; i < row_count; ++i) {
if (!row_bound_by_fast_path[static_cast<size_t>(i)] ||
!row_should_update_gallery[static_cast<size_t>(i)]) {
continue;
}
const int bound_sid = stable_ids[static_cast<size_t>(i)];
if (bound_sid < 0) {
continue;
}
const ReIDFeature& feature = features_by_row[static_cast<size_t>(i)];
if (feature.empty()) {
continue;
}
float bound_sim = -1.0f;
float best_sim = -1.0f;
float best_other_sim = -1.0f;
int best_sid = -1;
for (const auto& kv : tracks_) {
if (kv.second.gallery.empty()) {
continue;
}
const float sim = kv.second.gallery.best_similarity(feature);
if (kv.first == bound_sid) {
bound_sim = sim;
}
if (sim > best_sim) {
best_sim = sim;
best_sid = kv.first;
}
if (kv.first != bound_sid && sim > best_other_sim) {
best_other_sim = sim;
}
}
const bool current_sid_supported =
bound_sim >= config_.reid_match_threshold &&
best_sid == bound_sid &&
(bound_sim - best_other_sim) >= config_.reid_gap_threshold;
if (!current_sid_supported) {
row_gallery_refresh_allowed[static_cast<size_t>(i)] = false;
}
}
// Phase 3: ReID recovery for rows that ByteTrack/raw-id continuity did
// not bind. If a row matches an existing gallery, bind it immediately.
// Only brand-new unmatched rows go through the consecutive-hit gate in
// Phase 4 before receiving a fresh stable_id.
bool reid_recovery_attempted = false;
int gate_hard_rejections = 0;
int gate_soft_warnings = 0;
for (int i = 0; i < row_count; ++i) {
if (stable_ids[static_cast<size_t>(i)] >= 0) {
continue;
@@ -537,40 +754,32 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
if (feature.empty()) {
continue;
}
const EntryCloudEstimate& row_est = row_estimates[static_cast<size_t>(i)];
const int row_raw_id = static_cast<int>(tracks(i, 4));
int best_sid = -1;
float top1 = -1.0f;
float top2 = -1.0f;
for (const auto& kv : tracks_) {
if (taken_sids.count(kv.first) || kv.second.gallery.empty()) {
if (kv.second.gallery.empty()) {
continue;
}
// 3D gate: only apply when both sides have valid world positions.
bool gate_pass = true;
if (row_est.valid && kv.second.has_last_pos) {
const float dist =
(kv.second.last_pos_world - row_est.pos_world).norm();
const int64_t frames_since = std::max<int64_t>(
0, frame_index_ - kv.second.last_seen_frame);
const float dmax = config_.reid_3d_gate_base_m +
config_.reid_3d_gate_per_frame_m *
static_cast<float>(frames_since);
if (dist > dmax) {
if (config_.reid_3d_gate_hard) {
++gate_hard_rejections;
continue; // skip this (row, entry) pair
} else {
++gate_soft_warnings;
gate_pass = false; // (soft mode: still compare)
}
}
// Always compute the similarity — even against entries that are
// already taken by fast-path or a previous Phase 3 iteration —
// because this number is our only way to explain the binding
// decision in the log afterward. The "taken" check is enforced
// below on the binding candidate selection only.
const float sim = kv.second.gallery.best_similarity(feature);
if (debug_info && !config_.debug_reid) {
debug_info->reid_comparisons.emplace_back(
row_raw_id, kv.first, sim);
}
if (taken_sids.count(kv.first)) {
continue; // cannot bind to a claimed entry
}
(void)gate_pass;
reid_recovery_attempted = true;
const float sim = kv.second.gallery.best_similarity(feature);
if (sim > top1) {
top2 = top1;
top1 = sim;
@@ -594,15 +803,11 @@ std::vector<int> 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.
// Phase 3.5: 3D fallback is only for the single-person case. In multi-
// person frames, rely purely on raw-id continuity + ReID.
int fallback_rebinds = 0;
if (config_.reid_3d_fallback_enabled &&
row_count == 1 &&
current_target_stable_id_ >= 0 &&
!taken_sids.count(current_target_stable_id_)) {
auto target_it = tracks_.find(current_target_stable_id_);
@@ -618,6 +823,13 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
if (stable_ids[static_cast<size_t>(i)] >= 0) {
continue;
}
// Only rescue rows whose ReID path was blocked by missing
// appearance features. If a crop produced a feature but failed
// ReID matching, do not let pure 3D proximity override that
// negative signal and hijack the follow target.
if (!features_by_row[static_cast<size_t>(i)].empty()) {
continue;
}
const EntryCloudEstimate& row_est =
row_estimates[static_cast<size_t>(i)];
if (!row_est.valid) {
@@ -635,17 +847,42 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
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;
target_it->second.last_rebind_3d_distance = best_dist;
fallback_rebinds = 1;
}
}
}
// Phase 4: allocate a fresh stable_id for rows that still have no match.
// Apply the confirmed-track gate: a ByteTrack raw_id must show up for
// `reid_confirm_hits_before_allocate` consecutive frames before getting
// its own stable_id. Short-lived ByteTrack IDs (noise, brief re-splits)
// stay in pending and never pollute the stable_id space.
const int confirm_threshold =
std::max(1, config_.reid_confirm_hits_before_allocate);
for (int i = 0; i < row_count; ++i) {
if (stable_ids[static_cast<size_t>(i)] >= 0) {
continue;
}
const int raw_id = static_cast<int>(tracks(i, 4));
auto [pit, inserted] = pending_raw_ids_.try_emplace(raw_id);
PendingRaw& pending = pit->second;
if (inserted || pending.last_hit_frame < frame_index_ - 1) {
pending.consecutive_hits = 1; // streak restart
} else {
pending.consecutive_hits += 1;
}
pending.last_hit_frame = frame_index_;
if (pending.consecutive_hits < confirm_threshold) {
// Not yet promoted. Row stays unbound this frame — Phase 5 will
// skip it, snapshot_active_tracks won't emit it, and no observation
// fires. Pending hit count will keep growing while ByteTrack keeps
// the same raw_id alive.
continue;
}
// Promoted: allocate stable_id and drop the pending record.
const int new_sid = next_stable_id_++;
TrackEntry entry;
entry.stable_id = new_sid;
@@ -654,6 +891,16 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
tracks_.emplace(new_sid, std::move(entry));
stable_ids[static_cast<size_t>(i)] = new_sid;
taken_sids.insert(new_sid);
pending_raw_ids_.erase(pit);
}
// Prune pending raw_ids that weren't touched this frame (streak lost).
for (auto it = pending_raw_ids_.begin(); it != pending_raw_ids_.end();) {
if (it->second.last_hit_frame < frame_index_) {
it = pending_raw_ids_.erase(it);
} else {
++it;
}
}
// Phase 5: per-row bookkeeping + gallery update + 2D/3D cache writeback.
@@ -670,7 +917,10 @@ 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)];
if (!feature.empty()) {
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_);
}
@@ -707,14 +957,29 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
if (debug_info) {
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_gate_rejections = 0;
debug_info->reid_3d_gate_soft_warnings = 0;
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.
debug_info->depth_ms = depth_compute_ms;
debug_info->reid_extract_ms = reid_extract_ms;
// Snapshot the per-row raw→stable binding so downstream logs can
// show exactly which ByteTrack id landed on which TrackManager id.
debug_info->raw_track_ids.clear();
debug_info->row_stable_ids.clear();
debug_info->row_detection_indices.clear();
debug_info->raw_track_ids.reserve(row_count);
debug_info->row_stable_ids.reserve(row_count);
debug_info->row_detection_indices.reserve(row_count);
for (int i = 0; i < row_count; ++i) {
debug_info->raw_track_ids.push_back(static_cast<int>(tracks(i, 4)));
debug_info->row_stable_ids.push_back(
stable_ids[static_cast<size_t>(i)]);
debug_info->row_detection_indices.push_back(
static_cast<int>(tracks(i, 7)));
}
}
return stable_ids;
}
@@ -738,15 +1003,14 @@ void TargetObservationProcessor::age_out_tracks()
}
}
// Soft cap: evict least-recently-seen entries (never the follow target).
// Soft cap: evict least-recently-seen entries. Primary selection is
// resolved later; stable-id lifetime itself should not special-case the
// current follow target.
const int cap = std::max(1, config_.max_concurrent_tracks);
while (static_cast<int>(tracks_.size()) > cap) {
int lru_sid = -1;
int64_t oldest = std::numeric_limits<int64_t>::max();
for (const auto& kv : tracks_) {
if (kv.first == current_target_stable_id_) {
continue;
}
if (kv.second.last_seen_frame < oldest) {
oldest = kv.second.last_seen_frame;
lru_sid = kv.first;
@@ -840,6 +1104,7 @@ TargetObservation TargetObservationProcessor::select_follow_target(
observation.raw_track_id = selected_raw_track_id;
observation.detection_index = detection_index;
observation.confidence = pose.conf;
last_selected_primary_stable_id_ = selected_sid;
observation.bbox_xyxy = {
static_cast<float>(pose.box.x),
static_cast<float>(pose.box.y),
@@ -854,17 +1119,22 @@ TargetObservation TargetObservationProcessor::select_follow_target(
// 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.
// counts as "recovered" for downstream logging. The two paths report via
// independent debug fields so the log stays unambiguous: reid_similarity
// is strictly cosine ≥ 0; reid_3d_fallback_distance is metres ≥ 0.
bool recovered_by_reid = false;
float rebind_sim = -1.0f;
float rebind_3d_distance = -1.0f;
const auto it = tracks_.find(selected_sid);
if (it != tracks_.end()) {
recovered_by_reid = it->second.just_rebound_by_reid ||
it->second.just_rebound_by_3d_fallback;
rebind_sim = it->second.last_rebind_similarity;
rebind_3d_distance = it->second.last_rebind_3d_distance;
it->second.just_rebound_by_reid = false;
it->second.just_rebound_by_3d_fallback = false;
it->second.last_rebind_similarity = -1.0f;
it->second.last_rebind_3d_distance = -1.0f;
}
if (debug_info) {
debug_info->found_existing_target = found_existing;
@@ -875,6 +1145,7 @@ TargetObservation TargetObservationProcessor::select_follow_target(
debug_info->detection_index = detection_index;
debug_info->recovered_by_reid = recovered_by_reid;
debug_info->reid_similarity = rebind_sim;
debug_info->reid_3d_fallback_distance = rebind_3d_distance;
}
return observation;
}
@@ -1158,7 +1429,7 @@ TargetObservationProcessor::snapshot_active_tracks() const
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 == current_target_stable_id_);
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;
@@ -1185,7 +1456,7 @@ void draw_target_observation_overlay(
return;
}
const cv::Scalar box_color(0, 0, 255);
const cv::Scalar box_color = stable_track_color_bgr(observation.track_id);
const int x1 = static_cast<int>(std::lround(observation.bbox_xyxy[0]));
const int y1 = static_cast<int>(std::lround(observation.bbox_xyxy[1]));
const int x2 = static_cast<int>(std::lround(observation.bbox_xyxy[2]));
@@ -1199,7 +1470,7 @@ void draw_target_observation_overlay(
}
const int u = static_cast<int>(std::lround(observation.keypoints_xyc[i * 3 + 0]));
const int v = static_cast<int>(std::lround(observation.keypoints_xyc[i * 3 + 1]));
cv::circle(image_bgr, cv::Point(u, v), 3, cv::Scalar(0, 255, 0), -1);
cv::circle(image_bgr, cv::Point(u, v), 3, box_color, -1);
}
const std::string label = format_target_text(observation);