Rot3 modernization: now derives from LieGroup, SLOW_CAYLEY is gone, retract and localCoordinates auto-generated so no more flag. Might re-add instance-based expmap and logmap in LieGroup for convenienece.

gtsam/base/Matrix.cpp

@@ -660,19 +660,6 @@ Matrix expm(const Matrix& A, size_t K) {
   return E;
 }
 
-/* ************************************************************************* */
-Matrix Cayley(const Matrix& A) {
-  Matrix::Index n = A.cols();
-  assert(A.rows() == n);
-
-  // original
-//  const Matrix I = eye(n);
-//  return (I-A)*inverse(I+A);
-
-  // inlined to let Eigen do more optimization
-  return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse();
-}
-
 /* ************************************************************************* */
 std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal)
 {

gtsam/base/Matrix.h

@@ -528,17 +528,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9);
  */
 GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
 
-/// Cayley transform
-GTSAM_EXPORT Matrix Cayley(const Matrix& A);
-
-/// Implementation of Cayley transform using fixed size matrices to let
-/// Eigen do more optimization
-template<int N>
-Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
-  typedef Eigen::Matrix<double, N, N> FMat;
-  return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
-}
-
 std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
 
 } // namespace gtsam

gtsam/geometry/Quaternion.h

@@ -16,7 +16,7 @@
  **/
 
 #include <gtsam/base/Lie.h>
-#include <gtsam/base/Matrix.h>
+#include <gtsam/base/concepts.h>
 
 #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
 
@@ -96,8 +96,7 @@ struct traits_x<QUATERNION_TYPE> {
       _Scalar angle = omega.norm();
       return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
     }
-    if (H)
+    if (H) CONCEPT_NOT_IMPLEMENTED;
-      throw std::runtime_error("TODO: implement Jacobian");
   }
 
   /// We use our own Logmap, as there is a slight bug in Eigen
@@ -129,8 +128,7 @@ struct traits_x<QUATERNION_TYPE> {
       return (angle / s) * q.vec();
     }
 
-    if (H)
+    if (H) CONCEPT_NOT_IMPLEMENTED;
-      throw std::runtime_error("TODO: implement Jacobian");
   }
 
   /// @}

gtsam/geometry/Rot3.cpp

@@ -58,20 +58,6 @@ Rot3 Rot3::rodriguez(const Vector3& w) {
   return rodriguez(w/t, t);
 }
 
