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
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
OptionalJacobian<6, 6> H_x) const {
const Matrix6 &AdT = AdjointMap().transpose();
const Vector6 &AdTx = AdT * x;
const Matrix6 Ad = AdjointMap();
const Vector6 AdTx = Ad.transpose() * x;
// Jacobians
// See docs/math.pdf for more details.
if (H_pose) {
const auto &w_T_hat = skewSymmetric(AdTx.head<3>()),
&v_T_hat = skewSymmetric(AdTx.tail<3>());
*H_pose = (Matrix6() << w_T_hat, v_T_hat, //
/* */ v_T_hat, Z_3x3)
.finished();
const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
v_T_hat = skewSymmetric(AdTx.tail<3>());
*H_pose << w_T_hat, v_T_hat, //
/* */ v_T_hat, Z_3x3;
}
if (H_x) {
*H_x = AdT;
*H_x = Ad.transpose();
}
return AdTx;