change depth estimation from only HIP into UPPER+HIP

script/record_dataset.sh

@@ -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

@@ -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;
+            std::vector<float> inlier_depths;
-            inliers.reserve(per_keypoint_depths.size());
+            std::vector<float> inlier_weights;
-            for (const float depth : per_keypoint_depths) {
+            inlier_depths.reserve(per_keypoint_depths.size());
-                if (std::abs(depth - median_depth) < threshold) {
+            inlier_weights.reserve(per_keypoint_weights.size());
-                    inliers.push_back(depth);
+            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()) {
+            if (!inlier_depths.empty()) {
-                per_keypoint_depths.swap(inliers);
+                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 = weighted_median(per_keypoint_depths, per_keypoint_weights);
-    enriched.depth = median_in_place(final_depths);
     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;
+    std::vector<Eigen::Vector2f> valid_torso_pixels;
-    valid_hip_pixels.reserve(kHipKeypointIndices.size());
+    std::vector<float> valid_torso_weights;
-    for (const int kp_index : kHipKeypointIndices) {
+    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();
+        Eigen::Vector2f weighted_sum = Eigen::Vector2f::Zero();
-        for (const auto& pixel : valid_hip_pixels) {
+        float weight_total = 0.0f;
-            mean_pixel += pixel;
+        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>();
     }