diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 3c56a541a..533c042c0 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -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 GroupConcept { diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 756790f16..932e069ef 100644 --- a/gtsam/base/Lie.h +++ b/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 - 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));} - /** - * 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 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 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�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 - */ - /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? - 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)); + /** 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 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); - } +/** + * 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 + */ +/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? +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); +} } // 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; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; #define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 87506f31f..430be4f7d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -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 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 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