From 04e04eed52174ca2ad585563951b609079b47fc5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 00:15:34 -0500 Subject: [PATCH 1/6] inlined ExpmapTranslation --- gtsam/geometry/Pose3.cpp | 72 ++++++++++++++++------------------- gtsam/geometry/Pose3.h | 17 --------- gtsam/navigation/NavState.cpp | 48 +++++++++++++++-------- 3 files changed, 66 insertions(+), 71 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6d9256f93b..9c3bd77339 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -183,16 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + // Compute rotation using Expmap - Matrix3 Jr; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(v); +#else + const Rot3 R(local.expmap()); +#endif - // Compute translation and optionally its Jacobian Q in w - // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. - Matrix3 Q; - const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); + // The translation t = local.leftJacobian() * v. + // Here we call applyLeftJacobian, which is faster if you don't need + // Jacobians, and returns Jacobian of t with respect to w if asked. + // NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas + // t_parallel = w * w.dot(v); // translation parallel to axis + // w_cross_v = w.cross(v); // translation orthogonal to axis + // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; + // but functor does not need R, deals automatically with the case where theta2 + // is near zero, and also gives us the machinery for the Jacobians. + Matrix3 H; + const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr); 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; *Hxi << Jr, Z_3x3, // Q, Jr; } @@ -260,45 +281,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - Matrix3 Q; - ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); - return Q; -} - -/* ************************************************************************* */ -// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas -// t_parallel = w * w.dot(v); // translation parallel to axis -// w_cross_v = w.cross(v); // translation orthogonal to axis -// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; -// but functor does not need R, deals automatically with the case where theta2 -// is near zero, and also gives us the machinery for the Jacobians. -Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, - OptionalJacobian<3, 3> Q, - OptionalJacobian<3, 3> J, - double nearZeroThreshold) { - const double theta2 = w.dot(w); - bool nearZero = (theta2 <= nearZeroThreshold); // Instantiate functor for Dexp-related operations: + bool nearZero = (w.dot(w) <= nearZeroThreshold); so3::DexpFunctor local(w, nearZero); - // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you - // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + // Call applyLeftJacobian to get its Jacobian Matrix3 H; - Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); - - // We return Jacobians for use in Expmap, so we multiply with X, that - // translates from left to right for our right expmap convention: - if (Q) { - Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - *Q = X * H; - } - - if (J) { - *J = local.rightJacobian(); // = X * local.leftJacobian(); - } + local.applyLeftJacobian(v, H); - return t; + // Multiply with X, translates from left to right for our expmap convention: + const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + return X * H; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e4896ae26f..af14ac82cc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -220,27 +220,10 @@ class GTSAM_EXPORT Pose3: public LieGroup { * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. - * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); - /** - * Compute the translation part of the exponential map, with Jacobians. - * @param w 3D angular velocity - * @param v 3D velocity - * @param Q Optionally, compute 3x3 Jacobian wrpt w - * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) - * @param nearZeroThreshold threshold for small values - * @note This function returns Jacobians Q and J corresponding to the bottom - * blocks of the SE(3) exponential, and translated from left to right from the - * applyLeftJacobian Jacobians. - */ - static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, - OptionalJacobian<3, 3> Q = {}, - OptionalJacobian<3, 3> J = {}, - double nearZeroThreshold = 1e-5); - using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index bd482132e4..d033c96e88 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -117,20 +117,29 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // Get angular velocity w and components rho (for t) and nu (for v) from xi Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + // Compute rotation using Expmap - Matrix3 Jr; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(v); +#else + const Rot3 R(local.expmap()); +#endif - // Compute translations and optionally their Jacobians Q in w - // The Jacobians with respect to rho and nu are equal to Jr - Matrix3 Qt, Qv; - Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); - Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); + // Compute translation and velocity. See Pose3::Expmap + Matrix3 H_t_w, H_v_w; + const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr); + const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr); if (Hxi) { + // See Pose3::Expamp for explanation of the Jacobians + const Matrix3 Jr = local.rightJacobian(); + const Matrix3 X = Jr * local.leftJacobianInverse(); *Hxi << Jr, Z_3x3, Z_3x3, // - Qt, Jr, Z_3x3, // - Qv, Z_3x3, Jr; + X * H_t_w, Jr, Z_3x3, // + X * H_v_w, Z_3x3, Jr; } return NavState(R, t, v); @@ -231,11 +240,21 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { const Vector3 w = xi.head<3>(); Vector3 rho = xi.segment<3>(3); Vector3 nu = xi.tail<3>(); - - Matrix3 Qt, Qv; - const Rot3 R = Rot3::Expmap(w); - Pose3::ExpmapTranslation(w, rho, Qt); - Pose3::ExpmapTranslation(w, nu, Qv); + + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian to get its Jacobians + Matrix3 H_t_w, H_v_w; + 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; + const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; @@ -247,7 +266,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { return J; } - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { From d6581cdef98ed6acb263afca14d9ce579038ed47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 00:45:07 -0500 Subject: [PATCH 2/6] Fixed typo in quaternion path --- gtsam/geometry/Pose3.cpp | 2 +- gtsam/navigation/NavState.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9c3bd77339..0c47e3eed6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -189,7 +189,7 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Compute rotation using Expmap #ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(v); + const Rot3 R = traits::Expmap(w); #else const Rot3 R(local.expmap()); #endif diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index d033c96e88..d102340ce8 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -123,7 +123,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // Compute rotation using Expmap #ifdef GTSAM_USE_QUATERNIONS - const Rot3 R = traits::Expmap(v); + const Rot3 R = traits::Expmap(w); #else const Rot3 R(local.expmap()); #endif From b0e9b16cc40adcf266fd4e7d153f387097977666 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 18:09:01 -0500 Subject: [PATCH 3/6] expmap now returns Matrix3 --- gtsam/geometry/SO3.cpp | 12 ++++++------ gtsam/geometry/SO3.h | 2 +- gtsam/geometry/tests/testSO3.cpp | 8 ++++---- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 41f7ce8107..a1947953ba 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, init(nearZeroApprox); } -SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } +Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { @@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, template <> GTSAM_EXPORT SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - return so3::ExpmapFunctor(axis, theta).expmap(); + return SO3(so3::ExpmapFunctor(axis, theta).expmap()); } //****************************************************************************** @@ -251,11 +251,11 @@ template <> GTSAM_EXPORT SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { - so3::DexpFunctor impl(omega); - *H = impl.dexp(); - return impl.expmap(); + so3::DexpFunctor local(omega); + *H = local.dexp(); + return SO3(local.expmap()); } else { - return so3::ExpmapFunctor(omega).expmap(); + return SO3(so3::ExpmapFunctor(omega).expmap()); } } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 36329a19d5..1bdcd82c75 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor { ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); /// Rodrigues formula - SO3 expmap() const; + Matrix3 expmap() const; protected: void init(bool nearZeroApprox = false); diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index c6fd521613..41777ae3aa 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) { // axis angle version so3::ExpmapFunctor f1(axis, angle); - SO3 actual1 = f1.expmap(); + SO3 actual1(f1.expmap()); CHECK(assert_equal(expected, actual1.matrix(), 1e-5)); // axis angle version, negative angle so3::ExpmapFunctor f2(axis, angle - 2*M_PI); - SO3 actual2 = f2.expmap(); + SO3 actual2(f2.expmap()); CHECK(assert_equal(expected, actual2.matrix(), 1e-5)); // omega version so3::ExpmapFunctor f3(axis * angle); - SO3 actual3 = f3.expmap(); + SO3 actual3(f3.expmap()); CHECK(assert_equal(expected, actual3.matrix(), 1e-5)); // omega version, negative angle so3::ExpmapFunctor f4(axis * (angle - 2*M_PI)); - SO3 actual4 = f4.expmap(); + SO3 actual4(f4.expmap()); CHECK(assert_equal(expected, actual4.matrix(), 1e-5)); } From fdf8a51a44258f0360bd2ac9a980c93e4f4db09e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 Jan 2025 18:09:26 -0500 Subject: [PATCH 4/6] X = R^T, according to Barfoot Eq 8.83 --- gtsam/geometry/Pose3.cpp | 13 +++++-------- gtsam/navigation/NavState.cpp | 14 +++++++------- 2 files changed, 12 insertions(+), 15 deletions(-) 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; From e3e4736e5e052de05f5d3b43e91a76796ea63263 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 Jan 2025 00:41:08 -0500 Subject: [PATCH 5/6] Fix comments about R^T --- gtsam/geometry/Pose3.cpp | 9 +++++---- gtsam/navigation/NavState.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e9b628985b..0f039d20f0 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -209,8 +209,8 @@ 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 R^T - // to translate from left to right for our right expmap convention: + // We are creating a Pose3, so we still need to chain H with R^T, the + // Jacobian of Pose3::Create with respect to t. const Matrix3 Q = R.matrix().transpose() * H; *Hxi << Jr, Z_3x3, // Q, Jr; @@ -288,8 +288,9 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, Matrix3 H; local.applyLeftJacobian(v, H); - // Multiply with R^T, translates from left to right for our expmap convention: - return local.expmap().transpose() * H; + // Multiply with R^T to account for the Pose3::Create Jacobian. + const Matrix3 R = local.expmap(); + return R.transpose() * H; } /* ************************************************************************* */ diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index a0514c679b..8bdddb198d 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -134,8 +134,9 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr); if (Hxi) { - // See Pose3::Expamp for explanation of the Jacobians const Matrix3 Jr = local.rightJacobian(); + // We are creating a NavState, so we still need to chain H_t_w and H_v_w + // with R^T, the Jacobian of Navstate::Create with respect to both t and v. const Matrix3 M = R.matrix(); *Hxi << Jr, Z_3x3, Z_3x3, // M.transpose() * H_t_w, Jr, Z_3x3, // @@ -250,11 +251,12 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { local.applyLeftJacobian(rho, H_t_w); local.applyLeftJacobian(nu, H_v_w); - // Multiply with R^T, translates from left to right for our expmap convention: + // Multiply with R^T to account for NavState::Create Jacobian. const Matrix3 R = local.expmap(); const Matrix3 Qt = R.transpose() * H_t_w; const Matrix3 Qv = R.transpose() * H_v_w; + // Now compute the blocks of the LogmapDerivative Jacobian const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; From 2042977d28963de612fca7c1261eda6ee455c9fc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 Jan 2025 00:54:49 -0500 Subject: [PATCH 6/6] Last comment I hope --- gtsam/geometry/Pose3.cpp | 4 ++-- gtsam/navigation/NavState.cpp | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0f039d20f0..f6d0c43cb6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -212,8 +212,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // We are creating a Pose3, so we still need to chain H with R^T, the // Jacobian of Pose3::Create with respect to t. const Matrix3 Q = R.matrix().transpose() * H; - *Hxi << Jr, Z_3x3, // - Q, Jr; + *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap + Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v. } return Pose3(R, t); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 8bdddb198d..116efe90a2 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -138,9 +138,11 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // We are creating a NavState, so we still need to chain H_t_w and H_v_w // with R^T, the Jacobian of Navstate::Create with respect to both t and v. const Matrix3 M = R.matrix(); - *Hxi << Jr, Z_3x3, Z_3x3, // + *Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap M.transpose() * H_t_w, Jr, Z_3x3, // M.transpose() * H_v_w, Z_3x3, Jr; + // In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83). + // J_l is the Jacobian of applyLeftJacobian in the second argument. } return NavState(R, t, v);