VectorSpace.h
Go to the documentation of this file.
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 
32  static int GetDimension(const Class&) { return N;}
33 
34  static TangentVector Local(const Class& origin, const Class& other,
35  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
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 = {}, ChartJacobian H2 = {}) {
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 = {}) {
55  if (Hm) *Hm = Jacobian::Identity();
56  return m.vector();
57  }
58 
59  static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
60  if (Hv) *Hv = Jacobian::Identity();
61  return Class(v);
62  }
63 
64  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = {},
65  ChartJacobian H2 = {}) {
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 = {},
72  ChartJacobian H2 = {}) {
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 = {}) {
79  if (H) *H = - Jacobian::Identity();
80  return -v;
81  }
82 
84 };
85 
87 template<class Class>
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 = {}, ChartJacobian H2 = {}) {
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 = {}, ChartJacobian H2 = {}) {
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 = {}) {
129  if (Hm) *Hm = Eye(m);
130  return m.vector();
131  }
132 
133  static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
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 = {}) {
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 = {}) {
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 
170 
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
189 
191 
195  static Class Identity() { return Class::Identity();}
197 
200  enum { dimension = Class::dimension};
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 
235  enum { dimension = 1 };
238 
239  static TangentVector Local(Scalar origin, Scalar other,
240  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
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 = {}, ChartJacobian H2 = {}) {
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 = {}) {
259  if (H) (*H)[0] = 1.0;
260  return Local(0, m);
261  }
262 
263  static Scalar Expmap(const TangentVector& v, ChartJacobian H = {}) {
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 
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;
313 
314  static TangentVector Local(const Fixed& origin, const Fixed& other,
315  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
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 = {}, ChartJacobian H2 = {}) {
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 = {}) {
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 = {}) {
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 
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);
393  }
394 
395  static TangentVector Local(const Dynamic& m, const Dynamic& other, //
396  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
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 = {}, ChartJacobian H2 = {}) {
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 = {}) {
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 = {}) {
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 = {}) {
427  if (H) *H = -Eye(m);
428  return -m;
429  }
430 
431  static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
432  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
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 = {}, ChartJacobian H2 = {}) {
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 
474 
476  static_assert(
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 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
SCALAR Scalar
Definition: bench_gemm.cpp:46
static Class Between(const Class &v1, const Class &v2, ChartJacobian H1, ChartJacobian H2={})
Definition: VectorSpace.h:147
Requirements on type to pass it to Manifold template below.
Definition: VectorSpace.h:164
static Dynamic Retract(const Dynamic &m, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:404
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
static Class Inverse(const Class &v, ChartJacobian H)
Definition: VectorSpace.h:154
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
Vector v2
BOOST_CONCEPT_USAGE(IsVectorSpace)
Definition: VectorSpace.h:475
static bool Equals(const Dynamic &v1, const Dynamic &v2, double tol=1e-8)
Definition: VectorSpace.h:364
Vector v1
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
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
static int GetDimension(const Class &)
Definition: VectorSpace.h:32
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
Eigen::Matrix< double, N, N > Jacobian
Definition: VectorSpace.h:31
tag to assert a type is a Lie group
Definition: Lie.h:164
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Eigen::Matrix< double, M, N, Options, MaxRows, MaxCols > Dynamic
Definition: VectorSpace.h:357
static Dynamic Expmap(const TangentVector &, ChartJacobian H={})
Definition: VectorSpace.h:421
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
OptionalJacobian< N, N > ChartJacobian
Definition: VectorSpace.h:30
static Class Compose(const Class &v1, const Class &v2, ChartJacobian H1, ChartJacobian H2={})
Definition: VectorSpace.h:140
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::Matrix< double, M, N, Options, MaxRows, MaxCols > Fixed
Definition: VectorSpace.h:288
static bool Equals(Scalar v1, Scalar v2, double tol=1e-8)
Definition: VectorSpace.h:221
static TangentVector Logmap(const Class &m, ChartJacobian Hm={})
Definition: VectorSpace.h:128
static Dynamic Inverse(const Dynamic &m, ChartJacobian H={})
Definition: VectorSpace.h:426
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 set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
#define N
Definition: gksort.c:12
static TangentVector Local(const Dynamic &m, const Dynamic &other, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:395
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Vector Space concept.
Definition: VectorSpace.h:470
static TangentVector Logmap(const Fixed &m, ChartJacobian H={})
Definition: VectorSpace.h:333
static Class Compose(const Class &v1, const Class &v2, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:64
static Fixed Retract(const Fixed &origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:323
Eigen::Matrix< double, N, 1 > TangentVector
Definition: VectorSpace.h:29
static int GetDimension(const Dynamic &m)
Definition: VectorSpace.h:386
static Scalar Expmap(const TangentVector &v, ChartJacobian H={})
Definition: VectorSpace.h:263
static TangentVector Logmap(Scalar m, ChartJacobian H={})
Definition: VectorSpace.h:258
Eigen::VectorXd Vector
Definition: Vector.h:38
OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic > ChartJacobian
Definition: VectorSpace.h:100
Values result
static Class Between(const Class &v1, const Class &v2)
Definition: VectorSpace.h:93
traits< T >::structure_category structure_category_tag
Definition: VectorSpace.h:473
static Class Identity()
static TangentVector Logmap(const Class &m, ChartJacobian Hm={})
Definition: VectorSpace.h:54
static Class Between(const Class &v1, const Class &v2, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:71
Definition: pytypes.h:1403
static bool Equals(const Fixed &v1, const Fixed &v2, double tol=1e-8)
Definition: VectorSpace.h:295
Array< int, Dynamic, 1 > v
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:108
Array< double, 1, 3 > e(1./3., 0.5, 2.)
OptionalJacobian< dimension, dimension > ChartJacobian
Definition: VectorSpace.h:383
EIGEN_DEVICE_FUNC const Scalar & q
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:116
static Dynamic Compose(const Dynamic &v1, const Dynamic &v2, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:431
static void Print(Scalar m, const std::string &str="")
Definition: VectorSpace.h:218
Base class and basic functions for Lie types.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
additive_group_tag group_flavor
Definition: VectorSpace.h:228
VectorSpaceTraits Implementation for Fixed sizes.
Definition: VectorSpace.h:25
traits
Definition: chartTesting.h:28
tag to assert a type is a vector space
Definition: VectorSpace.h:16
Eigen::Matrix< double, 1, 1 > TangentVector
Definition: VectorSpace.h:236
vector_space_tag structure_category
Definition: VectorSpace.h:214
const Vector3 & vector() const
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:34
static Class Compose(const Class &v1, const Class &v2)
Definition: VectorSpace.h:92
static TangentVector Local(const Fixed &origin, const Fixed &other, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:314
static TangentVector Local(Scalar origin, Scalar other, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:239
static Class Expmap(const TangentVector &v, ChartJacobian Hv={})
Definition: VectorSpace.h:59
float * p
static Jacobian Eye(const Dynamic &m)
Definition: VectorSpace.h:390
OptionalJacobian< 1, 1 > ChartJacobian
Definition: VectorSpace.h:237
static Dynamic Between(const Dynamic &v1, const Dynamic &v2, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:438
additive_group_tag group_flavor
Definition: VectorSpace.h:372
static Class Expmap(const TangentVector &v, ChartJacobian Hv={})
Definition: VectorSpace.h:133
static void Print(const Dynamic &m, const std::string &str="")
Definition: VectorSpace.h:361
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs)
Definition: VectorSpace.h:171
const G double tol
Definition: Group.h:86
const int Dynamic
Definition: Constants.h:22
static Class Inverse(const Class &v, ChartJacobian H={})
Definition: VectorSpace.h:78
#define abs(x)
Definition: datatypes.h:17
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:42
vector_space_tag structure_category
Definition: VectorSpace.h:356
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
static Eigen::MatrixXd Eye(const Class &m)
Definition: VectorSpace.h:103
static Scalar Retract(Scalar origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
Definition: VectorSpace.h:248
static TangentVector Logmap(const Dynamic &m, ChartJacobian H={})
Definition: VectorSpace.h:414
static Fixed Expmap(const TangentVector &v, ChartJacobian H={})
Definition: VectorSpace.h:340
static void Print(const Fixed &m, const std::string &str="")
Definition: VectorSpace.h:292


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43