Lie/Manifold documentation
Alex Cunningham
parent
83ccc6211e
commit
8cf2b84f5a
|
|
@ -2,8 +2,6 @@
|
|||
* @file Group.h
|
||||
*
|
||||
* @brief Concept check class for variable types with Group properties
|
||||
* A Group concept extends a Manifold
|
||||
*
|
||||
* @date Nov 5, 2011
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
|
@ -13,7 +11,8 @@
|
|||
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>
|
||||
class GroupConcept {
|
||||
|
|
|
|||
202
gtsam/base/Lie.h
202
gtsam/base/Lie.h
|
|
@ -14,28 +14,6 @@
|
|||
* @brief Base class and basic functions for Lie types
|
||||
* @author Richard Roberts
|
||||
* @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 {
|
||||
|
||||
/**
|
||||
* These core global functions can be specialized by new Lie types
|
||||
* for better performance.
|
||||
*/
|
||||
/**
|
||||
* These core global functions can be specialized by new Lie types
|
||||
* for better performance.
|
||||
*/
|
||||
|
||||
/** Compute l0 s.t. l2=l1*l0 */
|
||||
template<class T>
|
||||
inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);}
|
||||
/** Compute l0 s.t. l2=l1*l0 */
|
||||
template<class T>
|
||||
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 */
|
||||
template<class T>
|
||||
inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));}
|
||||
/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
|
||||
template<class T>
|
||||
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) */
|
||||
template<class T>
|
||||
inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
|
||||
/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
|
||||
template<class T>
|
||||
inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
|
||||
|
||||
/**
|
||||
* Concept check class for Lie group type
|
||||
*
|
||||
* T is the Lie type, like Point2, Pose3, etc.
|
||||
*
|
||||
* By convention, we use capital letters to designate a static function
|
||||
*/
|
||||
template <class T>
|
||||
class LieConcept {
|
||||
private:
|
||||
/** concept checking function - implement the functions this demands */
|
||||
static void concept_check(const T& t) {
|
||||
/**
|
||||
* Concept check class for Lie group type
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* 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 */
|
||||
T t2 = t;
|
||||
/** assignment */
|
||||
T t2 = t;
|
||||
|
||||
/**
|
||||
* Returns dimensionality of the tangent space
|
||||
*/
|
||||
size_t dim_ret = t.dim();
|
||||
/**
|
||||
* Returns dimensionality of the tangent space
|
||||
*/
|
||||
size_t dim_ret = t.dim();
|
||||
|
||||
/** expmap around identity */
|
||||
T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
|
||||
/** expmap around identity */
|
||||
T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
|
||||
|
||||
/** Logmap around identity */
|
||||
Vector logmap_identity_ret = T::Logmap(t);
|
||||
/** Logmap around identity */
|
||||
Vector logmap_identity_ret = T::Logmap(t);
|
||||
|
||||
/** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
|
||||
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));
|
||||
/** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
|
||||
T between_ret = t.between(t2);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
/**
|
||||
* 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
|
||||
* 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
|
||||
|
||||
|
|
@ -141,11 +145,11 @@ namespace gtsam {
|
|||
* the gtsam namespace to be more easily enforced as testable
|
||||
*/
|
||||
#define GTSAM_CONCEPT_LIE_INST(T) \
|
||||
template class gtsam::ManifoldConcept<T>; \
|
||||
template class gtsam::GroupConcept<T>; \
|
||||
template class gtsam::LieConcept<T>;
|
||||
template class gtsam::ManifoldConcept<T>; \
|
||||
template class gtsam::GroupConcept<T>; \
|
||||
template class gtsam::LieConcept<T>;
|
||||
|
||||
#define GTSAM_CONCEPT_LIE_TYPE(T) \
|
||||
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
|
||||
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
|
||||
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
|
||||
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
|
||||
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
|
||||
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
|
||||
|
|
|
|||
|
|
@ -12,23 +12,7 @@
|
|||
/**
|
||||
* @file Manifold.h
|
||||
* @brief Base class and basic functions for Manifold types
|
||||
* @author Richard Roberts
|
||||
* @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
|
||||
|
|
@ -38,40 +22,70 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Concept check class for Manifold types
|
||||
* Requires a mapping between a linear tangent space and the underlying
|
||||
* manifold, of which Lie is a specialization.
|
||||
*
|
||||
* T is the Manifold type, like Point2, Pose3, etc.
|
||||
*
|
||||
* By convention, we use capital letters to designate a static function
|
||||
*/
|
||||
template <class T>
|
||||
class ManifoldConcept {
|
||||
private:
|
||||
/** concept checking function - implement the functions this demands */
|
||||
static void concept_check(const T& t) {
|
||||
/**
|
||||
* Concept check class for Manifold types
|
||||
* Requires a mapping between a linear tangent space and the underlying
|
||||
* manifold, of which Lie is a specialization.
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* A manifold defines a space in which there is a notion of a linear tangent space
|
||||
* that can be centered around a given point on the manifold. These nonlinear
|
||||
* spaces may have such properties as wrapping around (as is the case with rotations),
|
||||
* 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 */
|
||||
T t2 = t;
|
||||
/** assignment */
|
||||
T t2 = t;
|
||||
|
||||
/**
|
||||
* Returns dimensionality of the tangent space
|
||||
*/
|
||||
size_t dim_ret = t.dim();
|
||||
/**
|
||||
* Returns dimensionality of the tangent space
|
||||
*/
|
||||
size_t dim_ret = t.dim();
|
||||
|
||||
/**
|
||||
* Returns Retraction update of T
|
||||
*/
|
||||
T retract_ret = t.retract(gtsam::zero(dim_ret));
|
||||
/**
|
||||
* Returns Retraction update of T
|
||||
*/
|
||||
T retract_ret = t.retract(gtsam::zero(dim_ret));
|
||||
|
||||
/**
|
||||
* Returns local coordinates of another object
|
||||
*/
|
||||
Vector localCoords_ret = t.localCoordinates(t2);
|
||||
}
|
||||
};
|
||||
/**
|
||||
* Returns local coordinates of another object
|
||||
*/
|
||||
Vector localCoords_ret = t.localCoordinates(t2);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue