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

change depth estimation from only HIP into UPPER+HIP

Browse Source
This commit is contained in:
hjy
2026-04-19 15:18:00 +08:00
parent 983da3e207
commit a1fddc9638
2 changed files with 72 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
script/record_dataset.sh
+1
View File
@@ -3,5 +3,6 @@ ros2 bag record \
/odin1/sync/image/compressed \
/odin1/sync/odometry \
/odin1/sync/wiwc \
/lowstate \
# /odin1/combined_image/compressed
src/target_observation_processing.cpp
+71 -22
View File
@@ -16,7 +16,11 @@ namespace odin_ros_driver {
namespace {
constexpr std::array<int, 2> kHipKeypointIndices = {11, 12};
// 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
// variance) and legs (often truncated in follow scenes).
constexpr std::array<int, 4> kUpperTorsoKeypointIndices = {5, 6, 11, 12};
cv::Rect track_row_to_rect(const Eigen::MatrixXf& tracks, int row)
{
@@ -47,6 +51,36 @@ float median_in_place(std::vector<float>& values)
return *mid;
}
float weighted_median(const std::vector<float>& values, const std::vector<float>& weights)
{
if (values.empty()) {
return -1.0f;
}
std::vector<size_t> order(values.size());
std::iota(order.begin(), order.end(), size_t{0});
std::sort(
order.begin(), order.end(),
[&values](size_t a, size_t b) { return values[a] < values[b]; });
float total = 0.0f;
for (const float w : weights) {
total += w;
}
if (total <= 0.0f) {
std::vector<float> tmp = values;
return median_in_place(tmp);
}
const float half = 0.5f * total;
float cum = 0.0f;
for (const size_t i : order) {
cum += weights[i];
if (cum >= half) {
return values[i];
}
}
return values[order.back()];
}
std::string format_target_text(const TargetObservation& observation)
{
std::ostringstream oss;
@@ -652,11 +686,13 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
}
std::vector<float> per_keypoint_depths;
per_keypoint_depths.reserve(kHipKeypointIndices.size());
std::vector<float> per_keypoint_weights;
per_keypoint_depths.reserve(kUpperTorsoKeypointIndices.size());
per_keypoint_weights.reserve(kUpperTorsoKeypointIndices.size());
std::vector<cv::Point> valid_projected_pixels;
valid_projected_pixels.reserve(128);
for (const int kp_index : kHipKeypointIndices) {
for (const int kp_index : kUpperTorsoKeypointIndices) {
if (kp_index >= static_cast<int>(pose.keypoints.size())) {
continue;
}
@@ -683,6 +719,7 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
continue;
}
per_keypoint_depths.push_back(median_in_place(nearby_depths));
per_keypoint_weights.push_back(kp.confidence);
}
if (debug_info) {
debug_info->depth_sample_count = static_cast<int>(per_keypoint_depths.size());
@@ -697,6 +734,9 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
return TargetObservation{};
}
// MAD rejection uses the unweighted median as the robust center (classic
// MAD). Weights are filtered alongside depths so the downstream weighted
// median stays aligned.
if (per_keypoint_depths.size() >= 3) {
std::vector<float> deviations = per_keypoint_depths;
const float median_depth = median_in_place(deviations);
@@ -706,49 +746,58 @@ TargetObservation TargetObservationProcessor::enrich_target_with_cloud(
const float mad = median_in_place(deviations);
if (mad > 1e-3f) {
const float threshold = 2.5f * mad / 0.6745f;
std::vector<float> inliers;
inliers.reserve(per_keypoint_depths.size());
for (const float depth : per_keypoint_depths) {
if (std::abs(depth - median_depth) < threshold) {
inliers.push_back(depth);
std::vector<float> inlier_depths;
std::vector<float> inlier_weights;
inlier_depths.reserve(per_keypoint_depths.size());
inlier_weights.reserve(per_keypoint_weights.size());
for (size_t i = 0; i < per_keypoint_depths.size(); ++i) {
if (std::abs(per_keypoint_depths[i] - median_depth) < threshold) {
inlier_depths.push_back(per_keypoint_depths[i]);
inlier_weights.push_back(per_keypoint_weights[i]);
}
}
if (!inliers.empty()) {
per_keypoint_depths.swap(inliers);
if (!inlier_depths.empty()) {
per_keypoint_depths.swap(inlier_depths);
per_keypoint_weights.swap(inlier_weights);
}
}
}
TargetObservation enriched = target;
std::vector<float> final_depths = per_keypoint_depths;
enriched.depth = median_in_place(final_depths);
enriched.depth = weighted_median(per_keypoint_depths, per_keypoint_weights);
enriched.depth_confidence = std::min(
1.0f,
static_cast<float>(per_keypoint_depths.size()) /
static_cast<float>(kHipKeypointIndices.size()));
static_cast<float>(kUpperTorsoKeypointIndices.size()));
std::vector<Eigen::Vector2f> valid_hip_pixels;
valid_hip_pixels.reserve(kHipKeypointIndices.size());
for (const int kp_index : kHipKeypointIndices) {
std::vector<Eigen::Vector2f> valid_torso_pixels;
std::vector<float> valid_torso_weights;
valid_torso_pixels.reserve(kUpperTorsoKeypointIndices.size());
valid_torso_weights.reserve(kUpperTorsoKeypointIndices.size());
for (const int kp_index : kUpperTorsoKeypointIndices) {
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) {
valid_hip_pixels.emplace_back(kp.x, kp.y);
valid_torso_pixels.emplace_back(kp.x, kp.y);
valid_torso_weights.push_back(kp.confidence);
}
}
if (valid_hip_pixels.empty()) {
if (valid_torso_pixels.empty()) {
const float center_u = 0.5f * (target.bbox_xyxy[0] + target.bbox_xyxy[2]);
const float center_v = 0.5f * (target.bbox_xyxy[1] + target.bbox_xyxy[3]);
enriched.target_pos_cam = reprojector.pixelToCameraPoint(
center_u, center_v, enriched.depth).cast<float>();
} else {
Eigen::Vector2f mean_pixel = Eigen::Vector2f::Zero();
for (const auto& pixel : valid_hip_pixels) {
mean_pixel += pixel;
Eigen::Vector2f weighted_sum = Eigen::Vector2f::Zero();
float weight_total = 0.0f;
for (size_t i = 0; i < valid_torso_pixels.size(); ++i) {
weighted_sum += valid_torso_weights[i] * valid_torso_pixels[i];
weight_total += valid_torso_weights[i];
}
mean_pixel /= static_cast<float>(valid_hip_pixels.size());
const Eigen::Vector2f mean_pixel =
weighted_sum / std::max(weight_total, 1e-6f);
enriched.target_pos_cam = reprojector.pixelToCameraPoint(
mean_pixel.x(), mean_pixel.y(), enriched.depth).cast<float>();
}