Add nearZeroApprox flag and make sure those cases are covered in tests

gtsam/geometry/SO3.cpp

@@ -27,8 +27,8 @@ namespace gtsam {
 
 namespace so3 {
 
-void ExpmapFunctor::init() {
+void ExpmapFunctor::init(bool nearZeroApprox) {
-  nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
+  nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (nearZero) return;
   theta = std::sqrt(theta2);  // rotation angle
   sin_theta = std::sin(theta);
@@ -36,10 +36,11 @@ void ExpmapFunctor::init() {
   one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
 }
 
-ExpmapFunctor::ExpmapFunctor(const Vector3& omega) : theta2(omega.dot(omega)) {
+ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
+    : theta2(omega.dot(omega)) {
   const double wx = omega.x(), wy = omega.y(), wz = omega.z();
   W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
-  init();
+  init(nearZeroApprox);
   if (!nearZero) {
     theta = std::sqrt(theta2);
     K = W / theta;
@@ -47,12 +48,12 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega) : theta2(omega.dot(omega)) {
   }
 }
 
-ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle)
+ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
     : theta2(angle * angle) {
   const double ax = axis.x(), ay = axis.y(), az = axis.z();
   K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
   W = K * angle;
-  init();
+  init(nearZeroApprox);
   if (!nearZero) {
     theta = angle;
     KK = K * K;
@@ -66,8 +67,8 @@ SO3 ExpmapFunctor::expmap() const {
     return I_3x3 + sin_theta * K + one_minus_cos * KK;
 }
 
-DexpFunctor::DexpFunctor(const Vector3& omega)
+DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
-    : ExpmapFunctor(omega), omega(omega) {
+    : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
   if (nearZero)
     dexp_ = I_3x3 - 0.5 * W;
   else {
@@ -79,19 +80,18 @@ DexpFunctor::DexpFunctor(const Vector3& omega)
 
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  if (nearZero) {
-    if (H1) *H1 = 0.5 * skewSymmetric(v);
-    if (H2) *H2 = I_3x3;
-    return v - 0.5 * omega.cross(v);
-  }
   if (H1) {
-    // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
+    if (nearZero) {
-    const Vector3 Kv = K * v;
+      *H1 = 0.5 * skewSymmetric(v);
-    const double Da = (sin_theta - 2.0 * a) / theta2;
+    } else {
-    const double Db = (one_minus_cos - 3.0 * b) / theta2;
+      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-    *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
+      const Vector3 Kv = K * v;
-          skewSymmetric(Kv * b / theta) +
+      const double Da = (sin_theta - 2.0 * a) / theta2;
-          (a * I_3x3 - b * K) * skewSymmetric(v / theta);
+      const double Db = (one_minus_cos - 3.0 * b) / theta2;
+      *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
+            skewSymmetric(Kv * b / theta) +
+            (a * I_3x3 - b * K) * skewSymmetric(v / theta);
+    }
   }
   if (H2) *H2 = dexp_;
   return dexp_ * v;

gtsam/geometry/SO3.h

@@ -143,14 +143,14 @@ class ExpmapFunctor {
   bool nearZero;
   double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
 
-  void init();
+  void init(bool nearZeroApprox = false);
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  ExpmapFunctor(const Vector3& omega);
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   /// Constructor with axis-angle
-  ExpmapFunctor(const Vector3& axis, double angle);
+  ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
 
   /// Rodrigues formula
   SO3 expmap() const;
@@ -164,7 +164,7 @@ class DexpFunctor : public ExpmapFunctor {
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  DexpFunctor(const Vector3& omega);
+  DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,

gtsam/geometry/tests/testSO3.cpp

@@ -203,49 +203,49 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
-Vector3 apply(const Vector3& omega, const Vector3& v) {
-  so3::DexpFunctor local(omega);
-  return local.applyDexp(v);
-}
-
 /* ************************************************************************* */
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
-  for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+  for (bool nearZeroApprox : {true, false}) {
-                        Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+    boost::function<Vector3(const Vector3&, const Vector3&)> f =
-    so3::DexpFunctor local(omega);
+        [=](const Vector3& omega, const Vector3& v) {
-    for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+          return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
-                      Vector3(0.4, 0.3, 0.2)}) {
+        };
-      EXPECT(assert_equal(Vector3(local.dexp() * v),
+    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          local.applyDexp(v, aH1, aH2)));
+                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      EXPECT(assert_equal(numericalDerivative21(apply, omega, v), aH1));
+      so3::DexpFunctor local(omega, nearZeroApprox);
-      EXPECT(assert_equal(numericalDerivative22(apply, omega, v), aH2));
+      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-      EXPECT(assert_equal(local.dexp(), aH2));
+                        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));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(local.dexp(), aH2));
+      }
     }
   }
 }
 
-/* ************************************************************************* */
-Vector3 applyInv(const Vector3& omega, const Vector3& v) {
-  so3::DexpFunctor local(omega);
-  return local.applyInvDexp(v);
-}
-
 /* ************************************************************************* */
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
-  for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+  for (bool nearZeroApprox : {true, false}) {
-                        Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+    boost::function<Vector3(const Vector3&, const Vector3&)> f =
-    so3::DexpFunctor local(omega);
+        [=](const Vector3& omega, const Vector3& v) {
-    Matrix invDexp = local.dexp().inverse();
+          return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
-    for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+        };
-                      Vector3(0.4, 0.3, 0.2)}) {
+    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-      EXPECT(
+                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-          assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2)));
+      so3::DexpFunctor local(omega, nearZeroApprox);
-      EXPECT(assert_equal(numericalDerivative21(applyInv, omega, v), aH1));
+      Matrix invDexp = local.dexp().inverse();
-      EXPECT(assert_equal(numericalDerivative22(applyInv, omega, v), aH2));
+      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-      EXPECT(assert_equal(invDexp, aH2));
+                        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));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(invDexp, aH2));
+      }
     }
   }
 }