diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0c47e3eed6..e9b628985b 100644 --- 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 index d102340ce8..a0514c679b 100644 --- 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;