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 /// 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 = (Matrix6() << w_T_hat, v_T_hat, //
/* */ v_T_hat, Z_3x3) /* */ v_T_hat, Z_3x3)
.finished(); .finished();
} }
if (H_x) { if (H_x) {
*H_x = AdT; *H_x = Ad.transpose();
} }
return AdTx; return AdTx;