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

concept documentation cleanup

release/4.3a0
Chris Beall 2010-10-21 20:52:34 +00:00
parent 48be990ef1
commit 0245ab06d2
2 changed files with 155 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

283
base/Lie.h
View File

@ -14,8 +14,45 @@
* @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
*
* 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.
*
* Returns dimensionality of the tangent space
* inline size_t dim() const;
*
* Returns Exponential map update of T
* A default implementation of expmap(*this, lp) is available:
* expmap_default()
* T expmap(const Vector& v) const;
*
* expmap around identity
* static T Expmap(const Vector& v);
*
* Returns Log map
* A default implementation of logmap(*this, lp) is available:
* logmap_default()
* Vector logmap(const T& lp) const;
*
* 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;
*
* compose with another object
* T compose(const T& p) const;
*
* invert the object and yield a new one
* T inverse() const;
*
*/ */
#pragma once #pragma once
#include <string> #include <string>
@ -24,172 +61,126 @@
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));}
/** /**
* Base class for Lie group type * Base class for Lie group type
* This class uses the Curiously Recurring Template design pattern to allow for * This class uses the Curiously Recurring Template design pattern to allow for
* concept checking using a private function. * concept checking using a private function.
* *
* T is the derived Lie type, like Point2, Pose3, etc. * T is the derived Lie type, like Point2, Pose3, etc.
* *
* By convention, we use capital letters to designate a static function * By convention, we use capital letters to designate a static function
*/ */
template <class T> template <class T>
class Lie { class Lie {
private: private:
/** concept checking function - implement the functions this demands */ /** concept checking function - implement the functions this demands */
static void concept_check(const T& t) { 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 Exponential map update of T * Returns Exponential map update of T
* Default implementation calls global binary function * Default implementation calls global binary function
*/ */
T expmap_ret = t.expmap(gtsam::zero(dim_ret)); T expmap_ret = t.expmap(gtsam::zero(dim_ret));
/** 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));
/** /**
* Returns Log map * Returns Log map
* Default Implementation calls global binary function * Default Implementation calls global binary function
*/ */
Vector logmap_ret = t.logmap(t2); Vector logmap_ret = t.logmap(t2);
/** 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);
/** compose with another object */ /** compose with another object */
T compose_ret = t.compose(t2); T compose_ret = t.compose(t2);
/** invert the object and yield a new one */ /** invert the object and yield a new one */
T inverse_ret = t.inverse(); T inverse_ret = t.inverse();
} }
/** };
* The necessary functions to implement for Lie are defined
* below with additional details as to the interface. The
* concept checking function above will check whether or not
* the function exists and throw compile-time errors.
*/
/** 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 */
* Returns dimensionality of the tangent space template<class T>
*/ inline bool equal(const T& obj1, const T& obj2, double tol) {
// inline size_t dim() const; return obj1.equals(obj2, tol);
}
/** /** Call equal on the object without tolerance (use default tolerance) */
* Returns Exponential map update of T template<class T>
* A default implementation of expmap(*this, lp) is available: inline bool equal(const T& obj1, const T& obj2) {
* expmap_default() return obj1.equals(obj2);
*/ }
// T expmap(const Vector& v) const;
/** expmap around identity */ /**
// static T Expmap(const Vector& v); * 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
*/
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));
}
/** /**
* Returns Log map * Declaration of wedge (see Murray94book) used to convert
* A default implementation of logmap(*this, lp) is available: * from n exponential coordinates to n*n element of the Lie algebra
* logmap_default() */
*/ template <class T> Matrix wedge(const Vector& x);
// Vector logmap(const T& lp) const;
/** Logmap around identity */ /**
// static Vector Logmap(const T& p); * Exponential map given exponential coordinates
* class T needs a wedge<> function and a constructor from Matrix
/** * @param x exponential coordinates, vector of size n
* Compute l0 s.t. l2=l1*l0, where (*this) is l1 * @ return a T
* A default implementation of between(*this, lp) is available: */
* between_default() template <class T>
*/ T expm(const Vector& x, int K=7) {
// T between(const T& l2) const; Matrix xhat = wedge<T>(x);
return expm(xhat,K);
/** compose with another object */ }
// inline T compose(const T& p) const;
/** invert the object and yield a new one */
// inline T inverse() 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);
}
/**
* 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
*/
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 } // namespace gtsam

18
base/Testable.h
View File

@ -13,6 +13,20 @@
* @file Testable * @file Testable
* @brief Abstract base class for values that can be used in unit tests * @brief Abstract base class for values that can be used in unit tests
* @author Frank Dellaert * @author Frank Dellaert
*
* The necessary functions to implement for Testable are defined
* below with additional details as to the interface.
* The concept checking function will check whether or not
* the function exists in derived class and throw compile-time errors.
*
* print with optional string naming the object
* void print(const std::string& name) const = 0;
*
* equality up to tolerance
* tricky to implement, see NonlinearFactor1 for an example
* equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0;
*
*/ */
// \callgraph // \callgraph
@ -35,6 +49,10 @@ namespace gtsam {
class Testable { class Testable {
private: private:
/**
* This concept check is to make sure these methods exist in derived classes
* This is as an alternative to declaring those methods virtual, which is slower
*/
static void concept(const Derived& d) { static void concept(const Derived& d) {
const Derived *const_derived = static_cast<Derived*>(&d); const Derived *const_derived = static_cast<Derived*>(&d);
const_derived->print(std::string()); const_derived->print(std::string());