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

only compute jacobians when needed

release/4.3a0
Gerry Chen 2021-10-04 04:04:28 -04:00
parent fe59f3db19
commit bcafdb7557
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Pose3.cpp
View File

@ -67,9 +67,8 @@ Matrix6 Pose3::AdjointMap() const {
// Calculate AdjointMap applied to xi_b, with Jacobians // Calculate AdjointMap applied to xi_b, with Jacobians
Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this,
OptionalJacobian<6, 6> H_xib) const { OptionalJacobian<6, 6> H_xib) const {
// Ad * xi = [ R 0 * [w // Ad * xi = [ R 0 . [w
// [t]R R ] v] // [t]R R ] v]
// Declarations and aliases // Declarations and aliases
Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw;
Vector6 result; Vector6 result;
@ -77,14 +76,16 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this,
const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>();
// Calculations // Calculations
Rw = R_.rotate(w, Rw_H_R, Rw_H_w); Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr);
const Vector3 Rv = R_.rotate(v, Rv_H_R, Rv_H_v); const Vector3 Rv =
const Vector3 pRw = cross(t_, Rw, pRw_H_t, pRw_H_Rw); R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr);
const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr,
H_this || H_xib ? &pRw_H_Rw : nullptr);
result.tail<3>() = pRw + Rv; result.tail<3>() = pRw + Rv;
pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason
// Jacobians // Jacobians
if (H_this) { if (H_this) {
pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason
*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();