kinematics: measure ToF hit range from the trunk axis

kinematics/src/tof.rs

@@ -96,8 +96,9 @@ impl Reprojector {
     /// the prototype's tuned value.
     pub const FLOOR_SAFETY: f64 = 0.85;
 
-    /// Returns closer than this (horizontally) are discarded: under ~10 cm the
-    /// sensor's own crosstalk produces phantom short readings.
+    /// Returns closer than this horizontally to the sensor are discarded:
+    /// under ~10 cm its own crosstalk produces phantom short readings. This
+    /// is sensor-relative, unlike the trunk-axis distance in [`Zone::Hit`].
     pub const MIN_RANGE_M: f64 = 0.10;
 
     pub fn new(model: &'static Model) -> Self {
@@ -188,9 +189,14 @@ impl Reprojector {
                 zones[i] = Zone::TooClose;
                 continue;
             }
+            // The noise band above is the sensor's own, so it is judged from
+            // the sensor; the range a consumer compares against a stop
+            // threshold is from the trunk origin's vertical axis, as
+            // `Zone::Hit` documents (#430).
+            let point_level = level.rotate(point);
             zones[i] = Zone::Hit {
                 point,
-                range: horizontal,
+                range: point_level[0].hypot(point_level[1]),
             };
         }
         zones
@@ -260,7 +266,9 @@ mod tests {
         assert!((0.8..=1.2).contains(&point[0]), "x: {point:?}");
         assert!(point[1].abs() < 0.3, "y: {point:?}");
         assert!((point[2] - sensor.pos[2]).abs() < 0.3, "z: {point:?}");
-        assert!((0.8..=1.05).contains(&range), "range: {range}");
+        // The alpha mount is about 8.2 cm ahead of the trunk axis. A
+        // sensor-relative result (~0.999 m) must fail this sanity check.
+        assert!((1.08..=1.09).contains(&range), "range: {range}");
     }
 
     /// Head pitched down: a bottom-row beam at floor distance is the floor;
@@ -390,4 +398,46 @@ mod tests {
             "a yawed-left head must place the return further left: {a:?} vs {l:?}"
         );
     }
+
+    /// `range` is the horizontal distance from the trunk origin's vertical
+    /// axis, as `Zone::Hit` documents (#430) — not from the sensor, which
+    /// sits several centimetres forward of that axis.
+    fn assert_range_from_trunk_axis(head: [f64; 4], posture: &Posture, r: f64) {
+        let rp = Reprojector::alpha();
+        let zones = rp.project(&one_return(CENTRE, r), head, posture);
+        let Zone::Hit { point, range } = zones[CENTRE] else {
+            panic!("expected a hit, got {:?}", zones[CENTRE]);
+        };
+        let p = level_from_gravity(posture.gravity).rotate(point);
+        let expected = p[0].hypot(p[1]);
+        assert!(
+            (range - expected).abs() < 1e-9,
+            "range {range} is not the trunk-axis distance {expected} (head {head:?}, r {r})"
+        );
+    }
+
+    #[test]
+    fn hit_range_is_from_the_trunk_axis_level_head() {
+        for r in [0.3, 1.0, 2.5] {
+            assert_range_from_trunk_axis(LEVEL, &Posture::default(), r);
+        }
+    }
+
+    #[test]
+    fn hit_range_is_from_the_trunk_axis_walking_head() {
+        for r in [0.3, 1.0] {
+            assert_range_from_trunk_axis([0.3, 0.2, 0.0, 0.0], &Posture::default(), r);
+        }
+    }
+
+    /// With the trunk leaned, the distance is still measured in the levelled
+    /// frame: horizontal means the world's horizontal, not the trunk's.
+    #[test]
+    fn hit_range_is_from_the_trunk_axis_leaning_trunk() {
+        let posture = Posture {
+            gravity: [0.1, 0.05, -0.99],
+            ..Posture::default()
+        };
+        assert_range_from_trunk_axis(LEVEL, &posture, 0.5);
+    }
 }

kinematics/tests/tof_range.rs

