Skip to content

Commit

Permalink
Last comment I hope
Browse files Browse the repository at this point in the history
  • Loading branch information
dellaert committed Jan 16, 2025
1 parent e3e4736 commit 2042977
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 3 deletions.
4 changes: 2 additions & 2 deletions gtsam/geometry/Pose3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 3 additions & 1 deletion gtsam/navigation/NavState.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit 2042977

Please sign in to comment.