From 693ff77c4f0a193f6cc373667d733d63bccc681a Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 14 Nov 2024 13:03:54 -0500 Subject: [PATCH 1/2] Fix some bugs in IMU preintegration and SO3 exp --- src/residuals/imu_preint/delta.rs | 2 + src/residuals/imu_preint/newtypes.rs | 1 + src/residuals/imu_preint/residual.rs | 79 +++++++++++++++++++++++++++- src/variables/macros.rs | 2 +- src/variables/so3.rs | 12 +++-- 5 files changed, 90 insertions(+), 6 deletions(-) diff --git a/src/residuals/imu_preint/delta.rs b/src/residuals/imu_preint/delta.rs index ae47f63..faef279 100644 --- a/src/residuals/imu_preint/delta.rs +++ b/src/residuals/imu_preint/delta.rs @@ -253,6 +253,7 @@ mod test { assert_matrix_eq!(delta.xi_theta, gyro.0 * t, comp = abs, tol = 1e-5); } + // Test construction of state transtition matrix A #[test] fn make_a() { let dt = 0.1; @@ -333,6 +334,7 @@ mod test { ); } + // Test we get correct jacobian H for bias propagation #[test] #[allow(non_snake_case)] fn propagate_h() { diff --git a/src/residuals/imu_preint/newtypes.rs b/src/residuals/imu_preint/newtypes.rs index 16fb49a..ad5cd3a 100644 --- a/src/residuals/imu_preint/newtypes.rs +++ b/src/residuals/imu_preint/newtypes.rs @@ -79,6 +79,7 @@ impl Gravity { /// Struct to hold an Imu state /// /// Specifically holds an Imu state to which an ImuDelta can be applied +#[derive(Clone, Debug)] pub struct ImuState { pub r: SO3, pub v: Vector3, diff --git a/src/residuals/imu_preint/residual.rs b/src/residuals/imu_preint/residual.rs index 9b7d27c..ac9b0b8 100644 --- a/src/residuals/imu_preint/residual.rs +++ b/src/residuals/imu_preint/residual.rs @@ -296,7 +296,7 @@ pub struct ImuPreintegrationResidual { } impl Residual6 for ImuPreintegrationResidual { - type Differ = ForwardProp>; + type Differ = ForwardProp>; type DimIn = Const<30>; type DimOut = Const<15>; type V1 = SE3; @@ -361,3 +361,80 @@ impl fmt::Display for ImuPreintegrationResidual { write!(f, "ImuPreintegrationResidual({})", self.delta) } } + +#[cfg(test)] +mod test { + use crate::{ + assert_variable_eq, assign_symbols, + containers::{Graph, Values}, + linalg::Vector3, + optimizers::GaussNewton, + residuals::{Accel, Gyro, PriorResidual}, + variables::{ImuBias, VectorVar3, SE3}, + }; + + use super::*; + + assign_symbols!(X: SE3; V: VectorVar3; B: ImuBias); + + // Test using residual in an optimization problem + #[test] + fn optimize() { + // Initial conditions + let accel = Accel::new(0.1, 0.0, -9.81); + let gyro = Gyro::zeros(); + let x0 = SE3::identity(); + let v0 = VectorVar3::identity(); + let b0 = ImuBias::identity(); + let dt = 0.01; + let n = 100; + + // Integrate measurements + let mut preint = ImuPreintegrator::new(ImuCovariance::default(), b0.clone(), Gravity::up()); + for _ in 0..n { + preint.integrate(&gyro, &accel, dt); + } + println!("xi_pos : {}", preint.delta); + + // Build factor and graph + let mut graph = Graph::new(); + let factor = preint.build(X(0), V(0), B(0), X(1), V(1), B(1)); + let prior_x0 = FactorBuilder::new1(PriorResidual::new(x0.clone()), X(0)).build(); + let prior_v0 = FactorBuilder::new1(PriorResidual::new(v0.clone()), V(0)).build(); + let prior_b0 = FactorBuilder::new1(PriorResidual::new(b0.clone()), B(0)).build(); + graph.add_factor(factor); + graph.add_factor(prior_x0); + graph.add_factor(prior_v0); + graph.add_factor(prior_b0); + + // println!("{:?}", graph); + + let mut values = Values::new(); + values.insert(X(0), x0); + values.insert(V(0), v0); + values.insert(B(0), b0); + values.insert(X(1), SE3::identity()); + values.insert(V(1), VectorVar3::identity()); + values.insert(B(1), ImuBias::identity()); + + // Optimize + let mut opt: GaussNewton = GaussNewton::new(graph); + let results = opt.optimize(values).expect("Optimization failed"); + + // Check results + let t = n as dtype * dt; + let xyz = Vector3::new(accel.0.x * t * t / 2.0, 0.0, 0.0); + + let x1_exp = SE3::from_rot_trans(SO3::identity(), xyz); + let x1_got = results.get(X(1)).expect("Somehow missing X(1)").clone(); + println!("x1_exp: {}", x1_exp); + println!("x1_got: {}", x1_got); + assert_variable_eq!(x1_got, x1_exp, comp = abs, tol = 1e-5); + + let v1_exp = VectorVar3::new(accel.0.x * t, 0.0, 0.0); + let v1_got = results.get(V(1)).expect("Somehow missing V(1)").clone(); + println!("v1_exp: {}", v1_exp); + println!("v1_got: {}", v1_got); + assert_variable_eq!(v1_got, v1_exp, comp = abs, tol = 1e-5); + } +} diff --git a/src/variables/macros.rs b/src/variables/macros.rs index a4eb8a0..610cd89 100644 --- a/src/variables/macros.rs +++ b/src/variables/macros.rs @@ -13,7 +13,7 @@ macro_rules! assert_variable_eq { ($x:expr, $y:expr, comp = abs, tol = $tol:expr) => { matrixcompare::assert_matrix_eq!( $x.ominus(&$y), - $crate::linalg::VectorX::zeros($crate::variables::traits::Variable::dim(&$x)), + $crate::linalg::VectorX::zeros($crate::variables::Variable::dim(&$x)), comp = abs, tol = $tol ); diff --git a/src/variables/so3.rs b/src/variables/so3.rs index eae0cac..0696bda 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -112,16 +112,20 @@ impl Variable for SO3 { fn exp(xi: VectorViewX) -> Self { let mut xyzw = Vector4::zeros(); - let theta = xi.norm(); - - xyzw.w = (theta * T::from(0.5)).cos(); + let theta2 = xi.norm_squared(); - if theta < T::from(1e-3) { + if theta2 < T::from(1e-6) { + // cos(theta / 2) \approx 1 - theta^2 / 8 + xyzw.w = T::from(1.0) - theta2 / T::from(8.0); + // TODO: Investigate the proper approximation here let tmp = xyzw.w * T::from(0.5); xyzw.x = xi[0] * tmp; xyzw.y = xi[1] * tmp; xyzw.z = xi[2] * tmp; } else { + let theta = theta2.sqrt(); + xyzw.w = (theta * T::from(0.5)).cos(); + let omega = xi / theta; let sin_theta_half = (T::from(1.0) - xyzw.w * xyzw.w).sqrt(); xyzw.x = omega[0] * sin_theta_half; From 4b3e17f51746e7706b16a123754e69484767df57 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 14 Nov 2024 13:05:18 -0500 Subject: [PATCH 2/2] Finalize SO3 fixes --- src/variables/so3.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/variables/so3.rs b/src/variables/so3.rs index 0696bda..eed8310 100644 --- a/src/variables/so3.rs +++ b/src/variables/so3.rs @@ -117,8 +117,8 @@ impl Variable for SO3 { if theta2 < T::from(1e-6) { // cos(theta / 2) \approx 1 - theta^2 / 8 xyzw.w = T::from(1.0) - theta2 / T::from(8.0); - // TODO: Investigate the proper approximation here - let tmp = xyzw.w * T::from(0.5); + // Complete the square so that norm is one + let tmp = T::from(0.5); xyzw.x = xi[0] * tmp; xyzw.y = xi[1] * tmp; xyzw.z = xi[2] * tmp;