141 lines
4.1 KiB
C++
141 lines
4.1 KiB
C++
/*
|
||
* Lie.h
|
||
*
|
||
* Created on: Jan 5, 2010
|
||
* Author: Richard Roberts
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#include <string>
|
||
#include "Matrix.h"
|
||
|
||
namespace gtsam {
|
||
|
||
template<class T>
|
||
T expmap(const Vector& v); /* Exponential map about identity */
|
||
|
||
// Syntactic sugar
|
||
template<class T>
|
||
inline T operator*(const T& l1, const T& l0) { return compose(l1, l0); }
|
||
|
||
|
||
// The following functions may be overridden in your own class file
|
||
// with more efficient versions if possible.
|
||
|
||
// Compute l1 s.t. l2=l1*l0
|
||
template<class T>
|
||
inline T between(const T& l1, const T& l2) { return inverse(l1)*l2; }
|
||
|
||
// Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp
|
||
template<class T>
|
||
inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); }
|
||
|
||
/* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
|
||
template<class T>
|
||
inline T expmap(const T& t, const Vector& d) { return t * expmap<T>(d); }
|
||
|
||
/**
|
||
* Base class for Lie group type
|
||
*/
|
||
template <class T>
|
||
class Lie {
|
||
public:
|
||
|
||
/**
|
||
* Returns dimensionality of the tangent space
|
||
*/
|
||
size_t dim() const;
|
||
|
||
/**
|
||
* Returns Exponential mapy
|
||
*/
|
||
T expmap(const Vector& v) const;
|
||
|
||
/**
|
||
* Returns Log map
|
||
*/
|
||
Vector logmap(const T& lp) const;
|
||
|
||
};
|
||
|
||
/** Call print on the object */
|
||
template<class T>
|
||
inline void print_(const T& object, const std::string& s = "") {
|
||
object.print(s);
|
||
}
|
||
|
||
/** Call equal on the object */
|
||
template<class T>
|
||
inline bool equal(const T& obj1, const T& obj2, double tol) {
|
||
return obj1.equals(obj2, tol);
|
||
}
|
||
|
||
/** Call equal on the object without tolerance (use default tolerance) */
|
||
template<class T>
|
||
inline bool equal(const T& obj1, const T& obj2) {
|
||
return obj1.equals(obj2);
|
||
}
|
||
|
||
// The rest of the file makes double and Vector behave as a Lie type (with + as compose)
|
||
|
||
// double,+ group operations
|
||
inline double compose(double p1,double p2) { return p1+p2;}
|
||
inline double inverse(double p) { return -p;}
|
||
inline double between(double p1,double p2) { return p2-p1;}
|
||
|
||
// double,+ is a trivial Lie group
|
||
template<> inline double expmap(const Vector& d) { return d(0);}
|
||
template<> inline double expmap(const double& p,const Vector& d) { return p+d(0);}
|
||
inline Vector logmap(const double& p) { return repeat(1,p);}
|
||
inline Vector logmap(const double& p1,const double& p2) { return Vector_(1,p2-p1);}
|
||
|
||
// Global functions needed for double
|
||
inline size_t dim(const double& v) { return 1; }
|
||
|
||
// Vector group operations
|
||
inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
|
||
inline Vector inverse(const Vector& p) { return -p;}
|
||
inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
|
||
|
||
// Vector is a trivial Lie group
|
||
template<> inline Vector expmap(const Vector& d) { return d;}
|
||
template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
|
||
inline Vector logmap(const Vector& p) { return p;}
|
||
inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
|
||
|
||
/**
|
||
* Three term approximation of the Baker<65>Campbell<6C>Hausdorff formula
|
||
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
|
||
* it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
|
||
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
|
||
* http://en.wikipedia.org/wiki/Baker<65>Campbell<6C>Hausdorff_formula
|
||
*/
|
||
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 X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
|
||
bracket(X, X_Y));
|
||
}
|
||
|
||
/**
|
||
* Declaration of wedge (see Murray94book) used to convert
|
||
* from n exponential coordinates to n*n element of the Lie algebra
|
||
*/
|
||
template <class T> Matrix wedge(const Vector& x);
|
||
|
||
/**
|
||
* Exponential map given exponential coordinates
|
||
* class T needs a wedge<> function and a constructor from Matrix
|
||
* @param x exponential coordinates, vector of size n
|
||
* @ return a T
|
||
*/
|
||
template <class T>
|
||
T expm(const Vector& x, int K=7) {
|
||
Matrix xhat = wedge<T>(x);
|
||
return expm(xhat,K);
|
||
}
|
||
|
||
} // namespace gtsam
|