-/* ************************************************************************* */
-Rot3 Rot3::retract(const Vector& omega, OptionalJacobian<3, 3> Hthis,
-    OptionalJacobian<3, 3> Hv, Rot3::CoordinatesMode mode) const {
-  if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED;
-  return retract(omega, mode);
-}
-
-/* ************************************************************************* */
-Vector3 Rot3::localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin,
-    OptionalJacobian<3, 3> H2, Rot3::CoordinatesMode mode) const {
-  if (Horigin || H2) CONCEPT_NOT_IMPLEMENTED;
-  return localCoordinates(R2, mode);
-}
-
 /* ************************************************************************* */
 bool Rot3::equals(const Rot3 & R, double tol) const {
   return equal_with_abs_tol(matrix(), R.matrix(), tol);
@@ -248,8 +234,8 @@ ostream &operator<<(ostream &os, const Rot3& R) {
 Rot3 Rot3::slerp(double t, const Rot3& other) const {
   // amazingly simple in GTSAM :-)
   assert(t>=0 && t<=1);
-  Vector3 omega = localCoordinates(other, Rot3::EXPMAP);
+  Vector3 omega = Logmap(between(other));
-  return retract(t * omega, Rot3::EXPMAP);
+  return compose(Expmap(t * omega));
 }
 
 /* ************************************************************************* */

gtsam/geometry/Rot3.h

@@ -52,7 +52,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Rot3 {
+  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
 
   private:
 
@@ -64,8 +64,6 @@ namespace gtsam {
 #endif
 
   public:
-    /// The intrinsic dimension of this manifold.
-    enum { dimension = 3 };
 
     /// @name Constructors and named constructors
     /// @{
@@ -210,16 +208,13 @@ namespace gtsam {
       return Rot3();
     }
 
-    /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
-    Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const;
-
-    /// Compose two rotations i.e., R= (*this) * R2
-    Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
-        OptionalJacobian<3, 3> H2 = boost::none) const;
-
     /** compose two rotations */
     Rot3 operator*(const Rot3& R2) const;
 
+    Rot3 inverse() const {
+      return Rot3(Matrix3(transpose()));
+    }
+
     /**
      * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C
      * @param cRb rotation from B frame to C frame
@@ -230,23 +225,10 @@ namespace gtsam {
       return cRb * (*this) * cRb.inverse();
     }
 
-    /**
-     * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
-     */
-    Rot3 between(const Rot3& R2,
-        OptionalJacobian<3,3> H1=boost::none,
-        OptionalJacobian<3,3> H2=boost::none) const;
-
     /// @}
     /// @name Manifold
     /// @{
 
-    /// dimension of the variable - used to autodetect sizes
-    static size_t Dim() { return 3; }
-
-    /// return dimensionality of tangent space, DOF = 3
-    size_t dim() const { return 3; }
-
     /**
      * The method retract() is used to map from the tangent space back to the manifold.
      * Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the
@@ -260,21 +242,29 @@ namespace gtsam {
       EXPMAP, ///< Use the Lie group exponential map to retract
 #ifndef GTSAM_USE_QUATERNIONS
       CAYLEY, ///< Retract and localCoordinates using the Cayley transform.
-      SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only).
 #endif
       };
 
 #ifndef GTSAM_USE_QUATERNIONS
+
+    // Cayley chart around origin, no derivatives
+    struct CayleyChart {
+      static Rot3 Retract(const Vector3& v);
+      static Vector3 Local(const Rot3& r);
+    };
+
     /// Retraction from R^3 to Rot3 manifold using the Cayley transform
-    Rot3 retractCayley(const Vector& omega) const;
+    Rot3 retractCayley(const Vector& omega) const {
+      return compose(CayleyChart::Retract(omega));
+    }
+
+    /// Inverse of retractCayley
+    Vector3 localCayley(const Rot3& other) const {
+      return CayleyChart::Local(between(other));
+    }
+
 #endif
 
-    /// Retraction from R^3 \f$ [R_x,R_y,R_z] \f$ to Rot3 manifold neighborhood around current rotation
-    Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
-
-    /// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation
-    Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
-
     /// @}
     /// @name Lie Group
     /// @{
@@ -300,27 +290,16 @@ namespace gtsam {
     /// Derivative of Logmap
     static Matrix3 LogmapDerivative(const Vector3& x);
 
-    Rot3 retract(const Vector& omega, OptionalJacobian<3, 3> Hthis,
-        OptionalJacobian<3, 3> Hv = boost::none, Rot3::CoordinatesMode mode =
-            ROT3_DEFAULT_COORDINATES_MODE) const;
-
-    Vector3 localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin,
-        OptionalJacobian<3, 3> H2 = boost::none, Rot3::CoordinatesMode mode =
-            ROT3_DEFAULT_COORDINATES_MODE) const;
-
     /** Calculate Adjoint map */
     Matrix3 AdjointMap() const { return matrix(); }
 
-    /**
+    // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
-     * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
+    struct ChartAtOrigin {
-     * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+      static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
-     */
+      static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
-    static Matrix3 rightJacobianExpMapSO3(const Vector3& x);
+    };
 
-    /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
+    using LieGroup<Rot3, 3>::inverse; // version with derivative
-     * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-     */
-    static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x);
 
     /// @}
     /// @name Group Action on Point3

gtsam/geometry/Rot3M.cpp

@@ -139,13 +139,6 @@ Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
      -swy + C02,  swx + C12,    c + C22);
 }
 
