X = R^T, according to Barfoot Eq 8.83

borglab · Jan 15, 2025 · fdf8a51 · fdf8a51
1 parent b0e9b16
commit fdf8a51
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 15 deletions.
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
@@ -209,11 +209,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
   if (Hxi) {
     // The Jacobian of expmap is given by the right Jacobian of SO(3):
     const Matrix3 Jr = local.rightJacobian();
-    // We multiply H, the derivative of applyLeftJacobian in omega, with
-    //   X = Jr * Jl^{-1},
-    // which translates from left to right for our right expmap convention:
-    const Matrix3 X = Jr * local.leftJacobianInverse();
-    const Matrix3 Q = X * H;
+    // We multiply H, the derivative of applyLeftJacobian in omega, with R^T
+    // to translate from left to right for our right expmap convention:
+    const Matrix3 Q = R.matrix().transpose() * H;
     *Hxi << Jr, Z_3x3,  //
         Q, Jr;
   }
@@ -290,9 +288,8 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
   Matrix3 H;
   local.applyLeftJacobian(v, H);
 
-  // Multiply with X, translates from left to right for our expmap convention:
-  const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
-  return X * H;
+  // Multiply with R^T, translates from left to right for our expmap convention:
+  return local.expmap().transpose() * H;
 }
 
 /* ************************************************************************* */

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
@@ -136,10 +136,10 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
   if (Hxi) {
     // See Pose3::Expamp for explanation of the Jacobians
     const Matrix3 Jr = local.rightJacobian();
-    const Matrix3 X = Jr * local.leftJacobianInverse();
+    const Matrix3 M = R.matrix();
     *Hxi << Jr, Z_3x3, Z_3x3,  //
-        X * H_t_w, Jr, Z_3x3,  //
-        X * H_v_w, Z_3x3, Jr;
+        M.transpose() * H_t_w, Jr, Z_3x3,  //
+        M.transpose() * H_v_w, Z_3x3, Jr;
   }
 
   return NavState(R, t, v);
@@ -250,10 +250,10 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
   local.applyLeftJacobian(rho, H_t_w);
   local.applyLeftJacobian(nu, H_v_w);
 
-  // Multiply with X, translates from left to right for our expmap convention:
-  const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
-  const Matrix3 Qt = X * H_t_w;
-  const Matrix3 Qv = X * H_v_w;
+  // Multiply with R^T, translates from left to right for our expmap convention:
+  const Matrix3 R = local.expmap();
+  const Matrix3 Qt = R.transpose() * H_t_w;
+  const Matrix3 Qv = R.transpose() * H_v_w;
 
   const Matrix3 Jw = Rot3::LogmapDerivative(w);
   const Matrix3 Qt2 = -Jw * Qt * Jw;