12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose3.cpp
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 // We are creating a Pose3, so we still need to chain H with R^T, the
// Jacobian of Pose3::Create with respect to t. // Jacobian of Pose3::Create with respect to t.
const Matrix3 Q = R.matrix().transpose() * H; const Matrix3 Q = R.matrix().transpose() * H;
*Hxi << Jr, Z_3x3, // *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap
Q, Jr; Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v.
} }
return Pose3(R, t); return Pose3(R, t);

4
gtsam/navigation/NavState.cpp
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 // 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. // with R^T, the Jacobian of Navstate::Create with respect to both t and v.
const Matrix3 M = R.matrix(); 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_t_w, Jr, Z_3x3, //
M.transpose() * H_v_w, Z_3x3, Jr; 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); return NavState(R, t, v);