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

fix upstaris problem

Browse Source 
primary track一直都有，只是有时候会错误
This commit is contained in:
hjy
2026-04-19 18:11:22 +08:00
parent 5b177c90f4
commit 560a60068f
6 changed files with 196 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/control_command.yaml
+16 -1
View File
@@ -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}/
include/cloud_reprojection_ros_node.hpp
+2
View File
@@ -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_;
include/target_observation_processing.hpp
+16
View File
@@ -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;
script/record_dataset.sh
+6 -1
View File
@@ -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
src/cloud_reprojection_ros.cpp
+40 -4
View File
@@ -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_ : "<disabled>")
<< ((camera_image_compressed_ || always_send_sync_compressed_) ? sync_image_compressed_topic_ : "<disabled>")
<< "\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<PointCloud2>(sync_cloud_topic_, 10);
sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_topic_, 10);
if (always_send_sync_cloud_slam_) {
sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_topic_, 10);
}
sync_odom_pub_ = this->create_publisher<Odometry>(sync_odom_topic_, 10);
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
sync_image_pub_ = this->create_publisher<Image>(sync_image_topic_, 10);
if (camera_image_compressed_) {
if (camera_image_compressed_ || always_send_sync_compressed_) {
sync_image_compressed_pub_ =
this->create_publisher<CompressedImage>(sync_image_compressed_topic_, 10);
}
@@ -258,6 +262,8 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image");
this->declare_parameter<int>("register_keys.sync_camera_compressed", 0);
this->declare_parameter<int>("register_keys.always_send_sync_compressed", 0);
this->declare_parameter<int>("register_keys.always_send_sync_cloud_slam", 1);
this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
@@ -290,6 +296,10 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<int>("register_keys.target_reid_input_width", 128);
this->declare_parameter<int>("register_keys.target_reid_feature_dim", 512);
this->declare_parameter<int>("register_keys.target_reid_max_batch_size", 8);
this->declare_parameter<int>("register_keys.target_reid_crop_edge_margin_px", 0);
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);
#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<float>(
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.debug = debug_target_observation_;
target_observation_processor_ =
std::make_unique<odin_ros_driver::TargetObservationProcessor>();
@@ -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<uchar> 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);
}
src/target_observation_processing.cpp
+116 -22
View File
@@ -413,6 +413,7 @@ std::vector<int> 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<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.
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<int64_t>(
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<float>(frames_since);
int best_row = -1;
float best_dist = std::numeric_limits<float>::max();
for (int i = 0; i < row_count; ++i) {
if (stable_ids[static_cast<size_t>(i)] >= 0) {
continue;
}
const EntryCloudEstimate& row_est =
row_estimates[static_cast<size_t>(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<size_t>(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<size_t>(i)] >= 0) {
@@ -661,6 +709,7 @@ std::vector<int> 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<float> per_keypoint_depths;
std::vector<float> 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<int>(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<int>& kp_list, float weight_scale) {
for (const int kp_index : kp_list) {
if (kp_index >= static_cast<int>(pose.keypoints.size())) {
continue;
}
const auto& kp = pose.keypoints[static_cast<size_t>(kp_index)];
if (kp.confidence < 0.5f) {
continue;
}
std::vector<float> 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<int>(std::lround(projected.uv[i].x)),
static_cast<int>(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<size_t>(kp_index)];
if (kp.confidence < 0.5f) {
continue;
}
std::vector<float> 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<float> 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<int>(std::lround(projected.uv[i].x)),
static_cast<int>(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<int>(per_keypoint_depths.size());