-/* ************************************************************************* */
-Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
-  if (H1) *H1 = R2.transpose();
-  if (H2) *H2 = I_3x3;
-  return *this * R2;
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::operator*(const Rot3& R2) const {
   return Rot3(Matrix3(rot_*R2.rot_));
@@ -157,21 +150,6 @@ Matrix3 Rot3::transpose() const {
   return rot_.transpose();
 }
 
-/* ************************************************************************* */
-Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
-  if (H1) *H1 = -rot_;
-  return Rot3(Matrix3(transpose()));
-}
-
-/* ************************************************************************* */
-Rot3 Rot3::between (const Rot3& R2,
-    OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
-  if (H1) *H1 = -(R2.transpose()*rot_);
-  if (H2) *H2 = I_3x3;
-  Matrix3 R12 = transpose()*R2.rot_;
-  return Rot3(R12);
-}
-
 /* ************************************************************************* */
 Point3 Rot3::rotate(const Point3& p,
     OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
@@ -228,26 +206,41 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::retractCayley(const Vector& omega) const {
+Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) {
   const double x = omega(0), y = omega(1), z = omega(2);
   const double x2 = x * x, y2 = y * y, z2 = z * z;
   const double xy = x * y, xz = x * z, yz = y * z;
   const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
-  return (*this)
+  return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
-      * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
           (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
           (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
+Vector3 Rot3::CayleyChart::Local(const Rot3& R) {
-  if(mode == Rot3::EXPMAP) {
+  // Create a fixed-size matrix
-    return (*this)*Expmap(omega);
+  Matrix3 A = R.matrix();
-  } else if(mode == Rot3::CAYLEY) {
+  // Mathematica closed form optimization (procrastination?) gone wild:
-    return retractCayley(omega);
+  const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
-  } else if(mode == Rot3::SLOW_CAYLEY) {
+  const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
-    Matrix3 Omega = skewSymmetric(omega);
+  const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
-    return (*this)*CayleyFixed<3>(-Omega/2);
+  const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
+  const double M = 1 + e - f * h + i + e * i;
+  const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
+  const double x = a * f - cd + f;
+  const double y = b * f - ce - c;
+  const double z = fg - di - d;
+  return K * Vector3(x, y, z);
+}
+
+/* ************************************************************************* */
+Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
+  static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
+  if (mode == Rot3::EXPMAP) return Expmap(omega, H);
+
+  if (H) CONCEPT_NOT_IMPLEMENTED;
+  if(mode == Rot3::CAYLEY) {
+    return CayleyChart::Retract(omega);
   } else {
     assert(false);
     exit(1);
@@ -255,29 +248,13 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
 }
 
 /* ************************************************************************* */
-Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
+Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
-  if(mode == Rot3::EXPMAP) {
+  static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
-    return Logmap(between(T));
+  if (mode == Rot3::EXPMAP) return Logmap(R,H);
-  } else if(mode == Rot3::CAYLEY) {
+
-    // Create a fixed-size matrix
+  if (H) CONCEPT_NOT_IMPLEMENTED;
-    Matrix3 A = rot_.transpose() * T.matrix();
+  if(mode == Rot3::CAYLEY) {
-    // Mathematica closed form optimization (procrastination?) gone wild:
+    return CayleyChart::Local(R);
-    const double a=A(0,0),b=A(0,1),c=A(0,2);
-    const double d=A(1,0),e=A(1,1),f=A(1,2);
-    const double g=A(2,0),h=A(2,1),i=A(2,2);
-    const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
-    const double M = 1 + e - f*h + i + e*i;
-    const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
-    const double x = a * f - cd + f;
-    const double y = b * f - ce - c;
-    const double z = fg - di - d;
-    return K * Vector3(x, y, z);
-  } else if(mode == Rot3::SLOW_CAYLEY) {
-    // Create a fixed-size matrix
-    Matrix3 A(between(T).matrix());
-    // using templated version of Cayley
-    Matrix3 Omega = CayleyFixed<3>(A);
-    return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
   } else {
     assert(false);
     exit(1);

gtsam/geometry/tests/testRot3.cpp

@@ -294,12 +294,10 @@ TEST(Rot3, manifold_expmap)
   Rot3 origin;
 
   // log behaves correctly
-  Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP);
+  Vector d12 = Rot3::Logmap(gR1.between(gR2));
-  CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP)));
+  Vector d21 = Rot3::Logmap(gR2.between(gR1));
-  Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP);
-  CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP)));
 
