gtsam  4.1.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>
76  // Compile-time dimensionality
77  static int GetDimension(const Class&) {
78  return N;
79  }
80 };
81 
83 template<class Class>
84 struct GetDimensionImpl<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: GetDimensionImpl<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 } // \ namespace gtsam
171 
173 // * Macros for using the ManifoldConcept
174 // * - An instantiation for use inside unit tests
175 // * - A typedef for use inside generic algorithms
176 // *
177 // * NOTE: intentionally not in the gtsam namespace to allow for classes not in
178 // * the gtsam namespace to be more easily enforced as testable
179 // */
180 #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
181 #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T;
Testable.h
Concept check for values that can be used in unit tests.
gtsam::OptionalJacobian< dim, dim >
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::IsTestable
Definition: Testable.h:57
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition: Manifold.h:95
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam::internal::HasManifoldPrereqs
Requirements on type to pass it to Manifold template below.
Definition: Manifold.h:59
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:33
gtsam::BOOST_CONCEPT_REQUIRES
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:75