Fix some bugs in IMU preintegration and SO3 exp

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() {

src/residuals/imu_preint/newtypes.rs

@@ -79,6 +79,7 @@ impl<T: Numeric> Gravity<T> {
 /// Struct to hold an Imu state
 ///
 /// Specifically holds an Imu state to which an ImuDelta can be applied
+#[derive(Clone, Debug)]
 pub struct ImuState<T: Numeric = dtype> {
     pub r: SO3<T>,
     pub v: Vector3<T>,

src/residuals/imu_preint/residual.rs

@@ -296,7 +296,7 @@ pub struct ImuPreintegrationResidual {
 }
 
 impl Residual6 for ImuPreintegrationResidual {
-    type Differ = ForwardProp<Const<15>>;
+    type Differ = ForwardProp<Const<30>>;
     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);
+    }
+}

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
         );

src/variables/so3.rs

@@ -112,16 +112,20 @@ impl<T: Numeric> Variable<T> for SO3<T> {
     fn exp(xi: VectorViewX<T>) -> 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) {
-            let tmp = xyzw.w * T::from(0.5);
+        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);
+            // 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;
         } 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;