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 {
@@ -112,7 +113,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
 
 /* ************************************************************************* */
 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
+                       OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
   if (Hxi) {
     Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
@@ -159,7 +160,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
 /* ************************************************************************* */
 Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
   return Pose3(interpolate<Rot3>(R_, T.R_, t),
-                interpolate<Point3>(t_, T.t_, t));
+               interpolate<Point3>(t_, T.t_, t));
 }
 
 /* ************************************************************************* */
@@ -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 (nearZero) {
-    return v + 0.5 * w.cross(v);
+  if (Q) {
+    // Instantiate functor for Dexp-related operations:
+    so3::DexpFunctor local(w, nearZero);
+    // 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
-    // formulas below convey geometric insight and creating functor is not free.
+    // NOTE(Frank): if Q is not asked we use formulas below instead of calling
+    // 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;
@@ -332,14 +325,14 @@ Matrix4 Pose3::matrix() const {
 
 /* ************************************************************************* */
 Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
-                                                 OptionalJacobian<6, 6> HaTb) const {
+                               OptionalJacobian<6, 6> HaTb) const {
   const Pose3& wTa = *this;
   return wTa.compose(aTb, Hself, HaTb);
 }
 
 /* ************************************************************************* */
 Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
-                                               OptionalJacobian<6, 6> HwTb) const {
+                             OptionalJacobian<6, 6> HwTb) const {
   if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
   if (HwTb) *HwTb = I_6x6;
   const Pose3& wTa = *this;
@@ -348,7 +341,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
 
 /* ************************************************************************* */
 Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
-    OptionalJacobian<3, 3> Hpoint) const {
+                            OptionalJacobian<3, 3> Hpoint) const {
   // Only get matrix once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 R = R_.matrix();
@@ -372,7 +365,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const {
 
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
-    OptionalJacobian<3, 3> Hpoint) const {
+                          OptionalJacobian<3, 3> Hpoint) const {
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 Rt = R_.transpose();
@@ -478,7 +471,7 @@ std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
 std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
   if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
     throw std::invalid_argument(
-      "Pose3:Align expects 3*N matrices of equal shape.");
+        "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
   for (Eigen::Index j = 0; j < a.cols(); j++) {

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;