diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e9b628985..0f039d20f 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 a0514c679..8bdddb198 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;