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

Use Eigen expressions more effectively and kill & in code.

release/4.3a0
Frank Dellaert 2021-11-06 13:46:19 -04:00
parent 1b1ea146ac
commit 4bd80357f5
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Pose3.cpp
View File

@ -85,20 +85,20 @@ 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>());
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();
}
if (H_x) {
*H_x = AdT;
*H_x = Ad.transpose();
}
return AdTx;