Loading...
Searching...
No Matches
Lie.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Group.h>
namespace gtsam {
template <class Class, int N>
struct LieGroup {
enum { dimension = N };
typedef OptionalJacobian<N, N> ChartJacobian;
typedef Eigen::Matrix<double, N, N> Jacobian;
typedef Eigen::Matrix<double, N, 1> TangentVector;
const Class & derived() const {
return static_cast<const Class&>(*this);
}
Class compose(const Class& g) const {
return derived() * g;
}
Class between(const Class& g) const {
return derived().inverse() * g;
}
Class compose(const Class& g, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
if (H1) *H1 = g.inverse().AdjointMap();
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
return derived() * g;
}
Class between(const Class& g, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Class result = derived().inverse() * g;
if (H1) *H1 = - result.inverse().AdjointMap();
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
return result;
}
Class inverse(ChartJacobian H) const {
if (H) *H = - derived().AdjointMap();
return derived().inverse();
}
Class expmap(const TangentVector& v) const {
return compose(Class::Expmap(v));
}
TangentVector logmap(const Class& g) const {
return Class::Logmap(between(g));
}
Class expmap(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below
if (H1) *H1 = g.inverse().AdjointMap();
if (H2) *H2 = D_g_v;
return h;
}
TangentVector logmap(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Class h = between(g); // derivatives inlined below
Jacobian D_v_h;
TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
if (H2) *H2 = D_v_h;
return v;
}
static Class Retract(const TangentVector& v) {
return Class::ChartAtOrigin::Retract(v);
}
static TangentVector LocalCoordinates(const Class& g) {
return Class::ChartAtOrigin::Local(g);
}
static Class Retract(const TangentVector& v, ChartJacobian H) {
return Class::ChartAtOrigin::Retract(v,H);
}
static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
return Class::ChartAtOrigin::Local(g,H);
}
Class retract(const TangentVector& v) const {
return compose(Class::ChartAtOrigin::Retract(v));
}
TangentVector localCoordinates(const Class& g) const {
return Class::ChartAtOrigin::Local(between(g));
}
Class retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below
if (H1) *H1 = g.inverse().AdjointMap();
if (H2) *H2 = D_g_v;
return h;
}
TangentVector localCoordinates(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Class h = between(g); // derivatives inlined below
Jacobian D_v_h;
TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
if (H2) *H2 = D_v_h;
return v;
}
};
struct lie_group_tag: public manifold_tag, public group_tag {};
namespace internal {
template<class Class>
struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
typedef lie_group_tag structure_category;
typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::Identity();}
typedef Class ManifoldType;
enum { dimension = Class::dimension };
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother);
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return origin.retract(v, Horigin, Hv);
}
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
return Class::Logmap(m, Hm);
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
return Class::Expmap(v, Hv);
}
static Class Compose(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
return m1.compose(m2, H1, H2);
}
static Class Between(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
return m1.between(m2, H1, H2);
}
static Class Inverse(const Class& m, //
ChartJacobian H = boost::none) {
return m.inverse(H);
}
};
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
} // \ namepsace internal
template<class Class>
inline Class between_default(const Class& l1, const Class& l2) {
return l1.inverse().compose(l2);
}
template<class Class>
inline Vector logmap_default(const Class& l0, const Class& lp) {
return Class::Logmap(l0.between(lp));
}
template<class Class>
inline Class expmap_default(const Class& t, const Vector& d) {
return t.compose(Class::Expmap(d));
}
template<typename T>
class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
public:
typedef typename traits<T>::structure_category structure_category_tag;
typedef typename traits<T>::ManifoldType ManifoldType;
typedef typename traits<T>::TangentVector TangentVector;
typedef typename traits<T>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsLieGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)");
// group opertations with Jacobians
g = traits<T>::Compose(g, h, Hg, Hh);
g = traits<T>::Between(g, h, Hg, Hh);
g = traits<T>::Inverse(g, Hg);
// log and exp map without Jacobians
g = traits<T>::Expmap(v);
v = traits<T>::Logmap(g);
// log and exponential map with Jacobians
g = traits<T>::Expmap(v, Hg);
v = traits<T>::Logmap(g, Hg);
}
private:
T g, h;
TangentVector v;
ChartJacobian Hg, Hh;
};
template<class T>
T BCH(const T& X, const T& Y) {
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
T X_Y = bracket(X, Y);
return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
}
template <class T> Matrix wedge(const Vector& x);
template <class T>
T expm(const Vector& x, int K=7) {
Matrix xhat = wedge<T>(x);
return T(expm(xhat,K));
}
template <typename T>
T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
if (Hx || Hy) {
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
const T between =
traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
const T Delta = traits<T>::Expmap(t * delta, exp_H);
const T result = traits<T>::Compose(
X, Delta, compose_H_x); // compose_H_xinv_y = identity
if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
if (Hy) *Hy = t * exp_H * log_H;
return result;
}
return traits<T>::Compose(
X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}
template<class T>
class TransformCovariance
{
private:
typename T::Jacobian adjointMap_;
public:
explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {}
typename T::Jacobian operator()(const typename T::Jacobian &covariance)
{ return adjointMap_ * covariance * adjointMap_.transpose(); }
};
} // namespace gtsam
#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;
Group.h
Concept check class for variable types with Group properties.
Manifold.h
Base class and basic functions for Manifold types.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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:317
gtsam::logmap_default
Vector logmap_default(const Class &l0, const Class &lp)
Log map centered at l0, s.t.
Definition Lie.h:246
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:240
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type Hy=boost::none)
Linear interpolation between X and Y by coefficient t.
Definition Lie.h:327
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:298
gtsam::expmap_default
Class expmap_default(const Class &t, const Vector &d)
Exponential map centered at l0, s.t.
Definition Lie.h:252
gtsam::wedge
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
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::group_tag
tag to assert a type is a group
Definition Group.h:34
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
gtsam::IsGroup
Group Concept.
Definition Group.h:46
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition Lie.h:121
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::expmap
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition Lie.h:89
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition Lie.h:152
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:84
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition Lie.h:100
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
gtsam::LieGroup::retract
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition Lie.h:141
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition Lie.h:126
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition Lie.h:131
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:78
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::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
gtsam::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151