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);