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

Merge pull request #917 from borglab/fix/AdjointTranspose

release/4.3a0
Frank Dellaert 2021-11-06 19:09:43 -04:00 committed by GitHub
parent 1b1ea146ac 238563f0e5
commit c0faaed885
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Pose3.cpp
View File

@ -85,20 +85,19 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
/// The dual version of Adjoint /// The dual version of Adjoint
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
OptionalJacobian<6, 6> H_x) const { OptionalJacobian<6, 6> H_x) const {
const Matrix6 &AdT = AdjointMap().transpose(); const Matrix6 Ad = AdjointMap();
const Vector6 &AdTx = AdT * x; const Vector6 AdTx = Ad.transpose() * x;
// Jacobians // Jacobians
// See docs/math.pdf for more details. // See docs/math.pdf for more details.
if (H_pose) { if (H_pose) {
const auto &w_T_hat = skewSymmetric(AdTx.head<3>()), const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
&v_T_hat = skewSymmetric(AdTx.tail<3>()); v_T_hat = skewSymmetric(AdTx.tail<3>());
*H_pose = (Matrix6() << w_T_hat, v_T_hat, // *H_pose << w_T_hat, v_T_hat, //
/* */ v_T_hat, Z_3x3) /* */ v_T_hat, Z_3x3;
.finished();
} }
if (H_x) { if (H_x) {
*H_x = AdT; *H_x = Ad.transpose();
} }
return AdTx; return AdTx;