From 8d97415239304e62893256abb39fac83e8f45b91 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 23 May 2015 22:00:21 -0700 Subject: [PATCH 01/32] Modernized group traits --- gtsam/base/Group.h | 42 +++++++++++++++++++++++++++-------------- gtsam/geometry/Cyclic.h | 24 ++++++----------------- 2 files changed, 34 insertions(+), 32 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index aec4b5f1c..f97be5376 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -13,8 +13,9 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * @date Nov 5, 2011 + * @date November, 2011 * @author Alex Cunningham + * @author Frank Dellaert */ #pragma once @@ -83,21 +84,34 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); } -/// Macro to add group traits, additive flavor -#define GTSAM_ADDITIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \ - static GROUP Inverse(const GROUP &g) { return -g;} +namespace internal { -/// Macro to add group traits, multiplicative flavor -#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ - static GROUP Inverse(const GROUP &g) { return g.inverse();} +/// A helper class that implements the traits interface for groups. +template +struct GroupTraits { + typedef group_tag structure_category; + static Class Identity() { return Class::Identity(); } +}; -} // \ namespace gtsam +/// A helper class that implements the traits interface for additive groups. +template +struct AdditiveGroupTraits : GroupTraits { + typedef additive_group_tag group_flavor; \ + static Class Compose(const Class &g, const Class & h) { return g + h;} \ + static Class Between(const Class &g, const Class & h) { return h - g;} \ + static Class Inverse(const Class &g) { return -g;} +}; + +/// A helper class that implements the traits interface for multiplicative groups. +template +struct MultiplicativeGroupTraits : GroupTraits { + typedef additive_group_tag group_flavor; \ + static Class Compose(const Class &g, const Class & h) { return g * h;} \ + static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \ + static Class Inverse(const Class &g) { return g.inverse();} +}; +} // namespace internal +} // namespace gtsam /** * Macros for using the IsGroup diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 2c883707f..8aa205aa3 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -16,10 +16,8 @@ **/ #include -#include -#include -#include -#include +#include +#include // for cout :-( namespace gtsam { @@ -33,7 +31,7 @@ public: i_(i) { assert(i < N); } - /// Idenity element + /// Identity element static Cyclic Identity() { return Cyclic(0); } @@ -63,20 +61,10 @@ public: } }; -/// Define cyclic group traits to be a model of the Group concept +/// Define cyclic group traits to be a model of the Additive Group concept template -struct traits > { - typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) - static Cyclic Identity() { - return Cyclic::Identity(); - } - static bool Equals(const Cyclic& a, const Cyclic& b, - double tol = 1e-9) { - return a.equals(b, tol); - } - static void Print(const Cyclic& c, const std::string &s = "") { - c.print(s); - } +struct traits > : internal::AdditiveGroupTraits >, // + Testable > { }; } // \namespace gtsam From 00621521206e507df05d2470cb55fd54595ea610 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 23 May 2015 22:00:39 -0700 Subject: [PATCH 02/32] prototyping direct sum --- gtsam/geometry/tests/testCyclic.cpp | 90 ++++++++++++++++++++++++++++- 1 file changed, 89 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index a15d7e2c2..eaacea5c3 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -58,18 +58,106 @@ TEST(Cyclic, Between) { } //****************************************************************************** -TEST(Cyclic, Ivnverse) { +TEST(Cyclic, Inverse) { EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); EXPECT_LONGS_EQUAL(1, traits::Inverse(G(2))); } +//****************************************************************************** +TEST(Cyclic, Negation) { + EXPECT_LONGS_EQUAL(0, -G(0)); + EXPECT_LONGS_EQUAL(2, -G(1)); + EXPECT_LONGS_EQUAL(1, -G(2)); +} + +//****************************************************************************** +TEST(Cyclic, Negation2) { + typedef Cyclic<2> Z2; + EXPECT_LONGS_EQUAL(0, -Z2(0)); + EXPECT_LONGS_EQUAL(1, -Z2(1)); +} + //****************************************************************************** TEST(Cyclic , Invariants) { G g(2), h(1); check_group_invariants(g,h); } +//****************************************************************************** +namespace gtsam { + +template +class DirectSum: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsTestable)); + + const G& g() const { return this->first; } + const H& h() const { return this->second;} + +public: + // Construct from two subgroup elements + DirectSum(const G& g, const H& h):std::pair(g,h) {} + + /// Default constructor yields identity + DirectSum():std::pair(G::Identity(),H::Identity()) { + } + + /// Identity element + static DirectSum Identity() { + return DirectSum(); + } + /// Addition + DirectSum operator+(const DirectSum& other) const { + return DirectSum(g()+other.g(), h()+other.h()); + } + /// Subtraction + DirectSum operator-(const DirectSum& other) const { + return DirectSum(g()-other.g(), h()-other.h()); + } + /// Inverse + DirectSum operator-() const { + return DirectSum(- g(), - h()); + } + /// print with optional string + void print(const std::string& s = "") const { + std::cout << s << "(\n"; + traits::Print(g()); + std::cout << ",\n"; + traits::Print(h()); + std::cout << ")" << std::endl; + } + /// equals with an tolerance, prints out message if unequal + bool equals(const DirectSum& other, double tol = 1e-9) const { + return *this == other; // uses std::pair operator + } +}; + +/// Define direct sums to be a model of the Additive Group concept +template +struct traits > : internal::AdditiveGroupTraits >, // + Testable > { +}; + +} // namespace gtsam + +TEST(Cyclic , DirectSum) { + // The Direct sum of Cyclic<2> and Cyclic<2> is *not* Cyclic<4>, but the + // smallest non-cyclic group called the Klein four-group: + typedef DirectSum, Cyclic<2> > K; + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsTestable)); + + K g(0, 1), h(1, 1); + EXPECT(assert_equal(K(0, 1), - g)); + EXPECT(assert_equal(K(), g + g)); + EXPECT(assert_equal(K(1, 0), g + h)); + EXPECT(assert_equal(K(1, 0), g - h)); + check_group_invariants(g, h); +} + //****************************************************************************** int main() { TestResult tr; From 58f3945605de00aac3daba22c6b272e01563b19a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 23 May 2015 22:25:44 -0700 Subject: [PATCH 03/32] Moved DirectSum template to Group.h header --- gtsam/base/Group.h | 36 +++++++++++ gtsam/geometry/tests/testCyclic.cpp | 93 +++++++++++------------------ 2 files changed, 70 insertions(+), 59 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index f97be5376..a9abf968d 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace gtsam { @@ -111,6 +112,41 @@ struct MultiplicativeGroupTraits : GroupTraits { static Class Inverse(const Class &g) { return g.inverse();} }; } // namespace internal + +/// Template to construct the direct sum of two additive groups +template +class DirectSum: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + + const G& g() const { return this->first; } + const H& h() const { return this->second;} + +public: + // Construct from two subgroup elements + DirectSum(const G& g, const H& h):std::pair(g,h) { + } + /// Default constructor yields identity + DirectSum():std::pair(G::Identity(),H::Identity()) { + } + static DirectSum Identity() { + return DirectSum(); + } + DirectSum operator+(const DirectSum& other) const { + return DirectSum(g()+other.g(), h()+other.h()); + } + DirectSum operator-(const DirectSum& other) const { + return DirectSum(g()-other.g(), h()-other.h()); + } + DirectSum operator-() const { + return DirectSum(- g(), - h()); + } +}; + +// Define direct sums to be a model of the Additive Group concept +template +struct traits > : internal::AdditiveGroupTraits > {}; + } // namespace gtsam /** diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index eaacea5c3..22b4dc209 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -85,60 +85,21 @@ TEST(Cyclic , Invariants) { } //****************************************************************************** +// The Direct sum of Cyclic<2> and Cyclic<2> is *not* Cyclic<4>, but the +// smallest non-cyclic group called the Klein four-group: +typedef DirectSum, Cyclic<2> > K4; + namespace gtsam { -template -class DirectSum: public std::pair { - BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive - BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive - BOOST_CONCEPT_ASSERT((IsTestable)); - BOOST_CONCEPT_ASSERT((IsTestable)); - - const G& g() const { return this->first; } - const H& h() const { return this->second;} - -public: - // Construct from two subgroup elements - DirectSum(const G& g, const H& h):std::pair(g,h) {} - - /// Default constructor yields identity - DirectSum():std::pair(G::Identity(),H::Identity()) { +/// Define K4 to be a model of the Additive Group concept, and provide Testable +template<> +struct traits : internal::AdditiveGroupTraits { + static void Print(const K4& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; } - - /// Identity element - static DirectSum Identity() { - return DirectSum(); + static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) { + return m1 == m2; } - /// Addition - DirectSum operator+(const DirectSum& other) const { - return DirectSum(g()+other.g(), h()+other.h()); - } - /// Subtraction - DirectSum operator-(const DirectSum& other) const { - return DirectSum(g()-other.g(), h()-other.h()); - } - /// Inverse - DirectSum operator-() const { - return DirectSum(- g(), - h()); - } - /// print with optional string - void print(const std::string& s = "") const { - std::cout << s << "(\n"; - traits::Print(g()); - std::cout << ",\n"; - traits::Print(h()); - std::cout << ")" << std::endl; - } - /// equals with an tolerance, prints out message if unequal - bool equals(const DirectSum& other, double tol = 1e-9) const { - return *this == other; // uses std::pair operator - } -}; - -/// Define direct sums to be a model of the Additive Group concept -template -struct traits > : internal::AdditiveGroupTraits >, // - Testable > { }; } // namespace gtsam @@ -146,16 +107,30 @@ struct traits > : internal::AdditiveGroupTraits TEST(Cyclic , DirectSum) { // The Direct sum of Cyclic<2> and Cyclic<2> is *not* Cyclic<4>, but the // smallest non-cyclic group called the Klein four-group: - typedef DirectSum, Cyclic<2> > K; - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsTestable)); + typedef DirectSum, Cyclic<2> > K4; + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsTestable)); - K g(0, 1), h(1, 1); - EXPECT(assert_equal(K(0, 1), - g)); - EXPECT(assert_equal(K(), g + g)); - EXPECT(assert_equal(K(1, 0), g + h)); - EXPECT(assert_equal(K(1, 0), g - h)); - check_group_invariants(g, h); + // Refer to http://en.wikipedia.org/wiki/Klein_four-group + K4 e(0,0), a(0, 1), b(1, 0), c(1, 1); + EXPECT(assert_equal(a, - a)); + EXPECT(assert_equal(b, - b)); + EXPECT(assert_equal(c, - c)); + EXPECT(assert_equal(a, a + e)); + EXPECT(assert_equal(b, b + e)); + EXPECT(assert_equal(c, c + e)); + EXPECT(assert_equal(e, a + a)); + EXPECT(assert_equal(e, b + b)); + EXPECT(assert_equal(e, c + c)); + EXPECT(assert_equal(c, a + b)); + EXPECT(assert_equal(b, a + c)); + EXPECT(assert_equal(a, b + c)); + EXPECT(assert_equal(c, a - b)); + EXPECT(assert_equal(a, b - c)); + EXPECT(assert_equal(b, c - a)); + check_group_invariants(a, b); + check_group_invariants(b, c); + check_group_invariants(c, a); } //****************************************************************************** From f335e70196392bb83d8a0bfcc8775beb47f0018b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 10:25:19 -0700 Subject: [PATCH 04/32] Fixed some issues and further testing --- gtsam/base/Group.h | 10 +++++++--- gtsam/geometry/Cyclic.h | 7 +++---- gtsam/geometry/tests/testCyclic.cpp | 8 ++++---- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index a9abf968d..f56ba9685 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -88,13 +88,15 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { namespace internal { /// A helper class that implements the traits interface for groups. +/// Assumes that constructor yields identity template struct GroupTraits { typedef group_tag structure_category; - static Class Identity() { return Class::Identity(); } + static Class Identity() { return Class(); } }; /// A helper class that implements the traits interface for additive groups. +/// Assumes existence of three additive operators template struct AdditiveGroupTraits : GroupTraits { typedef additive_group_tag group_flavor; \ @@ -104,9 +106,10 @@ struct AdditiveGroupTraits : GroupTraits { }; /// A helper class that implements the traits interface for multiplicative groups. +/// Assumes existence of operators * and /, as well as inverse method template struct MultiplicativeGroupTraits : GroupTraits { - typedef additive_group_tag group_flavor; \ + typedef multiplicative_group_tag group_flavor; \ static Class Compose(const Class &g, const Class & h) { return g * h;} \ static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \ static Class Inverse(const Class &g) { return g.inverse();} @@ -114,6 +117,7 @@ struct MultiplicativeGroupTraits : GroupTraits { } // namespace internal /// Template to construct the direct sum of two additive groups +/// Assumes existence of three additive operators for both groups template class DirectSum: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive @@ -127,7 +131,7 @@ public: DirectSum(const G& g, const H& h):std::pair(g,h) { } /// Default constructor yields identity - DirectSum():std::pair(G::Identity(),H::Identity()) { + DirectSum():std::pair(traits::Identity(),traits::Identity()) { } static DirectSum Identity() { return DirectSum(); diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 8aa205aa3..88a04ab2d 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -31,9 +31,8 @@ public: i_(i) { assert(i < N); } - /// Identity element - static Cyclic Identity() { - return Cyclic(0); + /// Default constructor yields identity + Cyclic():i_(0) { } /// Cast to size_t operator size_t() const { @@ -61,7 +60,7 @@ public: } }; -/// Define cyclic group traits to be a model of the Additive Group concept +/// Define cyclic group to be a model of the Additive Group concept template struct traits > : internal::AdditiveGroupTraits >, // Testable > { diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 22b4dc209..20404a14f 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -81,7 +81,7 @@ TEST(Cyclic, Negation2) { //****************************************************************************** TEST(Cyclic , Invariants) { G g(2), h(1); - check_group_invariants(g,h); + EXPECT(check_group_invariants(g,h)); } //****************************************************************************** @@ -128,9 +128,9 @@ TEST(Cyclic , DirectSum) { EXPECT(assert_equal(c, a - b)); EXPECT(assert_equal(a, b - c)); EXPECT(assert_equal(b, c - a)); - check_group_invariants(a, b); - check_group_invariants(b, c); - check_group_invariants(c, a); + EXPECT(check_group_invariants(a, b)); + EXPECT(check_group_invariants(b, c)); + EXPECT(check_group_invariants(c, a)); } //****************************************************************************** From 379e0c6edebbb98ae2b6ecf8768e9665c47944ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 10:25:42 -0700 Subject: [PATCH 05/32] Test non-abelian groups with permutation group --- gtsam/base/tests/testGroup.cpp | 78 ++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 gtsam/base/tests/testGroup.cpp diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp new file mode 100644 index 000000000..2cee30070 --- /dev/null +++ b/gtsam/base/tests/testGroup.cpp @@ -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 testGroup.cpp + * @brief Unit tests for groups + * @author Frank Dellaert + **/ + +#include +#include +#include + +namespace gtsam { + +template +class Permutation: public Eigen::PermutationMatrix { +public: + Permutation() { + Eigen::PermutationMatrix::setIdentity(); + } + Permutation(const Eigen::PermutationMatrix& P) : + Eigen::PermutationMatrix(P) { + } + Permutation inverse() const { + return Eigen::PermutationMatrix(Eigen::PermutationMatrix::inverse()); + } +}; + +/// Define permutation group traits to be a model of the Multiplicative Group concept +template +struct traits > : internal::MultiplicativeGroupTraits< + Permutation > { + static void Print(const Permutation& m, const std::string& str = "") { + std::cout << m << std::endl; + } + static bool Equals(const Permutation& m1, const Permutation& m2, + double tol = 1e-8) { + return m1.indices() == m2.indices(); + } +}; + +} // namespace gtsam + +#include +#include + +using namespace std; +using namespace gtsam; + +typedef Permutation<3> G; // Let's use the permutation group of order 3 + +//****************************************************************************** +TEST(Group, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); +} + +//****************************************************************************** +TEST(Group , Invariants) { + G g, h; + EXPECT(check_group_invariants(g, h)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + From b8e980258c2f3209c434b06abe006edd40ecc11c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 10:26:07 -0700 Subject: [PATCH 06/32] Fixed invariant tests (which were basically no-ops) --- gtsam/base/tests/testLieScalar.cpp | 4 ++-- gtsam/geometry/tests/testPoint2.cpp | 8 ++++---- gtsam/geometry/tests/testPoint3.cpp | 4 ++-- gtsam/geometry/tests/testPose2.cpp | 16 ++++++++-------- gtsam/geometry/tests/testPose3.cpp | 16 ++++++++-------- gtsam/geometry/tests/testQuaternion.cpp | 16 ++++++++-------- gtsam/geometry/tests/testRot2.cpp | 16 ++++++++-------- gtsam/geometry/tests/testRot3.cpp | 20 ++++++++++---------- gtsam/geometry/tests/testSO3.cpp | 16 ++++++++-------- 9 files changed, 58 insertions(+), 58 deletions(-) diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 141b55761..07e666fbe 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -37,8 +37,8 @@ TEST(LieScalar , Concept) { //****************************************************************************** TEST(LieScalar , Invariants) { LieScalar lie1(2), lie2(3); - check_group_invariants(lie1, lie2); - check_manifold_invariants(lie1, lie2); + CHECK(check_group_invariants(lie1, lie2)); + CHECK(check_manifold_invariants(lie1, lie2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index fce7955e9..7ed27ab2e 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -37,8 +37,8 @@ TEST(Double , Concept) { //****************************************************************************** TEST(Double , Invariants) { double p1(2), p2(5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } //****************************************************************************** @@ -51,8 +51,8 @@ TEST(Point2 , Concept) { //****************************************************************************** TEST(Point2 , Invariants) { Point2 p1(1, 2), p2(4, 5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5c6b32dca..6811ed122 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -36,8 +36,8 @@ TEST(Point3 , Concept) { //****************************************************************************** TEST(Point3 , Invariants) { Point3 p1(1, 2, 3), p2(4, 5, 6); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 803bb7c3f..5b835200a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); TEST(Pose2 , Invariants) { Pose2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index d749ba6af..98c80e356 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -760,15 +760,15 @@ TEST( Pose3, stream) TEST(Pose3 , Invariants) { Pose3 id; - check_group_invariants(id,id); - check_group_invariants(id,T3); - check_group_invariants(T2,id); - check_group_invariants(T2,T3); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T3); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T3); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 82d3283bd..0421e1e44 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) { //****************************************************************************** TEST(Quaternion , Invariants) { - check_group_invariants(id, id); - check_group_invariants(id, R1); - check_group_invariants(R2, id); - check_group_invariants(R2, R1); + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); - check_manifold_invariants(id, id); - check_manifold_invariants(id, R1); - check_manifold_invariants(R2, id); - check_manifold_invariants(R2, R1); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, R1)); + EXPECT(check_manifold_invariants(R2, id)); + EXPECT(check_manifold_invariants(R2, R1)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 37054fd83..6ead22860 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -163,15 +163,15 @@ Rot2 T2(0.2); TEST(Rot2 , Invariants) { Rot2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 7e0dcc43f..349f87029 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -659,17 +659,17 @@ Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); TEST(Rot3 , Invariants) { Rot3 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); - check_group_invariants(T1,T2); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); + EXPECT(check_group_invariants(T1,T2)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); - check_manifold_invariants(T1,T2); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); + EXPECT(check_manifold_invariants(T1,T2)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index acbf3bcf5..bc32e0df0 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -57,15 +57,15 @@ TEST(SO3 , Retract) { //****************************************************************************** TEST(SO3 , Invariants) { - check_group_invariants(id,id); - check_group_invariants(id,R1); - check_group_invariants(R2,id); - check_group_invariants(R2,R1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,R1)); + EXPECT(check_group_invariants(R2,id)); + EXPECT(check_group_invariants(R2,R1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,R1); - check_manifold_invariants(R2,id); - check_manifold_invariants(R2,R1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,R1)); + EXPECT(check_manifold_invariants(R2,id)); + EXPECT(check_manifold_invariants(R2,R1)); } //****************************************************************************** From 6c6abe0b6c929389c5f8daeeab02d89e3aa674a9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 12:38:53 -0700 Subject: [PATCH 07/32] compose_pow and DirectProduct --- gtsam/base/Group.h | 65 +++++++++++++++++++++++++++++++++++++++------- 1 file changed, 56 insertions(+), 9 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index f56ba9685..16c1e6a23 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -95,6 +95,16 @@ struct GroupTraits { static Class Identity() { return Class(); } }; +/// A helper class that implements the traits interface for multiplicative groups. +/// Assumes existence of operator* and inverse method +template +struct MultiplicativeGroupTraits : GroupTraits { + typedef multiplicative_group_tag group_flavor; \ + static Class Compose(const Class &g, const Class & h) { return g * h;} \ + static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \ + static Class Inverse(const Class &g) { return g.inverse();} +}; + /// A helper class that implements the traits interface for additive groups. /// Assumes existence of three additive operators template @@ -105,17 +115,54 @@ struct AdditiveGroupTraits : GroupTraits { static Class Inverse(const Class &g) { return -g;} }; -/// A helper class that implements the traits interface for multiplicative groups. -/// Assumes existence of operators * and /, as well as inverse method -template -struct MultiplicativeGroupTraits : GroupTraits { - typedef multiplicative_group_tag group_flavor; \ - static Class Compose(const Class &g, const Class & h) { return g * h;} \ - static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \ - static Class Inverse(const Class &g) { return g.inverse();} -}; } // namespace internal +/// compose multiple times +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(G)) // +compose_pow(const G& g, size_t n) { + if (n == 0) + return traits::Identity(); + else if (n == 1) + return g; + else + return traits::Compose(compose_pow(g, n - 1), g); +} + +/// Template to construct the direct product of two arbitrary groups +/// Assumes nothing except group structure from G and H +template +class DirectProduct: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); + + const G& g() const {return this->first;} + const H& h() const {return this->second;} + +public: + // Construct from two subgroup elements + DirectProduct(const G& g, const H& h):std::pair(g,h) { + } + /// Default constructor yields identity + DirectProduct():std::pair(traits::Identity(),traits::Identity()) { + } + static DirectProduct Identity() { + return DirectProduct(); + } + DirectProduct operator*(const DirectProduct& other) const { + return DirectProduct(traits::Compose(g(),other.g()), traits::Compose(h(),other.h())); + } + DirectProduct inverse() const { + return DirectProduct(g().inverse(), h().inverse()); + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::MultiplicativeGroupTraits< + DirectProduct > { +}; + /// Template to construct the direct sum of two additive groups /// Assumes existence of three additive operators for both groups template From 9fc9e70dd6b829e02b103011f46acae8a6ff748d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 12:39:41 -0700 Subject: [PATCH 08/32] Test DirectProduct by constructing dihedral group Dih6 --- gtsam/base/tests/testGroup.cpp | 110 ++++++++++++++++++++++++++------- 1 file changed, 87 insertions(+), 23 deletions(-) diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index 2cee30070..dadf2896c 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -16,36 +16,56 @@ **/ #include +#include #include #include +#include namespace gtsam { +/// Symmetric group template -class Permutation: public Eigen::PermutationMatrix { -public: - Permutation() { - Eigen::PermutationMatrix::setIdentity(); - } - Permutation(const Eigen::PermutationMatrix& P) : +class Symmetric: private Eigen::PermutationMatrix { + Symmetric(const Eigen::PermutationMatrix& P) : Eigen::PermutationMatrix(P) { } - Permutation inverse() const { +public: + Symmetric() { + Eigen::PermutationMatrix::setIdentity(); + } + static Symmetric Transposition(int i, int j) { + Symmetric g; + return g.applyTranspositionOnTheRight(i, j); + } + Symmetric operator*(const Symmetric& other) const { + return Eigen::PermutationMatrix::operator*(other); + } + bool operator==(const Symmetric& other) const { + for (size_t i = 0; i < N; i++) + if (this->indices()[i] != other.indices()[i]) + return false; + return true; + } + Symmetric inverse() const { return Eigen::PermutationMatrix(Eigen::PermutationMatrix::inverse()); } + friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) { + for (size_t i = 0; i < N; i++) + os << m.indices()[i] << " "; + return os; + } + void print(const std::string& s = "") const { + std::cout << s << *this << std::endl; + } + bool equals(const Symmetric& other, double tol = 0) const { + return this->indices() == other.indices(); + } }; /// Define permutation group traits to be a model of the Multiplicative Group concept template -struct traits > : internal::MultiplicativeGroupTraits< - Permutation > { - static void Print(const Permutation& m, const std::string& str = "") { - std::cout << m << std::endl; - } - static bool Equals(const Permutation& m1, const Permutation& m2, - double tol = 1e-8) { - return m1.indices() == m2.indices(); - } +struct traits > : internal::MultiplicativeGroupTraits >, + Testable > { }; } // namespace gtsam @@ -56,17 +76,61 @@ struct traits > : internal::MultiplicativeGroupTraits< using namespace std; using namespace gtsam; -typedef Permutation<3> G; // Let's use the permutation group of order 3 - //****************************************************************************** -TEST(Group, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); +typedef Symmetric<2> S2; +TEST(Group, S2) { + S2 e, s1 = S2::Transposition(0, 1); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); } //****************************************************************************** -TEST(Group , Invariants) { - G g, h; - EXPECT(check_group_invariants(g, h)); +typedef Symmetric<3> S3; +TEST(Group, S3) { + S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); + EXPECT(assert_equal(s1, s1 * e)); + EXPECT(assert_equal(s1, e * s1)); + EXPECT(assert_equal(e, s1 * s1)); + S3 g = s1 * s2; // 1 2 0 + EXPECT(assert_equal(s1, g * s2)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup +} + +//****************************************************************************** +// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon, +// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon) +typedef DirectProduct Dih6; + +std::ostream &operator<<(std::ostream &os, const Dih6& m) { + os << "( " << m.first << ", " << m.second << ")"; + return os; +} + +// Provide traits with Testable +namespace gtsam { +template<> +struct traits : internal::MultiplicativeGroupTraits { + static void Print(const Dih6& m, const string& s = "") { + cout << s << m << endl; + } + static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} // namespace gtsam + +TEST(Group, Dih6) { + Dih6 e, g(S2::Transposition(0, 1), + S3::Transposition(0, 1) * S3::Transposition(1, 2)); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, g)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup } //****************************************************************************** From 2c235d98beede725ce279b96ee5ab5d1f9f5e16f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 15:32:20 -0700 Subject: [PATCH 09/32] Formatting and comments only --- gtsam/base/Group.h | 35 +++++++++++++---------------------- gtsam/base/Manifold.h | 2 +- 2 files changed, 14 insertions(+), 23 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 16c1e6a23..8a1d69848 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -121,12 +121,9 @@ struct AdditiveGroupTraits : GroupTraits { template BOOST_CONCEPT_REQUIRES(((IsGroup)),(G)) // compose_pow(const G& g, size_t n) { - if (n == 0) - return traits::Identity(); - else if (n == 1) - return g; - else - return traits::Compose(compose_pow(g, n - 1), g); + if (n == 0) return traits::Identity(); + else if (n == 1) return g; + else return traits::Compose(compose_pow(g, n - 1), g); } /// Template to construct the direct product of two arbitrary groups @@ -140,15 +137,12 @@ class DirectProduct: public std::pair { const H& h() const {return this->second;} public: - // Construct from two subgroup elements - DirectProduct(const G& g, const H& h):std::pair(g,h) { - } /// Default constructor yields identity - DirectProduct():std::pair(traits::Identity(),traits::Identity()) { - } - static DirectProduct Identity() { - return DirectProduct(); - } + DirectProduct():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectProduct(const G& g, const H& h):std::pair(g,h) {} + DirectProduct operator*(const DirectProduct& other) const { return DirectProduct(traits::Compose(g(),other.g()), traits::Compose(h(),other.h())); } @@ -174,15 +168,12 @@ class DirectSum: public std::pair { const H& h() const { return this->second;} public: - // Construct from two subgroup elements - DirectSum(const G& g, const H& h):std::pair(g,h) { - } /// Default constructor yields identity - DirectSum():std::pair(traits::Identity(),traits::Identity()) { - } - static DirectSum Identity() { - return DirectSum(); - } + DirectSum():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectSum(const G& g, const H& h):std::pair(g,h) {} + DirectSum operator+(const DirectSum& other) const { return DirectSum(g()+other.g(), h()+other.h()); } diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 6998b3b18..eafd7e3e2 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -90,7 +90,7 @@ struct ManifoldImpl { /// A helper that implements the traits interface for GTSAM manifolds. /// To use this for your class type, define: -/// template<> struct traits : public Manifold { }; +/// template<> struct traits : public internal::Manifold { }; template struct Manifold: Testable, ManifoldImpl { From 387dfe52298b012c7f4af49f11d8dc218bb21d8d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 15:33:15 -0700 Subject: [PATCH 10/32] Some more tests on retract/localCoordinates --- gtsam/geometry/tests/testEssentialMatrix.cpp | 23 +++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index d2990a747..fe27b2911 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3) { + EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); + Pose3 pose(c1Rc2, c1Tc2); + EssentialMatrix actual = EssentialMatrix::FromPose3(pose); + EXPECT(assert_equal(expected, actual)); +} + //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; @@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) { Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); EXPECT(assert_equal(zero(5), actual, 1e-8)); - Vector d = zero(6); - d(4) += 1e-5; + Vector6 d; + d << 0.1, 0.2, 0.3, 0, 0, 0; Vector actual2 = hx.localCoordinates( EssentialMatrix::FromPose3(pose.retract(d))); - EXPECT(assert_equal(zero(5), actual2, 1e-8)); + EXPECT(assert_equal(d.head(5), actual2, 1e-8)); } //************************************************************************* @@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, RoundTrip) { + Vector5 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5; + EssentialMatrix e, hx = e.retract(d); + Vector actual = e.localCoordinates(hx); + EXPECT(assert_equal(d, actual, 1e-8)); +} + //************************************************************************* Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); From 994196d37d580260b635e6a6ba837f2a873cda56 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 24 May 2015 15:34:48 -0700 Subject: [PATCH 11/32] Successful representation of E as product manifold --- gtsam/geometry/EssentialMatrix.cpp | 54 ++++++----------- gtsam/geometry/EssentialMatrix.h | 97 +++++++++++++++++++----------- 2 files changed, 79 insertions(+), 72 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 062178fea..7b90edc39 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -13,64 +13,46 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { - const Rot3& _1R2_ = _1P2_.rotation(); - const Point3& _1T2_ = _1P2_.translation(); + const Rot3& aRb = aPb.rotation(); + const Point3& aTb = aPb.translation(); if (!H) { // just make a direction out of translation and create E - Unit3 direction(_1T2_); - return EssentialMatrix(_1R2_, direction); + Unit3 direction(aTb); + return EssentialMatrix(aRb, direction); } else { // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; - Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); + Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2); *H << I_3x3, Z_3x3, // - Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); - return EssentialMatrix(_1R2_, direction); + Matrix23::Zero(), D_direction_1T2 * aRb.matrix(); + return EssentialMatrix(aRb, direction); } } /* ************************************************************************* */ void EssentialMatrix::print(const string& s) const { cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); -} - -/* ************************************************************************* */ -EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Unit3 t = aTb_.retract(z); - return EssentialMatrix(R, t); -} - -/* ************************************************************************* */ -Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - Vector5 v; - v << aRb_.localCoordinates(other.aRb_), - aTb_.localCoordinates(other.aTb_); - return v; + rotation().print("R:\n"); + direction().print("d: "); } /* ************************************************************************* */ Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, OptionalJacobian<3, 3> Dpoint) const { - Pose3 pose(aRb_, aTb_.point3()); + Pose3 pose(rotation(), direction().point3()); Matrix36 DE_; Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // Duy made an educated guess that this needs to be rotated to the local frame Matrix35 H; - H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); + H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis(); *DE = H; } return q; @@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); + Rot3 c1Rc2 = rotation().conjugate(cRb); if (!HE && !HR) { // Rotate translation direction and return - Unit3 c1Tc2 = cRb * aTb_; + Unit3 c1Tc2 = cRb * direction(); return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives Matrix23 D_c1Tc2_cRb; // 2*3 Matrix2 D_c1Tc2_aTb; // 2*2 - Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) *HE << cRb.matrix(), Matrix32::Zero(), // Matrix23::Zero(), D_c1Tc2_aTb; @@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); *H << HR, HD; } return dot(vA, E_ * vB); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index c5243c68b..6a94c8b11 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,6 +7,55 @@ #pragma once +#include + +namespace gtsam { + +/// CRTP to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes manifold structure from M1 and M2, and binary constructor +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +private: + const M1& g() const {return this->first;} + const M2& h() const {return this->second;} + +public: + // Dimension of the manifold + enum { dimension = M1::dimension + M2::dimension }; + + typedef Eigen::Matrix TangentVector; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductManifold(const M1& g, const M2& h):std::pair(g,h) {} + + /// Retract delta to manifold + Derived retract(const TangentVector& xi) const { + return Derived(traits::Retract(g(),xi.head(M1::dimension)), + traits::Retract(h(),xi.tail(M2::dimension))); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const Derived& other) const { + TangentVector xi; + xi << traits::Local(g(),other.g()), traits::Local(h(),other.h()); + return xi; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold< + ProductManifold > { +}; + +} // namespace gtsam + #include #include #include @@ -20,18 +69,15 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix { +class GTSAM_EXPORT EssentialMatrix : public ProductManifold { private: - Rot3 aRb_; ///< Rotation between a and b - Unit3 aTb_; ///< translation direction from a to b + typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix public: - enum { dimension = 5 }; - /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { return Vector3(p.x(), p.y(), 1); @@ -42,12 +88,12 @@ public: /// Default constructor EssentialMatrix() : - aTb_(1, 0, 0), E_(aTb_.skew()) { + Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) { } /// Construct from rotation and translation EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : - aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { + Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) @@ -72,43 +118,23 @@ public: /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { - return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + return rotation().equals(other.rotation(), tol) + && direction().equals(other.direction(), tol); } /// @} - /// @name Manifold - /// @{ - - /// Dimensionality of tangent space = 5 DOF - inline static size_t Dim() { - return 5; - } - - /// Return the dimensionality of the tangent space - size_t dim() const { - return 5; - } - - /// Retract delta to manifold - EssentialMatrix retract(const Vector& xi) const; - - /// Compute the coordinates in the tangent space - Vector5 localCoordinates(const EssentialMatrix& other) const; - - /// @} - /// @name Essential matrix methods /// @{ /// Rotation inline const Rot3& rotation() const { - return aRb_; + return this->first; } /// Direction inline const Unit3& direction() const { - return aTb_; + return this->second; } /// Return 3*3 matrix representation @@ -118,12 +144,12 @@ public: /// Return epipole in image_a , as Unit3 to allow for infinity inline const Unit3& epipole_a() const { - return aTb_; // == direction() + return direction(); } /// Return epipole in image_b, as Unit3 to allow for infinity inline Unit3 epipole_b() const { - return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + return rotation().unrotate(direction()); } /** @@ -180,8 +206,8 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(aRb_); - ar & BOOST_SERIALIZATION_NVP(aTb_); + ar & BOOST_SERIALIZATION_NVP(rotation()); + ar & BOOST_SERIALIZATION_NVP(direction()); ar & boost::serialization::make_nvp("E11", E_(0,0)); ar & boost::serialization::make_nvp("E12", E_(0,1)); @@ -195,7 +221,6 @@ private: } /// @} - }; template<> From b525ef7c9d1e06a4d9475d15a71dbf9fb01d104e Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 10:30:06 -0700 Subject: [PATCH 12/32] Fixed serialization issue --- gtsam/geometry/EssentialMatrix.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 6a94c8b11..fbf657c49 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -206,8 +206,8 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(rotation()); - ar & BOOST_SERIALIZATION_NVP(direction()); + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); ar & boost::serialization::make_nvp("E11", E_(0,0)); ar & boost::serialization::make_nvp("E12", E_(0,1)); From 740f49576b70b8b845d35ce81e6bb3ddf23bd030 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 11:59:56 -0700 Subject: [PATCH 13/32] ProductManifold --- gtsam/base/Manifold.h | 44 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index eafd7e3e2..8ac678e65 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -165,6 +165,50 @@ struct FixedDimension { "FixedDimension instantiated for dymanically-sized type."); }; +/// CRTP to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes manifold structure from M1 and M2, and binary constructor +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +private: + const M1& g() const {return this->first;} + const M2& h() const {return this->second;} + +public: + enum { dimension = M1::dimension + M2::dimension }; + inline static size_t Dim() { return dimension;} + inline size_t dim() const { return dimension;} + + typedef Eigen::Matrix TangentVector; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductManifold(const M1& g, const M2& h):std::pair(g,h) {} + + /// Retract delta to manifold + Derived retract(const TangentVector& xi) const { + return Derived(traits::Retract(g(),xi.head(M1::dimension)), + traits::Retract(h(),xi.tail(M2::dimension))); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const Derived& other) const { + TangentVector xi; + xi << traits::Local(g(),other.g()), traits::Local(h(),other.h()); + return xi; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold< + ProductManifold > { +}; + } // \ namespace gtsam ///** From 3b7c4404f9aee51df5d6d5dd0856dc5f57f867fc Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 12:00:05 -0700 Subject: [PATCH 14/32] Now Private base class --- gtsam/geometry/EssentialMatrix.h | 70 +++++++++----------------------- 1 file changed, 19 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index fbf657c49..f69488171 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,58 +7,10 @@ #pragma once -#include - -namespace gtsam { - -/// CRTP to construct the product manifold of two other manifolds, M1 and M2 -/// Assumes manifold structure from M1 and M2, and binary constructor -template -class ProductManifold: public std::pair { - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsManifold)); - -private: - const M1& g() const {return this->first;} - const M2& h() const {return this->second;} - -public: - // Dimension of the manifold - enum { dimension = M1::dimension + M2::dimension }; - - typedef Eigen::Matrix TangentVector; - - /// Default constructor yields identity - ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} - - // Construct from two subgroup elements - ProductManifold(const M1& g, const M2& h):std::pair(g,h) {} - - /// Retract delta to manifold - Derived retract(const TangentVector& xi) const { - return Derived(traits::Retract(g(),xi.head(M1::dimension)), - traits::Retract(h(),xi.tail(M2::dimension))); - } - - /// Compute the coordinates in the tangent space - TangentVector localCoordinates(const Derived& other) const { - TangentVector xi; - xi << traits::Local(g(),other.g()), traits::Local(h(),other.h()); - return xi; - } -}; - -// Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : internal::Manifold< - ProductManifold > { -}; - -} // namespace gtsam - #include #include #include +#include #include namespace gtsam { @@ -69,10 +21,10 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix : public ProductManifold { +class GTSAM_EXPORT EssentialMatrix : private ProductManifold { private: - + friend class ProductManifold; typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix @@ -124,6 +76,22 @@ public: /// @} + /// @name Manifold + /// @{ + + using Base::dimension; + using Base::dim; + using Base::Dim; + using Base::retract; + using Base::localCoordinates; + + /// Retract delta to manifold + // EssentialMatrix retract(const Vector& xi) const; + /// Compute the coordinates in the tangent space + // Vector5 localCoordinates(const EssentialMatrix& other) const; + + /// @} + /// @name Essential matrix methods /// @{ From fe8f519109fe8b965f6829c5d40754c644e93bd5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 16:30:00 -0700 Subject: [PATCH 15/32] Two new group-related targets --- .cproject | 34 ++++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/.cproject b/.cproject index 7c8190e6a..520fd3300 100644 --- a/.cproject +++ b/.cproject @@ -1301,6 +1301,7 @@ make + testSimulated2DOriented.run true false @@ -1340,6 +1341,7 @@ make + testSimulated2D.run true false @@ -1347,6 +1349,7 @@ make + testSimulated3D.run true false @@ -1450,7 +1453,6 @@ make - testErrors.run true false @@ -1761,7 +1763,6 @@ cpack - -G DEB true false @@ -1769,7 +1770,6 @@ cpack - -G RPM true false @@ -1777,7 +1777,6 @@ cpack - -G TGZ true false @@ -1785,7 +1784,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1977,6 +1975,7 @@ make + tests/testGaussianISAM2 true false @@ -2112,7 +2111,6 @@ make - tests/testBayesTree.run true false @@ -2120,7 +2118,6 @@ make - testBinaryBayesNet.run true false @@ -2168,7 +2165,6 @@ make - testSymbolicBayesNet.run true false @@ -2176,7 +2172,6 @@ make - tests/testSymbolicFactor.run true false @@ -2184,7 +2179,6 @@ make - testSymbolicFactorGraph.run true false @@ -2200,7 +2194,6 @@ make - tests/testBayesTree true false @@ -2814,6 +2807,14 @@ true true + + make + -j4 + testCyclic.run + true + true + true + make -j5 @@ -3190,6 +3191,14 @@ true true + + make + -j4 + testGroup.run + true + true + true + make -j5 @@ -3328,7 +3337,6 @@ make - testGraph.run true false @@ -3336,7 +3344,6 @@ make - testJunctionTree.run true false @@ -3344,7 +3351,6 @@ make - testSymbolicBayesNetB.run true false From 6569bd49aa35317ef6356e344f193b1a9ae23884 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 16:31:56 -0700 Subject: [PATCH 16/32] Put Derived in Group, as well --- gtsam/base/Group.h | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 8a1d69848..a5ae6ba1c 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -128,7 +128,7 @@ compose_pow(const G& g, size_t n) { /// Template to construct the direct product of two arbitrary groups /// Assumes nothing except group structure from G and H -template +template class DirectProduct: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsGroup)); @@ -143,23 +143,22 @@ public: // Construct from two subgroup elements DirectProduct(const G& g, const H& h):std::pair(g,h) {} - DirectProduct operator*(const DirectProduct& other) const { - return DirectProduct(traits::Compose(g(),other.g()), traits::Compose(h(),other.h())); + Derived operator*(const Derived& other) const { + return Derived(traits::Compose(g(),other.g()), traits::Compose(h(),other.h())); } - DirectProduct inverse() const { - return DirectProduct(g().inverse(), h().inverse()); + Derived inverse() const { + return Derived(g().inverse(), h().inverse()); } }; // Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : internal::MultiplicativeGroupTraits< - DirectProduct > { -}; +template +struct traits > : + internal::MultiplicativeGroupTraits > {}; /// Template to construct the direct sum of two additive groups /// Assumes existence of three additive operators for both groups -template +template class DirectSum: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive @@ -174,20 +173,21 @@ public: // Construct from two subgroup elements DirectSum(const G& g, const H& h):std::pair(g,h) {} - DirectSum operator+(const DirectSum& other) const { + Derived operator+(const Derived& other) const { return DirectSum(g()+other.g(), h()+other.h()); } - DirectSum operator-(const DirectSum& other) const { - return DirectSum(g()-other.g(), h()-other.h()); + Derived operator-(const Derived& other) const { + return Derived(g()-other.g(), h()-other.h()); } - DirectSum operator-() const { - return DirectSum(- g(), - h()); + Derived operator-() const { + return Derived(- g(), - h()); } }; // Define direct sums to be a model of the Additive Group concept -template -struct traits > : internal::AdditiveGroupTraits > {}; +template +struct traits > : + internal::AdditiveGroupTraits > {}; } // namespace gtsam From b8a8a16348248b62fd55113f3efb2a3cf2685cd4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 16:51:08 -0700 Subject: [PATCH 17/32] typedefs are no longer possible with CRTP :-( --- gtsam/base/tests/testGroup.cpp | 6 ++- gtsam/geometry/tests/testCyclic.cpp | 60 +++++++++++++++-------------- 2 files changed, 37 insertions(+), 29 deletions(-) diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index dadf2896c..034c7acaf 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -103,7 +103,11 @@ TEST(Group, S3) { //****************************************************************************** // The direct product of S2=Z2 and S3 is the symmetry group of a hexagon, // i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon) -typedef DirectProduct Dih6; +struct Dih6 : DirectProduct { + typedef DirectProduct Base; + Dih6(const S2& g, const S3& h):Base(g,h) {} + Dih6() {} +}; std::ostream &operator<<(std::ostream &os, const Dih6& m) { os << "( " << m.first << ", " << m.second << ")"; diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 20404a14f..84125ef22 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -22,72 +22,77 @@ using namespace std; using namespace gtsam; -typedef Cyclic<3> G; // Let's use the cyclic group of order 3 +typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3 +typedef Cyclic<2> Z2; //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - EXPECT_LONGS_EQUAL(0, traits::Identity()); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT_LONGS_EQUAL(0, traits::Identity()); } //****************************************************************************** TEST(Cyclic, Constructor) { - G g(0); + Z3 g(0); } //****************************************************************************** TEST(Cyclic, Compose) { - EXPECT_LONGS_EQUAL(0, traits::Compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(0),Z3(2))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(2),G(0))); - EXPECT_LONGS_EQUAL(0, traits::Compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(2),Z3(2))); } //****************************************************************************** TEST(Cyclic, Between) { - EXPECT_LONGS_EQUAL(0, traits::Between(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(0),Z3(2))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(2),G(0))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(2),G(1))); - EXPECT_LONGS_EQUAL(0, traits::Between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(2),Z3(2))); } //****************************************************************************** TEST(Cyclic, Inverse) { - EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); - EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); - EXPECT_LONGS_EQUAL(1, traits::Inverse(G(2))); + EXPECT_LONGS_EQUAL(0, traits::Inverse(Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Inverse(Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Inverse(Z3(2))); } //****************************************************************************** TEST(Cyclic, Negation) { - EXPECT_LONGS_EQUAL(0, -G(0)); - EXPECT_LONGS_EQUAL(2, -G(1)); - EXPECT_LONGS_EQUAL(1, -G(2)); + EXPECT_LONGS_EQUAL(0, -Z3(0)); + EXPECT_LONGS_EQUAL(2, -Z3(1)); + EXPECT_LONGS_EQUAL(1, -Z3(2)); } //****************************************************************************** TEST(Cyclic, Negation2) { - typedef Cyclic<2> Z2; EXPECT_LONGS_EQUAL(0, -Z2(0)); EXPECT_LONGS_EQUAL(1, -Z2(1)); } //****************************************************************************** TEST(Cyclic , Invariants) { - G g(2), h(1); + Z3 g(2), h(1); EXPECT(check_group_invariants(g,h)); } //****************************************************************************** -// The Direct sum of Cyclic<2> and Cyclic<2> is *not* Cyclic<4>, but the +// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the // smallest non-cyclic group called the Klein four-group: -typedef DirectSum, Cyclic<2> > K4; +struct K4: DirectSum { + typedef DirectSum Base; + K4(const Z2& g, const Z2& h):Base(g,h) {} + K4(const Base& base):Base(base) {} + K4() {} +}; namespace gtsam { @@ -105,9 +110,8 @@ struct traits : internal::AdditiveGroupTraits { } // namespace gtsam TEST(Cyclic , DirectSum) { - // The Direct sum of Cyclic<2> and Cyclic<2> is *not* Cyclic<4>, but the + // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the // smallest non-cyclic group called the Klein four-group: - typedef DirectSum, Cyclic<2> > K4; BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsTestable)); From b23a51db6ddf5e5bf318b911a231c553f5f2df0a Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 17:13:08 -0700 Subject: [PATCH 18/32] PoseRTV as ProductManifold works --- gtsam_unstable/dynamics/PoseRTV.cpp | 48 +++++++++-------- gtsam_unstable/dynamics/PoseRTV.h | 81 ++++++++++++++++++++--------- 2 files changed, 82 insertions(+), 47 deletions(-) diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 818266d4c..2b1fd29f6 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -14,7 +14,7 @@ namespace gtsam { using namespace std; -static const Vector g = delta(3, 2, 9.81); +static const Vector kGravity = delta(3, 2, 9.81); /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -24,28 +24,30 @@ double bound(double a, double min, double max) { } /* ************************************************************************* */ -PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, - double vx, double vy, double vz) -: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {} +PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, + double z, double vx, double vy, double vz) : + Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), + Velocity3(vx, vy, vz)) { +} /* ************************************************************************* */ -PoseRTV::PoseRTV(const Vector& rtv) -: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3)) -{ +PoseRTV::PoseRTV(const Vector& rtv) : + Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), + Velocity3(rtv.tail(3))) { } /* ************************************************************************* */ Vector PoseRTV::vector() const { Vector rtv(9); - rtv.head(3) = Rt_.rotation().xyz(); - rtv.segment(3,3) = Rt_.translation().vector(); - rtv.tail(3) = v_.vector(); + rtv.head(3) = rotation().xyz(); + rtv.segment(3,3) = translation().vector(); + rtv.tail(3) = velocity().vector(); return rtv; } /* ************************************************************************* */ bool PoseRTV::equals(const PoseRTV& other, double tol) const { - return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol); + return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol); } /* ************************************************************************* */ @@ -53,7 +55,7 @@ void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); t().print(" T"); - v_.print(" V"); + velocity().print(" V"); } /* ************************************************************************* */ @@ -67,8 +69,8 @@ PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) { /* ************************************************************************* */ Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { if (H) CONCEPT_NOT_IMPLEMENTED; - Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = p.v_.vector(); + Vector6 Lx = Pose3::Logmap(p.pose()); + Vector3 Lv = p.velocity().vector(); return (Vector9() << Lx, Lv).finished(); } @@ -79,8 +81,8 @@ PoseRTV PoseRTV::retract(const Vector& v, if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; assert(v.size() == 9); // First order approximation - Pose3 newPose = Rt_.retract(sub(v, 0, 6)); - Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9)); + Pose3 newPose = pose().retract(sub(v, 0, 6)); + Velocity3 newVel = velocity() + rotation() * Point3(sub(v, 6, 9)); return PoseRTV(newPose, newVel); } @@ -92,7 +94,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1, const Pose3& x0 = pose(), &x1 = p1.pose(); // First order approximation Vector6 poseLogmap = x0.localCoordinates(x1); - Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); + Vector3 lv = rotation().unrotate(p1.velocity() - velocity()).vector(); return (Vector(9) << poseLogmap, lv).finished(); } @@ -100,7 +102,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1, PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } PoseRTV PoseRTV::inverse(ChartJacobian H1) const { if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), - v_); + return PoseRTV(pose().inverse(), - velocity()); } /* ************************************************************************* */ @@ -109,7 +111,7 @@ PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, ChartJacobian H2) const { if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_); + return PoseRTV(pose().compose(p.pose()), velocity()+p.velocity()); } /* ************************************************************************* */ @@ -187,7 +189,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity)); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -205,7 +207,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // acceleration Vector3 accel = (v2-v1).vector() / dt; - imu.head<3>() = r2.transpose() * (accel - g); + imu.head<3>() = r2.transpose() * (accel - kGravity); // rotation rates // just using euler angles based on matlab code @@ -249,7 +251,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, OptionalJacobian<9, 6> Dtrans) const { // Note that we rotate the velocity Matrix DVr, DTt; - Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); + Velocity3 newvel = trans.rotation().rotate(velocity(), DVr, DTt); if (!Dglobal && !Dtrans) return PoseRTV(trans.compose(pose()), newvel); @@ -273,7 +275,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, // insertSub(*Dtrans, DTc, 0, 0); // correct in tests // // // rotating the velocity - // Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z()); + // Matrix vRhat = skewSymmetric(-velocity().x(), -velocity().y(), -velocity().z()); // trans.rotation().print("Transform rotation"); // gtsam::print(vRhat, "vRhat"); // gtsam::print(DVr, "DVr"); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 20709ba89..d93f58bed 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -7,11 +7,46 @@ #pragma once #include -#include #include +#include namespace gtsam { +/// CRTP to construct the product Lie group of two other Lie groups, G and H +/// Assumes manifold structure from G and H, and binary constructor +template +class ProductLieGroup: public ProductManifold { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + typedef ProductManifold Base; + +public: + enum {dimension = G::dimension + H::dimension}; + inline static size_t Dim() {return dimension;} + inline size_t dim() const {return dimension;} + + typedef Eigen::Matrix TangentVector; + + /// Default constructor yields identity + ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h):Base(g,h) {} + + ProductLieGroup operator*(const Derived& other) const { + return Derived(traits::Compose(this->g(),other.g()), traits::Compose(this->h(),other.h())); + } + ProductLieGroup inverse() const { + return Derived(this->g().inverse(), this->h().inverse()); + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits< + ProductLieGroup > { +}; + /// Syntactic sugar to clarify components typedef Point3 Velocity3; @@ -19,12 +54,10 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV { +class GTSAM_UNSTABLE_EXPORT PoseRTV : private ProductLieGroup { protected: - Pose3 Rt_; - Velocity3 v_; - + typedef ProductLieGroup Base; typedef OptionalJacobian<9, 9> ChartJacobian; public: @@ -32,16 +65,16 @@ public: // constructors - with partial versions PoseRTV() {} - PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - explicit PoseRTV(const Point3& pt) - : Rt_(Rot3::identity(), pt) {} + PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + explicit PoseRTV(const Point3& t) + : Base(Pose3(Rot3(), t),Velocity3()) {} PoseRTV(const Pose3& pose, const Velocity3& vel) - : Rt_(pose), v_(vel) {} + : Base(pose, vel) {} explicit PoseRTV(const Pose3& pose) - : Rt_(pose) {} + : Base(pose,Velocity3()) {} /** build from components - useful for data files */ PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, @@ -51,21 +84,21 @@ public: explicit PoseRTV(const Vector& v); // access - const Point3& t() const { return Rt_.translation(); } - const Rot3& R() const { return Rt_.rotation(); } - const Velocity3& v() const { return v_; } - const Pose3& pose() const { return Rt_; } + const Pose3& pose() const { return first; } + const Velocity3& v() const { return second; } + const Point3& t() const { return pose().translation(); } + const Rot3& R() const { return pose().rotation(); } // longer function names - const Point3& translation() const { return Rt_.translation(); } - const Rot3& rotation() const { return Rt_.rotation(); } - const Velocity3& velocity() const { return v_; } + const Point3& translation() const { return pose().translation(); } + const Rot3& rotation() const { return pose().rotation(); } + const Velocity3& velocity() const { return second; } // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return Rt_.translation().vector(); } - Vector velocityVec() const { return v_.vector(); } + Vector translationVec() const { return pose().translation().vector(); } + Vector velocityVec() const { return velocity().vector(); } // testable bool equals(const PoseRTV& other, double tol=1e-6) const; @@ -183,8 +216,8 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(Rt_); - ar & BOOST_SERIALIZATION_NVP(v_); + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); } }; From 4aa7225585cb17c61d90946b3e94ca5082cdbcaf Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 18:30:40 -0700 Subject: [PATCH 19/32] Get rid of g() and h() --- gtsam/base/Group.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index a5ae6ba1c..f520b2ff7 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -133,9 +133,6 @@ class DirectProduct: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsGroup)); - const G& g() const {return this->first;} - const H& h() const {return this->second;} - public: /// Default constructor yields identity DirectProduct():std::pair(traits::Identity(),traits::Identity()) {} @@ -144,10 +141,11 @@ public: DirectProduct(const G& g, const H& h):std::pair(g,h) {} Derived operator*(const Derived& other) const { - return Derived(traits::Compose(g(),other.g()), traits::Compose(h(),other.h())); + return Derived(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); } Derived inverse() const { - return Derived(g().inverse(), h().inverse()); + return Derived(this->first.inverse(), this->second.inverse()); } }; From 8582357976b377a763531d361329c864a8771530 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 18:30:53 -0700 Subject: [PATCH 20/32] test Product --- gtsam/base/Manifold.h | 35 ++++++++++++++++++++++------------- tests/testManifold.cpp | 31 +++++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 8ac678e65..2f8dc5f68 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -46,7 +46,7 @@ struct manifold_tag {}; * There may be multiple possible retractions for a given manifold, which can be chosen * between depending on the computational complexity. The important criteria for * the creation for the retract and localCoordinates functions is that they be - * inverse operations. The new notion of a Chart guarantees that. + * inverse operations. * */ @@ -90,9 +90,9 @@ struct ManifoldImpl { /// A helper that implements the traits interface for GTSAM manifolds. /// To use this for your class type, define: -/// template<> struct traits : public internal::Manifold { }; +/// template<> struct traits : public internal::ManifoldTraits { }; template -struct Manifold: Testable, ManifoldImpl { +struct ManifoldTraits: ManifoldImpl { // Check that Class has the necessary machinery BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); @@ -116,6 +116,11 @@ struct Manifold: Testable, ManifoldImpl { } }; +/// Implement both manifold and testable traits at the same time +template +struct Manifold: Testable, ManifoldTraits { +}; + } // \ namespace internal /// Check invariants for Manifold type @@ -173,33 +178,37 @@ class ProductManifold: public std::pair { BOOST_CONCEPT_ASSERT((IsManifold)); private: - const M1& g() const {return this->first;} - const M2& h() const {return this->second;} + enum { dimension1 = traits::dimension }; + enum { dimension2 = traits::dimension }; public: - enum { dimension = M1::dimension + M2::dimension }; + enum { dimension = dimension1 + dimension2 }; inline static size_t Dim() { return dimension;} inline size_t dim() const { return dimension;} typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; /// Default constructor yields identity ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} - // Construct from two subgroup elements - ProductManifold(const M1& g, const M2& h):std::pair(g,h) {} + // Construct from two original manifold values + ProductManifold(const M1& m1, const M2& m2):std::pair(m1,m2) {} /// Retract delta to manifold Derived retract(const TangentVector& xi) const { - return Derived(traits::Retract(g(),xi.head(M1::dimension)), - traits::Retract(h(),xi.tail(M2::dimension))); + M1 m1 = traits::Retract(this->first, xi.template head()); + M2 m2 = traits::Retract(this->second, xi.template tail()); + return Derived(m1,m2); } /// Compute the coordinates in the tangent space TangentVector localCoordinates(const Derived& other) const { - TangentVector xi; - xi << traits::Local(g(),other.g()), traits::Local(h(),other.h()); - return xi; + typename traits::TangentVector v1 = traits::Local(this->first, other.first); + typename traits::TangentVector v2 = traits::Local(this->second, other.second); + TangentVector v; + v << v1, v2; + return v; } }; diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index ef0456146..496579b8d 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -148,6 +148,37 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(zero(3), traits::Local(R, R))); } +//****************************************************************************** +struct MyPoint2Pair : public ProductManifold { + typedef ProductManifold Base; + MyPoint2Pair(const Point2& p1, const Point2& p2):Base(p1,p2) {} + MyPoint2Pair(const Base& base):Base(base) {} + MyPoint2Pair() {} +}; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::ManifoldTraits { + static void Print(const MyPoint2Pair& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} + +TEST(Manifold, ProductManifold) { + BOOST_CONCEPT_ASSERT((IsManifold)); + MyPoint2Pair pair1; + Vector4 d; + d << 1,2,3,4; + MyPoint2Pair expected(Point2(1,2),Point2(3,4)); + MyPoint2Pair pair2 = pair1.retract(d); + EXPECT(assert_equal(expected,pair2,1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9)); +} + //****************************************************************************** int main() { TestResult tr; From 1bbbb7ad56c5ff8152653541683c168f89c225e3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 18:31:06 -0700 Subject: [PATCH 21/32] Get rid of obsolete comments --- gtsam/geometry/EssentialMatrix.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index f69488171..29675d640 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -85,11 +85,6 @@ public: using Base::retract; using Base::localCoordinates; - /// Retract delta to manifold - // EssentialMatrix retract(const Vector& xi) const; - /// Compute the coordinates in the tangent space - // Vector5 localCoordinates(const EssentialMatrix& other) const; - /// @} /// @name Essential matrix methods From 111d0d39dd2aa260869b3452f50719c5791016fc Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 20:51:50 -0700 Subject: [PATCH 22/32] Got rid of CRTP --- gtsam/base/Group.h | 34 ++++++++++++++--------------- gtsam/base/Manifold.h | 19 ++++++++-------- gtsam/base/tests/testGroup.cpp | 6 +---- gtsam/geometry/EssentialMatrix.h | 21 +++++++++++++----- gtsam/geometry/tests/testCyclic.cpp | 7 +----- tests/testManifold.cpp | 12 ++++------ 6 files changed, 48 insertions(+), 51 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index f520b2ff7..f35091757 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -128,7 +128,7 @@ compose_pow(const G& g, size_t n) { /// Template to construct the direct product of two arbitrary groups /// Assumes nothing except group structure from G and H -template +template class DirectProduct: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsGroup)); @@ -140,23 +140,23 @@ public: // Construct from two subgroup elements DirectProduct(const G& g, const H& h):std::pair(g,h) {} - Derived operator*(const Derived& other) const { - return Derived(traits::Compose(this->first, other.first), + DirectProduct operator*(const DirectProduct& other) const { + return DirectProduct(traits::Compose(this->first, other.first), traits::Compose(this->second, other.second)); } - Derived inverse() const { - return Derived(this->first.inverse(), this->second.inverse()); + DirectProduct inverse() const { + return DirectProduct(this->first.inverse(), this->second.inverse()); } }; // Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : - internal::MultiplicativeGroupTraits > {}; +template +struct traits > : + internal::MultiplicativeGroupTraits > {}; /// Template to construct the direct sum of two additive groups /// Assumes existence of three additive operators for both groups -template +template class DirectSum: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive @@ -171,21 +171,21 @@ public: // Construct from two subgroup elements DirectSum(const G& g, const H& h):std::pair(g,h) {} - Derived operator+(const Derived& other) const { + DirectSum operator+(const DirectSum& other) const { return DirectSum(g()+other.g(), h()+other.h()); } - Derived operator-(const Derived& other) const { - return Derived(g()-other.g(), h()-other.h()); + DirectSum operator-(const DirectSum& other) const { + return DirectSum(g()-other.g(), h()-other.h()); } - Derived operator-() const { - return Derived(- g(), - h()); + DirectSum operator-() const { + return DirectSum(- g(), - h()); } }; // Define direct sums to be a model of the Additive Group concept -template -struct traits > : - internal::AdditiveGroupTraits > {}; +template +struct traits > : + internal::AdditiveGroupTraits > {}; } // namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 2f8dc5f68..12df84819 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -170,14 +170,14 @@ struct FixedDimension { "FixedDimension instantiated for dymanically-sized type."); }; -/// CRTP to construct the product manifold of two other manifolds, M1 and M2 -/// Assumes manifold structure from M1 and M2, and binary constructor -template +/// Helper class to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes nothing except manifold structure from M1 and M2 +template class ProductManifold: public std::pair { BOOST_CONCEPT_ASSERT((IsManifold)); BOOST_CONCEPT_ASSERT((IsManifold)); -private: +protected: enum { dimension1 = traits::dimension }; enum { dimension2 = traits::dimension }; @@ -196,14 +196,14 @@ public: ProductManifold(const M1& m1, const M2& m2):std::pair(m1,m2) {} /// Retract delta to manifold - Derived retract(const TangentVector& xi) const { + ProductManifold retract(const TangentVector& xi) const { M1 m1 = traits::Retract(this->first, xi.template head()); M2 m2 = traits::Retract(this->second, xi.template tail()); - return Derived(m1,m2); + return ProductManifold(m1,m2); } /// Compute the coordinates in the tangent space - TangentVector localCoordinates(const Derived& other) const { + TangentVector localCoordinates(const ProductManifold& other) const { typename traits::TangentVector v1 = traits::Local(this->first, other.first); typename traits::TangentVector v2 = traits::Local(this->second, other.second); TangentVector v; @@ -213,9 +213,8 @@ public: }; // Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : internal::Manifold< - ProductManifold > { +template +struct traits > : internal::Manifold > { }; } // \ namespace gtsam diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index 034c7acaf..dadf2896c 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -103,11 +103,7 @@ TEST(Group, S3) { //****************************************************************************** // The direct product of S2=Z2 and S3 is the symmetry group of a hexagon, // i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon) -struct Dih6 : DirectProduct { - typedef DirectProduct Base; - Dih6(const S2& g, const S3& h):Base(g,h) {} - Dih6() {} -}; +typedef DirectProduct Dih6; std::ostream &operator<<(std::ostream &os, const Dih6& m) { os << "( " << m.first << ", " << m.second << ")"; diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 29675d640..697bd462d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -21,13 +21,17 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix : private ProductManifold { +class GTSAM_EXPORT EssentialMatrix : private ProductManifold { private: - friend class ProductManifold; - typedef ProductManifold Base; + typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix + /// Construct from Base + EssentialMatrix(const Base& base) : + Base(base), E_(direction().skew() * rotation().matrix()) { + } + public: /// Static function to convert Point2 to homogeneous coordinates @@ -82,9 +86,16 @@ public: using Base::dimension; using Base::dim; using Base::Dim; - using Base::retract; - using Base::localCoordinates; + /// Retract delta to manifold + EssentialMatrix retract(const TangentVector& v) const { + return Base::retract(v); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const EssentialMatrix& other) const { + return Base::localCoordinates(other); + } /// @} /// @name Essential matrix methods diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 84125ef22..7becfc75f 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -87,12 +87,7 @@ TEST(Cyclic , Invariants) { //****************************************************************************** // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the // smallest non-cyclic group called the Klein four-group: -struct K4: DirectSum { - typedef DirectSum Base; - K4(const Z2& g, const Z2& h):Base(g,h) {} - K4(const Base& base):Base(base) {} - K4() {} -}; +typedef DirectSum K4; namespace gtsam { diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 496579b8d..89b296824 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -10,13 +10,14 @@ * -------------------------------1------------------------------------------- */ /** - * @file testExpression.cpp + * @file testManifold.cpp * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale - * @brief unit tests for Block Automatic Differentiation + * @brief unit tests for Manifold type machinery */ +#include #include #include #include @@ -149,12 +150,7 @@ TEST(Manifold, DefaultChart) { } //****************************************************************************** -struct MyPoint2Pair : public ProductManifold { - typedef ProductManifold Base; - MyPoint2Pair(const Point2& p1, const Point2& p2):Base(p1,p2) {} - MyPoint2Pair(const Base& base):Base(base) {} - MyPoint2Pair() {} -}; +typedef ProductManifold MyPoint2Pair; // Define any direct product group to be a model of the multiplicative Group concept namespace gtsam { From e65f510ebf04a804e8269c28066aa8052be409e8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 22:02:42 -0700 Subject: [PATCH 23/32] Harmonized traits internal helper classes --- gtsam/base/Group.h | 50 +++++++++++++++++++++++++--------------- gtsam/base/Lie.h | 11 +++++---- gtsam/base/Manifold.h | 6 ++--- gtsam/base/VectorSpace.h | 15 +++++++----- 4 files changed, 48 insertions(+), 34 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index f35091757..39541416e 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -20,6 +20,8 @@ #pragma once +#include + #include #include #include @@ -87,34 +89,38 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { namespace internal { -/// A helper class that implements the traits interface for groups. -/// Assumes that constructor yields identity +/// A helper class that implements the traits interface for multiplicative groups. +/// Assumes existence of identity, operator* and inverse method template -struct GroupTraits { +struct MultiplicativeGroupTraits { typedef group_tag structure_category; - static Class Identity() { return Class(); } + typedef multiplicative_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g * h;} + static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} + static Class Inverse(const Class &g) { return g.inverse();} }; -/// A helper class that implements the traits interface for multiplicative groups. -/// Assumes existence of operator* and inverse method +/// Both multiplicative group traits and Testable template -struct MultiplicativeGroupTraits : GroupTraits { - typedef multiplicative_group_tag group_flavor; \ - static Class Compose(const Class &g, const Class & h) { return g * h;} \ - static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \ - static Class Inverse(const Class &g) { return g.inverse();} -}; +struct MultiplicativeGroup : MultiplicativeGroupTraits, Testable {}; /// A helper class that implements the traits interface for additive groups. -/// Assumes existence of three additive operators +/// Assumes existence of identity and three additive operators template -struct AdditiveGroupTraits : GroupTraits { - typedef additive_group_tag group_flavor; \ - static Class Compose(const Class &g, const Class & h) { return g + h;} \ - static Class Between(const Class &g, const Class & h) { return h - g;} \ - static Class Inverse(const Class &g) { return -g;} +struct AdditiveGroupTraits { + typedef group_tag structure_category; + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g + h;} + static Class Between(const Class &g, const Class & h) { return h - g;} + static Class Inverse(const Class &g) { return -g;} }; +/// Both additive group traits and Testable +template +struct AdditiveGroup : AdditiveGroupTraits, Testable {}; + } // namespace internal /// compose multiple times @@ -127,7 +133,7 @@ compose_pow(const G& g, size_t n) { } /// Template to construct the direct product of two arbitrary groups -/// Assumes nothing except group structure from G and H +/// Assumes nothing except group structure and Testable from G and H template class DirectProduct: public std::pair { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -140,6 +146,9 @@ public: // Construct from two subgroup elements DirectProduct(const G& g, const H& h):std::pair(g,h) {} + // identity + static DirectProduct identity() { return DirectProduct(); } + DirectProduct operator*(const DirectProduct& other) const { return DirectProduct(traits::Compose(this->first, other.first), traits::Compose(this->second, other.second)); @@ -171,6 +180,9 @@ public: // Construct from two subgroup elements DirectSum(const G& g, const H& h):std::pair(g,h) {} + // identity + static DirectSum identity() { return DirectSum(); } + DirectSum operator+(const DirectSum& other) const { return DirectSum(g()+other.g(), h()+other.h()); } diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index c9f788992..05c7bc20f 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -136,8 +136,10 @@ namespace internal { /// A helper class that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public internal::LieGroupTraits {}; +/// Assumes existence of: identity, dimension, localCoordinates, retract, +/// and additionally Logmap, Expmap, compose, between, and inverse template -struct LieGroupTraits : Testable { +struct LieGroupTraits { typedef lie_group_tag structure_category; /// @name Group @@ -167,12 +169,10 @@ struct LieGroupTraits : Testable { ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return origin.retract(v, Horigin, Hv); } - /// @} /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { return Class::Logmap(m, Hm); } @@ -195,11 +195,12 @@ struct LieGroupTraits : Testable { ChartJacobian H = boost::none) { return m.inverse(H); } - /// @} - }; +/// Both LieGroupTraits and Testable +template struct LieGroup: LieGroupTraits, Testable {}; + } // \ namepsace internal /** diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 12df84819..b20c60822 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -116,10 +116,8 @@ struct ManifoldTraits: ManifoldImpl { } }; -/// Implement both manifold and testable traits at the same time -template -struct Manifold: Testable, ManifoldTraits { -}; +/// Both ManifoldTraits and Testable +template struct Manifold: ManifoldTraits, Testable {}; } // \ namespace internal diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 558fe52c9..c356dcc07 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -20,7 +20,7 @@ template struct traits; namespace internal { -/// VectorSpace Implementation for Fixed sizes +/// VectorSpaceTraits Implementation for Fixed sizes template struct VectorSpaceImpl { @@ -83,7 +83,7 @@ struct VectorSpaceImpl { /// @} }; -/// VectorSpace implementation for dynamic types. +/// VectorSpaceTraits implementation for dynamic types. template struct VectorSpaceImpl { @@ -159,11 +159,11 @@ struct VectorSpaceImpl { /// @} }; -/// A helper that implements the traits interface for GTSAM lie groups. +/// A helper that implements the traits interface for GTSAM vector space types. /// To use this for your gtsam type, define: -/// template<> struct traits : public VectorSpace { }; +/// template<> struct traits : public VectorSpaceTraits { }; template -struct VectorSpace: Testable, VectorSpaceImpl { +struct VectorSpaceTraits: VectorSpaceImpl { typedef vector_space_tag structure_category; @@ -178,9 +178,12 @@ struct VectorSpace: Testable, VectorSpaceImpl { enum { dimension = Class::dimension}; typedef Class ManifoldType; /// @} - }; +/// Both VectorSpaceTRaits and Testable +template +struct VectorSpace: Testable, VectorSpaceTraits {}; + /// A helper that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public ScalarTraits { }; From 42b0f0f4d438ea24cd8909e75d56310fac850ce4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 22:02:54 -0700 Subject: [PATCH 24/32] We need identity --- gtsam/geometry/Cyclic.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 88a04ab2d..15d8154b8 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -34,6 +34,8 @@ public: /// Default constructor yields identity Cyclic():i_(0) { } + static Cyclic identity() { return Cyclic();} + /// Cast to size_t operator size_t() const { return i_; From 9d522c72f38d494883ac5681d0790abd202103f2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 22:04:04 -0700 Subject: [PATCH 25/32] internal::LieGroup needed if you also want Testable traits --- gtsam/geometry/Pose2.h | 4 ++-- gtsam/geometry/Pose3.h | 4 ++-- gtsam/geometry/Rot2.h | 4 ++-- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/SO3.h | 4 ++-- gtsam_unstable/dynamics/PoseRTV.cpp | 24 ------------------------ gtsam_unstable/dynamics/PoseRTV.h | 2 +- gtsam_unstable/geometry/Similarity3.h | 2 +- 8 files changed, 12 insertions(+), 36 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 59d53305a..8d8d5b7b6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -291,10 +291,10 @@ typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b11ae2587..fcfceae65 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -324,10 +324,10 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); typedef std::vector Pose3Vector; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 9500f0d64..198cd5862 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -208,9 +208,9 @@ namespace gtsam { }; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7520d8d56..881c58579 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -463,9 +463,9 @@ namespace gtsam { GTSAM_EXPORT std::pair RQ(const Matrix3& A); template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index a07c3601d..a08168ed8 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -129,11 +129,11 @@ public: }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; } // end namespace gtsam diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 2b1fd29f6..1918008f3 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -74,30 +74,6 @@ Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { return (Vector9() << Lx, Lv).finished(); } -/* ************************************************************************* */ -PoseRTV PoseRTV::retract(const Vector& v, - ChartJacobian Horigin, - ChartJacobian Hv) const { - if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; - assert(v.size() == 9); - // First order approximation - Pose3 newPose = pose().retract(sub(v, 0, 6)); - Velocity3 newVel = velocity() + rotation() * Point3(sub(v, 6, 9)); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector PoseRTV::localCoordinates(const PoseRTV& p1, - ChartJacobian Horigin, - ChartJacobian Hp) const { - if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED; - const Pose3& x0 = pose(), &x1 = p1.pose(); - // First order approximation - Vector6 poseLogmap = x0.localCoordinates(x1); - Vector3 lv = rotation().unrotate(p1.velocity() - velocity()).vector(); - return (Vector(9) << poseLogmap, lv).finished(); -} - /* ************************************************************************* */ PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } PoseRTV PoseRTV::inverse(ChartJacobian H1) const { diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index d93f58bed..b2b9b8ece 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -223,6 +223,6 @@ private: template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // \namespace gtsam diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 65de32589..eec2124ee 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -148,5 +148,5 @@ public: }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } From 043bebe8efbc6dcba1f8b658cae7e4d9ec54fd56 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 22:04:18 -0700 Subject: [PATCH 26/32] Group-related targets --- .cproject | 42 ++++++++++++++++++++++++++++++------------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/.cproject b/.cproject index 520fd3300..ac9b166ec 100644 --- a/.cproject +++ b/.cproject @@ -1035,6 +1035,14 @@ true true + + make + -j4 + testCyclic.run + true + true + true + make -j2 @@ -1301,7 +1309,6 @@ make - testSimulated2DOriented.run true false @@ -1341,7 +1348,6 @@ make - testSimulated2D.run true false @@ -1349,7 +1355,6 @@ make - testSimulated3D.run true false @@ -1453,6 +1458,7 @@ make + testErrors.run true false @@ -1763,6 +1769,7 @@ cpack + -G DEB true false @@ -1770,6 +1777,7 @@ cpack + -G RPM true false @@ -1777,6 +1785,7 @@ cpack + -G TGZ true false @@ -1784,6 +1793,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1975,7 +1985,6 @@ make - tests/testGaussianISAM2 true false @@ -2111,6 +2120,7 @@ make + tests/testBayesTree.run true false @@ -2118,6 +2128,7 @@ make + testBinaryBayesNet.run true false @@ -2165,6 +2176,7 @@ make + testSymbolicBayesNet.run true false @@ -2172,6 +2184,7 @@ make + tests/testSymbolicFactor.run true false @@ -2179,6 +2192,7 @@ make + testSymbolicFactorGraph.run true false @@ -2194,6 +2208,7 @@ make + tests/testBayesTree true false @@ -2807,14 +2822,6 @@ true true - - make - -j4 - testCyclic.run - true - true - true - make -j5 @@ -3337,6 +3344,7 @@ make + testGraph.run true false @@ -3344,6 +3352,7 @@ make + testJunctionTree.run true false @@ -3351,6 +3360,7 @@ make + testSymbolicBayesNetB.run true false @@ -3420,6 +3430,14 @@ true true + + make + -j4 + testLie.run + true + true + true + make -j2 From 512e37353048e826e9e6048f17d92c772971286d Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 22:04:42 -0700 Subject: [PATCH 27/32] ProductLieGroup prototype --- tests/testLie.cpp | 157 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 157 insertions(+) create mode 100644 tests/testLie.cpp diff --git a/tests/testLie.cpp b/tests/testLie.cpp new file mode 100644 index 000000000..99337230b --- /dev/null +++ b/tests/testLie.cpp @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testLie.cpp + * @date May, 2015 + * @author Frank Dellaert + * @brief unit tests for Lie group type machinery + */ + +#include +#include + +namespace gtsam { + +/// Template to construct the product Lie group of two other Lie groups, G and H +/// Assumes manifold structure from G and H, and binary constructor +template +class ProductLieGroup: public std::pair, public LieGroup< + ProductLieGroup, traits::dimension + traits::dimension> { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + typedef std::pair Base; + +protected: + enum {dimension1 = traits::dimension}; + enum {dimension2 = traits::dimension}; + +public: + /// Default constructor yields identity + ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h):Base(g,h) {} + + // Construct from base + ProductLieGroup(const Base& base):Base(base) {} + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static ProductLieGroup identity() {return ProductLieGroup();} + + ProductLieGroup operator*(const ProductLieGroup& other) const { + return ProductLieGroup(traits::Compose(this->first,other.first), + traits::Compose(this->second,other.second)); + } + ProductLieGroup inverse() const { + return ProductLieGroup(this->first.inverse(), this->second.inverse()); + } + /// @} + + /// @name Manifold (but with derivatives) + /// @{ + enum {dimension = dimension1 + dimension2}; + inline static size_t Dim() {return dimension;} + inline size_t dim() const {return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + /// @} + + /// @name Lie Group + /// @{ + Eigen::Matrix AdjointMap() const { + Eigen::Matrix A; + A.setIdentity(); + throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); +// A.template topLeftCorner() = this->first.AdjointMap(); +// A.template bottomRightCorner() = this->second.AdjointMap(); + return A; + } + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + G g = traits::Expmap(v.template head()); + H h = traits::Expmap(v.template tail()); + return ProductLieGroup(g,h); + } + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Logmap(p.first); + typename traits::TangentVector v2 = traits::Logmap(p.second); + TangentVector v; + v << v1, v2; + return v; + } + struct ChartAtOrigin { + static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { + return Logmap(m, Hm); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Expmap(v, Hv); + } + }; + using LieGroup::inverse; // with derivative + /// @} +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits< + ProductLieGroup > { +}; +} +#include +#include + +#undef CHECK +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +typedef ProductLieGroup MyPoint2Pair; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::LieGroupTraits { + static void Print(const MyPoint2Pair& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, + double tol = 1e-8) { + return m1 == m2; + } +}; +} + +TEST(Lie, ProductLieGroup) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + MyPoint2Pair pair1; + Vector4 d; + d << 1, 2, 3, 4; + MyPoint2Pair expected(Point2(1, 2), Point2(3, 4)); + MyPoint2Pair pair2 = pair1.retract(d); + EXPECT(assert_equal(expected, pair2, 1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + From 8939adc7991b90e2e52e33187e02f6162dbe56b1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 23:49:41 -0700 Subject: [PATCH 28/32] Moved ProductLieGroup to its own header --- gtsam/base/ProductLieGroup.h | 115 +++++++++++++++++++++++++++++++++++ tests/testLie.cpp | 95 +---------------------------- 2 files changed, 116 insertions(+), 94 deletions(-) create mode 100644 gtsam/base/ProductLieGroup.h diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h new file mode 100644 index 000000000..2bcb49dba --- /dev/null +++ b/gtsam/base/ProductLieGroup.h @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file ProductLieGroup.h + * @date May, 2015 + * @author Frank Dellaert + * @brief Group product of two Lie Groups + */ + +#pragma once + +#include +#include // pair + +namespace gtsam { + +/// Template to construct the product Lie group of two other Lie groups, G and H +/// Assumes manifold structure from G and H, and binary constructor +template +class ProductLieGroup: public std::pair, public LieGroup< + ProductLieGroup, traits::dimension + traits::dimension> { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + typedef std::pair Base; + +protected: + enum {dimension1 = traits::dimension}; + enum {dimension2 = traits::dimension}; + +public: + /// Default constructor yields identity + ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h):Base(g,h) {} + + // Construct from base + ProductLieGroup(const Base& base):Base(base) {} + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static ProductLieGroup identity() {return ProductLieGroup();} + + ProductLieGroup operator*(const ProductLieGroup& other) const { + return ProductLieGroup(traits::Compose(this->first,other.first), + traits::Compose(this->second,other.second)); + } + ProductLieGroup inverse() const { + return ProductLieGroup(this->first.inverse(), this->second.inverse()); + } + /// @} + + /// @name Manifold (but with derivatives) + /// @{ + enum {dimension = dimension1 + dimension2}; + inline static size_t Dim() {return dimension;} + inline size_t dim() const {return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + /// @} + + /// @name Lie Group + /// @{ + Eigen::Matrix AdjointMap() const { + Eigen::Matrix A; + A.setIdentity(); + throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + // A.template topLeftCorner() = this->first.AdjointMap(); + // A.template bottomRightCorner() = this->second.AdjointMap(); + return A; + } + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + G g = traits::Expmap(v.template head()); + H h = traits::Expmap(v.template tail()); + return ProductLieGroup(g,h); + } + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Logmap(p.first); + typename traits::TangentVector v2 = traits::Logmap(p.second); + TangentVector v; + v << v1, v2; + return v; + } + struct ChartAtOrigin { + static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { + return Logmap(m, Hm); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Expmap(v, Hv); + } + }; + using LieGroup::inverse; // with derivative + /// @} +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits< + ProductLieGroup > { +}; +} // namespace gtsam + diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 99337230b..f004dcc0f 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -16,104 +16,11 @@ * @brief unit tests for Lie group type machinery */ -#include -#include +#include -namespace gtsam { - -/// Template to construct the product Lie group of two other Lie groups, G and H -/// Assumes manifold structure from G and H, and binary constructor -template -class ProductLieGroup: public std::pair, public LieGroup< - ProductLieGroup, traits::dimension + traits::dimension> { - BOOST_CONCEPT_ASSERT((IsLieGroup)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); - typedef std::pair Base; - -protected: - enum {dimension1 = traits::dimension}; - enum {dimension2 = traits::dimension}; - -public: - /// Default constructor yields identity - ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} - - // Construct from two subgroup elements - ProductLieGroup(const G& g, const H& h):Base(g,h) {} - - // Construct from base - ProductLieGroup(const Base& base):Base(base) {} - - /// @name Group - /// @{ - typedef multiplicative_group_tag group_flavor; - static ProductLieGroup identity() {return ProductLieGroup();} - - ProductLieGroup operator*(const ProductLieGroup& other) const { - return ProductLieGroup(traits::Compose(this->first,other.first), - traits::Compose(this->second,other.second)); - } - ProductLieGroup inverse() const { - return ProductLieGroup(this->first.inverse(), this->second.inverse()); - } - /// @} - - /// @name Manifold (but with derivatives) - /// @{ - enum {dimension = dimension1 + dimension2}; - inline static size_t Dim() {return dimension;} - inline size_t dim() const {return dimension;} - - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; - /// @} - - /// @name Lie Group - /// @{ - Eigen::Matrix AdjointMap() const { - Eigen::Matrix A; - A.setIdentity(); - throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); -// A.template topLeftCorner() = this->first.AdjointMap(); -// A.template bottomRightCorner() = this->second.AdjointMap(); - return A; - } - static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); - G g = traits::Expmap(v.template head()); - H h = traits::Expmap(v.template tail()); - return ProductLieGroup(g,h); - } - static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); - typename traits::TangentVector v1 = traits::Logmap(p.first); - typename traits::TangentVector v2 = traits::Logmap(p.second); - TangentVector v; - v << v1, v2; - return v; - } - struct ChartAtOrigin { - static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { - return Logmap(m, Hm); - } - static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { - return Expmap(v, Hv); - } - }; - using LieGroup::inverse; // with derivative - /// @} -}; - -// Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : internal::LieGroupTraits< - ProductLieGroup > { -}; -} #include #include -#undef CHECK #include using namespace std; From d060d4621e69ff2a6bac23d00bc3b41d3754e570 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 25 May 2015 23:50:00 -0700 Subject: [PATCH 29/32] PoseRTV is now implemented using ProductLieGroup --- gtsam_unstable/dynamics/PoseRTV.cpp | 103 ++++------------- gtsam_unstable/dynamics/PoseRTV.h | 105 +++++------------- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 11 +- 3 files changed, 53 insertions(+), 166 deletions(-) diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 1918008f3..942e1ab55 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -3,12 +3,9 @@ * @author Alex Cunningham */ -#include -#include - -#include - #include +#include +#include namespace gtsam { @@ -58,48 +55,6 @@ void PoseRTV::print(const string& s) const { velocity().print(" V"); } -/* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3(v.tail<3>()); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Vector6 Lx = Pose3::Logmap(p.pose()); - Vector3 Lv = p.velocity().vector(); - return (Vector9() << Lx, Lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } -PoseRTV PoseRTV::inverse(ChartJacobian H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(pose().inverse(), - velocity()); -} - -/* ************************************************************************* */ -PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(pose().compose(p.pose()), velocity()+p.velocity()); -} - -/* ************************************************************************* */ -PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } -PoseRTV PoseRTV::between(const PoseRTV& p, - ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); - return inverse().compose(p); -} - /* ************************************************************************* */ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const { @@ -210,54 +165,40 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub } /* ************************************************************************* */ -double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { - if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); - if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); - return t().distance(other.t()); + Matrix36 D_t1_pose, D_t2_other; + const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); + const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); + Matrix13 D_d_t1, D_d_t2; + double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; + if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; + return d; } /* ************************************************************************* */ -PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { - return global.transformed_from(transform); -} - PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, OptionalJacobian<9, 6> Dtrans) const { - // Note that we rotate the velocity - Matrix DVr, DTt; - Velocity3 newvel = trans.rotation().rotate(velocity(), DVr, DTt); - if (!Dglobal && !Dtrans) - return PoseRTV(trans.compose(pose()), newvel); // Pose3 transform is just compose - Matrix DTc, DGc; - Pose3 newpose = trans.compose(pose(), DTc, DGc); + Matrix6 D_newpose_trans, D_newpose_pose; + Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose); + + // Note that we rotate the velocity + Matrix3 D_newvel_R, D_newvel_v; + Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v); if (Dglobal) { - *Dglobal = zeros(9,9); - insertSub(*Dglobal, DGc, 0, 0); - - // Rotate velocity - insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix? + Dglobal->setZero(); + Dglobal->topLeftCorner<6,6>() = D_newpose_pose; + Dglobal->bottomRightCorner<3,3>() = D_newvel_v; } if (Dtrans) { - *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8); - // - // *Dtrans = zeros(9,6); - // // directly affecting the pose - // insertSub(*Dtrans, DTc, 0, 0); // correct in tests - // - // // rotating the velocity - // Matrix vRhat = skewSymmetric(-velocity().x(), -velocity().y(), -velocity().z()); - // trans.rotation().print("Transform rotation"); - // gtsam::print(vRhat, "vRhat"); - // gtsam::print(DVr, "DVr"); - // // FIXME: find analytic derivative - //// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I - //// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail + Dtrans->setZero(); + Dtrans->topLeftCorner<6,6>() = D_newpose_trans; + Dtrans->bottomLeftCorner<3,3>() = D_newvel_R; } return PoseRTV(newpose, newvel); } diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b2b9b8ece..78bd1fe6f 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -8,45 +8,10 @@ #include #include -#include +#include namespace gtsam { -/// CRTP to construct the product Lie group of two other Lie groups, G and H -/// Assumes manifold structure from G and H, and binary constructor -template -class ProductLieGroup: public ProductManifold { - BOOST_CONCEPT_ASSERT((IsLieGroup)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); - typedef ProductManifold Base; - -public: - enum {dimension = G::dimension + H::dimension}; - inline static size_t Dim() {return dimension;} - inline size_t dim() const {return dimension;} - - typedef Eigen::Matrix TangentVector; - - /// Default constructor yields identity - ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} - - // Construct from two subgroup elements - ProductLieGroup(const G& g, const H& h):Base(g,h) {} - - ProductLieGroup operator*(const Derived& other) const { - return Derived(traits::Compose(this->g(),other.g()), traits::Compose(this->h(),other.h())); - } - ProductLieGroup inverse() const { - return Derived(this->g().inverse(), this->h().inverse()); - } -}; - -// Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : internal::LieGroupTraits< - ProductLieGroup > { -}; - /// Syntactic sugar to clarify components typedef Point3 Velocity3; @@ -54,14 +19,13 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV : private ProductLieGroup { +class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup { protected: - typedef ProductLieGroup Base; + typedef ProductLieGroup Base; typedef OptionalJacobian<9, 9> ChartJacobian; public: - enum { dimension = 9 }; // constructors - with partial versions PoseRTV() {} @@ -76,6 +40,10 @@ public: explicit PoseRTV(const Pose3& pose) : Base(pose,Velocity3()) {} + // Construct from Base + PoseRTV(const Base& base) + : Base(base) {} + /** build from components - useful for data files */ PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); @@ -104,52 +72,26 @@ public: bool equals(const PoseRTV& other, double tol=1e-6) const; void print(const std::string& s="") const; - // Manifold - static size_t Dim() { return 9; } - size_t dim() const { return Dim(); } + /// @name Manifold + /// @{ + using Base::dimension; + using Base::dim; + using Base::Dim; + using Base::retract; + using Base::localCoordinates; + /// @} - /** - * retract/unretract assume independence of components - * Tangent space parameterization: - * - v(0-2): Rot3 (roll, pitch, yaw) - * - v(3-5): Point3 - * - v(6-8): Translational velocity - */ - PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const; - Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const; + /// @name measurement functions + /// @{ - // Lie TODO IS this a Lie group or just a Manifold???? - /** - * expmap/logmap are poor approximations that assume independence of components - * Currently implemented using the poor retract/unretract approximations - */ - static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none); - static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none); - - static PoseRTV identity() { return PoseRTV(); } - - /** Derivatives calculated numerically */ - PoseRTV inverse(ChartJacobian H1=boost::none) const; - - /** Derivatives calculated numerically */ - PoseRTV compose(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - PoseRTV operator*(const PoseRTV& p) const { return compose(p); } - - /** Derivatives calculated numerically */ - PoseRTV between(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - // measurement functions - /** Derivatives calculated numerically */ + /** range between translations */ double range(const PoseRTV& other, OptionalJacobian<1,9> H1=boost::none, OptionalJacobian<1,9> H2=boost::none) const; + /// @} - // IMU-specific + /// @name IMU-specific + /// @{ /// Dynamics integrator for ground robots /// Always move from time 1 to time 2 @@ -197,7 +139,9 @@ public: ChartJacobian Dglobal = boost::none, OptionalJacobian<9, 6> Dtrans = boost::none) const; - // Utility functions + /// @} + /// @name Utility functions + /// @{ /// RRTMbn - Function computes the rotation rate transformation matrix from /// body axis rates to euler angle (global) rates @@ -210,6 +154,7 @@ public: static Matrix RRTMnb(const Vector& euler); static Matrix RRTMnb(const Rot3& att); + /// @} private: /** Serialization function */ diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0261257be..d29af526e 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) { /* ************************************************************************* */ TEST( testPoseRTV, Lie ) { // origin and zero deltas - EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol)); + PoseRTV identity; + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol)); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished(); @@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) { Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); PoseRTV state2(pt2, rot2, vel2); - EXPECT(assert_equal(state2, state1.retract(delta), 1e-1)); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1)); EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation + EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1)); } /* ************************************************************************* */ From 69c9017b36c43dd886c527da1f8bb272dd1f6974 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 26 May 2015 01:03:13 -0700 Subject: [PATCH 30/32] Added identity --- gtsam/base/tests/testGroup.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index dadf2896c..241f71802 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -30,6 +30,7 @@ class Symmetric: private Eigen::PermutationMatrix { Eigen::PermutationMatrix(P) { } public: + static Symmetric identity() { return Symmetric(); } Symmetric() { Eigen::PermutationMatrix::setIdentity(); } From d385984f264956079aaa2ddcf7750bef73367290 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 26 May 2015 01:08:27 -0700 Subject: [PATCH 31/32] Working compose/between/inverse derivatives --- gtsam/base/ProductLieGroup.h | 112 +++++++++++++++--- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 37 ++++++ 2 files changed, 134 insertions(+), 15 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 2bcb49dba..90aeb54d1 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -23,11 +23,10 @@ namespace gtsam { -/// Template to construct the product Lie group of two other Lie groups, G and H -/// Assumes manifold structure from G and H, and binary constructor +/// Template to construct the product Lie group of two other Lie groups +/// Assumes Lie group structure for G and H template -class ProductLieGroup: public std::pair, public LieGroup< - ProductLieGroup, traits::dimension + traits::dimension> { +class ProductLieGroup: public std::pair { BOOST_CONCEPT_ASSERT((IsLieGroup)); BOOST_CONCEPT_ASSERT((IsLieGroup)); typedef std::pair Base; @@ -58,9 +57,15 @@ public: ProductLieGroup inverse() const { return ProductLieGroup(this->first.inverse(), this->second.inverse()); } + ProductLieGroup compose(const ProductLieGroup& g) const { + return (*this) * g; + } + ProductLieGroup between(const ProductLieGroup& g) const { + return this->inverse() * g; + } /// @} - /// @name Manifold (but with derivatives) + /// @name Manifold /// @{ enum {dimension = dimension1 + dimension2}; inline static size_t Dim() {return dimension;} @@ -68,26 +73,74 @@ public: typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; + + static ProductLieGroup Retract(const TangentVector& v) { + return ProductLieGroup::ChartAtOrigin::Retract(v); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g) { + return ProductLieGroup::ChartAtOrigin::Local(g); + } + ProductLieGroup retract(const TangentVector& v) const { + return compose(ProductLieGroup::ChartAtOrigin::Retract(v)); + } + TangentVector localCoordinates(const ProductLieGroup& g) const { + return ProductLieGroup::ChartAtOrigin::Local(between(g)); + } /// @} /// @name Lie Group /// @{ - Eigen::Matrix AdjointMap() const { - Eigen::Matrix A; - A.setIdentity(); - throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); - // A.template topLeftCorner() = this->first.AdjointMap(); - // A.template bottomRightCorner() = this->second.AdjointMap(); - return A; +protected: + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian1; + typedef Eigen::Matrix Jacobian2; + +public: + ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup inverse(ChartJacobian D) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : 0); + H h = traits::Inverse(this->second, D ? &D_h_second : 0); + if (D) { + D->setZero(); + D->template topLeftCorner() = D_g_first; + D->template bottomRightCorner() = D_h_second; + } + return ProductLieGroup(g,h); } static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); G g = traits::Expmap(v.template head()); H h = traits::Expmap(v.template tail()); return ProductLieGroup(g,h); } static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); typename traits::TangentVector v1 = traits::Logmap(p.first); typename traits::TangentVector v2 = traits::Logmap(p.second); TangentVector v; @@ -102,8 +155,37 @@ public: return Expmap(v, Hv); } }; - using LieGroup::inverse; // with derivative + ProductLieGroup expmap(const TangentVector& v) const { + return compose(ProductLieGroup::Expmap(v)); + } + TangentVector logmap(const ProductLieGroup& g) const { + return ProductLieGroup::Logmap(between(g)); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Retract(v,H1); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Local(g,H1); + } + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); + ProductLieGroup h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ProductLieGroup h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } /// @} + }; // Define any direct product group to be a model of the multiplicative Group concept diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index d29af526e..2ea582292 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -120,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) { } +/* ************************************************************************* */ +PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); } +TEST( testPoseRTV, compose ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); } +TEST( testPoseRTV, between ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); } +TEST( testPoseRTV, inverse ) { + PoseRTV state1(pt, rot, vel); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) { From 6a67ea86afdbe8b64f50dcd04a1b6c6efed1881b Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 26 May 2015 01:53:58 -0700 Subject: [PATCH 32/32] Made testLie a bit stronger --- tests/testLie.cpp | 77 ++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 62 insertions(+), 15 deletions(-) diff --git a/tests/testLie.cpp b/tests/testLie.cpp index f004dcc0f..7be629053 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -18,6 +18,7 @@ #include +#include #include #include @@ -26,35 +27,81 @@ using namespace std; using namespace gtsam; +static const double tol = 1e-9; + //****************************************************************************** -typedef ProductLieGroup MyPoint2Pair; +typedef ProductLieGroup Product; // Define any direct product group to be a model of the multiplicative Group concept namespace gtsam { -template<> struct traits : internal::LieGroupTraits { - static void Print(const MyPoint2Pair& m, const string& s = "") { - cout << s << "(" << m.first << "," << m.second << ")" << endl; +template<> struct traits : internal::LieGroupTraits { + static void Print(const Product& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second.translation() << "/" + << m.second.theta() << ")" << endl; } - static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, - double tol = 1e-8) { - return m1 == m2; + static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { + return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); } }; } +//****************************************************************************** TEST(Lie, ProductLieGroup) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); - MyPoint2Pair pair1; - Vector4 d; - d << 1, 2, 3, 4; - MyPoint2Pair expected(Point2(1, 2), Point2(3, 4)); - MyPoint2Pair pair2 = pair1.retract(d); + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + Product pair1; + Vector5 d; + d << 1, 2, 0.1, 0.2, 0.3; + Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); + Product pair2 = pair1.retract(d); EXPECT(assert_equal(expected, pair2, 1e-9)); EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); } +/* ************************************************************************* */ +Product compose_proxy(const Product& A, const Product& B) { + return A.compose(B); +} +TEST( testProduct, compose ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product between_proxy(const Product& A, const Product& B) { + return A.between(B); +} +TEST( testProduct, between ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product inverse_proxy(const Product& A) { + return A.inverse(); +} +TEST( testProduct, inverse ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + //****************************************************************************** int main() { TestResult tr;