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

correct jacobians

release/4.3a0
Gerry Chen 2021-10-16 01:01:02 -04:00
parent 561037f073
commit 33e16aa7d2
1 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/Pose3.cpp
View File

@ -79,22 +79,23 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this,
Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr);
const Vector3 Rv = const Vector3 Rv =
R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr);
// Since we use the Point3 version of cross, the jacobian of pRw wrpt t
// (pRw_H_t) needs special treatment as detailed below.
const Vector3 pRw = const Vector3 pRw =
cross(t_, Rw, boost::none, (H_this || H_xib) ? &pRw_H_Rw : nullptr); cross(t_, Rw, pRw_H_t, (H_this || H_xib) ? &pRw_H_Rw : nullptr);
result.tail<3>() = pRw + Rv; result.tail<3>() = pRw + Rv;
// Jacobians // Jacobians
if (H_this) { if (H_this) {
// By applying the chain rule to the matrix-matrix product of [t]R, we can
// compute a simplified derivative which is the same as Rw_H_R. Details:
// https://github.com/borglab/gtsam/pull/885#discussion_r726591370
pRw_H_t = Rw_H_R;
*H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, //
/* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t)
.finished(); .finished();
/* This is the "full" calculation:
Matrix36 R_H_this, t_H_this;
rotation(R_H_this); // I_3x3, Z_3x3
translation(t_H_this); // Z_3x3, R
(*H_this) *= (Matrix6() << R_H_this, t_H_this).finished();
*/
// But we can simplify those calculations since it's mostly I and Z:
H_this->bottomRightCorner<3, 3>() *= R_.matrix();
} }
if (H_xib) { if (H_xib) {
*H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, //
@ -130,9 +131,11 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this,
// Jacobians // Jacobians
if (H_this) { if (H_this) {
*H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, -Rtv_H_tv * tv_H_t,
/* */ Rv_H_R, /* */ Z_3x3) /* */ Rv_H_R, /* */ Z_3x3)
.finished(); .finished();
// See Adjoint(xi) jacobian calculation for why we multiply by R
H_this->topRightCorner<3, 3>() *= R_.matrix();
} }
if (H_x) { if (H_x) {
*H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, //