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 #include <gtsam/base/concepts.h>
26 
27 namespace gtsam {
28 
30 struct manifold_tag {};
31 
50 template <typename T> struct traits;
51 
52 namespace internal {
53 
55 template<class Class>
57 
58  enum { dim = Class::dimension };
59 
60  Class p, q;
63 
65  v = p.localCoordinates(q);
66  q = p.retract(v);
67  }
68 };
69 
71 template<class Class, int N>
73  // Compile-time dimensionality
74  static int GetDimension(const Class&) {
75  return N;
76  }
77 };
78 
80 template<class Class>
82  // Run-time dimensionality
83  static int GetDimension(const Class& m) {
84  return m.dim();
85  }
86 };
87 
91 template<class Class>
92 struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
93 
94  // Check that Class has the necessary machinery
96 
97  // Dimension of the manifold
98  enum { dimension = Class::dimension };
99 
100  // Typedefs required by all manifold types.
104 
105  // Local coordinates
106  static TangentVector Local(const Class& origin, const Class& other) {
107  return origin.localCoordinates(other);
108  }
109 
110  // Retraction back to manifold
111  static Class Retract(const Class& origin, const TangentVector& v) {
112  return origin.retract(v);
113  }
114 };
115 
117 template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
118 
119 } // \ namespace internal
120 
122 template<typename T>
124 check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
127  T c = traits<T>::Retract(a,v);
128  return v0.norm() < tol && traits<T>::Equals(b,c,tol);
129 }
130 
132 template<typename T>
133 class IsManifold {
134 
135 public:
136 
137  typedef typename traits<T>::structure_category structure_category_tag;
138  static const int dim = traits<T>::dimension;
139  typedef typename traits<T>::ManifoldType ManifoldType;
140  typedef typename traits<T>::TangentVector TangentVector;
141 
142  BOOST_CONCEPT_USAGE(IsManifold) {
143  static_assert(
145  "This type's structure_category trait does not assert it as a manifold (or derived)");
146  static_assert(TangentVector::SizeAtCompileTime == dim);
147 
148  // make sure Chart methods are defined
149  v = traits<T>::Local(p, q);
150  q = traits<T>::Retract(p, v);
151  }
152 
153 private:
154 
155  TangentVector v;
156  ManifoldType p, q;
157 };
158 
160 template<typename T>
162  typedef const int value_type;
163  static const int value = traits<T>::dimension;
164  static_assert(value != Eigen::Dynamic,
165  "FixedDimension instantiated for dymanically-sized type.");
166 };
167 } // \ namespace gtsam
168 
170 // * Macros for using the ManifoldConcept
171 // * - An instantiation for use inside unit tests
172 // * - A typedef for use inside generic algorithms
173 // *
174 // * NOTE: intentionally not in the gtsam namespace to allow for classes not in
175 // * the gtsam namespace to be more easily enforced as testable
176 // */
177 #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
178 #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;
Matrix3f m
tag to assert a type is a manifold
Definition: Manifold.h:30
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Eigen::Matrix< double, dimension, 1 > TangentVector
Definition: Manifold.h:103
Requirements on type to pass it to Manifold template below.
Definition: Manifold.h:56
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
Concept check for values that can be used in unit tests.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Eigen::Matrix< double, dim, 1 > v
Definition: Manifold.h:61
const int value_type
Definition: Manifold.h:162
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
#define N
Definition: gksort.c:12
#define BOOST_CONCEPT_USAGE(concept)
Definition: base/concepts.h:20
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:72
BOOST_CONCEPT_USAGE(HasManifoldPrereqs)
Definition: Manifold.h:64
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EIGEN_DEVICE_FUNC const Scalar & q
const G & b
Definition: Group.h:86
static int GetDimension(const Class &)
Definition: Manifold.h:74
traits
Definition: chartTesting.h:28
OptionalJacobian< dim, dim > Hv
Definition: Manifold.h:62
static const double v0
float * p
static Class Retract(const Class &origin, const TangentVector &v)
Definition: Manifold.h:111
GTSAM_CONCEPT_REQUIRES(IsGroup< G >, bool) check_group_invariants(const G &a
Check invariants.
static TangentVector Local(const Class &origin, const Class &other)
Definition: Manifold.h:106
Special class for optional Jacobian arguments.
const G double tol
Definition: Group.h:86
const int Dynamic
Definition: Constants.h:22


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:35