gtsam  4.1.0
gtsam
VectorSpace.h
1 /*
2  * VectorSpace.h
3  *
4  * @date December 21, 2014
5  * @author Mike Bosse
6  * @author Frank Dellaert
7  */
8 
9 #pragma once
10 
11 #include <gtsam/base/Lie.h>
12 
13 namespace gtsam {
14 
17 };
18 
19 template<typename T> struct traits;
20 
21 namespace internal {
22 
24 template<class Class, int N>
26 
29  typedef Eigen::Matrix<double, N, 1> TangentVector;
31  typedef Eigen::Matrix<double, N, N> Jacobian;
32  static int GetDimension(const Class&) { return N;}
33 
34  static TangentVector Local(const Class& origin, const Class& other,
35  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
36  if (H1) *H1 = - Jacobian::Identity();
37  if (H2) *H2 = Jacobian::Identity();
38  Class v = other-origin;
39  return v.vector();
40  }
41 
42  static Class Retract(const Class& origin, const TangentVector& v,
43  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
44  if (H1) *H1 = Jacobian::Identity();
45  if (H2) *H2 = Jacobian::Identity();
46  return origin + v;
47  }
48 
50 
53 
54  static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
55  if (Hm) *Hm = Jacobian::Identity();
56  return m.vector();
57  }
58 
59  static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
60  if (Hv) *Hv = Jacobian::Identity();
61  return Class(v);
62  }
63 
64  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
65  ChartJacobian H2 = boost::none) {
66  if (H1) *H1 = Jacobian::Identity();
67  if (H2) *H2 = Jacobian::Identity();
68  return v1 + v2;
69  }
70 
71  static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
72  ChartJacobian H2 = boost::none) {
73  if (H1) *H1 = - Jacobian::Identity();
74  if (H2) *H2 = Jacobian::Identity();
75  return v2 - v1;
76  }
77 
78  static Class Inverse(const Class& v, ChartJacobian H = boost::none) {
79  if (H) *H = - Jacobian::Identity();
80  return -v;
81  }
82 
84 };
85 
87 template<class Class>
88 struct VectorSpaceImpl<Class,Eigen::Dynamic> {
89 
92  static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
93  static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
94  static Class Inverse(const Class& m) { return -m;}
96 
99  typedef Eigen::VectorXd TangentVector;
101  static int GetDimension(const Class& m) { return m.dim();}
102 
103  static Eigen::MatrixXd Eye(const Class& m) {
104  int dim = GetDimension(m);
105  return Eigen::MatrixXd::Identity(dim, dim);
106  }
107 
108  static TangentVector Local(const Class& origin, const Class& other,
109  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
110  if (H1) *H1 = - Eye(origin);
111  if (H2) *H2 = Eye(other);
112  Class v = other-origin;
113  return v.vector();
114  }
115 
116  static Class Retract(const Class& origin, const TangentVector& v,
117  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
118  if (H1) *H1 = Eye(origin);
119  if (H2) *H2 = Eye(origin);
120  return origin + v;
121  }
122 
124 
127 
128  static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
129  if (Hm) *Hm = Eye(m);
130  return m.vector();
131  }
132 
133  static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
134  Class result(v);
135  if (Hv)
136  *Hv = Eye(v);
137  return result;
138  }
139 
140  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
141  ChartJacobian H2 = boost::none) {
142  if (H1) *H1 = Eye(v1);
143  if (H2) *H2 = Eye(v2);
144  return v1 + v2;
145  }
146 
147  static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
148  ChartJacobian H2 = boost::none) {
149  if (H1) *H1 = - Eye(v1);
150  if (H2) *H2 = Eye(v2);
151  return v2 - v1;
152  }
153 
154  static Class Inverse(const Class& v, ChartJacobian H) {
155  if (H) *H = -Eye(v);
156  return -v;
157  }
158 
160 };
161 
163 template<class Class>
165 
166  enum { dim = Class::dimension };
167 
168  Class p, q;
169  Vector v;
170 
171  BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
172  p = Class::identity(); // identity
173  q = p + p; // addition
174  q = p - p; // subtraction
175  v = p.vector(); // conversion to vector
176  q = p + v; // addition of a vector on the right
177  }
178 };
179 
184 template<class Class>
185 struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
186 
187  // Check that Class has the necessary machinery
188  BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
189 
191 
195  static Class Identity() { return Class::identity();}
197 
200  enum { dimension = Class::dimension};
201  typedef Class ManifoldType;
203 };
204 
206 template<class Class>
207 struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
208 
211 template<typename Scalar>
212 struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
213 
215 
218  static void Print(Scalar m, const std::string& str = "") {
219  gtsam::print(m, str);
220  }
221  static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
222  return std::abs(v1 - v2) < tol;
223  }
225 
229  static Scalar Identity() { return 0;}
231 
234  typedef Scalar ManifoldType;
235  enum { dimension = 1 };
236  typedef Eigen::Matrix<double, 1, 1> TangentVector;
238 
239  static TangentVector Local(Scalar origin, Scalar other,
240  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
241  if (H1) (*H1)[0] = -1.0;
242  if (H2) (*H2)[0] = 1.0;
243  TangentVector result;
244  result(0) = other - origin;
245  return result;
246  }
247 
248  static Scalar Retract(Scalar origin, const TangentVector& v,
249  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
250  if (H1) (*H1)[0] = 1.0;
251  if (H2) (*H2)[0] = 1.0;
252  return origin + v[0];
253  }
255 
258  static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
259  if (H) (*H)[0] = 1.0;
260  return Local(0, m);
261  }
262 
263  static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
264  if (H) (*H)[0] = 1.0;
265  return v[0];
266  }
268 
269 };
270 
271 } // namespace internal
272 
274 template<> struct traits<double> : public internal::ScalarTraits<double> {
275 };
276 
278 template<> struct traits<float> : public internal::ScalarTraits<float> {
279 };
280 
281 // traits for any fixed double Eigen matrix
282 template<int M, int N, int Options, int MaxRows, int MaxCols>
283 struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
285  Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
286 
288  typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
289 
292  static void Print(const Fixed& m, const std::string& str = "") {
293  gtsam::print(Eigen::MatrixXd(m), str);
294  }
295  static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) {
296  return equal_with_abs_tol(v1, v2, tol);
297  }
299 
303  static Fixed Identity() { return Fixed::Zero();}
305 
308  enum { dimension = M*N};
309  typedef Fixed ManifoldType;
310  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
311  typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
313 
314  static TangentVector Local(const Fixed& origin, const Fixed& other,
315  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
316  if (H1) (*H1) = -Jacobian::Identity();
317  if (H2) (*H2) = Jacobian::Identity();
318  TangentVector result;
319  Eigen::Map<Fixed>(result.data()) = other - origin;
320  return result;
321  }
322 
323  static Fixed Retract(const Fixed& origin, const TangentVector& v,
324  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
325  if (H1) (*H1) = Jacobian::Identity();
326  if (H2) (*H2) = Jacobian::Identity();
327  return origin + Eigen::Map<const Fixed>(v.data());
328  }
330 
333  static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
334  if (H) *H = Jacobian::Identity();
335  TangentVector result;
336  Eigen::Map<Fixed>(result.data()) = m;
337  return result;
338  }
339 
340  static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
341  Fixed m;
342  m.setZero();
343  if (H) *H = Jacobian::Identity();
344  return m + Eigen::Map<const Fixed>(v.data());
345  }
347 };
348 
349 
350 namespace internal {
351 
352 // traits for dynamic Eigen matrices
353 template<int M, int N, int Options, int MaxRows, int MaxCols>
355 
357  typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Dynamic;
358 
361  static void Print(const Dynamic& m, const std::string& str = "") {
362  gtsam::print(Eigen::MatrixXd(m), str);
363  }
364  static bool Equals(const Dynamic& v1, const Dynamic& v2,
365  double tol = 1e-8) {
366  return equal_with_abs_tol(v1, v2, tol);
367  }
369 
373  static Dynamic Identity() {
374  throw std::runtime_error("Identity not defined for dynamic types");
375  }
377 
380  enum { dimension = Eigen::Dynamic };
381  typedef Eigen::VectorXd TangentVector;
382  typedef Eigen::MatrixXd Jacobian;
384  typedef Dynamic ManifoldType;
385 
386  static int GetDimension(const Dynamic& m) {
387  return m.rows() * m.cols();
388  }
389 
390  static Jacobian Eye(const Dynamic& m) {
391  int dim = GetDimension(m);
392  return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
393  }
394 
395  static TangentVector Local(const Dynamic& m, const Dynamic& other, //
396  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
397  if (H1) *H1 = -Eye(m);
398  if (H2) *H2 = Eye(m);
399  TangentVector v(GetDimension(m));
400  Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
401  return v;
402  }
403 
404  static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
405  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
406  if (H1) *H1 = Eye(m);
407  if (H2) *H2 = Eye(m);
408  return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
409  }
411 
414  static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) {
415  if (H) *H = Eye(m);
416  TangentVector result(GetDimension(m));
417  Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
418  return result;
419  }
420 
421  static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) {
422  static_cast<void>(H);
423  throw std::runtime_error("Expmap not defined for dynamic types");
424  }
425 
426  static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) {
427  if (H) *H = -Eye(m);
428  return -m;
429  }
430 
431  static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
432  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
433  if (H1) *H1 = Eye(v1);
434  if (H2) *H2 = Eye(v1);
435  return v1 + v2;
436  }
437 
438  static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
439  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
440  if (H1) *H1 = -Eye(v1);
441  if (H2) *H2 = Eye(v1);
442  return v2 - v1;
443  }
445 
446 };
447 
448 } // \ internal
449 
450 // traits for fully dynamic matrix
451 template<int Options, int MaxRows, int MaxCols>
452 struct traits<Eigen::Matrix<double, -1, -1, Options, MaxRows, MaxCols> > :
453  public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> {
454 };
455 
456 // traits for dynamic column vector
457 template<int Options, int MaxRows, int MaxCols>
458 struct traits<Eigen::Matrix<double, -1, 1, Options, MaxRows, MaxCols> > :
459  public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> {
460 };
461 
462 // traits for dynamic row vector
463 template<int Options, int MaxRows, int MaxCols>
464 struct traits<Eigen::Matrix<double, 1, -1, Options, MaxRows, MaxCols> > :
465  public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> {
466 };
467 
469 template<typename T>
470 class IsVectorSpace: public IsLieGroup<T> {
471 public:
472 
473  typedef typename traits<T>::structure_category structure_category_tag;
474 
475  BOOST_CONCEPT_USAGE(IsVectorSpace) {
476  BOOST_STATIC_ASSERT_MSG(
477  (boost::is_base_of<vector_space_tag, structure_category_tag>::value),
478  "This type's trait does not assert it as a vector space (or derived)");
479  r = p + q;
480  r = -p;
481  r = p - q;
482  }
483 
484 private:
485  T p, q, r;
486 };
487 
488 } // namespace gtsam
489 
gtsam::IsLieGroup
Lie Group Concept.
Definition: Lie.h:259
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition: Lie.h:163
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:84
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
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::internal::VectorSpaceImpl
VectorSpaceTraits Implementation for Fixed sizes.
Definition: VectorSpace.h:25
gtsam::additive_group_tag
Definition: Group.h:38
gtsam::internal::HasVectorSpacePrereqs
Requirements on type to pass it to Manifold template below.
Definition: VectorSpace.h:164
gtsam::internal::DynamicTraits
Definition: VectorSpace.h:354
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
gtsam::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition: OptionalJacobian.h:144
gtsam::internal::VectorSpaceTraits
A helper that implements the traits interface for classes that define vector spaces To use this for y...
Definition: VectorSpace.h:185
gtsam::vector_space_tag
tag to assert a type is a vector space
Definition: VectorSpace.h:16
gtsam::internal::ScalarTraits
A helper that implements the traits interface for scalar vector spaces.
Definition: VectorSpace.h:212
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
Definition: OptionalJacobian.h:161
Lie.h
Base class and basic functions for Lie types.