Go to the documentation of this file.
35 template <
class Class,
int N>
38 enum { dimension = N };
40 typedef Eigen::Matrix<double, N, N> Jacobian;
41 typedef Eigen::Matrix<double, N, 1> TangentVector;
43 const Class & derived()
const {
44 return static_cast<const Class&
>(*this);
47 Class compose(
const Class& g)
const {
51 Class between(
const Class& g)
const {
52 return derived().inverse() * g;
57 if (H1) *H1 = g.inverse().AdjointMap();
58 if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
64 Class result = derived().inverse() * g;
65 if (H1) *H1 = - result.inverse().AdjointMap();
66 if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
71 if (H) *H = - derived().AdjointMap();
72 return derived().inverse();
77 Class
expmap(
const TangentVector& v)
const {
78 return compose(Class::Expmap(v));
83 TangentVector
logmap(
const Class& g)
const {
84 return Class::Logmap(between(g));
91 Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
93 if (H1) *H1 = g.inverse().AdjointMap();
101 Class h = between(g);
103 TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
104 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
110 static Class
Retract(
const TangentVector& v) {
111 return Class::ChartAtOrigin::Retract(v);
116 return Class::ChartAtOrigin::Local(g);
121 return Class::ChartAtOrigin::Retract(v,H);
126 return Class::ChartAtOrigin::Local(g,H);
131 return compose(Class::ChartAtOrigin::Retract(v));
136 return Class::ChartAtOrigin::Local(between(g));
143 Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
144 Class h = compose(g);
145 if (H1) *H1 = g.inverse().AdjointMap();
153 Class h = between(g);
155 TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
156 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
172 template<
class Class>
179 static Class Identity() {
return Class::identity();}
184 typedef Class ManifoldType;
185 enum { dimension = Class::dimension };
186 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
189 static TangentVector Local(
const Class& origin,
const Class& other,
191 return origin.localCoordinates(other, Horigin, Hother);
194 static Class Retract(
const Class& origin,
const TangentVector& v,
196 return origin.retract(v, Horigin, Hv);
202 static TangentVector Logmap(
const Class& m,
ChartJacobian Hm = boost::none) {
203 return Class::Logmap(m, Hm);
206 static Class Expmap(
const TangentVector& v,
ChartJacobian Hv = boost::none) {
207 return Class::Expmap(v, Hv);
210 static Class Compose(
const Class& m1,
const Class& m2,
212 return m1.compose(m2, H1, H2);
215 static Class Between(
const Class& m1,
const Class& m2,
217 return m1.between(m2, H1, H2);
220 static Class Inverse(
const Class& m,
238 template<
class Class>
240 return l1.inverse().compose(l2);
244 template<
class Class>
246 return Class::Logmap(l0.between(lp));
250 template<
class Class>
252 return t.compose(Class::Expmap(d));
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)");
285 ChartJacobian Hg, Hh;
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)));
307 template <
class T> Matrix
wedge(
const Vector& x);
316 T
expm(
const Vector& x,
int K=7) {
317 Matrix xhat = wedge<T>(x);
318 return T(
expm(xhat,K));
324 template <
typename T>
326 assert(t >= 0 && t <= 1);
338 typename T::Jacobian adjointMap_;
341 typename T::Jacobian operator()(
const typename T::Jacobian &covariance)
342 {
return adjointMap_ * covariance * adjointMap_.transpose(); }
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;
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
Lie Group Concept.
Definition: Lie.h:259
tag to assert a type is a Lie group
Definition: Lie.h:163
Class expmap_default(const Class &t, const Vector &d)
Exponential map centered at l0, s.t.
Definition: Lie.h:251
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Both LieGroupTraits and Testable.
Definition: Lie.h:228
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition: Lie.h:140
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:110
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
Functor for transforming covariance of T.
Definition: Lie.h:336
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition: Lie.h:99
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition: Lie.h:151
Group operator syntax flavors.
Definition: Group.h:37
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:135
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition: Lie.h:115
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
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
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
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition: Lie.h:125
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
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
Vector logmap_default(const Class &l0, const Class &lp)
Log map centered at l0, s.t.
Definition: Lie.h:245
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition: Lie.h:120
Concept check class for variable types with Group properties.
tag to assert a type is a manifold
Definition: Manifold.h:33
Group Concept.
Definition: Group.h:46
A helper class that implements the traits interface for GTSAM lie groups.
Definition: Lie.h:173
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
Extra manifold traits for fixed-dimension types.
Definition: Manifold.h:75
Base class and basic functions for Manifold types.
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition: Lie.h:88
tag to assert a type is a group
Definition: Group.h:34