diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 484fb9ca9..5a3b20782 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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); const Vector3 Rv = 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 = - 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; // Jacobians 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, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .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) { *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // @@ -130,9 +131,11 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, // Jacobians 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) .finished(); + // See Adjoint(xi) jacobian calculation for why we multiply by R + H_this->topRightCorner<3, 3>() *= R_.matrix(); } if (H_x) { *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, //