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 /// @{