-  // Check that it is expmap
+  // Check expmap
   CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
   CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
 
@@ -596,6 +594,12 @@ TEST(Rot3, quaternion) {
 }
 
 /* ************************************************************************* */
+Matrix Cayley(const Matrix& A) {
+  Matrix::Index n = A.cols();
+  const Matrix I = eye(n);
+  return (I-A)*inverse(I+A);
+}
+
 TEST( Rot3, Cayley ) {
   Matrix A = skewSymmetric(1,2,-3);
   Matrix Q = Cayley(A);

gtsam/geometry/tests/testRot3M.cpp

@@ -10,21 +10,14 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    testRot3.cpp
+ * @file    testRot3M.cpp
- * @brief   Unit tests for Rot3 class
+ * @brief   Unit tests for Rot3 class, matrix version
  * @author  Alireza Fathi
  * @author  Frank Dellaert
  */
 
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
-
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/lieProxies.h>
-
-#include <boost/math/constants/constants.hpp>
-
 #include <CppUnitLite/TestHarness.h>
 
 #ifndef GTSAM_USE_QUATERNIONS
@@ -46,38 +39,10 @@ TEST(Rot3, manifold_cayley)
   Rot3 origin;
 
   // log behaves correctly
-  Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY);
+  Vector d12 = gR1.localCayley(gR2);
-  CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY)));
+  CHECK(assert_equal(gR2, gR1.retractCayley(d12)));
-  Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY);
+  Vector d21 = gR2.localCayley(gR1);
-  CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY)));
+  CHECK(assert_equal(gR1, gR2.retractCayley(d21)));
-
-  // Check that log(t1,t2)=-log(t2,t1)
-  CHECK(assert_equal(d12,-d21));
-
-  // lines in canonical coordinates correspond to Abelian subgroups in SO(3)
-  Vector d = Vector3(0.1, 0.2, 0.3);
-  // exp(-d)=inverse(exp(d))
-  CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
-  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
-  Rot3 R2 = Rot3::Expmap (2 * d);
-  Rot3 R3 = Rot3::Expmap (3 * d);
-  Rot3 R5 = Rot3::Expmap (5 * d);
-  CHECK(assert_equal(R5,R2*R3));
-  CHECK(assert_equal(R5,R3*R2));
-}
-
-/* ************************************************************************* */
-TEST(Rot3, manifold_slow_cayley)
-{
-  Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
-  Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
-  Rot3 origin;
-
-  // log behaves correctly
-  Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY);
-  CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY)));
-  Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY);
-  CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY)));
 
   // Check that log(t1,t2)=-log(t2,t1)
   CHECK(assert_equal(d12,-d21));

gtsam/navigation/AHRSFactor.cpp

@@ -140,8 +140,7 @@ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
     boost::optional<Matrix&> H) const {
   const Vector3 biasOmegaIncr = bias - biasHat_;
   Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
-  const Rot3 deltaRij_biascorrected = deltaRij_.retract(
+  const Rot3 deltaRij_biascorrected = deltaRij_ * Rot3::Expmap(delRdelBiasOmega_biasOmegaIncr);
-      delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
   const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
   if (H) {
     const Matrix3 Jrinv_theta_bc = //

gtsam/navigation/CombinedImuFactor.cpp

@@ -280,7 +280,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
 
   // We compute factor's Jacobians, according to [3]
   /* ---------------------------------------------------------------------------------------------------- */
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_ * Rot3::Expmap(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
 
   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
@@ -480,7 +480,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3&
     vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
   }
 
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_ * Rot3::Expmap(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr);
   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
   Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -

gtsam/navigation/ImuFactor.cpp

@@ -262,7 +262,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
 
   // We compute factor's Jacobians
   /* ---------------------------------------------------------------------------------------------------- */
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_ * Rot3::Expmap(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
 
   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
@@ -422,7 +422,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
     vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
   }
 
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_ * Rot3::Expmap(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr);
   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
   Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -