From 4bd80357f5a1d5e451dc54982bf8efb517ea7136 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 6 Nov 2021 13:46:19 -0400 Subject: [PATCH 1/2] Use Eigen expressions more effectively and kill & in code. --- gtsam/geometry/Pose3.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1b..8e43105cd0 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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; From 238563f0e5c164014e1624982ba797eb41b14d7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 6 Nov 2021 13:51:15 -0400 Subject: [PATCH 2/2] Cleaner Jacobian. --- gtsam/geometry/Pose3.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8e43105cd0..d0e60f3fdd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -93,9 +93,8 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, 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(); + *H_pose << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3; } if (H_x) { *H_x = Ad.transpose();