@@ -0,0 +1,103 @@
+use kinematics::tof::{COLS, Posture, ROWS, Reprojector, Zone};
+
+const CENTRE: usize = 3 * COLS + 3;
+
+fn one_return(r: f64) -> [Option<f64>; ROWS * COLS] {
+    let mut ranges = [None; ROWS * COLS];
+    ranges[CENTRE] = Some(r);
+    ranges
+}
+
+// Distance to the line through the origin parallel to gravity: |p × g| / |g|.
+// This oracle does not use the production levelling rotation, so a shared
+// quaternion/sign mistake cannot make both the implementation and test agree.
+fn distance_to_axis(p: [f64; 3], g: [f64; 3]) -> f64 {
+    let cross = [
+        p[1] * g[2] - p[2] * g[1],
+        p[2] * g[0] - p[0] * g[2],
+        p[0] * g[1] - p[1] * g[0],
+    ];
+    cross[0].hypot(cross[1]).hypot(cross[2]) / g[0].hypot(g[1]).hypot(g[2])
+}
+
+#[test]
+fn hit_range_is_distance_to_gravity_axis_across_grid_and_head_poses() {
+    let rp = Reprojector::alpha();
+    for head in [
+        [0.0; 4],
+        [0.3, 0.2, 0.0, 0.0],
+        [0.1, -0.2, 0.8, 0.3],
+        [-0.1, 0.2, -1.2, -0.3],
+    ] {
+        for gravity in [
+            [0.0, 0.0, -1.0],
+            [0.3, -0.4, -0.8],
+            [0.6, -0.8, -1.6], // gravity magnitude must not affect the distance
+            [0.0, 0.0, 1.0],   // the antiparallel levelling branch
+        ] {
+            let posture = Posture {
+                gravity,
+                // Isolate range geometry from the floor filter, including
+                // downward beams; floor classification has its own tests.
+                trunk_height_m: Some(10.0),
+            };
+            for slant in [0.3, 1.0, 2.5] {
+                for (i, zone) in rp
+                    .project(&[Some(slant); ROWS * COLS], head, &posture)
+                    .into_iter()
+                    .enumerate()
+                {
+                    let Zone::Hit { point, range } = zone else {
+                        panic!("expected hit: {zone:?}, head {head:?}, zone {i}");
+                    };
+                    let expected = distance_to_axis(point, gravity);
+                    assert!(
+                        (range - expected).abs() < 1e-9,
+                        "range {range}, axis distance {expected}, head {head:?}, gravity {gravity:?}, slant {slant}, zone {i}"
+                    );
+                }
+            }
+        }
+    }
+}
+
+#[test]
+fn noise_threshold_stays_sensor_relative_even_when_trunk_range_is_larger() {
+    let rp = Reprojector::alpha();
+    for gravity in [[0.0, 0.0, -1.0], [0.3, -0.4, -0.8]] {
+        let posture = Posture {
+            gravity,
+            trunk_height_m: Some(10.0),
+        };
+        let head = [0.0; 4];
+        let sensor = rp.sensor_in_trunk(head);
+        let dir = sensor.quat.rotate(rp.beams()[CENTRE]);
+        let horizontal_per_metre = distance_to_axis(dir, gravity);
+        for delta in [-1e-6, 1e-6] {
+            let slant = (Reprojector::MIN_RANGE_M + delta) / horizontal_per_metre;
+            let point = sensor.transform_point(rp.beams()[CENTRE].map(|v| v * slant));
+            assert!(distance_to_axis(point, gravity) > Reprojector::MIN_RANGE_M + 0.05);
+            let zones = rp.project(&one_return(slant), head, &posture);
+            if delta < 0.0 {
+                assert_eq!(zones[CENTRE], Zone::TooClose);
+            } else {
+                assert!(matches!(zones[CENTRE], Zone::Hit { .. }));
+            }
+            assert_eq!(zones[0], Zone::Empty);
+        }
+    }
+}
+
+#[test]
+fn untrusted_gravity_keeps_the_upright_range_fallback() {
+    let rp = Reprojector::alpha();
+    let ranges = one_return(0.3);
+    let upright = rp.project(&ranges, [0.0; 4], &Posture::default());
+    for gravity in [[0.0; 3], [0.1, -0.1, -0.1]] {
+        let posture = Posture {
+            gravity,
+            ..Posture::default()
+        };
+        assert_eq!(rp.project(&ranges, [0.0; 4], &posture), upright);
+    }
+}