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

54
src/ego/orientation.rs
View File

@@ -9,7 +9,8 @@ pub struct OrientationEstimator {
down: Option<Unit<Vector3<f32>>>,
forward: Option<Unit<Vector3<f32>>>,
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 {
@@ -42,37 +43,42 @@ impl OrientationEstimator {
},
Some(down) => {
// 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();
if horiz.norm() < 5.0 {
if horiz.norm() < 9.0 {
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() {
// If we already had a forward, allow reversal check
if let Some(prev) = &self.forward {
// Positive dot means similar direction, negative means opposite direction
if forward.dot(prev) < -0.7 {
// Strong opposite -> flip
forward = Unit::new_unchecked(-forward.into_inner());
warn!("Forwards is flipped!!! prev={prev:?} forward={forward:?}");
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 let Some(prev) = &self.forward {
// Positive dot means similar direction, negative means opposite direction
if unit_forward.dot(prev) < -0.7 {
// Strong opposite -> flip
//warn!("Forwards is very different!!! prev={prev:?} forward={unit_forward:?} dot={}", unit_forward.dot(prev));
self.forward = Some(Unit::new_unchecked(-unit_forward.into_inner()));
}
} else {
info!("Found forwards: {unit_forward:?} avg.norm={}", horiz.norm());
self.forward = Some(unit_forward);
}
} else {
info!("Found forwards: {forward:?}")
// Build body axes
let x = unit_forward.into_inner();
let z = down.into_inner();
let y = z.cross(&x).normalize();
let x = y.cross(&z).normalize(); // re-orthogonalize
let mat = Matrix3::from_columns(&[x, y, z]);
self.rotation_sb = Rotation3::from_matrix_unchecked(mat);
}
self.forward = Some(forward);
// Build body axes
let x = forward.into_inner();
let z = down.into_inner();
let y = z.cross(&x).normalize();
let x = y.cross(&z).normalize(); // re-orthogonalize
let mat = Matrix3::from_columns(&[x, y, z]);
self.rotation_sb = Rotation3::from_matrix_unchecked(mat);
}
}
}