Added left and fake_exp to tests

.github/workflows/ci.yml

@@ -48,7 +48,11 @@ jobs:
 
     - name: default
       run: cargo test 
-    - name: Serde
+    - name: serde
       run: cargo test --features="serde,serde_json"
     - name: f32
-      run: cargo test --features f32
+      run: cargo test --features f32
+    - name: left
+      run: cargo test --features left
+    - name: fake_exp
+      run: cargo test --features fake_exp

src/residuals/imu_preint/delta.rs

@@ -90,7 +90,7 @@ impl<T: Numeric> ImuDelta<T> {
         // Setup
         self.dt += dt;
         let accel_world = SO3::exp(self.xi_theta.as_view()).apply(accel.0.as_view());
-        let H = SO3::dexp(self.xi_theta.as_view());
+        let H = SO3::dexp_right(self.xi_theta.as_view());
         let Hinv = H.try_inverse().expect("Failed to invert H(theta)");
 
         // Integrate (make sure integration occurs in the correct order)
@@ -118,7 +118,7 @@ impl<T: Numeric> ImuDelta<T> {
     // that's faster
     #[allow(non_snake_case)]
     fn A(&self, gyro: &GyroUnbiased<T>, accel: &AccelUnbiased<T>, dt: T) -> Matrix<15, 15, T> {
-        let H = SO3::dexp(self.xi_theta.as_view());
+        let H = SO3::dexp_right(self.xi_theta.as_view());
         let Hinv = H.try_inverse().expect("Failed to invert H(theta)");
         let R: nalgebra::Matrix<
             T,

src/variables/so3.rs

@@ -14,7 +14,7 @@ use crate::{
 /// 3D Special Orthogonal Group
 ///
 /// Implementation of SO(3) for 3D rotations. Specifically, we use quaternions
-/// to represent rotations due to their underyling efficiency when computing
+/// to represent rotations due to their underlying efficiency when computing
 /// log/exp maps.
 #[derive(Clone)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
@@ -52,6 +52,14 @@ impl<T: Numeric> SO3<T> {
     }
 
     pub fn dexp(xi: VectorView3<T>) -> Matrix3<T> {
+        if cfg!(feature = "left") {
+            Self::dexp_left(xi)
+        } else {
+            Self::dexp_right(xi)
+        }
+    }
+
+    pub fn dexp_right(xi: VectorView3<T>) -> Matrix3<T> {
         let theta2 = xi.norm_squared();
 
         let (a, b) = if theta2 < T::from(1e-6) {
@@ -64,11 +72,27 @@ impl<T: Numeric> SO3<T> {
         };
 
         let hat = SO3::hat(xi);
-        // gtsam says minus here for -hat a, but ethan eade says plus
-        // Empirically (via our test & jac in ImuDelta), minus is correct
-        // Need to find reference to confirm
+        // Right has a minus
         Matrix3::identity() - hat * a + hat * hat * b
     }
+
+    pub fn dexp_left(xi: VectorView3<T>) -> Matrix3<T> {
+        let theta2 = xi.norm_squared();
+
+        let (a, b) = if theta2 < T::from(1e-6) {
+            // TODO: Higher order terms using theta2?
+            (T::from(0.5), T::from(1.0) / T::from(6.0))
+        } else {
+            let theta = theta2.sqrt();
+            let a = (T::from(1.0) - theta.cos()) / theta2;
+            let b = (theta - theta.sin()) / (theta * theta2);
+            (a, b)
+        };
+
+        let hat = SO3::hat(xi);
+        // Left has a plus
+        Matrix3::identity() + hat * a + hat * hat * b
+    }
 }
 
 #[factrs::mark]

src/variables/traits.rs

@@ -29,9 +29,9 @@ pub trait Variable<T: Numeric = dtype>: Clone + Sized + Display + Debug {
     fn inverse(&self) -> Self;
     /// Composition of two group elements
     fn compose(&self, other: &Self) -> Self;
-    /// Exponential map (trivial if a vectorspace)
+    /// Exponential map (trivial if a vector space)
     fn exp(delta: VectorViewX<T>) -> Self;
-    /// Logarithm map (trivial if a vectorspace)
+    /// Logarithm map (trivial if a vector space)
     fn log(&self) -> VectorX<T>;
 
     /// Conversion to dual space