diff --git a/base/Lie.h b/base/Lie.h index fac0df490..ccd65c5ea 100644 --- a/base/Lie.h +++ b/base/Lie.h @@ -14,8 +14,45 @@ * @brief Base class and basic functions for Lie types * @author Richard Roberts * @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 #include @@ -24,172 +61,126 @@ 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 - inline T between_default(const T& l1, const T& l2) { return l1.inverse().compose(l2); } + /** Compute l0 s.t. l2=l1*l0 */ + template + 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 - 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 + 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 - 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 + inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} - /** - * Base class for Lie group type - * This class uses the Curiously Recurring Template design pattern to allow for - * concept checking using a private function. - * - * T is the derived Lie type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class Lie { - private: + /** + * Base class for Lie group type + * This class uses the Curiously Recurring Template design pattern to allow for + * concept checking using a private function. + * + * T is the derived Lie type, like Point2, Pose3, etc. + * + * By convention, we use capital letters to designate a static function + */ + template + class Lie { + private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { + /** 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 Exponential map update of T - * Default implementation calls global binary function - */ - T expmap_ret = t.expmap(gtsam::zero(dim_ret)); + /** + * Returns Exponential map update of T + * Default implementation calls global binary function + */ + T expmap_ret = t.expmap(gtsam::zero(dim_ret)); - /** 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)); - /** - * Returns Log map - * Default Implementation calls global binary function - */ - Vector logmap_ret = t.logmap(t2); + /** + * Returns Log map + * Default Implementation calls global binary function + */ + Vector logmap_ret = t.logmap(t2); - /** 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); + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = t.between(t2); - /** compose with another object */ - T compose_ret = t.compose(t2); + /** compose with another object */ + T compose_ret = t.compose(t2); - /** invert the object and yield a new one */ - T inverse_ret = t.inverse(); - } + /** invert the object and yield a new one */ + 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 + inline void print(const T& object, const std::string& s = "") { + object.print(s); + } - /** - * Returns dimensionality of the tangent space - */ -// inline size_t dim() const; + /** Call equal on the object */ + template + inline bool equal(const T& obj1, const T& obj2, double tol) { + return obj1.equals(obj2, tol); + } - /** - * Returns Exponential map update of T - * A default implementation of expmap(*this, lp) is available: - * expmap_default() - */ -// T expmap(const Vector& v) const; + /** Call equal on the object without tolerance (use default tolerance) */ + template + inline bool equal(const T& obj1, const T& obj2) { + return obj1.equals(obj2); + } - /** expmap around identity */ -// static T Expmap(const Vector& v); + /** + * Three term approximation of the Baker�Campbell�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�Campbell�Hausdorff_formula + */ + template + 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 - * A default implementation of logmap(*this, lp) is available: - * logmap_default() - */ -// Vector logmap(const T& lp) const; + /** + * Declaration of wedge (see Murray94book) used to convert + * from n exponential coordinates to n*n element of the Lie algebra + */ + template Matrix wedge(const Vector& x); - /** 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 */ -// 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 - inline void print(const T& object, const std::string& s = "") { - object.print(s); - } - - /** Call equal on the object */ - template - 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 - inline bool equal(const T& obj1, const T& obj2) { - return obj1.equals(obj2); - } - - /** - * Three term approximation of the Baker�Campbell�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�Campbell�Hausdorff_formula - */ - template - 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 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 - T expm(const Vector& x, int K=7) { - Matrix xhat = wedge(x); - return expm(xhat,K); - } + /** + * 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 + T expm(const Vector& x, int K=7) { + Matrix xhat = wedge(x); + return expm(xhat,K); + } } // namespace gtsam diff --git a/base/Testable.h b/base/Testable.h index fb5189567..e02cb2e10 100644 --- a/base/Testable.h +++ b/base/Testable.h @@ -13,6 +13,20 @@ * @file Testable * @brief Abstract base class for values that can be used in unit tests * @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 @@ -35,6 +49,10 @@ namespace gtsam { class Testable { 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) { const Derived *const_derived = static_cast(&d); const_derived->print(std::string());