Formatting, comments

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 T expmap_default(const T&, const Vector&);
-  template bool equal(const T&, const T&, double); \
-  template bool equal(const T&, const T&);
 
 

gtsam/base/Lie.h

@@ -96,24 +96,6 @@ namespace gtsam {
 
 	};
 
-	/** 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)

gtsam/base/Testable.h

@@ -63,6 +63,24 @@ namespace gtsam {
 	  }
 	};
 
+	/** 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);
+	}
+
 	/**
 	 * 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<T>;
 #define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;

gtsam/base/lieProxies.h

@@ -47,6 +47,7 @@ namespace testing {
 	template<class T, class P>
 	P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
 
-}
+} // \namespace testing
-}
+} // \namespace gtsam
+
 

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 */
+	// Manifold requirements
-	inline Pose2 operator*(const Pose2& p2) const {
-		return Pose2(r_*p2.r(), t_ + r_*p2.t());
-	}
 
 	/** 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; }
 
-	/**
+	/** Apply tangent space update to an object */
-	 * inverse transformation with derivatives
+	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<Matrix&> H1=boost::none) const;
 
 	/**
@@ -116,22 +120,11 @@ public:
 		return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
 	}
 
-	/** syntactic sugar for transform_from */
+	/** compose syntactic sugar */
-	inline Point2 operator*(const Point2& point) const { return transform_from(point);}
+	inline Pose2 operator*(const Pose2& p2) const {
-
+		return Pose2(r_*p2.r(), t_ + r_*p2.t());
-	/** identity */
-	inline static Pose2 identity() {
-		return Pose2();
 	}
 
-	/** 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<Pose2>(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<Matrix&> H1=boost::none,
 			boost::optional<Matrix&> 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

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
+ *  are elements on manifolds, 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.
+ *  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;