Fixed numerical problems near +-pi

gtsam/geometry/SO3.cpp

@@ -261,25 +261,37 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
 
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
-  if (tr + 1.0 < 1e-10) {
+  if (tr + 1.0 < 1e-4) {
-    if (std::abs(R33 + 1.0) > 1e-5)
+    std::vector<double> diags = {R11, R22, R33};
-      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
+    size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end()));
-    else if (std::abs(R22 + 1.0) > 1e-5)
+    if (max_elem == 2) {
-      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
+      const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0;
-    else
+      const double r = sqrt(2.0 + 2.0 * R33);
+      const double correction = 1.0 - M_1_PI / r * (R21 - R12);
+      omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33);
+    } else if (max_elem == 1) {
+      const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0;
+      const double r = sqrt(2.0 + 2.0 * R22);
+      const double correction = 1.0 - M_1_PI / r * (R13 - R31);
+      omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32);
+    } else {
       // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
-      omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
+      const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0;
+      const double r = sqrt(2.0 + 2.0 * R11);
+      const double correction = 1.0 - M_1_PI / r * (R32 - R23);
+      omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31);
+    }
   } else {
     double magnitude;
     const double tr_3 = tr - 3.0;  // always negative
-    if (tr_3 < -1e-7) {
+    if (tr_3 < -1e-6) {
       double theta = acos((tr - 1.0) / 2.0);
       magnitude = theta / (2.0 * sin(theta));
     } else {
       // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
       // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
       // see https://github.com/borglab/gtsam/issues/746 for details
-      magnitude = 0.5 - tr_3 / 12.0;
+      magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
     }
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }

gtsam/geometry/tests/testRot3.cpp

@@ -134,7 +134,7 @@ TEST( Rot3, AxisAngle2)
   std::tie(actualAxis, actualAngle) = R1.axisAngle();
   
   double expectedAngle = 3.1396582;
-  CHECK(assert_equal(expectedAngle, actualAngle, 1e-7));
+  CHECK(assert_equal(expectedAngle, actualAngle, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -196,13 +196,13 @@ TEST( Rot3, retract)
 }
 
 /* ************************************************************************* */
-TEST(Rot3, log) {
+TEST( Rot3, log) {
   static const double PI = boost::math::constants::pi<double>();
   Vector w;
   Rot3 R;
 
 #define CHECK_OMEGA(X, Y, Z)             \
-  w = (Vector(3) << X, Y, Z).finished(); \
+  w = (Vector(3) << (X), (Y), (Z)).finished(); \
   R = Rot3::Rodrigues(w);                \
   EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
 
@@ -234,17 +234,17 @@ TEST(Rot3, log) {
   CHECK_OMEGA(0, 0, PI)
 
   // Windows and Linux have flipped sign in quaternion mode
-#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
+//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
   w = (Vector(3) << x * PI, y * PI, z * PI).finished();
   R = Rot3::Rodrigues(w);
   EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
-#else
+//#else
-  CHECK_OMEGA(x * PI, y * PI, z * PI)
+//  CHECK_OMEGA(x * PI, y * PI, z * PI)
-#endif
+//#endif
 
   // Check 360 degree rotations
 #define CHECK_OMEGA_ZERO(X, Y, Z)        \
-  w = (Vector(3) << X, Y, Z).finished(); \
+  w = (Vector(3) << (X), (Y), (Z)).finished(); \
   R = Rot3::Rodrigues(w);                \
   EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
 
@@ -262,15 +262,15 @@ TEST(Rot3, log) {
   // Rot3's Logmap returns different, but equivalent compacted
   // axis-angle vectors depending on whether Rot3 is implemented
   // by Quaternions or SO3.
-  #if defined(GTSAM_USE_QUATERNIONS)
+#if defined(GTSAM_USE_QUATERNIONS)
-    // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
+  // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
-    EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
+  EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
+                      (Vector)Rot3::Logmap(Rlund), 1e-8));
+#else
+  // SO3 will be approximate because of the non-orthogonality
+  EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444),
                         (Vector)Rot3::Logmap(Rlund), 1e-8));
-  #else
+#endif
-    // SO3 does not bound angle resulting in ~180.1 degrees
-    EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
-                        (Vector)Rot3::Logmap(Rlund), 1e-8));
-  #endif
 }
 
 /* ************************************************************************* */