Last comment I hope
parent
e3e4736e5e
commit
2042977d28
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue