No more functor for Q

gtsam/geometry/Pose3.cpp

@@ -22,6 +22,7 @@
 #include <cmath>
 #include <iostream>
 #include <string>
+
 #include "gtsam/geometry/Rot3.h"
 
 namespace gtsam {
@@ -239,30 +240,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 #endif
 }
 
-/* ************************************************************************* */
-namespace pose3 {
-struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
-      : so3::DexpFunctor(omega, nearZeroApprox) {
-  }
-
-  // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative.
-  Matrix3 computeQ(const Vector3& v) const {
-    // X translate from left to right for our right expmap convention:
-    Matrix X = rightJacobian() * leftJacobianInverse();
-    Matrix3 H;
-    applyLeftJacobian(v, H);
-    return X * H;
-  }
-};
-}  // namespace pose3
-
 /* ************************************************************************* */
 Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
                                            double nearZeroThreshold) {
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  return pose3::ExpmapFunctor(w).computeQ(v);
+  Matrix3 Q;
+  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
+  return Q;
 }
 
 /* ************************************************************************* */
@@ -273,13 +258,22 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   const double theta2 = w.dot(w);
   bool nearZero = (theta2 <= nearZeroThreshold);
 
-  if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
+  if (Q) {
-
+    // Instantiate functor for Dexp-related operations:
-  if (nearZero) {
+    so3::DexpFunctor local(w, nearZero);
-    return v + 0.5 * w.cross(v);
+    // X translate from left to right for our right expmap convention:
+    Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
+    Matrix3 H;
+    Vector t = local.applyLeftJacobian(v, H);
+    *Q = X * H;
+    return t;
+  } else if (nearZero) {
+    // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6
+    Vector3 Wv = w.cross(v);
+    return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth;
   } else {
-    // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but
+    // NOTE(Frank): if Q is not asked we use formulas below instead of calling
-    // formulas below convey geometric insight and creating functor is not free.
+    // applyLeftJacobian(v), as they convey geometric insight and are faster.
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
@@ -314,7 +308,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
 }
 
 /* ************************************************************************* */
-
 const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
   if (Hself) {
     *Hself << I_3x3, Z_3x3;

gtsam/geometry/tests/testPose3.cpp

@@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) {
   }
 }
 
-TEST( Pose3, ExpmapDerivativeQr) {
-  Vector6 w = Vector6::Random();
-  w.head<3>().normalize();
-  w.head<3>() = w.head<3>() * 0.9e-2;
-  Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
-  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
-}
-
 /* ************************************************************************* */
 TEST( Pose3, LogmapDerivative) {
   Matrix6 actualH;