Merge pull request #917 from borglab/fix/AdjointTranspose

borglab · Nov 6, 2021 · c0faaed · c0faaed
2 parents 1b1ea14 + 238563f
commit c0faaed
Showing 1 changed file with 7 additions and 8 deletions.
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
@@ -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;