diff --git a/gtsam/base/Lie-inl.h b/gtsam/base/Lie-inl.h index 2238958ff..f1bb947a2 100644 --- a/gtsam/base/Lie-inl.h +++ b/gtsam/base/Lie-inl.h @@ -23,8 +23,6 @@ #define INSTANTIATE_LIE(T) \ template T between_default(const T&, const T&); \ template Vector logmap_default(const T&, const T&); \ - template T expmap_default(const T&, const Vector&); \ - template bool equal(const T&, const T&, double); \ - template bool equal(const T&, const T&); + template T expmap_default(const T&, const Vector&); diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fb2146752..756790f16 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -96,24 +96,6 @@ namespace gtsam { }; - /** 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) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 7fa8bca80..d8829798b 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -63,6 +63,24 @@ namespace gtsam { } }; + /** 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); + } + /** * This template works for any type with equals */ @@ -111,6 +129,5 @@ namespace gtsam { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -/// TODO: find better name for "INST" macro, something like "UNIT" or similar #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; #define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index ecdb974d5..b1992f741 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -47,6 +47,7 @@ namespace testing { template P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } -} -} +} // \namespace testing +} // \namespace gtsam + diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 06a4ad54b..496400a04 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -79,28 +79,32 @@ public: *this = Expmap(v); } + // Testable requirements + /** print with optional string */ void print(const std::string& s = "") const; /** assert equality up to a tolerance */ bool equals(const Pose2& pose, double tol = 1e-9) const; - /** compose syntactic sugar */ - inline Pose2 operator*(const Pose2& p2) const { - return Pose2(r_*p2.r(), t_ + r_*p2.t()); - } + // Manifold requirements /** dimension of the variable - used to autodetect sizes */ inline static size_t Dim() { return dimension; } - /** Lie requirements */ /** return DOF, dimensionality of tangent space = 3 */ inline size_t dim() const { return dimension; } - /** - * inverse transformation with derivatives - */ + /** Apply tangent space update to an object */ + Pose2 retract(const Vector& v) const; + + /** @return local coordinates of p2 centered at *this */ + Vector localCoordinates(const Pose2& p2) const; + + // Group Requirements + + /** inverse transformation with derivatives */ Pose2 inverse(boost::optional H1=boost::none) const; /** @@ -116,22 +120,11 @@ public: return boost::shared_ptr(new Pose2(compose(p2))); } - /** syntactic sugar for transform_from */ - inline Point2 operator*(const Point2& point) const { return transform_from(point);} - - /** identity */ - inline static Pose2 identity() { - return Pose2(); + /** compose syntactic sugar */ + inline Pose2 operator*(const Pose2& p2) const { + return Pose2(r_*p2.r(), t_ + r_*p2.t()); } - /** Real versions of Expmap/Logmap */ - static Pose2 Expmap(const Vector& xi); - static Vector Logmap(const Pose2& p); - - /** default implementations of binary functions */ - Pose2 retract(const Vector& v) const; - Vector localCoordinates(const Pose2& p2) const; - /** * Return relative pose between p1 and p2, in p1 coordinate frame */ @@ -144,6 +137,19 @@ public: return boost::shared_ptr(new Pose2(between(p2))); } + /** identity */ + inline static Pose2 identity() { + return Pose2(); + } + + // Lie Group requirements + + /** Exponential map using canonical coordinates */ + static Pose2 Expmap(const Vector& xi); + + /** Log map returning local coordinates */ + static Vector Logmap(const Pose2& p); + /** return transformation matrix */ Matrix matrix() const; @@ -161,6 +167,9 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** syntactic sugar for transform_from */ + inline Point2 operator*(const Point2& point) const { return transform_from(point);} + /** * Calculate bearing to a landmark * @param point 2D location of landmark diff --git a/gtsam/nonlinear/LieValues.h b/gtsam/nonlinear/LieValues.h index b6b3ffdf4..6d06cad43 100644 --- a/gtsam/nonlinear/LieValues.h +++ b/gtsam/nonlinear/LieValues.h @@ -18,8 +18,8 @@ * Detailed story: * A values structure is a map from keys to values. It is used to specify the value of a bunch * of variables in a factor graph. A LieValues is a values structure which can hold variables that - * are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type - * which is also a Lie group, and hence supports operations dim, expmap, and logmap. + * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type + * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ #pragma once @@ -66,6 +66,7 @@ namespace gtsam { /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(Value) + GTSAM_CONCEPT_MANIFOLD_TYPE(Value) KeyValueMap values_; @@ -105,9 +106,6 @@ namespace gtsam { /** whether the config is empty */ bool empty() const { return values_.empty(); } - /** The dimensionality of the tangent space */ - size_t dim() const; - /** Get a zero VectorValues of the correct structure */ VectorValues zero(const Ordering& ordering) const; @@ -116,7 +114,10 @@ namespace gtsam { iterator begin() { return values_.begin(); } iterator end() { return values_.end(); } - // Lie operations + // Manifold operations + + /** The dimensionality of the tangent space */ + size_t dim() const; /** Add a delta config to current config and returns a new config */ LieValues retract(const VectorValues& delta, const Ordering& ordering) const;