gtsam  4.0.0
gtsam
Manifold.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #pragma once
21 
22 #include <gtsam/base/Matrix.h>
23 #include <gtsam/base/Testable.h>
25 
26 #include <boost/concept_check.hpp>
27 #include <boost/concept/requires.hpp>
28 #include <boost/type_traits/is_base_of.hpp>
29 
30 namespace gtsam {
31 
33 struct manifold_tag {};
34 
53 template <typename T> struct traits;
54 
55 namespace internal {
56 
58 template<class Class>
60 
61  enum { dim = Class::dimension };
62 
63  Class p, q;
64  Eigen::Matrix<double, dim, 1> v;
65  OptionalJacobian<dim, dim> Hp, Hq, Hv;
66 
67  BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
68  v = p.localCoordinates(q);
69  q = p.retract(v);
70  }
71 };
72 
74 template<class Class, int N>
75 struct ManifoldImpl {
76  // Compile-time dimensionality
77  static int GetDimension(const Class&) {
78  return N;
79  }
80 };
81 
83 template<class Class>
84 struct ManifoldImpl<Class, Eigen::Dynamic> {
85  // Run-time dimensionality
86  static int GetDimension(const Class& m) {
87  return m.dim();
88  }
89 };
90 
94 template<class Class>
95 struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
96 
97  // Check that Class has the necessary machinery
98  BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
99 
100  // Dimension of the manifold
101  enum { dimension = Class::dimension };
102 
103  // Typedefs required by all manifold types.
104  typedef Class ManifoldType;
106  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
107 
108  // Local coordinates
109  static TangentVector Local(const Class& origin, const Class& other) {
110  return origin.localCoordinates(other);
111  }
112 
113  // Retraction back to manifold
114  static Class Retract(const Class& origin, const TangentVector& v) {
115  return origin.retract(v);
116  }
117 };
118 
120 template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
121 
122 } // \ namespace internal
123 
125 template<typename T>
127 check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
128  typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
129  typename traits<T>::TangentVector v = traits<T>::Local(a,b);
130  T c = traits<T>::Retract(a,v);
131  return v0.norm() < tol && traits<T>::Equals(b,c,tol);
132 }
133 
135 template<typename T>
136 class IsManifold {
137 
138 public:
139 
140  typedef typename traits<T>::structure_category structure_category_tag;
141  static const int dim = traits<T>::dimension;
142  typedef typename traits<T>::ManifoldType ManifoldType;
143  typedef typename traits<T>::TangentVector TangentVector;
144 
145  BOOST_CONCEPT_USAGE(IsManifold) {
146  BOOST_STATIC_ASSERT_MSG(
147  (boost::is_base_of<manifold_tag, structure_category_tag>::value),
148  "This type's structure_category trait does not assert it as a manifold (or derived)");
149  BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
150 
151  // make sure Chart methods are defined
152  v = traits<T>::Local(p, q);
153  q = traits<T>::Retract(p, v);
154  }
155 
156 private:
157 
158  TangentVector v;
159  ManifoldType p, q;
160 };
161 
163 template<typename T>
165  typedef const int value_type;
166  static const int value = traits<T>::dimension;
167  BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
168  "FixedDimension instantiated for dymanically-sized type.");
169 };
170 
171 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
172 template<typename M1, typename M2>
175 class ProductManifold: public std::pair<M1, M2> {
176  BOOST_CONCEPT_ASSERT((IsManifold<M1>));
177  BOOST_CONCEPT_ASSERT((IsManifold<M2>));
178 
179 protected:
180  enum { dimension1 = traits<M1>::dimension };
181  enum { dimension2 = traits<M2>::dimension };
182 
183 public:
184  enum { dimension = dimension1 + dimension2 };
185  inline static size_t Dim() { return dimension;}
186  inline size_t dim() const { return dimension;}
187 
188  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
189  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
190 
192  ProductManifold():std::pair<M1,M2>(M1(),M2()) {}
193 
194  // Construct from two original manifold values
195  ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
196 
198  ProductManifold retract(const TangentVector& xi) const {
199  M1 m1 = traits<M1>::Retract(this->first, xi.template head<dimension1>());
200  M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
201  return ProductManifold(m1,m2);
202  }
203 
205  TangentVector localCoordinates(const ProductManifold& other) const {
206  typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first, other.first);
207  typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
208  TangentVector v;
209  v << v1, v2;
210  return v;
211  }
212 
213  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
214  enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
215  };
216 public:
217  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
218 };
219 
220 // Define any direct product group to be a model of the multiplicative Group concept
221 template<typename M1, typename M2>
222 struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
223 };
224 #endif
225 
226 } // \ namespace gtsam
227 
229 // * Macros for using the ManifoldConcept
230 // * - An instantiation for use inside unit tests
231 // * - A typedef for use inside generic algorithms
232 // *
233 // * NOTE: intentionally not in the gtsam namespace to allow for classes not in
234 // * the gtsam namespace to be more easily enforced as testable
235 // */
236 #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
237 #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T;
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:75
A helper that implements the traits interface for GTSAM manifolds.
Definition: Manifold.h:95
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
Requirements on type to pass it to Manifold template below.
Definition: Manifold.h:59
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
tag to assert a type is a manifold
Definition: Manifold.h:33
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Definition: Testable.h:57
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Concept check for values that can be used in unit tests.
typedef and functions to augment Eigen's MatrixXd
Special class for optional Jacobian arguments.