12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Lie/Manifold documentation

release/4.3a0
Alex Cunningham 2011-11-14 17:57:12 +00:00
parent 83ccc6211e
commit 8cf2b84f5a
3 changed files with 165 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/base/Group.h
View File

@ -2,8 +2,6 @@
* @file Group.h * @file Group.h
* *
* @brief Concept check class for variable types with Group properties * @brief Concept check class for variable types with Group properties
* A Group concept extends a Manifold
*
* @date Nov 5, 2011 * @date Nov 5, 2011
* @author Alex Cunningham * @author Alex Cunningham
*/ */
@ -13,7 +11,8 @@
namespace gtsam { namespace gtsam {
/** /**
* Concept check for general Group structure * This concept check enforces a Group structure on a variable type,
* in which we require the existence of basic algebraic operations.
*/ */
template<class T> template<class T>
class GroupConcept { class GroupConcept {

202
gtsam/base/Lie.h
View File

@ -14,28 +14,6 @@
* @brief Base class and basic functions for Lie types * @brief Base class and basic functions for Lie types
* @author Richard Roberts * @author Richard Roberts
* @author Alex Cunningham * @author Alex Cunningham
*
* This concept check provides a specialization on the Manifold type,
* in which the Manifolds represented require an algebra and group structure.
* All Lie types must also be a Manifold.
*
* The necessary functions to implement for Lie are defined
* below with additional details as to the interface. The
* concept checking function in class Lie will check whether or not
* the function exists and throw compile-time errors.
*
* Expmap around identity
* static T Expmap(const Vector& v);
*
*
* Logmap around identity
* static Vector Logmap(const T& p);
*
* Compute l0 s.t. l2=l1*l0, where (*this) is l1
* A default implementation of between(*this, lp) is available:
* between_default()
* T between(const T& l2) const;
*
*/ */
@ -46,89 +24,115 @@
namespace gtsam { namespace gtsam {
/** /**
* These core global functions can be specialized by new Lie types * These core global functions can be specialized by new Lie types
* for better performance. * for better performance.
*/ */
/** Compute l0 s.t. l2=l1*l0 */ /** Compute l0 s.t. l2=l1*l0 */
template<class T> template<class T>
inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);}
/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
template<class T> template<class T>
inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));}
/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
template<class T> template<class T>
inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
/** /**
* Concept check class for Lie group type * Concept check class for Lie group type
* *
* T is the Lie type, like Point2, Pose3, etc. * This concept check provides a specialization on the Manifold type,
* * in which the Manifolds represented require an algebra and group structure.
* By convention, we use capital letters to designate a static function * All Lie types must also be a Manifold.
*/ *
template <class T> * The necessary functions to implement for Lie are defined
class LieConcept { * below with additional details as to the interface. The
private: * concept checking function in class Lie will check whether or not
/** concept checking function - implement the functions this demands */ * the function exists and throw compile-time errors.
static void concept_check(const T& t) { *
* The exponential map is a specific retraction for Lie groups,
* which maps the tangent space in canonical coordinates back to
* the underlying manifold. Because we can enforce a group structure,
* a retraction of delta v, with tangent space centered at x1 can be performed
* as x2 = x1.compose(Expmap(v)).
*
* Expmap around identity
* static T Expmap(const Vector& v);
*
* Logmap around identity
* static Vector Logmap(const T& p);
*
* Compute l0 s.t. l2=l1*l0, where (*this) is l1
* A default implementation of between(*this, lp) is available:
* between_default()
* T between(const T& l2) const;
*
* By convention, we use capital letters to designate a static function
*
* @tparam T is a Lie type, like Point2, Pose3, etc.
*/
template <class T>
class LieConcept {
private:
/** concept checking function - implement the functions this demands */
static void concept_check(const T& t) {
/** assignment */ /** assignment */
T t2 = t; T t2 = t;
/** /**
* Returns dimensionality of the tangent space * Returns dimensionality of the tangent space
*/ */
size_t dim_ret = t.dim(); size_t dim_ret = t.dim();
/** expmap around identity */ /** expmap around identity */
T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
/** Logmap around identity */ /** Logmap around identity */
Vector logmap_identity_ret = T::Logmap(t); Vector logmap_identity_ret = T::Logmap(t);
/** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
T between_ret = t.between(t2); T between_ret = t.between(t2);
}
};
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>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
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
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 * Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
* class T needs a wedge<> function and a constructor from Matrix * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
* @param x exponential coordinates, vector of size n * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
* @ return a T * 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 expm(const Vector& x, int K=7) { /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
Matrix xhat = wedge<T>(x); template<class T>
return expm(xhat,K); 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 } // namespace gtsam
@ -141,11 +145,11 @@ namespace gtsam {
* the gtsam namespace to be more easily enforced as testable * the gtsam namespace to be more easily enforced as testable
*/ */
#define GTSAM_CONCEPT_LIE_INST(T) \ #define GTSAM_CONCEPT_LIE_INST(T) \
template class gtsam::ManifoldConcept<T>; \ template class gtsam::ManifoldConcept<T>; \
template class gtsam::GroupConcept<T>; \ template class gtsam::GroupConcept<T>; \
template class gtsam::LieConcept<T>; template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) \ #define GTSAM_CONCEPT_LIE_TYPE(T) \
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \ typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \ typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T; typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;

106
gtsam/base/Manifold.h
View File

@ -12,23 +12,7 @@
/** /**
* @file Manifold.h * @file Manifold.h
* @brief Base class and basic functions for Manifold types * @brief Base class and basic functions for Manifold types
* @author Richard Roberts
* @author Alex Cunningham * @author Alex Cunningham
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
*
* Returns dimensionality of the tangent space
* inline size_t dim() const;
*
* Returns Retraction update of T
* T retract(const Vector& v) const;
*
* Returns inverse retraction operation
* Vector localCoordinates(const T& lp) const;
*
*/ */
#pragma once #pragma once
@ -38,40 +22,70 @@
namespace gtsam { namespace gtsam {
/** /**
* Concept check class for Manifold types * Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying * Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization. * manifold, of which Lie is a specialization.
* *
* T is the Manifold type, like Point2, Pose3, etc. * The necessary functions to implement for Manifold are defined
* * below with additional details as to the interface. The
* By convention, we use capital letters to designate a static function * concept checking function in class Manifold will check whether or not
*/ * the function exists and throw compile-time errors.
template <class T> *
class ManifoldConcept { * A manifold defines a space in which there is a notion of a linear tangent space
private: * that can be centered around a given point on the manifold. These nonlinear
/** concept checking function - implement the functions this demands */ * spaces may have such properties as wrapping around (as is the case with rotations),
static void concept_check(const T& t) { * which might make linear operations on parameters not return a viable element of
* the manifold.
*
* We perform optimization by computing a linear delta in the tangent space of the
* current estimate, and then apply this change using a retraction operation, which
* maps the change in tangent space back to the manifold itself.
*
* There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be
* inverse operations.
*
* Returns dimensionality of the tangent space, which may be smaller than the number
* of nonlinear parameters.
* size_t dim() const;
*
* Returns a new T that is a result of updating *this with the delta v after pulling
* the updated value back to the manifold T.
* T retract(const Vector& v) const;
*
* Returns the linear coordinates of lp in the tangent space centered around *this.
* Vector localCoordinates(const T& lp) const;
*
* By convention, we use capital letters to designate a static function
* @tparam T is a Lie type, like Point2, Pose3, etc.
*/
template <class T>
class ManifoldConcept {
private:
/** concept checking function - implement the functions this demands */
static void concept_check(const T& t) {
/** assignment */ /** assignment */
T t2 = t; T t2 = t;
/** /**
* Returns dimensionality of the tangent space * Returns dimensionality of the tangent space
*/ */
size_t dim_ret = t.dim(); size_t dim_ret = t.dim();
/** /**
* Returns Retraction update of T * Returns Retraction update of T
*/ */
T retract_ret = t.retract(gtsam::zero(dim_ret)); T retract_ret = t.retract(gtsam::zero(dim_ret));
/** /**
* Returns local coordinates of another object * Returns local coordinates of another object
*/ */
Vector localCoords_ret = t.localCoordinates(t2); Vector localCoords_ret = t.localCoordinates(t2);
} }
}; };
} // namespace gtsam } // namespace gtsam