Lie.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 
23 #pragma once
24 
25 #include <gtsam/base/Manifold.h>
26 #include <gtsam/base/Group.h>
27 
28 namespace gtsam {
29 
35 template <class Class, int N>
36 struct LieGroup {
37 
38  enum { dimension = N };
42 
43  const Class & derived() const {
44  return static_cast<const Class&>(*this);
45  }
46 
47  Class compose(const Class& g) const {
48  return derived() * g;
49  }
50 
51  Class between(const Class& g) const {
52  return derived().inverse() * g;
53  }
54 
55  Class compose(const Class& g, ChartJacobian H1,
56  ChartJacobian H2 = boost::none) const {
57  if (H1) *H1 = g.inverse().AdjointMap();
59  return derived() * g;
60  }
61 
62  Class between(const Class& g, ChartJacobian H1,
63  ChartJacobian H2 = boost::none) const {
64  Class result = derived().inverse() * g;
65  if (H1) *H1 = - result.inverse().AdjointMap();
67  return result;
68  }
69 
70  Class inverse(ChartJacobian H) const {
71  if (H) *H = - derived().AdjointMap();
72  return derived().inverse();
73  }
74 
77  Class expmap(const TangentVector& v) const {
78  return compose(Class::Expmap(v));
79  }
80 
83  TangentVector logmap(const Class& g) const {
84  return Class::Logmap(between(g));
85  }
86 
88  Class expmap(const TangentVector& v, //
89  ChartJacobian H1, ChartJacobian H2 = boost::none) const {
90  Jacobian D_g_v;
91  Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
92  Class h = compose(g); // derivatives inlined below
93  if (H1) *H1 = g.inverse().AdjointMap();
94  if (H2) *H2 = D_g_v;
95  return h;
96  }
97 
99  TangentVector logmap(const Class& g, //
100  ChartJacobian H1, ChartJacobian H2 = boost::none) const {
101  Class h = between(g); // derivatives inlined below
102  Jacobian D_v_h;
103  TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
104  if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
105  if (H2) *H2 = D_v_h;
106  return v;
107  }
108 
110  static Class Retract(const TangentVector& v) {
111  return Class::ChartAtOrigin::Retract(v);
112  }
113 
115  static TangentVector LocalCoordinates(const Class& g) {
116  return Class::ChartAtOrigin::Local(g);
117  }
118 
120  static Class Retract(const TangentVector& v, ChartJacobian H) {
121  return Class::ChartAtOrigin::Retract(v,H);
122  }
123 
125  static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
126  return Class::ChartAtOrigin::Local(g,H);
127  }
128 
130  Class retract(const TangentVector& v) const {
131  return compose(Class::ChartAtOrigin::Retract(v));
132  }
133 
135  TangentVector localCoordinates(const Class& g) const {
136  return Class::ChartAtOrigin::Local(between(g));
137  }
138 
140  Class retract(const TangentVector& v, //
141  ChartJacobian H1, ChartJacobian H2 = boost::none) const {
142  Jacobian D_g_v;
143  Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
144  Class h = compose(g); // derivatives inlined below
145  if (H1) *H1 = g.inverse().AdjointMap();
146  if (H2) *H2 = D_g_v;
147  return h;
148  }
149 
151  TangentVector localCoordinates(const Class& g, //
152  ChartJacobian H1, ChartJacobian H2 = boost::none) const {
153  Class h = between(g); // derivatives inlined below
154  Jacobian D_v_h;
155  TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
156  if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
157  if (H2) *H2 = D_v_h;
158  return v;
159  }
160 };
161 
163 struct lie_group_tag: public manifold_tag, public group_tag {};
164 
165 namespace internal {
166 
172 template<class Class>
173 struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
175 
179  static Class Identity() { return Class::identity();}
181 
188 
189  static TangentVector Local(const Class& origin, const Class& other,
190  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
191  return origin.localCoordinates(other, Horigin, Hother);
192  }
193 
194  static Class Retract(const Class& origin, const TangentVector& v,
195  ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
196  return origin.retract(v, Horigin, Hv);
197  }
199 
202  static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
203  return Class::Logmap(m, Hm);
204  }
205 
206  static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
207  return Class::Expmap(v, Hv);
208  }
209 
210  static Class Compose(const Class& m1, const Class& m2, //
211  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
212  return m1.compose(m2, H1, H2);
213  }
214 
215  static Class Between(const Class& m1, const Class& m2, //
216  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
217  return m1.between(m2, H1, H2);
218  }
219 
220  static Class Inverse(const Class& m, //
221  ChartJacobian H = boost::none) {
222  return m.inverse(H);
223  }
225 };
226 
228 template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
229 
230 } // \ namepsace internal
231 
238 template<class Class>
239 inline Class between_default(const Class& l1, const Class& l2) {
240  return l1.inverse().compose(l2);
241 }
242 
244 template<class Class>
245 inline Vector logmap_default(const Class& l0, const Class& lp) {
246  return Class::Logmap(l0.between(lp));
247 }
248 
250 template<class Class>
251 inline Class expmap_default(const Class& t, const Vector& d) {
252  return t.compose(Class::Expmap(d));
253 }
254 
258 template<typename T>
259 class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
260 public:
265 
267  BOOST_STATIC_ASSERT_MSG(
269  "This type's trait does not assert it is a Lie group (or derived)");
270 
271  // group opertations with Jacobians
272  g = traits<T>::Compose(g, h, Hg, Hh);
273  g = traits<T>::Between(g, h, Hg, Hh);
274  g = traits<T>::Inverse(g, Hg);
275  // log and exp map without Jacobians
276  g = traits<T>::Expmap(v);
277  v = traits<T>::Logmap(g);
278  // log and exponential map with Jacobians
279  g = traits<T>::Expmap(v, Hg);
280  v = traits<T>::Logmap(g, Hg);
281  }
282 private:
283  T g, h;
284  TangentVector v;
285  ChartJacobian Hg, Hh;
286 };
287 
295 template<class T>
297 T BCH(const T& X, const T& Y) {
298  static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
299  T X_Y = bracket(X, Y);
300  return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
301 }
302 
307 template <class T> Matrix wedge(const Vector& x);
308 
315 template <class T>
316 T expm(const Vector& x, int K=7) {
317  Matrix xhat = wedge<T>(x);
318  return T(expm(xhat,K));
319 }
320 
324 template <typename T>
325 T interpolate(const T& X, const T& Y, double t) {
326  assert(t >= 0 && t <= 1);
328 }
329 
334 template<class T>
336 {
337 private:
338  typename T::Jacobian adjointMap_;
339 public:
340  explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {}
341  typename T::Jacobian operator()(const typename T::Jacobian &covariance)
342  { return adjointMap_ * covariance * adjointMap_.transpose(); }
343 };
344 
345 } // namespace gtsam
346 
355 #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
356 #define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T;
T::Jacobian operator()(const typename T::Jacobian &covariance)
Definition: Lie.h:341
Matrix3f m
tag to assert a type is a manifold
Definition: Manifold.h:33
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition: Lie.h:151
OptionalJacobian< dimension, dimension > ChartJacobian
Definition: Lie.h:187
static Class Expmap(const TangentVector &v, ChartJacobian Hv=boost::none)
Definition: Lie.h:206
static Class identity()
AngularVelocity bracket(const AngularVelocity &X, const AngularVelocity &Y)
Definition: testRot3.cpp:326
Class between_default(const Class &l1, const Class &l2)
Definition: Lie.h:239
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
Concept check class for variable types with Group properties.
tag to assert a type is a Lie group
Definition: Lie.h:163
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none)
Definition: Lie.h:194
MatrixType m2(n_dims)
ArrayXcf v
Definition: Cwise_arg.cpp:1
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
gtsam::Key l2
ChartJacobian Hh
Definition: Lie.h:285
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Pose2_ Expmap(const Vector3_ &xi)
static Class Compose(const Class &m1, const Class &m2, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none)
Definition: Lie.h:210
T BCH(const T &X, const T &Y)
AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
Definition: Lie.h:297
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition: Lie.h:99
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition: Lie.h:140
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
Class compose(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
Definition: Lie.h:55
lie_group_tag structure_category
Definition: Lie.h:174
TransformCovariance(const T &X)
Definition: Lie.h:340
T expm(const Vector &x, int K=7)
Definition: Lie.h:316
void g(const string &key, int i)
Definition: testBTree.cpp:43
traits< T >::structure_category structure_category_tag
Definition: Lie.h:261
Class between(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
Definition: Lie.h:62
Class inverse(ChartJacobian H) const
Definition: Lie.h:70
T interpolate(const T &X, const T &Y, double t)
Definition: Lie.h:325
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:75
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition: Lie.h:120
Class compose(const Class &g) const
Definition: Lie.h:47
Matrix wedge(const Vector &x)
Eigen::VectorXd Vector
Definition: Vector.h:38
Eigen::Matrix< double, N, N > Jacobian
Definition: Lie.h:40
Values result
OptionalJacobian< N, N > ChartJacobian
Definition: Lie.h:39
Matrix3d m1
Definition: IOFormat.cpp:2
Class expmap(const TangentVector &v) const
Definition: Lie.h:77
Base class and basic functions for Manifold types.
Group operator syntax flavors.
Definition: Group.h:37
T::Jacobian adjointMap_
Definition: Lie.h:338
Eigen::Triplet< double > T
traits< T >::ManifoldType ManifoldType
Definition: Lie.h:262
BetweenFactor< Rot3 > Between
traits< T >::TangentVector TangentVector
Definition: Lie.h:263
Class expmap_default(const Class &t, const Vector &d)
Definition: Lie.h:251
multiplicative_group_tag group_flavor
Definition: Lie.h:178
gtsam::Key l1
TangentVector logmap(const Class &g) const
Definition: Lie.h:83
traits
Definition: chartTesting.h:28
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
const double h
BOOST_CONCEPT_USAGE(IsLieGroup)
Definition: Lie.h:266
Eigen::Matrix< double, N, 1 > TangentVector
Definition: Lie.h:41
Both LieGroupTraits and Testable.
Definition: Lie.h:228
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition: Lie.h:115
static Class Inverse(const Class &m, ChartJacobian H=boost::none)
Definition: Lie.h:220
Class between(const Class &g) const
Definition: Lie.h:51
static Class Identity()
Definition: Lie.h:179
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:110
static TangentVector Logmap(const Class &m, ChartJacobian Hm=boost::none)
Definition: Lie.h:202
const Class & derived() const
Definition: Lie.h:43
traits< T >::ChartJacobian ChartJacobian
Definition: Lie.h:264
static Class Between(const Class &m1, const Class &m2, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none)
Definition: Lie.h:215
tag to assert a type is a group
Definition: Group.h:34
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition: Lie.h:125
#define X
Definition: icosphere.cpp:20
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition: Lie.h:88
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 x
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none)
Definition: Lie.h:189
Eigen::Matrix< double, dimension, 1 > TangentVector
Definition: Lie.h:186
Vector logmap_default(const Class &l0, const Class &lp)
Definition: Lie.h:245
Point2 t(10, 10)
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:135
TangentVector v
Definition: Lie.h:284


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:31