From f5b57ce59f6520980282c9fc2b0660957eb4d096 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 14 May 2019 20:43:44 -0400 Subject: [PATCH] Python tests --- cython/gtsam/tests/test_SO4.py | 14 ++++---- cython/gtsam/tests/test_SOn.py | 59 ++++++++++++++++++++++++++++++++++ gtsam.h | 19 ++++++++++- gtsam/geometry/SOn.h | 11 +++++-- 4 files changed, 93 insertions(+), 10 deletions(-) create mode 100644 cython/gtsam/tests/test_SOn.py diff --git a/cython/gtsam/tests/test_SO4.py b/cython/gtsam/tests/test_SO4.py index 3b30a8e89..648bd1710 100644 --- a/cython/gtsam/tests/test_SO4.py +++ b/cython/gtsam/tests/test_SO4.py @@ -14,10 +14,10 @@ import unittest import numpy as np from gtsam import SO4 -id = SO4() -v1 = np.array([0.1, 0, 0, 0, 0, 0]) +I4 = SO4() +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) - self.assertTrue(actual.equals(id, 1e-9)) + 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 = 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) diff --git a/cython/gtsam/tests/test_SOn.py b/cython/gtsam/tests/test_SOn.py new file mode 100644 index 000000000..7599363e2 --- /dev/null +++ b/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() diff --git a/gtsam.h b/gtsam.h index efe36a595..e3f682187 100644 --- a/gtsam.h +++ b/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; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index fca229682..eea24f1a8 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -76,14 +76,21 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// Construct SO identity for N == Eigen::Dynamic template > 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 explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} + /// Named constructor from Eigen Matrix + template + static SO FromMatrix(const Eigen::MatrixBase& R) { + return SO(R); + } + /// Construct dynamic SO(n) from Fixed SO template > explicit SO(const SO& R) : matrix_(R.matrix()) {}