Last comment I hope

release/4.3a0
Frank Dellaert 2025-01-16 00:54:49 -05:00
parent e3e4736e5e
commit 2042977d28
2 changed files with 5 additions and 3 deletions

View File

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

View File

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