Python tests

cython/gtsam/tests/test_SO4.py

@@ -14,10 +14,10 @@ import unittest
 import numpy as np
 from gtsam import SO4
 
-id = SO4()
+I4 = SO4()
-v1 = np.array([0.1, 0, 0, 0, 0, 0])
+v1 = np.array([0, 0, 0, .1, 0, 0])
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
 Q1 = SO4.Expmap(v1)
-v2 = np.array([0.01, 0.02, 0.03, 0, 0, 0])
 Q2 = SO4.Expmap(v2)
 
 
@@ -33,13 +33,13 @@ class TestSO4(unittest.TestCase):
     def test_retract(self):
         """Test retraction to manifold."""
         v = np.zeros((6,), np.float)
-        actual = id.retract(v)
+        actual = I4.retract(v)
-        self.assertTrue(actual.equals(id, 1e-9))
+        self.assertTrue(actual.equals(I4, 1e-9))
 
     def test_local(self):
         """Check localCoordinates for trivial case."""
         v0 = np.zeros((6,), np.float)
-        actual = id.localCoordinates(id)
+        actual = I4.localCoordinates(I4)
         np.testing.assert_array_almost_equal(actual, v0)
 
     def test_compose(self):
@@ -51,7 +51,7 @@ class TestSO4(unittest.TestCase):
     def test_vec(self):
         """Check wrapping of vec."""
         expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
-        actual = id.vec()
+        actual = I4.vec()
         np.testing.assert_array_equal(actual, expected)
 
 

cython/gtsam/tests/test_SOn.py

@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Dynamic SO(n) unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error
+import unittest
+
+import numpy as np
+from gtsam import SOn
+
+I4 = SOn(4)
+v1 = np.array([0, 0, 0, .1, 0, 0])
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q1 = I4.retract(v1)
+Q2 = I4.retract(v2)
+
+
+class TestSO4(unittest.TestCase):
+    """Test selected SOn methods."""
+
+    def test_constructor(self):
+        """Construct from matrix."""
+        matrix = np.eye(4)
+        so4 = SOn.FromMatrix(matrix)
+        self.assertIsInstance(so4, SOn)
+
+    def test_retract(self):
+        """Test retraction to manifold."""
+        v = np.zeros((6,), np.float)
+        actual = I4.retract(v)
+        self.assertTrue(actual.equals(I4, 1e-9))
+
+    def test_local(self):
+        """Check localCoordinates for trivial case."""
+        v0 = np.zeros((6,), np.float)
+        actual = I4.localCoordinates(I4)
+        np.testing.assert_array_almost_equal(actual, v0)
+
+    def test_compose(self):
+        """Check compose works in subgroup."""
+        expected = I4.retract(2*v1)
+        actual = Q1.compose(Q1)
+        self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
+
+    def test_vec(self):
+        """Check wrapping of vec."""
+        expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
+        actual = I4.vec()
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

gtsam.h

@@ -542,6 +542,7 @@ class SO3 {
   // Standard Constructors
   SO3();
   SO3(Matrix R);
+  static gtsam::SO3 FromMatrix(Matrix R);
   static gtsam::SO3 AxisAngle(const Vector axis, double theta);
   static gtsam::SO3 ClosestTo(const Matrix M);
 
@@ -570,6 +571,7 @@ class SO4 {
   // Standard Constructors
   SO4();
   SO4(Matrix R);
+  static gtsam::SO4 FromMatrix(Matrix R);
 
   // Testable
   void print(string s) const;
@@ -595,7 +597,22 @@ class SO4 {
 class SOn {
   // Standard Constructors
   SOn(size_t n);
-  SOn(Matrix R);
+  static gtsam::SOn FromMatrix(Matrix R);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SOn& other, double tol) const;
+
+  // Group
+  static gtsam::SOn identity();
+  gtsam::SOn inverse() const;
+  gtsam::SOn between(const gtsam::SOn& Q) const;
+  gtsam::SOn compose(const gtsam::SOn& Q) const;
+
+  // Manifold
+  gtsam::SOn retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SOn& Q) const;
+  static gtsam::SOn Expmap(Vector v);
 
   // Other methods
   Vector vec() const;

gtsam/geometry/SOn.h

@@ -76,14 +76,21 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// Construct SO<N> identity for N == Eigen::Dynamic
   template <int N_ = N, typename = IsDynamic<N_>>
   explicit SO(size_t n = 0) {
-    if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
+    // We allow for n=0 as the default constructor, needed for serialization,
+    // wrappers etc.
     matrix_ = Eigen::MatrixXd::Identity(n, n);
   }
 
-  /// Constructor from Eigen Matrix
+  /// Constructor from Eigen Matrix, dynamic version
   template <typename Derived>
   explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
 
+  /// Named constructor from Eigen Matrix
+  template <typename Derived>
+  static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
+    return SO(R);
+  }
+
   /// Construct dynamic SO(n) from Fixed SO<M>
   template <int M, int N_ = N, typename = IsDynamic<N_>>
   explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}