gtsam  4.1.0
gtsam
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 };
40  typedef Eigen::Matrix<double, N, N> Jacobian;
41  typedef Eigen::Matrix<double, N, 1> TangentVector;
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();
58  if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
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();
66  if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
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 
184  typedef Class ManifoldType;
185  enum { dimension = Class::dimension };
186  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
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:
261  typedef typename traits<T>::structure_category structure_category_tag;
262  typedef typename traits<T>::ManifoldType ManifoldType;
263  typedef typename traits<T>::TangentVector TangentVector;
264  typedef typename traits<T>::ChartJacobian ChartJacobian;
265 
266  BOOST_CONCEPT_USAGE(IsLieGroup) {
267  BOOST_STATIC_ASSERT_MSG(
268  (boost::is_base_of<lie_group_tag, structure_category_tag>::value),
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;
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
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::expmap_default
Class expmap_default(const Class &t, const Vector &d)
Exponential map centered at l0, s.t.
Definition: Lie.h:251
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::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:228
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::wedge
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
gtsam::LieGroup::retract
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition: Lie.h:140
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:110
gtsam::between_default
Class between_default(const Class &l1, const Class &l2)
These core global functions can be specialized by new Lie types for better performance.
Definition: Lie.h:239
gtsam::TransformCovariance
Functor for transforming covariance of T.
Definition: Lie.h:336
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition: Lie.h:99
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition: Lie.h:151
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition: Group.h:37
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:135
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition: Lie.h:115
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g) const
logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
Definition: Lie.h:83
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t)
Linear interpolation between X and Y by coefficient t in [0, 1].
Definition: Lie.h:325
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition: Lie.h:125
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition: Lie.h:77
gtsam::expm
T expm(const Vector &x, int K=7)
Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from...
Definition: Lie.h:316
gtsam::logmap_default
Vector logmap_default(const Class &l0, const Class &lp)
Log map centered at l0, s.t.
Definition: Lie.h:245
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition: Lie.h:120
Group.h
Concept check class for variable types with Group properties.
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:33
gtsam::IsGroup
Group Concept.
Definition: Group.h:46
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition: Lie.h:173
gtsam::BCH
T BCH(const T &X, const T &Y)
Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups,...
Definition: Lie.h:297
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:75
Manifold.h
Base class and basic functions for Manifold types.
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition: Lie.h:88
gtsam::group_tag
tag to assert a type is a group
Definition: Group.h:34