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 
24 #pragma once
25 
26 #include <gtsam/base/Manifold.h>
27 #include <gtsam/base/Group.h>
28 
29 namespace gtsam {
30 
36 template <class Class, int N>
37 struct LieGroup {
38 
39  enum { dimension = N };
43 
44  const Class & derived() const {
45  return static_cast<const Class&>(*this);
46  }
47 
48  Class compose(const Class& g) const {
49  return derived() * g;
50  }
51 
52  Class between(const Class& g) const {
53  return derived().inverse() * g;
54  }
55 
57  ChartJacobian H2 = {}) const {
58  if (H1) *H1 = g.inverse().AdjointMap();
60  return derived() * g;
61  }
62 
64  ChartJacobian H2 = {}) const {
65  Class result = derived().inverse() * g;
66  if (H1) *H1 = - result.inverse().AdjointMap();
68  return result;
69  }
70 
72  if (H) *H = - derived().AdjointMap();
73  return derived().inverse();
74  }
75 
78  Class expmap(const TangentVector& v) const {
79  return compose(Class::Expmap(v));
80  }
81 
84  TangentVector logmap(const Class& g) const {
85  return Class::Logmap(between(g));
86  }
87 
90  ChartJacobian H1, ChartJacobian H2 = {}) const {
91  Jacobian D_g_v;
92  Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
93  Class h = compose(g); // derivatives inlined below
94  if (H1) *H1 = g.inverse().AdjointMap();
95  if (H2) *H2 = D_g_v;
96  return h;
97  }
98 
101  ChartJacobian H1, ChartJacobian H2 = {}) const {
102  Class h = between(g); // derivatives inlined below
103  Jacobian D_v_h;
104  TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
105  if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
106  if (H2) *H2 = D_v_h;
107  return v;
108  }
109 
111  static Class Retract(const TangentVector& v) {
112  return Class::ChartAtOrigin::Retract(v);
113  }
114 
117  return Class::ChartAtOrigin::Local(g);
118  }
119 
122  return Class::ChartAtOrigin::Retract(v,H);
123  }
124 
127  return Class::ChartAtOrigin::Local(g,H);
128  }
129 
131  Class retract(const TangentVector& v) const {
132  return compose(Class::ChartAtOrigin::Retract(v));
133  }
134 
137  return Class::ChartAtOrigin::Local(between(g));
138  }
139 
142  ChartJacobian H1, ChartJacobian H2 = {}) const {
143  Jacobian D_g_v;
144  Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
145  Class h = compose(g); // derivatives inlined below
146  if (H1) *H1 = g.inverse().AdjointMap();
147  if (H2) *H2 = D_g_v;
148  return h;
149  }
150 
153  ChartJacobian H1, ChartJacobian H2 = {}) const {
154  Class h = between(g); // derivatives inlined below
155  Jacobian D_v_h;
156  TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
157  if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
158  if (H2) *H2 = D_v_h;
159  return v;
160  }
161 };
162 
164 struct lie_group_tag: public manifold_tag, public group_tag {};
165 
166 namespace internal {
167 
173 template<class Class>
174 struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
176 
180  static Class Identity() { return Class::Identity();}
182 
189 
190  static TangentVector Local(const Class& origin, const Class& other,
191  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
192  return origin.localCoordinates(other, Horigin, Hother);
193  }
194 
195  static Class Retract(const Class& origin, const TangentVector& v,
196  ChartJacobian Horigin = {}, ChartJacobian Hv = {}) {
197  return origin.retract(v, Horigin, Hv);
198  }
200 
203  static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) {
204  return Class::Logmap(m, Hm);
205  }
206 
207  static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
208  return Class::Expmap(v, Hv);
209  }
210 
211  static Class Compose(const Class& m1, const Class& m2, //
212  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
213  return m1.compose(m2, H1, H2);
214  }
215 
216  static Class Between(const Class& m1, const Class& m2, //
217  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
218  return m1.between(m2, H1, H2);
219  }
220 
221  static Class Inverse(const Class& m, //
222  ChartJacobian H = {}) {
223  return m.inverse(H);
224  }
226 };
227 
229 template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
230 
231 } // \ namepsace internal
232 
239 template<class Class>
240 inline Class between_default(const Class& l1, const Class& l2) {
241  return l1.inverse().compose(l2);
242 }
243 
245 template<class Class>
246 inline Vector logmap_default(const Class& l0, const Class& lp) {
247  return Class::Logmap(l0.between(lp));
248 }
249 
251 template<class Class>
252 inline Class expmap_default(const Class& t, const Vector& d) {
253  return t.compose(Class::Expmap(d));
254 }
255 
259 template<typename T>
260 class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
261 public:
266 
268  static_assert(
270  "This type's trait does not assert it is a Lie group (or derived)");
271 
272  // group opertations with Jacobians
273  g = traits<T>::Compose(g, h, Hg, Hh);
274  g = traits<T>::Between(g, h, Hg, Hh);
275  g = traits<T>::Inverse(g, Hg);
276  // log and exp map without Jacobians
277  g = traits<T>::Expmap(v);
278  v = traits<T>::Logmap(g);
279  // log and exponential map with Jacobians
280  g = traits<T>::Expmap(v, Hg);
281  v = traits<T>::Logmap(g, Hg);
282  }
283 private:
284  T g, h;
287 };
288 
296 template<class T>
298 T BCH(const T& X, const T& Y) {
299  static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
300  T X_Y = bracket(X, Y);
301  return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
302 }
303 
308 template <class T> Matrix wedge(const Vector& x);
309 
316 template <class T>
317 T expm(const Vector& x, int K=7) {
318  Matrix xhat = wedge<T>(x);
319  return T(expm(xhat,K));
320 }
321 
326 template <typename T>
327 T interpolate(const T& X, const T& Y, double t,
328  typename MakeOptionalJacobian<T, T>::type Hx = {},
329  typename MakeOptionalJacobian<T, T>::type Hy = {}) {
330  if (Hx || Hy) {
331  typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
332  const T between =
333  traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
334  typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
335  const T Delta = traits<T>::Expmap(t * delta, exp_H);
336  const T result = traits<T>::Compose(
337  X, Delta, compose_H_x); // compose_H_xinv_y = identity
338 
339  if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
340  if (Hy) *Hy = t * exp_H * log_H;
341  return result;
342  }
343  return traits<T>::Compose(
344  X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
345 }
346 
351 template<class T>
353 {
354 private:
355  typename T::Jacobian adjointMap_;
356 public:
357  explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {}
358  typename T::Jacobian operator()(const typename T::Jacobian &covariance)
359  { return adjointMap_ * covariance * adjointMap_.transpose(); }
360 };
361 
362 } // namespace gtsam
363 
372 #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
373 #define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
gtsam::wedge
Matrix wedge(const Vector &x)
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::TransformCovariance
Definition: Lie.h:352
gtsam::IsLieGroup::ManifoldType
traits< T >::ManifoldType ManifoldType
Definition: Lie.h:263
Y
const char Y
Definition: test/EulerAngles.cpp:31
gtsam::internal::LieGroupTraits::ChartJacobian
OptionalJacobian< dimension, dimension > ChartJacobian
Definition: Lie.h:188
gtsam::LieGroup::retract
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
retract with optional derivatives
Definition: Lie.h:141
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
d
static const double d[K][N]
Definition: igam.h:11
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
gtsam::LieGroup::Jacobian
Eigen::Matrix< double, N, N > Jacobian
Definition: Lie.h:41
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
expmap with optional derivatives
Definition: Lie.h:89
gtsam::LieGroup::ChartJacobian
OptionalJacobian< N, N > ChartJacobian
Definition: Lie.h:40
gtsam::IsLieGroup::structure_category_tag
traits< T >::structure_category structure_category_tag
Definition: Lie.h:262
gtsam::internal::LieGroupTraits::group_flavor
multiplicative_group_tag group_flavor
Definition: Lie.h:179
gtsam::expm
T expm(const Vector &x, int K=7)
Definition: Lie.h:317
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
x
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
Definition: gnuplot_common_settings.hh:12
Group.h
Concept check class for variable types with Group properties.
gtsam::TransformCovariance::TransformCovariance
TransformCovariance(const T &X)
Definition: Lie.h:357
l0
static Symbol l0('L', 0)
m1
Matrix3d m1
Definition: IOFormat.cpp:2
gtsam::internal::LieGroupTraits::Logmap
static TangentVector Logmap(const Class &m, ChartJacobian Hm={})
Definition: Lie.h:203
gtsam::internal::LieGroupTraits::ManifoldType
Class ManifoldType
Definition: Lie.h:185
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::IsLieGroup::ChartJacobian
traits< T >::ChartJacobian ChartJacobian
Definition: Lie.h:265
X
#define X
Definition: icosphere.cpp:20
gtsam::TransformCovariance::adjointMap_
T::Jacobian adjointMap_
Definition: Lie.h:355
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
logmap with optional derivatives
Definition: Lie.h:100
gtsam::between_default
Class between_default(const Class &l1, const Class &l2)
Definition: Lie.h:240
Class::dimension
@ dimension
Definition: testExpression.cpp:118
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::IsGroup
Definition: Group.h:42
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::BCH
T BCH(const T &X, const T &Y)
AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
Definition: Lie.h:298
gtsam::internal::LieGroupTraits::dimension
@ dimension
Definition: Lie.h:186
gtsam::logmap_default
Vector logmap_default(const Class &l0, const Class &lp)
Definition: Lie.h:246
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition: Group.h:33
bracket
AngularVelocity bracket(const AngularVelocity &X, const AngularVelocity &Y)
Definition: testRot3.cpp:337
gtsam::IsLieGroup::g
T g
Definition: Lie.h:284
m2
MatrixType m2(n_dims)
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
localCoordinates with optional derivatives
Definition: Lie.h:152
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:111
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition: Lie.h:126
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::internal::LieGroupTraits::Local
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian Horigin={}, ChartJacobian Hother={})
Definition: Lie.h:190
gtsam::LieGroup::TangentVector
Eigen::Matrix< double, N, 1 > TangentVector
Definition: Lie.h:42
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:72
gtsam::MakeOptionalJacobian::type
OptionalJacobian< traits< T >::dimension, traits< A >::dimension > type
Definition: OptionalJacobian.h:260
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:229
gtsam::IsLieGroup::v
TangentVector v
Definition: Lie.h:285
gtsam::IsLieGroup
Definition: Lie.h:260
gtsam::IsLieGroup::TangentVector
traits< T >::TangentVector TangentVector
Definition: Lie.h:264
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
Definition: Lie.h:327
gtsam::LieGroup::dimension
@ dimension
Definition: Lie.h:39
gtsam::IsLieGroup::Hh
ChartJacobian Hh
Definition: Lie.h:286
Manifold.h
Base class and basic functions for Manifold types.
gtsam::internal::LieGroupTraits
Definition: Lie.h:174
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::internal::LieGroupTraits::Expmap
static Class Expmap(const TangentVector &v, ChartJacobian Hv={})
Definition: Lie.h:207
Eigen::Triplet< double >
gtsam::IsLieGroup::h
T h
Definition: Lie.h:284
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:136
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g) const
Definition: Lie.h:84
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::MakeJacobian::type
Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > type
Definition: OptionalJacobian.h:248
gtsam::internal::LieGroupTraits::Between
static Class Between(const Class &m1, const Class &m2, ChartJacobian H1={}, ChartJacobian H2={})
Definition: Lie.h:216
gtsam::TransformCovariance::operator()
T::Jacobian operator()(const typename T::Jacobian &covariance)
Definition: Lie.h:358
gtsam::internal::LieGroupTraits::Identity
static Class Identity()
Definition: Lie.h:180
Between
BetweenFactor< Rot3 > Between
Definition: testRot3Optimization.cpp:31
gtsam::manifold_tag
tag to assert a type is a manifold
Definition: Manifold.h:30
gtsam::internal::LieGroupTraits::Retract
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian Horigin={}, ChartJacobian Hv={})
Definition: Lie.h:195
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition: Lie.h:121
gtsam
traits
Definition: chartTesting.h:28
gtsam::Testable
Definition: Testable.h:152
gtsam::internal::LieGroupTraits::structure_category
lie_group_tag structure_category
Definition: Lie.h:175
Class::Identity
static Class Identity()
Definition: testExpression.cpp:121
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::group_tag
tag to assert a type is a group
Definition: Group.h:30
Class
Definition: testExpression.cpp:116
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
origin
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
Definition: gnuplot_common_settings.hh:45
gtsam::internal::LieGroupTraits::Compose
static Class Compose(const Class &m1, const Class &m2, ChartJacobian H1={}, ChartJacobian H2={})
Definition: Lie.h:211
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition: Lie.h:164
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::LieGroup
Definition: Lie.h:37
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Eigen::Matrix< double, N, N >
gtsam::internal::LieGroupTraits::TangentVector
Eigen::Matrix< double, dimension, 1 > TangentVector
Definition: Lie.h:187
N
#define N
Definition: igam.h:9
internal
Definition: BandTriangularSolver.h:13
gtsam::LieGroup::derived
const Class & derived() const
Definition: Lie.h:44
gtsam::IsLieGroup::Hg
ChartJacobian Hg
Definition: Lie.h:286
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
Definition: Lie.h:78
gtsam::LieGroup::compose
Class compose(const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
Definition: Lie.h:56
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition: Lie.h:116
align_3::t
Point2 t(10, 10)
gtsam::internal::LieGroupTraits::Inverse
static Class Inverse(const Class &m, ChartJacobian H={})
Definition: Lie.h:221
gtsam::IsLieGroup::BOOST_CONCEPT_USAGE
BOOST_CONCEPT_USAGE(IsLieGroup)
Definition: Lie.h:267
gtsam::LieGroup::between
Class between(const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
Definition: Lie.h:63
test_callbacks.value
value
Definition: test_callbacks.py:158
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::LieGroup::inverse
Class inverse(ChartJacobian H) const
Definition: Lie.h:71
gtsam::expmap_default
Class expmap_default(const Class &t, const Vector &d)
Definition: Lie.h:252


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:03:00