New constants for cross and doubleCross

gtsam/geometry/SO3.cpp

@@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
 }
 
 void ExpmapFunctor::init(bool nearZeroApprox) {
-  WW = W * W;
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
-    sin_theta = std::sin(theta);
+    const double sin_theta = std::sin(theta);
-    const double s2 = std::sin(theta / 2.0);
-    one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
     A = sin_theta / theta;
+    const double s2 = std::sin(theta / 2.0);
+    const double one_minus_cos =
+        2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
     B = one_minus_cos / theta2;
+  } else {
+    // Limits as theta -> 0:
+    A = 1.0;
+    B = 0.5;
   }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
-    : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
+    : theta2(omega.dot(omega)),
-  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
+      theta(std::sqrt(theta2)),
-  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+      W(skewSymmetric(omega)),
+      WW(W * W) {
   init(nearZeroApprox);
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
-    : theta2(angle * angle), theta(angle) {
+    : theta2(angle * angle),
-  const double ax = axis.x(), ay = axis.y(), az = axis.z();
+      theta(angle),
-  W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
+      W(skewSymmetric(axis * angle)),
-  W *= angle;
+      WW(W * W) {
   init(nearZeroApprox);
 }
 
-SO3 ExpmapFunctor::expmap() const {
+SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
-  if (nearZero)
-    return SO3(I_3x3 + W + 0.5 * WW);
-  else
-    return SO3(I_3x3 + A * W + B * WW);
-}
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  C = (1 - A) / theta2;
+  C = nearZero ? one_sixth : (1 - A) / theta2;
+  D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
+  E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
 }
 
 Matrix3 DexpFunctor::rightJacobian() const {
   if (nearZero) {
-    return I_3x3 - 0.5 * W;  // + one_sixth * WW;
+    return I_3x3 - B * W;  // + C * WW;
   } else {
     return I_3x3 - B * W + C * WW;
   }
@@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
   // Wv = omega x * v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
-    // 1x3 Jacobian of B with respect to omega
-    const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
     // Apply product rule:
-    *H = Wv * HB - B * skewSymmetric(v);
+    // D * omega.transpose() is 1x3 Jacobian of B with respect to omega
+    // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v)
+    *H = Wv * D * omega.transpose() - B * skewSymmetric(v);
   }
   return B * Wv;
 }
@@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
   const Vector3 WWv =
       gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
   if (H) {
-    // 1x3 Jacobian of C with respect to omega
-    const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
     // Apply product rule:
-    *H = WWv * HC + C * doubleCrossJacobian;
+    // E * omega.transpose() is 1x3 Jacobian of C with respect to omega
+    // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v)
+    *H = WWv * E * omega.transpose() + C * doubleCrossJacobian;
   }
   return C * WWv;
 }
@@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
 template <>
 GTSAM_EXPORT
 Matrix3 SO3::Hat(const Vector3& xi) {
-  // skew symmetric matrix X = xi^
+  return skewSymmetric(xi);
-  Matrix3 Y = Z_3x3;
-  Y(0, 1) = -xi(2);
-  Y(0, 2) = +xi(1);
-  Y(1, 2) = -xi(0);
-  return Y - Y.transpose();
 }
 
 //******************************************************************************

gtsam/geometry/SO3.h

@@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 /// Functor implementing Exponential map
 class GTSAM_EXPORT ExpmapFunctor {
  protected:
-  const double theta2;
+  const double theta2, theta;
-  Matrix3 W, WW;
+  const Matrix3 W, WW;
-  double A, B; // Ethan Eade's constants
   bool nearZero;
-  double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
+  // Ethan Eade's constants:
+  double A; // A = sin(theta) / theta or 1 for theta->0
+  double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
 
   void init(bool nearZeroApprox = false);
 
@@ -156,17 +157,19 @@ class GTSAM_EXPORT ExpmapFunctor {
 /// Functor that implements Exponential map *and* its derivatives
 class DexpFunctor : public ExpmapFunctor {
  protected:
-  static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
-  double C;  // Ethan Eade's C constant
+  double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
+  // Constants used in cross and doubleCross
+  double D;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
+  double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
+ public:
   /// Computes B * (omega x v).
   Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
   /// Computes C * (omega x (omega x v)).
   Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
- public:
   /// Constructor with element of Lie algebra so(3)
   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
                                     bool nearZeroApprox = false);
@@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor {
   GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
                                          OptionalJacobian<3, 3> H1 = {},
                                          OptionalJacobian<3, 3> H2 = {}) const;
+
+ protected:
+  static constexpr double one_sixth = 1.0 / 6.0;
+  static constexpr double _one_twelfth = -1.0 / 12.0;
+  static constexpr double _one_sixtieth = -1.0 / 60.0;
 };
 }  //  namespace so3
 

gtsam/geometry/tests/testSO3.cpp

@@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
+namespace test_cases {
+std::vector<Vector3> nearZeros{
+    {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
+std::vector<Vector3> omegas{
+    {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
+std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
+}  // namespace test_cases
+
+//******************************************************************************
+TEST(SO3, Cross) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).cross(v);
+        };
+    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
+    for (Vector3 omega : omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
+        local.cross(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, DoubleCross) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).doubleCross(v);
+        };
+    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
+    for (Vector3 omega : omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
+        local.doubleCross(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+    for (Vector3 omega : test_cases::omegas) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      for (Vector3 v : test_cases::vs) {
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -329,16 +373,14 @@ TEST(SO3, ApplyDexp) {
 //******************************************************************************
 TEST(SO3, ApplyLeftJacobian) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {false, true}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+    for (Vector3 omega : test_cases::omegas) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      for (Vector3 v : test_cases::vs) {
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
         CHECK(assert_equal(Vector3(local.leftJacobian() * v),
                            local.applyLeftJacobian(v, aH1, aH2)));
         CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) {
 //******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+    for (Vector3 omega : test_cases::omegas) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
       Matrix invDexp = local.dexp().inverse();
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+      for (Vector3 v : test_cases::vs) {
-                        Vector3(0.4, 0.3, 0.2)}) {
         EXPECT(assert_equal(Vector3(invDexp * v),
                             local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));