Merge pull request #887 from borglab/add-failing-axis-angle-test-c++

add failing unit test on axisAngle for Rot3 in c++

gtsam/geometry/SO3.cpp

@@ -261,25 +261,38 @@ 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 (std::abs(R33 + 1.0) > 1e-5)
-      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
-    else if (std::abs(R22 + 1.0) > 1e-5)
-      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, 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);
+  if (tr + 1.0 < 1e-4) {
+    if (R33 > R22 && R33 > R11) {
+      // R33 is the largest diagonal
+      const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0;
+      const double r = sqrt(2.0 + 2.0 * R33);
+      const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12);
+      omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33);
+    } else if (R22 > R11) {
+      // R22 is the largest diagonal
+      const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0;
+      const double r = sqrt(2.0 + 2.0 * R22);
+      const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31);
+      omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32);
+    } else {
+      // R11 is the largest diagonal
+      const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0;
+      const double r = sqrt(2.0 + 2.0 * R11);
+      const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23);
+      omega = sgn_w * scale * 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) {
+    const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
+    if (tr_3 < -1e-6) {
+      // this is the normal case -1 < trace < 3
       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

@@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle)
   CHECK(assert_equal(expected,actual3,1e-5));
 }
 
+/* ************************************************************************* */
+TEST( Rot3, AxisAngle2)
+{
+  // constructor from a rotation matrix, as doubles in *row-major* order.
+  Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781);
+  
+  Unit3 actualAxis;
+  double actualAngle;
+  // convert Rot3 to quaternion using GTSAM
+  std::tie(actualAxis, actualAngle) = R1.axisAngle();
+  
+  double expectedAngle = 3.1396582;
+  CHECK(assert_equal(expectedAngle, actualAngle, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Rot3, Rodrigues)
 {
@@ -181,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));
 
@@ -219,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
-  CHECK_OMEGA(x * PI, y * PI, z * PI)
-#endif
+//#else
+//  CHECK_OMEGA(x * PI, y * PI, z * PI)
+//#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)));
 
@@ -247,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
   EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
                       (Vector)Rot3::Logmap(Rlund), 1e-8));
-  #else
-    // SO3 does not bound angle resulting in ~180.1 degrees
-    EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
+#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));
-  #endif
+#endif
 }
 
 /* ************************************************************************* */

python/gtsam/tests/test_Rot3.py

@@ -0,0 +1,41 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+Rot3 unit tests.
+Author: John Lambert
+"""
+# pylint: disable=no-name-in-module
+
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import Rot3
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestRot3(GtsamTestCase):
+    """Test selected Rot3 methods."""
+
+    def test_axisangle(self) -> None:
+        """Test .axisAngle() method."""
+        # fmt: off
+        R = np.array(
+          [
+            [ -0.999957, 0.00922903, 0.00203116],
+            [ 0.00926964, 0.999739, 0.0208927],
+            [ -0.0018374, 0.0209105, -0.999781]
+          ])
+        # fmt: on
+        
+        # get back angle in radians
+        _, actual_angle = Rot3(R).axisAngle()
+        expected_angle = 3.1396582
+        np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7)
+
+
+if __name__ == "__main__":
+    unittest.main()