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 {