diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 577a13304..138f1ff13 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -6,8 +6,9 @@ All Rights Reserved See LICENSE for the license information Pose3 unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert, Duy Nguyen Ta """ +# pylint: disable=no-name-in-module import math import unittest diff --git a/cython/gtsam/tests/test_SO4.py b/cython/gtsam/tests/test_SO4.py new file mode 100644 index 000000000..3b30a8e89 --- /dev/null +++ b/cython/gtsam/tests/test_SO4.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 + +SO4 unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error +import unittest + +import numpy as np +from gtsam import SO4 + +id = SO4() +v1 = np.array([0.1, 0, 0, 0, 0, 0]) +Q1 = SO4.Expmap(v1) +v2 = np.array([0.01, 0.02, 0.03, 0, 0, 0]) +Q2 = SO4.Expmap(v2) + + +class TestSO4(unittest.TestCase): + """Test selected SO4 methods.""" + + def test_constructor(self): + """Construct from matrix.""" + matrix = np.eye(4) + so4 = SO4(matrix) + self.assertIsInstance(so4, SO4) + + 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)) + + def test_local(self): + """Check localCoordinates for trivial case.""" + v0 = np.zeros((6,), np.float) + actual = id.localCoordinates(id) + np.testing.assert_array_almost_equal(actual, v0) + + def test_compose(self): + """Check compose works in subgroup.""" + expected = SO4.Expmap(2*v1) + actual = Q1.compose(Q1) + self.assertTrue(actual.equals(expected, 1e-9)) + + 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() + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index bf3575580..d06d83a56 100644 --- a/gtsam.h +++ b/gtsam.h @@ -537,6 +537,60 @@ class Rot2 { void serialize() const; }; +#include +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); + + // Testable + void print(string s) const; + bool equals(const gtsam::SO3& other, double tol) const; + + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; + + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + + // Testable + void print(string s) const; + bool equals(const gtsam::SO4& other, double tol) const; + + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; + + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + #include class Rot3 { // Standard Constructors and Named Constructors @@ -559,6 +613,7 @@ class Rot3 { static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); // Testable void print(string s) const; @@ -1974,6 +2029,8 @@ class Values { void insert(size_t j, const gtsam::Point3& point3); void insert(size_t j, const gtsam::Rot2& rot2); void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -1989,6 +2046,8 @@ class Values { void update(size_t j, const gtsam::Point3& point3); void update(size_t j, const gtsam::Rot2& rot2); void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -1999,7 +2058,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double @@ -2332,7 +2391,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2343,7 +2402,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2355,7 +2414,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2614,6 +2673,12 @@ pair readG2o(string filename, bool void writeG2o(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& estimate, string filename); +#include +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; + //************************************************************************* // Navigation //*************************************************************************