diff --git a/.travis.sh b/.travis.sh index 5de2d8e69..8b2b21e66 100755 --- a/.travis.sh +++ b/.travis.sh @@ -24,6 +24,7 @@ function configure() -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_USE_QUATERNIONS=${GTSAM_BUILD_UNSTABLE:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ diff --git a/.travis.yml b/.travis.yml index bcb6ceb4a..14fe66ac1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -28,6 +28,7 @@ install: stages: - compile - test + - special env: global: @@ -82,8 +83,8 @@ jobs: compiler: clang env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release script: bash .travis.sh -b -# on Linux, with deprecated ON to make sure that path still compiles - - stage: compile +# on Linux, with deprecated ON to make sure that path still compiles/tests + - stage: special os: linux compiler: clang env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON @@ -105,15 +106,19 @@ jobs: compiler: gcc env: CMAKE_BUILD_TYPE=Release script: bash .travis.sh -t -# Exclude g++ debug on Linux as it consistently times out -# - stage: test -# os: linux -# compiler: gcc -# env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF -# script: bash .travis.sh -t -# Exclude clang on Linux/clang in release until issue #57 is solved -# - stage: test -# os: linux -# compiler: clang -# env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release -# script: bash .travis.sh -t + - stage: test + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + script: bash .travis.sh -t + - stage: test + os: linux + compiler: clang + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -t +# on Linux, with quaternions ON to make sure that path still compiles/tests + - stage: special + os: linux + compiler: clang + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON + script: bash .travis.sh -t diff --git a/cython/gtsam/tests/test_KarcherMeanFactor.py b/cython/gtsam/tests/test_KarcherMeanFactor.py new file mode 100644 index 000000000..6976decc1 --- /dev/null +++ b/cython/gtsam/tests/test_KarcherMeanFactor.py @@ -0,0 +1,80 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KarcherMeanFactor unit tests. +Author: Frank Dellaert +""" + +# pylint: disable=invalid-name, no-name-in-module, no-member + +import unittest + +import gtsam +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +KEY = 0 +MODEL = gtsam.noiseModel_Unit.Create(3) + + +def find_Karcher_mean_Rot3(rotations): + """Find the Karcher mean of given values.""" + # Cost function C(R) = \sum PriorFactor(R_i)::error(R) + # No closed form solution. + graph = gtsam.NonlinearFactorGraph() + for R in rotations: + graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) + initial = gtsam.Values() + initial.insert(KEY, gtsam.Rot3()) + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + return result.atRot3(KEY) + + +# Rot3 version +R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0])) + + +class TestKarcherMean(GtsamTestCase): + + def test_find(self): + # Check that optimizing for Karcher mean (which minimizes Between distance) + # gets correct result. + rotations = {R, R.inverse()} + expected = gtsam.Rot3() + actual = find_Karcher_mean_Rot3(rotations) + self.gtsamAssertEquals(expected, actual) + + def test_factor(self): + """Check that the InnerConstraint factor leaves the mean unchanged.""" + # Make a graph with two variables, one between, and one InnerConstraint + # The optimal result should satisfy the between, while moving the other + # variable to make the mean the same as before. + # Mean of R and R' is identity. Let's make a BetweenFactor making R21 = + # R*R*R, i.e. geodesic length is 3 rather than 2. + graph = gtsam.NonlinearFactorGraph() + R12 = R.compose(R.compose(R)) + graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) + keys = gtsam.KeyVector() + keys.push_back(1) + keys.push_back(2) + graph.add(gtsam.KarcherMeanFactorRot3(keys)) + + initial = gtsam.Values() + initial.insert(1, R.inverse()) + initial.insert(2, R) + expected = find_Karcher_mean_Rot3([R, R.inverse()]) + + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + actual = find_Karcher_mean_Rot3( + [result.atRot3(1), result.atRot3(2)]) + self.gtsamAssertEquals(expected, actual) + self.gtsamAssertEquals( + R12, result.atRot3(1).between(result.atRot3(2))) + + +if __name__ == "__main__": + unittest.main() 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..648bd1710 --- /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 + +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) +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 = 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 = 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 = I4.vec() + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() 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 070a3c42f..9b4a5501e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -537,6 +537,88 @@ class Rot2 { void serialize() const; }; +#include +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); + + // 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); + static gtsam::SO4 FromMatrix(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 SOn { + // Standard Constructors + SOn(size_t n); + 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; + Matrix matrix() const; +}; + #include class Rot3 { // Standard Constructors and Named Constructors @@ -559,6 +641,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; @@ -2032,6 +2115,9 @@ 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::SOn& P); 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); @@ -2047,6 +2133,9 @@ 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::SOn& P); 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); @@ -2057,7 +2146,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 @@ -2394,7 +2483,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; @@ -2405,7 +2494,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; @@ -2417,7 +2506,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); @@ -2676,6 +2765,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 //************************************************************************* diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bbed26992..26c8a80d2 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -35,9 +35,6 @@ namespace gtsam { template struct LieGroup { - BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic, - "LieGroup not yet specialized for dynamically sized types."); - enum { dimension = N }; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix Jacobian; @@ -189,9 +186,6 @@ struct LieGroupTraits { typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic, - "LieGroupTraits not yet specialized for dynamically sized types."); - static int GetDimension(const Class&) {return dimension;} static TangentVector Local(const Class& origin, const Class& other, diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 166297d5f..82a5ae7f4 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -176,10 +176,11 @@ public: pointer_(NULL) { } - /// Constructor that will resize a dynamic matrix (unless already correct) - OptionalJacobian(Eigen::MatrixXd& dynamic) : - pointer_(&dynamic) { - } + /// Construct from pointer to dynamic matrix + OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {} + + /// Construct from refrence to dynamic matrix + OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {} #ifndef OPTIONALJACOBIAN_NOBOOST diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 01611d739..0acadf5bc 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -52,7 +52,6 @@ Pose3 Pose3::inverse() const { /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) -// Experimental - unit tests of derivatives based on it do not check out yet Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; @@ -418,24 +417,11 @@ boost::optional Pose3::Align(const std::vector& abPointPairs) for(const Point3Pair& abPair: abPointPairs) { Point3 da = abPair.first - aCentroid; Point3 db = abPair.second - bCentroid; - H += db * da.transpose(); + H += da * db.transpose(); } - // Compute SVD - Eigen::JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV); - Matrix U = svd.matrixU(); - Vector S = svd.singularValues(); - Matrix V = svd.matrixV(); - - // Check rank - if (S[1] < 1e-10) - return boost::none; - - // Recover transform with correction from Eggert97machinevisionandapplications - Matrix3 UVtranspose = U * V.transpose(); - Matrix3 detWeighting = I_3x3; - detWeighting(2, 2) = UVtranspose.determinant(); - Rot3 aRb(Matrix(V * detWeighting * U.transpose())); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); return Pose3(aRb, aTb); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 985c521a2..ab43b2d42 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -61,7 +61,7 @@ namespace gtsam { /** Internal Eigen Quaternion */ gtsam::Quaternion quaternion_; #else - Matrix3 rot_; + SO3 rot_; #endif public: @@ -92,26 +92,34 @@ namespace gtsam { * allow assignment/construction from a generic matrix. * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top */ - template - inline explicit Rot3(const Eigen::MatrixBase& R) { - #ifdef GTSAM_USE_QUATERNIONS - quaternion_=Matrix3(R); - #else - rot_ = R; - #endif + template +#ifdef GTSAM_USE_QUATERNIONS + explicit Rot3(const Eigen::MatrixBase& R) { + quaternion_ = Matrix3(R); } +#else + explicit Rot3(const Eigen::MatrixBase& R) : rot_(R) { + } +#endif /** * Constructor from a rotation matrix * Overload version for Matrix3 to avoid casting in quaternion mode. */ - inline explicit Rot3(const Matrix3& R) { - #ifdef GTSAM_USE_QUATERNIONS - quaternion_=R; - #else - rot_ = R; - #endif - } +#ifdef GTSAM_USE_QUATERNIONS + explicit Rot3(const Matrix3& R) : quaternion_(R) {} +#else + explicit Rot3(const Matrix3& R) : rot_(R) {} +#endif + + /** + * Constructor from an SO3 instance + */ +#ifdef GTSAM_USE_QUATERNIONS + explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {} +#else + explicit Rot3(const SO3& R) : rot_(R) {} +#endif /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion @@ -228,6 +236,9 @@ namespace gtsam { static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // const Unit3& a_q, const Unit3& b_q); + /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); } + /// @} /// @name Testable /// @{ @@ -483,28 +494,27 @@ namespace gtsam { /// @} #endif - private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) - { + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { #ifndef GTSAM_USE_QUATERNIONS - ar & boost::serialization::make_nvp("rot11", rot_(0,0)); - ar & boost::serialization::make_nvp("rot12", rot_(0,1)); - ar & boost::serialization::make_nvp("rot13", rot_(0,2)); - ar & boost::serialization::make_nvp("rot21", rot_(1,0)); - ar & boost::serialization::make_nvp("rot22", rot_(1,1)); - ar & boost::serialization::make_nvp("rot23", rot_(1,2)); - ar & boost::serialization::make_nvp("rot31", rot_(2,0)); - ar & boost::serialization::make_nvp("rot32", rot_(2,1)); - ar & boost::serialization::make_nvp("rot33", rot_(2,2)); + Matrix3& M = rot_.matrix_; + ar& boost::serialization::make_nvp("rot11", M(0, 0)); + ar& boost::serialization::make_nvp("rot12", M(0, 1)); + ar& boost::serialization::make_nvp("rot13", M(0, 2)); + ar& boost::serialization::make_nvp("rot21", M(1, 0)); + ar& boost::serialization::make_nvp("rot22", M(1, 1)); + ar& boost::serialization::make_nvp("rot23", M(1, 2)); + ar& boost::serialization::make_nvp("rot31", M(2, 0)); + ar& boost::serialization::make_nvp("rot32", M(2, 1)); + ar& boost::serialization::make_nvp("rot33", M(2, 2)); #else - ar & boost::serialization::make_nvp("w", quaternion_.w()); - ar & boost::serialization::make_nvp("x", quaternion_.x()); - ar & boost::serialization::make_nvp("y", quaternion_.y()); - ar & boost::serialization::make_nvp("z", quaternion_.z()); + ar& boost::serialization::make_nvp("w", quaternion_.w()); + ar& boost::serialization::make_nvp("x", quaternion_.x()); + ar& boost::serialization::make_nvp("y", quaternion_.y()); + ar& boost::serialization::make_nvp("z", quaternion_.z()); #endif } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 529c64973..d38d33bf1 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -36,18 +36,17 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1; - rot_.col(1) = col2; - rot_.col(2) = col3; + Matrix3 R; + R << col1, col2, col3; + rot_ = SO3(R); } /* ************************************************************************* */ -Rot3::Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) { - rot_ << R11, R12, R13, - R21, R22, R23, - R31, R32, R33; +Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, + double R23, double R31, double R32, double R33) { + Matrix3 R; + R << R11, R12, R13, R21, R22, R23, R31, R32, R33; + rot_ = SO3(R); } /* ************************************************************************* */ @@ -107,21 +106,21 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(Matrix3(rot_*R2.rot_)); + return Rot3(rot_*R2.rot_); } /* ************************************************************************* */ // TODO const Eigen::Transpose Rot3::transpose() const { Matrix3 Rot3::transpose() const { - return rot_.transpose(); + return rot_.matrix().transpose(); } /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; - return rot_ * p; + if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_.matrix(); + return rot_.matrix() * p; } /* ************************************************************************* */ @@ -178,21 +177,21 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { /* ************************************************************************* */ Matrix3 Rot3::matrix() const { - return rot_; + return rot_.matrix(); } /* ************************************************************************* */ -Point3 Rot3::r1() const { return Point3(rot_.col(0)); } +Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); } /* ************************************************************************* */ -Point3 Rot3::r2() const { return Point3(rot_.col(1)); } +Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); } /* ************************************************************************* */ -Point3 Rot3::r3() const { return Point3(rot_.col(2)); } +Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); } /* ************************************************************************* */ gtsam::Quaternion Rot3::toQuaternion() const { - return gtsam::Quaternion(rot_); + return gtsam::Quaternion(rot_.matrix()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 034956e99..7ea9dfb07 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -18,18 +18,38 @@ * @date December 2014 */ -#include #include +#include + +#include + #include -#include #include +#include namespace gtsam { +//****************************************************************************** namespace so3 { +Matrix99 Dcompose(const SO3& Q) { + Matrix99 H; + auto R = Q.matrix(); + H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), // + I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), // + I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2); + return H; +} + +Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { + Matrix3 MR = M * R.matrix(); + if (H) *H = Dcompose(R); + return MR; +} + void ExpmapFunctor::init(bool nearZeroApprox) { - nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); + nearZero = + nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); @@ -48,7 +68,8 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) } } -ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) +ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, + bool nearZeroApprox) : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; @@ -61,16 +82,16 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp SO3 ExpmapFunctor::expmap() const { if (nearZero) - return I_3x3 + W; + return SO3(I_3x3 + W); else - return I_3x3 + sin_theta * K + one_minus_cos * KK; + return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - if (nearZero) + if (nearZero) { dexp_ = I_3x3 - 0.5 * W; - else { + } else { a = one_minus_cos / theta; b = 1.0 - sin_theta / theta; dexp_ = I_3x3 - a * K + b * KK; @@ -103,7 +124,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, if (H1) { Matrix3 D_dexpv_omega; applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp* D_dexpv_omega; + *H1 = -invDexp * D_dexpv_omega; } if (H2) *H2 = invDexp; return c; @@ -111,39 +132,117 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, } // namespace so3 -/* ************************************************************************* */ +//****************************************************************************** +template <> SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } -/* ************************************************************************* */ -void SO3::print(const std::string& s) const { - std::cout << s << *this << std::endl; - } +//****************************************************************************** +template <> +SO3 SO3::ClosestTo(const Matrix3& M) { + Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); + const auto& U = svd.matrixU(); + const auto& V = svd.matrixV(); + const double det = (U * V.transpose()).determinant(); + return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose()); +} -/* ************************************************************************* */ +//****************************************************************************** +template <> +SO3 SO3::ChordalMean(const std::vector& rotations) { + // See Hartley13ijcv: + // Cost function C(R) = \sum sqr(|R-R_i|_F) + // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!! + Matrix3 C_e{Z_3x3}; + for (const auto& R_i : rotations) { + C_e += R_i.matrix(); + } + return ClosestTo(C_e); +} + +//****************************************************************************** +template <> +Matrix3 SO3::Hat(const Vector3& xi) { + // skew symmetric matrix X = xi^ + Matrix3 Y = Z_3x3; + Y(0, 1) = -xi(2); + Y(0, 2) = +xi(1); + Y(1, 2) = -xi(0); + return Y - Y.transpose(); +} + +//****************************************************************************** +template <> +Vector3 SO3::Vee(const Matrix3& X) { + Vector3 xi; + xi(0) = -X(1, 2); + xi(1) = +X(0, 2); + xi(2) = -X(0, 1); + return xi; +} + +//****************************************************************************** +template <> +Matrix3 SO3::AdjointMap() const { + return matrix_; +} + +//****************************************************************************** +template <> SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); *H = impl.dexp(); return impl.expmap(); - } else + } else { return so3::ExpmapFunctor(omega).expmap(); + } } +template <> Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { return so3::DexpFunctor(omega).dexp(); } -/* ************************************************************************* */ -Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { - using std::sqrt; +//****************************************************************************** +/* Right Jacobian for Log map in SO(3) - equation (10.86) and following + equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie + Groups", Volume 2, 2008. + + logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega + + where Jrinv = LogmapDerivative(omega). This maps a perturbation on the + manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv * + omega) + */ +template <> +Matrix3 SO3::LogmapDerivative(const Vector3& omega) { + using std::cos; using std::sin; + double theta2 = omega.dot(omega); + if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; + double theta = std::sqrt(theta2); // rotation angle + + // element of Lie algebra so(3): W = omega^ + const Matrix3 W = Hat(omega); + return I_3x3 + 0.5 * W + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * + W * W; +} + +//****************************************************************************** +template <> +Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { + using std::sin; + using std::sqrt; + // note switch to base 1 - const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); - const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); - const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); + const Matrix3& R = Q.matrix(); + const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); + const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); + const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); // Get trace(R) const double tr = R.trace(); @@ -162,7 +261,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; - const double tr_3 = tr - 3.0; // always negative + const double tr_3 = tr - 3.0; // always negative if (tr_3 < -1e-7) { double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); @@ -174,32 +273,49 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } - if(H) *H = LogmapDerivative(omega); + if (H) *H = LogmapDerivative(omega); return omega; } -/* ************************************************************************* */ -Matrix3 SO3::LogmapDerivative(const Vector3& omega) { - using std::cos; - using std::sin; +//****************************************************************************** +// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap - double theta2 = omega.dot(omega); - if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; - double theta = std::sqrt(theta2); // rotation angle - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega - * where Jrinv = LogmapDerivative(omega); - * This maps a perturbation on the manifold (expmap(omega)) - * to a perturbation in the tangent space (Jrinv * omega) - */ - const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^ - return I_3x3 + 0.5 * W + - (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * - W * W; +template <> +SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + return Expmap(omega, H); } -/* ************************************************************************* */ +template <> +Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) { + return Logmap(R, H); +} -} // end namespace gtsam +//****************************************************************************** +// local vectorize +static Vector9 vec3(const Matrix3& R) { + return Eigen::Map(R.data()); +} +// so<3> generators +static std::vector G3({SO3::Hat(Vector3::Unit(0)), + SO3::Hat(Vector3::Unit(1)), + SO3::Hat(Vector3::Unit(2))}); + +// vectorized generators +static const Matrix93 P3 = + (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished(); + +//****************************************************************************** +template <> +Vector9 SO3::vec(OptionalJacobian<9, 3> H) const { + const Matrix3& R = matrix_; + if (H) { + // As Luca calculated (for SO4), this is (I3 \oplus R) * P3 + *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0), + R * P3.block<3, 3>(6, 0); + } + return gtsam::vec3(R); +} +//****************************************************************************** + +} // end namespace gtsam diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 5f1c7d1bf..eb6129ac8 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -20,120 +20,103 @@ #pragma once -#include +#include + #include +#include #include -#include +#include namespace gtsam { +using SO3 = SO<3>; + +// Below are all declarations of SO<3> specializations. +// They are *defined* in SO3.cpp. + +template <> +SO3 SO3::AxisAngle(const Vector3& axis, double theta); + +template <> +SO3 SO3::ClosestTo(const Matrix3& M); + +template <> +SO3 SO3::ChordalMean(const std::vector& rotations); + +template <> +Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix + +template <> +Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat + +/// Adjoint map +template <> +Matrix3 SO3::AdjointMap() const; + /** - * True SO(3), i.e., 3*3 matrix subgroup - * We guarantee (all but first) constructors only generate from sub-manifold. - * However, round-off errors in repeated composition could move off it... + * Exponential map at identity - create a rotation from canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ -class SO3: public Matrix3, public LieGroup { +template <> +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); -protected: +/// Derivative of Expmap +template <> +Matrix3 SO3::ExpmapDerivative(const Vector3& omega); -public: - enum { - dimension = 3 - }; +/** + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation + */ +template <> +Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); - /// @name Constructors - /// @{ +/// Derivative of Logmap +template <> +Matrix3 SO3::LogmapDerivative(const Vector3& omega); - /// Constructor from AngleAxisd - SO3() : - Matrix3(I_3x3) { - } +// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap +template <> +SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H); - /// Constructor from Eigen Matrix - template - SO3(const MatrixBase& R) : - Matrix3(R.eval()) { - } +template <> +Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H); - /// Constructor from AngleAxisd - SO3(const Eigen::AngleAxisd& angleAxis) : - Matrix3(angleAxis) { - } +template <> +Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; - /// Static, named constructor TODO think about relation with above - GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta); +/** Serialization function */ +template +void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) { + Matrix3& M = R.matrix_; + ar& boost::serialization::make_nvp("R11", M(0, 0)); + ar& boost::serialization::make_nvp("R12", M(0, 1)); + ar& boost::serialization::make_nvp("R13", M(0, 2)); + ar& boost::serialization::make_nvp("R21", M(1, 0)); + ar& boost::serialization::make_nvp("R22", M(1, 1)); + ar& boost::serialization::make_nvp("R23", M(1, 2)); + ar& boost::serialization::make_nvp("R31", M(2, 0)); + ar& boost::serialization::make_nvp("R32", M(2, 1)); + ar& boost::serialization::make_nvp("R33", M(2, 2)); +} - /// @} - /// @name Testable - /// @{ - - GTSAM_EXPORT void print(const std::string& s) const; - - bool equals(const SO3 & R, double tol) const { - return equal_with_abs_tol(*this, R, tol); - } - - /// @} - /// @name Group - /// @{ - - /// identity rotation for group operation - static SO3 identity() { - return I_3x3; - } - - /// inverse of a rotation = transpose - SO3 inverse() const { - return this->Matrix3::inverse(); - } - - /// @} - /// @name Lie Group - /// @{ - - /** - * Exponential map at identity - create a rotation from canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula - */ - GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); - - /// Derivative of Expmap - GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& omega); - - /** - * Log map at identity - returns the canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ of this rotation - */ - GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); - - /// Derivative of Logmap - GTSAM_EXPORT static Matrix3 LogmapDerivative(const Vector3& omega); - - Matrix3 AdjointMap() const { - return *this; - } - - // Chart at origin - struct ChartAtOrigin { - static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) { - return Expmap(omega, H); - } - static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { - return Logmap(R, H); - } - }; - - using LieGroup::inverse; - - /// @} -}; - -// This namespace exposes two functors that allow for saving computation when -// exponential map and its derivatives are needed at the same location in so<3> -// The second functor also implements dedicated methods to apply dexp and/or inv(dexp) namespace so3 { +/** + * Compose general matrix with an SO(3) element. + * We only provide the 9*9 derivative in the first argument M. + */ +Matrix3 compose(const Matrix3& M, const SO3& R, + OptionalJacobian<9, 9> H = boost::none); + +/// (constant) Jacobian of compose wrpt M +Matrix99 Dcompose(const SO3& R); + +// Below are two functors that allow for saving computation when exponential map +// and its derivatives are needed at the same location in so<3>. The second +// functor also implements dedicated methods to apply dexp and/or inv(dexp). + /// Functor implementing Exponential map class GTSAM_EXPORT ExpmapFunctor { protected: @@ -146,7 +129,7 @@ class GTSAM_EXPORT ExpmapFunctor { public: /// Constructor with element of Lie algebra so(3) - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); + explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); /// Constructor with axis-angle ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); @@ -163,7 +146,7 @@ class DexpFunctor : public ExpmapFunctor { public: /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -184,12 +167,14 @@ class DexpFunctor : public ExpmapFunctor { }; } // namespace so3 -template<> -struct traits : public internal::LieGroup { -}; +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ -template<> -struct traits : public internal::LieGroup { -}; -} // end namespace gtsam +template <> +struct traits : public internal::LieGroup {}; +template <> +struct traits : public internal::LieGroup {}; + +} // end namespace gtsam diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp new file mode 100644 index 000000000..dd15807c3 --- /dev/null +++ b/gtsam/geometry/SO4.cpp @@ -0,0 +1,226 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SO4.cpp + * @brief 4*4 matrix representation of SO(4) + * @author Frank Dellaert + * @author Luca Carlone + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +// /* ************************************************************************* +// */ static Vector3 randomOmega(boost::mt19937 &rng) { +// static boost::uniform_real randomAngle(-M_PI, M_PI); +// return Unit3::Random(rng).unitVector() * randomAngle(rng); +// } + +// /* ************************************************************************* +// */ +// // Create random SO(4) element using direct product of lie algebras. +// SO4 SO4::Random(boost::mt19937 &rng) { +// Vector6 delta; +// delta << randomOmega(rng), randomOmega(rng); +// return SO4::Expmap(delta); +// } + +//****************************************************************************** +template <> +Matrix4 SO4::Hat(const Vector6& xi) { + // skew symmetric matrix X = xi^ + // Unlike Luca, makes upper-left the SO(3) subgroup. + Matrix4 Y = Z_4x4; + Y(0, 1) = -xi(5); + Y(0, 2) = +xi(4); + Y(1, 2) = -xi(3); + Y(0, 3) = -xi(2); + Y(1, 3) = +xi(1); + Y(2, 3) = -xi(0); + return Y - Y.transpose(); +} + +//****************************************************************************** +template <> +Vector6 SO4::Vee(const Matrix4& X) { + Vector6 xi; + xi(5) = -X(0, 1); + xi(4) = +X(0, 2); + xi(3) = -X(1, 2); + xi(2) = -X(0, 3); + xi(1) = +X(1, 3); + xi(0) = -X(2, 3); + return xi; +} + +//****************************************************************************** +/* Exponential map, porting MATLAB implementation by Luca, which follows + * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by + * Ramona-Andreaa Rohan */ +template <> +SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { + using namespace std; + if (H) throw std::runtime_error("SO4::Expmap Jacobian"); + + // skew symmetric matrix X = xi^ + const Matrix4 X = Hat(xi); + + // do eigen-decomposition + auto eig = Eigen::EigenSolver(X); + Eigen::Vector4cd e = eig.eigenvalues(); + using std::abs; + sort(e.data(), e.data() + 4, [](complex a, complex b) { + return abs(a.imag()) > abs(b.imag()); + }); + + // Get a and b from eigenvalues +/i ai and +/- bi + double a = e[0].imag(), b = e[2].imag(); + if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) { + throw runtime_error("SO4::Expmap: wrong eigenvalues."); + } + + // Build expX = exp(xi^) + Matrix4 expX; + using std::cos; + using std::sin; + const auto X2 = X * X; + const auto X3 = X2 * X; + double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b; + if (a != 0 && b == 0) { + double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3; + return SO4(I_4x4 + X + c2 * X2 + c3 * X3); + } else if (a == b && b != 0) { + double sin_a = sin(a), cos_a = cos(a); + double c0 = (a * sin_a + 2 * cos_a) / 2, + c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a), + c3 = (sin_a - a * cos_a) / (2 * a3); + return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); + } else if (a != b) { + double sin_a = sin(a), cos_a = cos(a); + double sin_b = sin(b), cos_b = cos(b); + double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2), + c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)), + c2 = (cos_a - cos_b) / (b2 - a2), + c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2)); + return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); + } else { + return SO4(); + } +} + +//****************************************************************************** +// local vectorize +static SO4::VectorN2 vec4(const Matrix4& Q) { + return Eigen::Map(Q.data()); +} + +// so<4> generators +static std::vector > G4( + {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), + SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), + SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); + +// vectorized generators +static const Eigen::Matrix P4 = + (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), + vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) + .finished(); + +//****************************************************************************** +template <> +Matrix6 SO4::AdjointMap() const { + // Elaborate way of calculating the AdjointMap + // TODO(frank): find a closed form solution. In SO(3) is just R :-/ + const Matrix4& Q = matrix_; + const Matrix4 Qt = Q.transpose(); + Matrix6 A; + for (size_t i = 0; i < 6; i++) { + // Calculate column i of linear map for coeffcient of Gi + A.col(i) = SO4::Vee(Q * G4[i] * Qt); + } + return A; +} + +//****************************************************************************** +template <> +SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { + const Matrix& Q = matrix_; + if (H) { + // As Luca calculated, this is (I4 \oplus Q) * P4 + *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0), + Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0); + } + return gtsam::vec4(Q); +} + +///****************************************************************************** +template <> +SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { + if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); + gttic(SO4_Retract); + const Matrix4 X = Hat(xi / 2); + return SO4((I_4x4 + X) * (I_4x4 - X).inverse()); +} + +//****************************************************************************** +template <> +Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) { + if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); + const Matrix4& R = Q.matrix(); + const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse(); + return -2 * Vee(X); +} + +//****************************************************************************** +Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) { + const Matrix4& R = Q.matrix(); + const Matrix3 M = R.topLeftCorner<3, 3>(); + if (H) { + const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), + q = R.topRightCorner<3, 1>(); + *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, // + Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, // + q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; + } + return M; +} + +//****************************************************************************** +Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) { + const Matrix4& R = Q.matrix(); + const Matrix43 M = R.leftCols<3>(); + if (H) { + const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3); + *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, // + Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, // + q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; + } + return M; +} + +//****************************************************************************** + +} // end namespace gtsam diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h new file mode 100644 index 000000000..c8e85b63f --- /dev/null +++ b/gtsam/geometry/SO4.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SO4.h + * @brief 4*4 matrix representation of SO(4) + * @author Frank Dellaert + * @author Luca Carlone + * @date March 2019 + */ + +#pragma once + +#include + +#include +#include +#include +#include + +#include + +#include + +namespace gtsam { + +using SO4 = SO<4>; + +// /// Random SO(4) element (no big claims about uniformity) +// static SO4 Random(boost::mt19937 &rng); + +// Below are all declarations of SO<4> specializations. +// They are *defined* in SO4.cpp. + +template <> +Matrix4 SO4::Hat(const TangentVector &xi); + +template <> +Vector6 SO4::Vee(const Matrix4 &X); + +template <> +SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H); + +template <> +Matrix6 SO4::AdjointMap() const; + +template <> +SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const; + +template <> +SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H); + +template <> +Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H); + +/** + * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). + */ +Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none); + +/** + * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) + * -> S \in St(3,4). + */ +Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none); + +/** Serialization function */ +template +void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) { + Matrix4 &M = Q.matrix_; + ar &boost::serialization::make_nvp("Q11", M(0, 0)); + ar &boost::serialization::make_nvp("Q12", M(0, 1)); + ar &boost::serialization::make_nvp("Q13", M(0, 2)); + ar &boost::serialization::make_nvp("Q14", M(0, 3)); + + ar &boost::serialization::make_nvp("Q21", M(1, 0)); + ar &boost::serialization::make_nvp("Q22", M(1, 1)); + ar &boost::serialization::make_nvp("Q23", M(1, 2)); + ar &boost::serialization::make_nvp("Q24", M(1, 3)); + + ar &boost::serialization::make_nvp("Q31", M(2, 0)); + ar &boost::serialization::make_nvp("Q32", M(2, 1)); + ar &boost::serialization::make_nvp("Q33", M(2, 2)); + ar &boost::serialization::make_nvp("Q34", M(2, 3)); + + ar &boost::serialization::make_nvp("Q41", M(3, 0)); + ar &boost::serialization::make_nvp("Q42", M(3, 1)); + ar &boost::serialization::make_nvp("Q43", M(3, 2)); + ar &boost::serialization::make_nvp("Q44", M(3, 3)); +} + +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ + +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +} // end namespace gtsam diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h new file mode 100644 index 000000000..344822c1f --- /dev/null +++ b/gtsam/geometry/SOn-inl.h @@ -0,0 +1,112 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file SOn-inl.h + * @brief Template implementations for SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#include + +namespace gtsam { + +// Implementation for N>5 just uses dynamic version +template +typename SO::MatrixNN SO::Hat(const TangentVector& xi) { + return SOn::Hat(xi); +} + +// Implementation for N>5 just uses dynamic version +template +typename SO::TangentVector SO::Vee(const MatrixNN& X) { + return SOn::Vee(X); +} + +template +SO SO::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) { + if (H) throw std::runtime_error("SO::Retract jacobian not implemented."); + const Matrix X = Hat(xi / 2.0); + size_t n = AmbientDim(xi.size()); + const auto I = Eigen::MatrixXd::Identity(n, n); + // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf + return SO((I + X) * (I - X).inverse()); +} + +template +typename SO::TangentVector SO::ChartAtOrigin::Local(const SO& R, + ChartJacobian H) { + if (H) throw std::runtime_error("SO::Local jacobian not implemented."); + const size_t n = R.rows(); + const auto I = Eigen::MatrixXd::Identity(n, n); + const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); + return -2 * Vee(X); +} + +template +typename SO::MatrixDD SO::AdjointMap() const { + throw std::runtime_error( + "SO::AdjointMap only implemented for SO3 and SO4."); +} + +template +SO SO::Expmap(const TangentVector& omega, ChartJacobian H) { + throw std::runtime_error("SO::Expmap only implemented for SO3 and SO4."); +} + +template +typename SO::MatrixDD SO::ExpmapDerivative(const TangentVector& omega) { + throw std::runtime_error("SO::ExpmapDerivative only implemented for SO3."); +} + +template +typename SO::TangentVector SO::Logmap(const SO& R, ChartJacobian H) { + throw std::runtime_error("SO::Logmap only implemented for SO3."); +} + +template +typename SO::MatrixDD SO::LogmapDerivative(const TangentVector& omega) { + throw std::runtime_error("O::LogmapDerivative only implemented for SO3."); +} + +template +typename SO::VectorN2 SO::vec( + OptionalJacobian H) const { + const size_t n = rows(); + const size_t n2 = n * n; + + // Vectorize + VectorN2 X(n2); + X << Eigen::Map(matrix_.data(), n2, 1); + + // If requested, calculate H as (I \oplus Q) * P, + // where Q is the N*N rotation matrix, and P is calculated below. + if (H) { + // Calculate P matrix of vectorized generators + // TODO(duy): Should we refactor this as the jacobian of Hat? + const size_t d = dim(); + Matrix P(n2, d); + for (size_t j = 0; j < d; j++) { + const auto X = Hat(Eigen::VectorXd::Unit(d, j)); + P.col(j) = Eigen::Map(X.data(), n2, 1); + } + H->resize(n2, d); + for (size_t i = 0; i < n; i++) { + H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); + } + } + return X; +} + +} // namespace gtsam diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp new file mode 100644 index 000000000..06d24472e --- /dev/null +++ b/gtsam/geometry/SOn.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SOn.cpp + * @brief Definitions of dynamic specializations of SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#include + +namespace gtsam { + +template <> +Matrix SOn::Hat(const Vector& xi) { + size_t n = AmbientDim(xi.size()); + if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); + + Matrix X(n, n); // allocate space for n*n skew-symmetric matrix + X.setZero(); + if (n == 2) { + // Handle SO(2) case as recursion bottom + assert(xi.size() == 1); + X << 0, -xi(0), xi(0), 0; + } else { + // Recursively call SO(n-1) call for top-left block + const size_t dmin = (n - 1) * (n - 2) / 2; + X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); + + // Now fill last row and column + double sign = 1.0; + for (size_t i = 0; i < n - 1; i++) { + const size_t j = n - 2 - i; + X(n - 1, j) = sign * xi(i); + X(j, n - 1) = -X(n - 1, j); + sign = -sign; + } + } + return X; +} + +template <> +Vector SOn::Vee(const Matrix& X) { + const size_t n = X.rows(); + if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); + + if (n == 2) { + // Handle SO(2) case as recursion bottom + Vector xi(1); + xi(0) = X(1, 0); + return xi; + } else { + // Calculate dimension and allocate space + const size_t d = n * (n - 1) / 2; + Vector xi(d); + + // Fill first n-1 spots from last row of X + double sign = 1.0; + for (size_t i = 0; i < n - 1; i++) { + const size_t j = n - 2 - i; + xi(i) = sign * X(n - 1, j); + sign = -sign; + } + + // Recursively call Vee to fill remainder of x + const size_t dmin = (n - 1) * (n - 2) / 2; + xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1)); + return xi; + } +} + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); + return derived() * g; +} + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + SOn result = derived().inverse() * g; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); + return result; +} + +} // namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h new file mode 100644 index 000000000..7954c4d6c --- /dev/null +++ b/gtsam/geometry/SOn.h @@ -0,0 +1,332 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SOn.h + * @brief N*N matrix representation of SO(N). N can be Eigen::Dynamic + * @author Frank Dellaert + * @date March 2019 + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace gtsam { + +namespace internal { +/// Calculate dimensionality of SO manifold, or return Dynamic if so +constexpr int DimensionSO(int N) { + return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; +} + +// Calculate N^2 at compile time, or return Dynamic if so +constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } +} // namespace internal + +/** + * Manifold of special orthogonal rotation matrices SO. + * Template paramater N can be a fixed integer or can be Eigen::Dynamic + */ +template +class SO : public LieGroup, internal::DimensionSO(N)> { + public: + enum { dimension = internal::DimensionSO(N) }; + using MatrixNN = Eigen::Matrix; + using VectorN2 = Eigen::Matrix; + using MatrixDD = Eigen::Matrix; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + protected: + MatrixNN matrix_; ///< Rotation matrix + + // enable_if_t aliases, used to specialize constructors/methods, see + // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/ + template + using IsDynamic = typename std::enable_if::type; + template + using IsFixed = typename std::enable_if= 2, void>::type; + template + using IsSO3 = typename std::enable_if::type; + + public: + /// @name Constructors + /// @{ + + /// Construct SO identity for N >= 2 + template > + SO() : matrix_(MatrixNN::Identity()) {} + + /// Construct SO identity for N == Eigen::Dynamic + template > + explicit SO(size_t n = 0) { + // We allow for n=0 as the default constructor, needed for serialization, + // wrappers etc. + matrix_ = Eigen::MatrixXd::Identity(n, n); + } + + /// 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()) {} + + /// Constructor from AngleAxisd + template > + explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} + + /// Constructor from axis and angle. Only defined for SO3 + static SO AxisAngle(const Vector3& axis, double theta); + + /// Named constructor that finds SO(n) matrix closest to M in Frobenius norm, + /// currently only defined for SO3. + static SO ClosestTo(const MatrixNN& M); + + /// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F), + /// currently only defined for SO3. + static SO ChordalMean(const std::vector& rotations); + + /// Random SO(n) element (no big claims about uniformity) + template > + static SO Random(boost::mt19937& rng, size_t n = 0) { + if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); + // TODO(frank): This needs to be re-thought! + static boost::uniform_real randomAngle(-M_PI, M_PI); + const size_t d = SO::Dimension(n); + Vector xi(d); + for (size_t j = 0; j < d; j++) { + xi(j) = randomAngle(rng); + } + return SO::Retract(xi); + } + + /// Random SO(N) element (no big claims about uniformity) + template > + static SO Random(boost::mt19937& rng) { + // By default, use dynamic implementation above. Specialized for SO(3). + return SO(SO::Random(rng, N).matrix()); + } + + /// @} + /// @name Standard methods + /// @{ + + /// Return matrix + const MatrixNN& matrix() const { return matrix_; } + + size_t rows() const { return matrix_.rows(); } + size_t cols() const { return matrix_.cols(); } + + /// @} + /// @name Testable + /// @{ + + void print(const std::string& s) const { + std::cout << s << matrix_ << std::endl; + } + + bool equals(const SO& other, double tol) const { + return equal_with_abs_tol(matrix_, other.matrix_, tol); + } + + /// @} + /// @name Group + /// @{ + + /// Multiplication + SO operator*(const SO& other) const { + assert(dim() == other.dim()); + return SO(matrix_ * other.matrix_); + } + + /// SO identity for N >= 2 + template > + static SO identity() { + return SO(); + } + + /// SO identity for N == Eigen::Dynamic + template > + static SO identity(size_t n = 0) { + return SO(n); + } + + /// inverse of a rotation = transpose + SO inverse() const { return SO(matrix_.transpose()); } + + /// @} + /// @name Manifold + /// @{ + + using TangentVector = Eigen::Matrix; + using ChartJacobian = OptionalJacobian; + + /// Return compile-time dimensionality: fixed size N or Eigen::Dynamic + static int Dim() { return dimension; } + + // Calculate manifold dimensionality for SO(n). + // Available as dimension or Dim() for fixed N. + static size_t Dimension(size_t n) { return n * (n - 1) / 2; } + + // Calculate ambient dimension n from manifold dimensionality d. + static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; } + + // Calculate run-time dimensionality of manifold. + // Available as dimension or Dim() for fixed N. + size_t dim() const { return Dimension(matrix_.rows()); } + + /** + * Hat operator creates Lie algebra element corresponding to d-vector, where d + * is the dimensionality of the manifold. This function is implemented + * recursively, and the d-vector is assumed to laid out such that the last + * element corresponds to so(2), the last 3 to so(3), the last 6 to so(4) + * etc... For example, the vector-space isomorphic to so(5) is laid out as: + * a b c d | u v w | x y | z + * where the latter elements correspond to "telescoping" sub-algebras: + * 0 -z y -w d + * z 0 -x v -c + * -y x 0 -u b + * w -v u 0 -a + * -d c -b a 0 + * This scheme behaves exactly as expected for SO(2) and SO(3). + */ + static MatrixNN Hat(const TangentVector& xi); + + /** + * Inverse of Hat. See note about xi element order in Hat. + */ + static TangentVector Vee(const MatrixNN& X); + + // Chart at origin + struct ChartAtOrigin { + /** + * Retract uses Cayley map. See note about xi element order in Hat. + * Deafault implementation has no Jacobian implemented + */ + static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none); + + /** + * Inverse of Retract. See note about xi element order in Hat. + */ + static TangentVector Local(const SO& R, ChartJacobian H = boost::none); + }; + + // Return dynamic identity DxD Jacobian for given SO(n) + template > + static MatrixDD IdentityJacobian(size_t n) { + const size_t d = Dimension(n); + return MatrixDD::Identity(d, d); + } + + /// @} + /// @name Lie Group + /// @{ + + /// Adjoint map + MatrixDD AdjointMap() const; + + /** + * Exponential map at identity - create a rotation from canonical coordinates + */ + static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none); + + /// Derivative of Expmap, currently only defined for SO3 + static MatrixDD ExpmapDerivative(const TangentVector& omega); + + /** + * Log map at identity - returns the canonical coordinates of this rotation + */ + static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none); + + /// Derivative of Logmap, currently only defined for SO3 + static MatrixDD LogmapDerivative(const TangentVector& omega); + + // inverse with optional derivative + using LieGroup, internal::DimensionSO(N)>::inverse; + + /// @} + /// @name Other methods + /// @{ + + /** + * Return vectorized rotation matrix in column order. + * Will use dynamic matrices as intermediate results, but returns a fixed size + * X and fixed-size Jacobian if dimension is known at compile time. + * */ + VectorN2 vec(OptionalJacobian H = + boost::none) const; + /// @} + + template + friend void serialize(Archive&, SO&, const unsigned int); + friend class boost::serialization::access; + friend class Rot3; // for serialize +}; + +using SOn = SO; + +/* + * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature. + * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version, + * and implementation for other fixed N is in SOn-inl.h. + */ + +template <> +Matrix SOn::Hat(const Vector& xi); + +template <> +Vector SOn::Vee(const Matrix& X); + +/* + * Specialize dynamic compose and between, because the derivative is unknowable + * by the LieGroup implementations, who return a fixed-size matrix for H2. + */ + +using DynamicJacobian = OptionalJacobian; + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const; + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const; + +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ + +template +struct traits> : public internal::LieGroup> {}; + +template +struct traits> : public internal::LieGroup> {}; + +} // namespace gtsam + +#include "SOn-inl.h" diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index e468eaf55..c95b85f21 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -645,6 +645,22 @@ TEST(Rot3 , ChartDerivatives) { } } +/* ************************************************************************* */ +TEST(Rot3, ClosestTo) { + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + Matrix expected(3, 3); + expected << 0.790687, 0.605096, -0.0931312, // + 0.415746, -0.642355, -0.643844, // + -0.449411, 0.47036, -0.759468; + + auto actual = Rot3::ClosestTo(3*M); + EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 56751fa06..b2c781b35 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -10,19 +10,31 @@ * -------------------------------------------------------------------------- */ /** - * @file testQuaternion.cpp + * @file testSO3.cpp * @brief Unit tests for SO3, as a GTSAM-adapted Lie Group * @author Frank Dellaert **/ #include -#include + #include +#include + #include using namespace std; using namespace gtsam; +//****************************************************************************** +TEST(SO3, Identity) { + const SO3 R; + EXPECT_LONGS_EQUAL(3, R.rows()); + EXPECT_LONGS_EQUAL(3, SO3::dimension); + EXPECT_LONGS_EQUAL(3, SO3::Dim()); + EXPECT_LONGS_EQUAL(3, R.dim()); + EXPECT_LONGS_EQUAL(3, traits::GetDimension(R)); +} + //****************************************************************************** TEST(SO3, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -31,14 +43,59 @@ TEST(SO3, Concept) { } //****************************************************************************** -TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } +TEST(SO3, Constructors) { + const Vector3 axis(0, 0, 1); + const double angle = 2.5; + SO3 Q(Eigen::AngleAxisd(angle, axis)); + SO3 R = SO3::AxisAngle(axis, angle); + EXPECT(assert_equal(Q, R)); +} + +/* ************************************************************************* */ +TEST(SO3, ClosestTo) { + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + Matrix expected(3, 3); + expected << 0.790687, 0.605096, -0.0931312, // + 0.415746, -0.642355, -0.643844, // + -0.449411, 0.47036, -0.759468; + + auto actual = SO3::ClosestTo(3 * M); + EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); +} //****************************************************************************** SO3 id; -Vector3 z_axis(0, 0, 1); +Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +/* ************************************************************************* */ +TEST(SO3, ChordalMean) { + std::vector rotations = {R1, R1.inverse()}; + EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations))); +} + +//****************************************************************************** +TEST(SO3, Hat) { + // Check that Hat specialization is equal to dynamic version + EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis))); + EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2))); + EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3))); +} + +//****************************************************************************** +TEST(SO3, Vee) { + // Check that Hat specialization is equal to dynamic version + auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); + EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1))); + EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2))); + EXPECT(assert_equal(SO3::Vee(X3), SOn::Vee(X3))); +} + //****************************************************************************** TEST(SO3, Local) { Vector3 expected(0, 0, 0.1); @@ -50,7 +107,21 @@ TEST(SO3, Local) { TEST(SO3, Retract) { Vector3 v(0, 0, 0.1); SO3 actual = traits::Retract(R1, v); - EXPECT(actual.isApprox(R2)); + EXPECT(assert_equal(R2, actual)); +} + +//****************************************************************************** +TEST(SO3, Logmap) { + Vector3 expected(0, 0, 0.1); + Vector3 actual = SO3::Logmap(R1.between(R2)); + EXPECT(assert_equal((Vector)expected, actual)); +} + +//****************************************************************************** +TEST(SO3, Expmap) { + Vector3 v(0, 0, 0.1); + SO3 actual = R1 * SO3::Expmap(v); + EXPECT(assert_equal(R2, actual)); } //****************************************************************************** @@ -83,7 +154,7 @@ TEST(SO3, ChartDerivatives) { } /* ************************************************************************* */ -TEST(SO3, Expmap) { +TEST(SO3, ExpmapFunctor) { Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; Matrix expected(3,3); @@ -134,7 +205,7 @@ TEST(SO3, ExpmapDerivative) { EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( @@ -145,7 +216,7 @@ TEST(SO3, ExpmapDerivative2) { SO3::ExpmapDerivative(-theta))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( @@ -156,7 +227,7 @@ TEST(SO3, ExpmapDerivative3) { SO3::ExpmapDerivative(-theta))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative4) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) @@ -184,7 +255,7 @@ TEST(SO3, ExpmapDerivative4) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative5) { auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; @@ -200,7 +271,7 @@ TEST(SO3, ExpmapDerivative5) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( @@ -210,7 +281,7 @@ TEST(SO3, ExpmapDerivative6) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation @@ -220,7 +291,7 @@ TEST(SO3, LogmapDerivative) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation @@ -231,7 +302,7 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ApplyDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { @@ -254,7 +325,7 @@ TEST(SO3, ApplyDexp) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { @@ -278,6 +349,33 @@ TEST(SO3, ApplyInvDexp) { } } +//****************************************************************************** +TEST(SO3, vec) { + const Vector9 expected = Eigen::Map(R2.matrix().data()); + Matrix actualH; + const Vector9 actual = R2.vec(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO3& Q) { return Q.vec(); }; + const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} + +//****************************************************************************** +TEST(Matrix, compose) { + Matrix3 M; + M << 1, 2, 3, 4, 5, 6, 7, 8, 9; + SO3 R = SO3::Expmap(Vector3(1, 2, 3)); + const Matrix3 expected = M * R.matrix(); + Matrix actualH; + const Matrix3 actual = so3::compose(M, R, actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [R](const Matrix3& M) { + return so3::compose(M, R); + }; + Matrix numericalH = numericalDerivative11(f, M, 1e-2); + CHECK(assert_equal(numericalH, actualH)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp new file mode 100644 index 000000000..594da01f6 --- /dev/null +++ b/gtsam/geometry/tests/testSO4.cpp @@ -0,0 +1,206 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSO4.cpp + * @brief Unit tests for SO4, as a GTSAM-adapted Lie Group + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST(SO4, Identity) { + const SO4 R; + EXPECT_LONGS_EQUAL(4, R.rows()); + EXPECT_LONGS_EQUAL(6, SO4::dimension); + EXPECT_LONGS_EQUAL(6, SO4::Dim()); + EXPECT_LONGS_EQUAL(6, R.dim()); + EXPECT_LONGS_EQUAL(6, traits::GetDimension(R)); +} + +//****************************************************************************** +TEST(SO4, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +SO4 id; +Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished(); +SO4 Q2 = SO4::Expmap(v2); +Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); +SO4 Q3 = SO4::Expmap(v3); + +//****************************************************************************** +TEST(SO4, Random) { + boost::mt19937 rng(42); + auto Q = SO4::Random(rng); + EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); +} +//****************************************************************************** +TEST(SO4, Expmap) { + // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. + auto R1 = SO3::Expmap(v1.tail<3>()).matrix(); + EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1))); + + // Same here + auto R2 = SO3::Expmap(v2.tail<3>()).matrix(); + EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2))); + + // Check commutative subgroups + for (size_t i = 0; i < 6; i++) { + Vector6 xi = Vector6::Zero(); + xi[i] = 2; + SO4 Q1 = SO4::Expmap(xi); + xi[i] = 3; + SO4 Q2 = SO4::Expmap(xi); + EXPECT(assert_equal(Q1 * Q2, Q2 * Q1)); + } +} + +//****************************************************************************** +TEST(SO4, Hat) { + // Check that Hat specialization is equal to dynamic version + EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1))); + EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2))); + EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3))); +} + +//****************************************************************************** +TEST(SO4, Vee) { + // Check that Hat specialization is equal to dynamic version + auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); + EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1))); + EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2))); + EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3))); +} + +//****************************************************************************** +TEST(SO4, Retract) { + // Check that Cayley yields the same as Expmap for small values + EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); + EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); + + // Check that Cayley is identical to dynamic version + EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100))); + EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100))); + + auto v = Vector6::Zero(); + SO4 actual = traits::Retract(id, v); + EXPECT(assert_equal(id, actual)); +} + +//****************************************************************************** +TEST(SO4, Local) { + // Check that Cayley is identical to dynamic version + EXPECT( + assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1)))); + EXPECT( + assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2)))); + + auto v0 = Vector6::Zero(); + Vector6 actual = traits::Local(id, id); + EXPECT(assert_equal((Vector)v0, actual)); +} + +//****************************************************************************** +TEST(SO4, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, Q1)); + EXPECT(check_group_invariants(Q2, id)); + EXPECT(check_group_invariants(Q2, Q1)); + EXPECT(check_group_invariants(Q1, Q2)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, Q1)); + EXPECT(check_manifold_invariants(Q2, id)); + EXPECT(check_manifold_invariants(Q2, Q1)); + EXPECT(check_manifold_invariants(Q1, Q2)); +} + +//****************************************************************************** +TEST(SO4, compose) { + SO4 expected = Q1 * Q2; + Matrix actualH1, actualH2; + SO4 actual = Q1.compose(Q2, actualH1, actualH2); + EXPECT(assert_equal(expected, actual)); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, Q1, Q2, 1e-2); + EXPECT(assert_equal(numericalH1, actualH1)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, Q1, Q2, 1e-2); + EXPECT(assert_equal(numericalH2, actualH2)); +} + +//****************************************************************************** +TEST(SO4, vec) { + using Vector16 = SO4::VectorN2; + const Vector16 expected = Eigen::Map(Q2.matrix().data()); + Matrix actualH; + const Vector16 actual = Q2.vec(actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q) { + return Q.vec(); + }; + const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); + EXPECT(assert_equal(numericalH, actualH)); +} + +//****************************************************************************** +TEST(SO4, topLeft) { + const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>(); + Matrix actualH; + const Matrix3 actual = topLeft(Q3, actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return topLeft(Q3); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + EXPECT(assert_equal(numericalH, actualH)); +} + +//****************************************************************************** +TEST(SO4, stiefel) { + const Matrix43 expected = Q3.matrix().leftCols<3>(); + Matrix actualH; + const Matrix43 actual = stiefel(Q3, actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return stiefel(Q3); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + EXPECT(assert_equal(numericalH, actualH)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp new file mode 100644 index 000000000..4e5f12c0c --- /dev/null +++ b/gtsam/geometry/tests/testSOn.cpp @@ -0,0 +1,170 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSOn.cpp + * @brief Unit tests for dynamic SO(n) classes. + * @author Frank Dellaert + **/ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +// Test dhynamic with n=0 +TEST(SOn, SO0) { + const auto R = SOn(0); + EXPECT_LONGS_EQUAL(0, R.rows()); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); + EXPECT_LONGS_EQUAL(0, R.dim()); + EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); +} + +//****************************************************************************** +TEST(SOn, SO5) { + const auto R = SOn(5); + EXPECT_LONGS_EQUAL(5, R.rows()); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); + EXPECT_LONGS_EQUAL(10, R.dim()); + EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); +} + +//****************************************************************************** +TEST(SOn, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(SOn, CopyConstructor) { + const auto R = SOn(5); + const auto B(R); + EXPECT_LONGS_EQUAL(5, B.rows()); + EXPECT_LONGS_EQUAL(10, B.dim()); +} + +//****************************************************************************** +TEST(SOn, Values) { + const auto R = SOn(5); + Values values; + const Key key(0); + values.insert(key, R); + const auto B = values.at(key); + EXPECT_LONGS_EQUAL(5, B.rows()); + EXPECT_LONGS_EQUAL(10, B.dim()); +} + +//****************************************************************************** +TEST(SOn, Random) { + static boost::mt19937 rng(42); + EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); + EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows()); + EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); +} + +//****************************************************************************** +TEST(SOn, HatVee) { + Vector6 v; + v << 1, 2, 3, 4, 5, 6; + + Matrix expected2(2, 2); + expected2 << 0, -1, 1, 0; + const auto actual2 = SOn::Hat(v.head<1>()); + CHECK(assert_equal(expected2, actual2)); + CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); + + Matrix expected3(3, 3); + expected3 << 0, -3, 2, // + 3, 0, -1, // + -2, 1, 0; + const auto actual3 = SOn::Hat(v.head<3>()); + CHECK(assert_equal(expected3, actual3)); + CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); + CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); + + Matrix expected4(4, 4); + expected4 << 0, -6, 5, -3, // + 6, 0, -4, 2, // + -5, 4, 0, -1, // + 3, -2, 1, 0; + const auto actual4 = SOn::Hat(v); + CHECK(assert_equal(expected4, actual4)); + CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); +} + +//****************************************************************************** +TEST(SOn, RetractLocal) { + Vector6 v1 = (Vector(6) << 0, 0, 0, 1, 0, 0).finished() / 10000; + Vector6 v2 = (Vector(6) << 0, 0, 0, 1, 2, 3).finished() / 10000; + Vector6 v3 = (Vector(6) << 3, 2, 1, 1, 2, 3).finished() / 10000; + + // Check that Cayley yields the same as Expmap for small values + SOn id(4); + EXPECT(assert_equal(id.retract(v1), SOn(SO4::Expmap(v1)))); + EXPECT(assert_equal(id.retract(v2), SOn(SO4::Expmap(v2)))); + EXPECT(assert_equal(id.retract(v3), SOn(SO4::Expmap(v3)))); + + // Same for SO3: + SOn I3(3); + EXPECT( + assert_equal(I3.retract(v1.tail<3>()), SOn(SO3::Expmap(v1.tail<3>())))); + EXPECT( + assert_equal(I3.retract(v2.tail<3>()), SOn(SO3::Expmap(v2.tail<3>())))); + + // If we do expmap in SO(3) subgroup, topleft should be equal to R1. + Matrix R1 = SO3().retract(v1.tail<3>()).matrix(); + SOn Q1 = SOn::Retract(v1); + CHECK(assert_equal(R1, Q1.matrix().block(0, 0, 3, 3), 1e-7)); + CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7)); +} + +//****************************************************************************** +TEST(SOn, vec) { + Vector10 v; + v << 0, 0, 0, 0, 1, 2, 3, 4, 5, 6; + SOn Q = SOn::ChartAtOrigin::Retract(v); + Matrix actualH; + const Vector actual = Q.vec(actualH); + boost::function h = [](const SOn& Q) { return Q.vec(); }; + const Matrix H = numericalDerivative11(h, Q, 1e-5); + CHECK(assert_equal(H, actualH)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 317ba1c44..6266e961c 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -86,6 +86,8 @@ namespace gtsam { FastVector children; int problemSize_; + bool is_root = false; + /// Fill the elimination result produced during elimination. Here this just stores the /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique /// to also cache the remaining factor. @@ -165,8 +167,14 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + if(!parent_.lock()) { + is_root = true; + } + ar & BOOST_SERIALIZATION_NVP(is_root); ar & BOOST_SERIALIZATION_NVP(conditional_); - ar & BOOST_SERIALIZATION_NVP(parent_); + if (!is_root) { // TODO(fan): Workaround for boost/serialization #119 + ar & BOOST_SERIALIZATION_NVP(parent_); + } ar & BOOST_SERIALIZATION_NVP(children); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index c8d77c554..53ce4b9ca 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -25,6 +25,8 @@ #include #include +#include +#include namespace gtsam { @@ -406,13 +408,41 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(Ab_); - ar & BOOST_SERIALIZATION_NVP(model_); + void save(ARCHIVE & ar, const unsigned int version) const { + // TODO(fan): This is a hack for Boost < 1.66 + // We really need to introduce proper versioning in the archives + // As otherwise this will not read objects serialized by older + // versions of GTSAM + ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar << BOOST_SERIALIZATION_NVP(Ab_); + bool model_null = false; + if(model_.get() == nullptr) { + model_null = true; + ar << boost::serialization::make_nvp("model_null", model_null); + } else { + ar << boost::serialization::make_nvp("model_null", model_null); + ar << BOOST_SERIALIZATION_NVP(model_); + } } - }; // JacobianFactor + template + void load(ARCHIVE & ar, const unsigned int version) { + // invoke serialization of the base class + ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar >> BOOST_SERIALIZATION_NVP(Ab_); + if (version < 1) { + ar >> BOOST_SERIALIZATION_NVP(model_); + } else { + bool model_null; + ar >> BOOST_SERIALIZATION_NVP(model_null); + if (!model_null) { + ar >> BOOST_SERIALIZATION_NVP(model_); + } + } + } + + BOOST_SERIALIZATION_SPLIT_MEMBER() + }; // JacobianFactor /// traits template<> struct traits : public Testable { @@ -420,6 +450,8 @@ struct traits : public Testable { } // \ namespace gtsam +BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1) + #include diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index ca3ebf91c..17dc6a425 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -25,7 +25,6 @@ using namespace boost::assign; #include #include -#include #include #include #include diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 4229d4f0c..55efdb151 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -68,30 +68,30 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, Matrix3 w_tangent_H_theta, invH; const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const SO3 R = local.expmap(); + const Rot3 R(local.expmap()); const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent * dt, // theta - position + velocity * dt + a_nav * dt22, // position - velocity + a_nav * dt; // velocity + preintegratedPlus << // new preintegrated vector: + theta + w_tangent * dt, // theta + position + velocity * dt + a_nav * dt22, // position + velocity + a_nav * dt; // velocity if (A) { // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp(); A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta } if (B) { B->block<3, 3>(0, 0) = Z_3x3; - B->block<3, 3>(3, 0) = R * dt22; - B->block<3, 3>(6, 0) = R * dt; + B->block<3, 3>(3, 0) = R.matrix() * dt22; + B->block<3, 3>(6, 0) = R.matrix() * dt; } if (C) { C->block<3, 3>(0, 0) = invH * dt; diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index a1baab5fa..1e398cd99 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -75,27 +75,15 @@ Values InitializePose3::normalizeRelaxedRotations( const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); - Matrix ppm = Z_3x3; // plus plus minus - ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; - Values validRot3; for(const auto& it: relaxedRot3) { Key key = it.first; if (key != kAnchorKey) { - Matrix3 R; - R << Eigen::Map(it.second.data()); // Recover M from vectorized + Matrix3 M; + M << Eigen::Map(it.second.data()); // Recover M from vectorized // ClosestTo finds rotation matrix closest to H in Frobenius sense - // Rot3 initRot = Rot3::ClosestTo(M.transpose()); - - Matrix U, V; Vector s; - svd(R.transpose(), U, s, V); - Matrix3 normalizedRotMat = U * V.transpose(); - - if(normalizedRotMat.determinant() < 0) - normalizedRotMat = U * ppm * V.transpose(); - - Rot3 initRot = Rot3(normalizedRotMat); + Rot3 initRot = Rot3::ClosestTo(M.transpose()); validRot3.insert(key, initRot); } } diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h new file mode 100644 index 000000000..076b0ff0c --- /dev/null +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file KarcherMeanFactor.cpp + * @author Frank Dellaert + * @date March 2019 + */ + +#pragma once + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +template +T FindKarcherMeanImpl(const vector& rotations) { + // Cost function C(R) = \sum PriorFactor(R_i)::error(R) + // No closed form solution. + NonlinearFactorGraph graph; + static const Key kKey(0); + for (const auto& R : rotations) { + graph.emplace_shared >(kKey, R); + } + Values initial; + initial.insert(kKey, T()); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + return result.at(kKey); +} + +template ::value >::type > +T FindKarcherMean(const std::vector& rotations) { + return FindKarcherMeanImpl(rotations); +} + +template +T FindKarcherMean(const std::vector>& rotations) { + return FindKarcherMeanImpl(rotations); +} + +template +T FindKarcherMean(std::initializer_list&& rotations) { + return FindKarcherMeanImpl(std::vector >(rotations)); +} + +template +template +KarcherMeanFactor::KarcherMeanFactor(const CONTAINER& keys, int d) + : NonlinearFactor(keys) { + if (d <= 0) { + throw std::invalid_argument( + "KarcherMeanFactor needs dimension for dynamic types."); + } + // Create the constant Jacobian made of D*D identity matrices, + // where D is the dimensionality of the manifold. + const auto I = Eigen::MatrixXd::Identity(d, d); + std::map terms; + for (Key j : keys) { + terms[j] = I; + } + jacobian_ = + boost::make_shared(terms, Eigen::VectorXd::Zero(d)); +} +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h new file mode 100644 index 000000000..54b3930d4 --- /dev/null +++ b/gtsam/slam/KarcherMeanFactor.h @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file KarcherMeanFactor.h + * @author Frank Dellaert + * @date March 2019 + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { +/** + * Optimize for the Karcher mean, minimizing the geodesic distance to each of + * the given rotations, by constructing a factor graph out of simple + * PriorFactors. + */ +template +T FindKarcherMean(const std::vector>& rotations); + +template +T FindKarcherMean(std::initializer_list&& rotations); + +/** + * The KarcherMeanFactor creates a constraint on all SO(n) variables with + * given keys that the Karcher mean (see above) will stay the same. Note the + * mean itself is irrelevant to the constraint and is not a parameter: the + * constraint is implemented as enforcing that the sum of local updates is + * equal to zero, hence creating a rank-dim constraint. Note it is implemented as + * a soft constraint, as typically it is used to fix a gauge freedom. + * */ +template +class KarcherMeanFactor : public NonlinearFactor { + /// Constant Jacobian made of d*d identity matrices + boost::shared_ptr jacobian_; + + enum {D = traits::dimension}; + + public: + /// Construct from given keys. + template + KarcherMeanFactor(const CONTAINER& keys, int d=D); + + /// Destructor + virtual ~KarcherMeanFactor() {} + + /// Calculate the error of the factor: always zero + double error(const Values& c) const override { return 0; } + + /// get the dimension of the factor (number of rows on linearization) + size_t dim() const override { return D; } + + /// linearize to a GaussianFactor + boost::shared_ptr linearize(const Values& c) const override { + return jacobian_; + } +}; +// \KarcherMeanFactor +} // namespace gtsam diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp new file mode 100644 index 000000000..c129b8fa3 --- /dev/null +++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testKarcherMeanFactor.cpp + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Rot3 version +/* ************************************************************************* */ +static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0)); + +/* ************************************************************************* */ +// Check that optimizing for Karcher mean (which minimizes Between distance) +// gets correct result. +TEST(KarcherMean, FindRot3) { + std::vector rotations = {R, R.inverse()}; + Rot3 expected; + auto actual = FindKarcherMean(rotations); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +// Check that the InnerConstraint factor leaves the mean unchanged. +TEST(KarcherMean, FactorRot3) { + // Make a graph with two variables, one between, and one InnerConstraint + // The optimal result should satisfy the between, while moving the other + // variable to make the mean the same as before. + // Mean of R and R' is identity. Let's make a BetweenFactor making R21 = + // R*R*R, i.e. geodesic length is 3 rather than 2. + NonlinearFactorGraph graph; + graph.emplace_shared>(1, 2, R * R * R); + std::vector keys{1, 2}; + graph.emplace_shared>(keys); + + Values initial; + initial.insert(1, R.inverse()); + initial.insert(2, R); + const auto expected = FindKarcherMean({R, R.inverse()}); + + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + const auto actual = + FindKarcherMean({result.at(1), result.at(2)}); + EXPECT(assert_equal(expected, actual)); + EXPECT( + assert_equal(R * R * R, result.at(1).between(result.at(2)))); +} + +/* ************************************************************************* */ +// SO(4) version +/* ************************************************************************* */ +static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished()); + +/* ************************************************************************* */ +TEST(KarcherMean, FindSO4) { + std::vector> rotations = {Q, Q.inverse()}; + auto expected = SO4(); //::ChordalMean(rotations); + auto actual = FindKarcherMean(rotations); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(KarcherMean, FactorSO4) { + NonlinearFactorGraph graph; + graph.emplace_shared>(1, 2, Q * Q * Q); + std::vector keys{1, 2}; + graph.emplace_shared>(keys); + + Values initial; + initial.insert(1, Q.inverse()); + initial.insert(2, Q); + + std::vector > rotations = {Q, Q.inverse()}; + const auto expected = FindKarcherMean(rotations); + + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + const auto actual = + FindKarcherMean({result.at(1), result.at(2)}); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(), + result.at(1).between(result.at(2)).matrix())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index 00c02c250..3a6a156d5 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -23,33 +23,33 @@ using namespace std; using namespace gtsam; -#define TEST(TITLE,STATEMENT) \ - cout << endl << TITLE << endl;\ - timeLog = clock();\ - for(int i = 0; i < n; i++)\ - STATEMENT;\ - timeLog2 = clock();\ - seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;\ - cout << seconds << " seconds" << endl;\ - cout << ((double)n/seconds) << " calls/second" << endl; +#define TEST(TITLE, STATEMENT) \ + cout << endl << TITLE << endl; \ + timeLog = clock(); \ + for (int i = 0; i < n; i++) STATEMENT; \ + timeLog2 = clock(); \ + seconds = static_cast(timeLog2 - timeLog) / CLOCKS_PER_SEC; \ + cout << 1000 * seconds << " milliseconds" << endl; \ + cout << (1e9 * seconds / static_cast(n)) << " nanosecs/call" << endl; -int main() -{ - int n = 100000; long timeLog, timeLog2; double seconds; +int main() { + int n = 100000; + clock_t timeLog, timeLog2; + double seconds; // create a random direction: - double norm=sqrt(1.0+16.0+4.0); - double x=1.0/norm, y=4.0/norm, z=2.0/norm; + double norm = sqrt(1.0 + 16.0 + 4.0); + double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm; Vector v = (Vector(3) << x, y, z).finished(); Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v); - TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001)) + TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v, 0.001)) TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v)) - TEST("Expmap", R*Rot3::Expmap(v)) + TEST("Expmap", R * Rot3::Expmap(v)) TEST("Retract", R.retract(v)) TEST("Logmap", Rot3::Logmap(R.between(R2))) TEST("localCoordinates", R.localCoordinates(R2)) - TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x)) - TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z)) + TEST("Slow rotation matrix", Rot3::Rz(z) * Rot3::Ry(y) * Rot3::Rx(x)) + TEST("Fast Rotation matrix", Rot3::RzRyRx(x, y, z)) return 0; }