tdfischer/renderbug-bike
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ego: orientation: account for sensor bias when determining what 'forwards' is, and let forwards be more flexible if our detection finds it needs changed

Browse Source
This commit is contained in:
Victoria Fischer
2025-12-24 09:44:01 +01:00
parent 9059066206
commit 2630c97609
1 changed files with 30 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/ego/orientation.rs
View File

@@ -9,7 +9,8 @@ pub struct OrientationEstimator {
down: Option<Unit<Vector3<f32>>>, down: Option<Unit<Vector3<f32>>>,
forward: Option<Unit<Vector3<f32>>>, forward: Option<Unit<Vector3<f32>>>,
rotation_sb: Rotation3<f32>, // sensor -> body rotation_sb: Rotation3<f32>, // sensor -> body
accel_history: CircularBuffer<Vector3<f32>, 200> accel_history: CircularBuffer<Vector3<f32>, 200>,
forwards_history: CircularBuffer<Vector3<f32>, 200>
} }
impl OrientationEstimator { impl OrientationEstimator {
@@ -42,31 +43,35 @@ impl OrientationEstimator {
}, },
Some(down) => { Some(down) => {
// Project motion into horizontal plane // Project motion into horizontal plane
let avg = self.accel_history.data().iter().sum::<Vector3<f32>>(); let avg = self.accel_history.data().iter().sum::<Vector3<f32>>() - self.sensor_bias;
let horiz = avg - avg.dot(&down) * down.into_inner(); let horiz = avg - avg.dot(&down) * down.into_inner();
if horiz.norm() < 5.0 { if horiz.norm() < 9.0 {
return; // not enough motion return; // not enough motion
} }
//warn!("motion accel={accel:?} accel.norm={} horiz={horiz:?} horiz.norm={}", accel.norm(), horiz.norm());
let mut forward = Unit::new_normalize(horiz); self.forwards_history.insert(horiz);
if self.forwards_history.is_filled() {
let avg_forwards = self.forwards_history.data().iter().sum::<Vector3<f32>>() / (self.forwards_history.data().len() as f32);
let unit_forward = Unit::new_normalize(avg_forwards);
// If we already had a forward, allow reversal check // If we already had a forward, allow reversal check
if let Some(prev) = &self.forward { if let Some(prev) = &self.forward {
// Positive dot means similar direction, negative means opposite direction // Positive dot means similar direction, negative means opposite direction
if forward.dot(prev) < -0.7 { if unit_forward.dot(prev) < -0.7 {
// Strong opposite -> flip // Strong opposite -> flip
forward = Unit::new_unchecked(-forward.into_inner()); //warn!("Forwards is very different!!! prev={prev:?} forward={unit_forward:?} dot={}", unit_forward.dot(prev));
warn!("Forwards is flipped!!! prev={prev:?} forward={forward:?}"); self.forward = Some(Unit::new_unchecked(-unit_forward.into_inner()));
} }
} else { } else {
info!("Found forwards: {forward:?}") info!("Found forwards: {unit_forward:?} avg.norm={}", horiz.norm());
self.forward = Some(unit_forward);
} }
self.forward = Some(forward);
// Build body axes // Build body axes
let x = forward.into_inner(); let x = unit_forward.into_inner();
let z = down.into_inner(); let z = down.into_inner();
let y = z.cross(&x).normalize(); let y = z.cross(&x).normalize();
let x = y.cross(&z).normalize(); // re-orthogonalize let x = y.cross(&z).normalize(); // re-orthogonalize
@@ -76,6 +81,7 @@ impl OrientationEstimator {
} }
} }
} }
}
pub fn apply(&self, v_sensor: Vector3<f32>) -> Vector3<f32> { pub fn apply(&self, v_sensor: Vector3<f32>) -> Vector3<f32> {
self.rotation_sb * (v_sensor - self.sensor_bias) self.rotation_sb * (v_sensor - self.sensor_bias)