test Qr with old codebase fails

gtsam/geometry/Pose3.cpp

@@ -190,15 +190,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 }
 
 /* ************************************************************************* */
-/**
+Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
- * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix
- *  J(xi) = [J_(w) Z_3x3;
- *             Q   J_(w)]
- *  where J_(w) is the SO3 Expmap derivative.
- *  (see Chirikjian11book2, pg 44, eq 10.95.
- *  The closed-form formula is similar to formula 102 in Barfoot14tro)
- */
-Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
   const Matrix3 V = skewSymmetric(v);
@@ -230,8 +222,8 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
   }
   else {
     Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
-        - 1./24.*(W*W*V + V*W*W - 3*W*V*W)
+        + 1./24.*(W*W*V + V*W*W - 3*W*V*W)
-        + 1./120.*(W*V*W*W + W*W*V*W);
+        - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
   }
 #endif
 
@@ -242,7 +234,7 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
   const Vector3 w = xi.head<3>();
   const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  const Matrix3 Q = computeQforExpmapDerivative(xi);
+  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   Matrix6 J;
   J << Jw, Z_3x3, Q, Jw;
   return J;
@@ -253,7 +245,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
   const Vector6 xi = Logmap(pose);
   const Vector3 w = xi.head<3>();
   const Matrix3 Jw = Rot3::LogmapDerivative(w);
-  const Matrix3 Q = computeQforExpmapDerivative(xi);
+  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   const Matrix3 Q2 = -Jw*Q*Jw;
   Matrix6 J;
   J << Jw, Z_3x3, Q2, Jw;

gtsam/geometry/Pose3.h

@@ -181,7 +181,16 @@ public:
     static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
   };
 
-  static Matrix3 computeQforExpmapDerivative(
+  /**
+  * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix
+  *  J_r(xi) = [J_(w) Z_3x3;
+  *             Q_r   J_(w)]
+  *  where J_(w) is the SO3 Expmap right derivative.
+  *  (see Chirikjian11book2, pg 44, eq 10.95.
+  *  The closed-form formula is identical to formula 102 in Barfoot14tro where
+  *  Q_l of the SE3 Expmap left derivative matrix is given.
+  */
+  static Matrix3 ComputeQforExpmapDerivative(
       const Vector6& xi, double nearZeroThreshold = 1e-5);
 
   using LieGroup<Pose3, 6>::inverse; // version with derivative

gtsam/geometry/tests/testPose3.cpp

@@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) {
   }
 }
 
+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, boost::none);
+  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
+  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, LogmapDerivative) {
   Matrix6 actualH;
@@ -818,15 +829,6 @@ TEST( Pose3, LogmapDerivative) {
   EXPECT(assert_equal(expectedH, actualH));
 }
 
-TEST( Pose3, computeQforExpmapDerivative) {
-  Vector6 w = Vector6::Random();
-  w.head<3>().normalize();
-  w.head<3>() = w.head<3>() * 0.09;
-  Matrix3 Qexact = Pose3::computeQforExpmapDerivative(w);
-  Matrix3 Qapprox = Pose3::computeQforExpmapDerivative(w, 0.1);
-  EXPECT(assert_equal(Qapprox, Qexact, 1e-5));
-}
-
 /* ************************************************************************* */
 Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
   return Pose3::adjointMap(xi) * v;