Added Manifold and Lie Concept checks, fixed LieScalar and LieVector

.cproject

@@ -375,6 +375,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testGaussianFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -401,7 +409,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -409,7 +416,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -457,7 +463,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -465,7 +470,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -473,7 +477,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -489,20 +492,11 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -529,6 +523,7 @@
 			</target>
 			<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -600,6 +595,7 @@
 			</target>
 			<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testInference.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -607,6 +603,7 @@
 			</target>
 			<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGaussianFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -614,6 +611,7 @@
 			</target>
 			<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -621,6 +619,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -628,6 +627,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -721,15 +721,7 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -737,14 +729,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -785,7 +769,15 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
 				<buildTarget>check</buildTarget>
@@ -793,6 +785,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1107,7 +1107,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1385,6 +1384,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1507,6 +1514,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1546,6 +1554,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1553,6 +1562,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1800,6 +1810,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1821,46 +1832,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>dist</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1957,6 +1928,62 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>dist</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>install</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1997,22 +2024,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>install</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>

gtsam/base/Lie.h

@@ -15,6 +15,10 @@
  * @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
@@ -55,8 +59,7 @@
 
 #pragma once
 
-#include <string>
+#include <gtsam/base/Manifold.h>
-#include <gtsam/base/Matrix.h>
 
 namespace gtsam {
 
@@ -78,18 +81,15 @@ namespace gtsam {
 	inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
 
 	/**
-	 * Base class for Lie group type
+	 * Concept check 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.
+	 * T is the Lie type, like Point2, Pose3, etc.
 	 *
 	 * By convention, we use capital letters to designate a static function
 	 */
 	template <class T>
-	class Lie {
+	class LieConcept {
 	private:
-
 		/** concept checking function - implement the functions this demands */
 		static void concept_check(const T& t) {
 
@@ -182,25 +182,16 @@ namespace gtsam {
 		return expm(xhat,K);
 	}
 
-	/**
-	 * function wrappers for full versions of expmap/logmap
-	 * these will default simple types to using the existing expmap/logmap,
-	 * but more complex ones can be specialized to use improved versions
-	 *
-	 * TODO: replace this approach with a naming scheme that doesn't call
-	 * non-expmap operations "expmap" - use same approach, but with "update"
-	 */
-
-  /** unary versions */
-	template<class T>
-	T ExpmapFull(const Vector& xi) { return T::Expmap(xi); }
-	template<class T>
-  Vector LogmapFull(const T& p) { return T::Logmap(p); }
-
-  /** binary versions */
-	template<class T>
-  T expmapFull(const T& t, const Vector& v) { return t.expmap(v); }
-	template<class T>
-  Vector logmapFull(const T& t, const T& p2) { return t.logmap(p2); }
-
 } // namespace gtsam
+
+/**
+ * Macros for using the ManifoldConcept
+ *  - An instantiation for use inside unit tests
+ *  - A typedef for use inside generic algorithms
+ *
+ * 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_LIE_INST(T) template class gtsam::LieConcept<T>;
+#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;

gtsam/base/LieScalar.h

@@ -24,7 +24,7 @@ namespace gtsam {
 	/**
 	 * LieScalar is a wrapper around double to allow it to be a Lie type
 	 */
-	struct LieScalar : public Lie<LieScalar> {
+	struct LieScalar {
 
 		/** default constructor - should be unnecessary */
 		LieScalar() {}
@@ -72,6 +72,28 @@ namespace gtsam {
 		/** Logmap around identity - just returns with default cast back */
 		static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); }
 
+		inline LieScalar between(const LieScalar& t2) const { return LieScalar(t2.value() - d_); }
+
+		/** compose with another object */
+		inline LieScalar compose(const LieScalar& t2) const { return LieScalar(t2.value() + d_); }
+
+		/** invert the object and yield a new one */
+		inline LieScalar inverse() const { return LieScalar(-d_); }
+
+		// Manifold requirements
+
+		inline LieScalar retract(const Vector& v) const { return expmap(v); }
+
+		/** expmap around identity */
+		inline static LieScalar Retract(const Vector& v) { return Expmap(v); }
+
+		/**
+		 * Returns inverse retraction
+		 */
+		inline Vector unretract(const LieScalar& t2) const { return logmap(t2); }
+
+		/** Unretract around identity */
+		inline static Vector Unretract(const LieScalar& t) { return Logmap(t); }
 
 	private:
 	    double d_;

gtsam/base/LieVector.h

@@ -25,7 +25,7 @@ namespace gtsam {
 /**
  * LieVector is a wrapper around vector to allow it to be a Lie type
  */
-struct LieVector : public Vector, public Lie<LieVector> {
+struct LieVector : public Vector {
 
 	/** default constructor - should be unnecessary */
 	LieVector() {}
@@ -100,5 +100,20 @@ struct LieVector : public Vector, public Lie<LieVector> {
 	inline LieVector inverse() const {
 		return LieVector(-1.0 * vector());
 	}
+
+	// Manifold requirements
+
+	inline LieVector retract(const Vector& v) const { return expmap(v); }
+
+	/** expmap around identity */
+	inline static LieVector Retract(const Vector& v) { return Expmap(v); }
+
+	/**
+	 * Returns inverse retraction
+	 */
+	inline Vector unretract(const LieVector& t2) const { return logmap(t2); }
+
+	/** Unretract around identity */
+	inline static Vector Unretract(const LieVector& t) { return Logmap(t); }
 };
 } // \namespace gtsam

gtsam/base/Makefile.am

@@ -25,7 +25,8 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h
 sources += timing.cpp debug.cpp
 check_PROGRAMS += tests/testDebug tests/testTestableAssertions
 
-# Lie Groups
+# Manifolds and Lie Groups
+headers += Manifold.h
 headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
 sources += LieVector.cpp
 check_PROGRAMS += tests/testLieVector tests/testLieScalar

gtsam/base/Manifold.h

@@ -0,0 +1,102 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @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;
+ *
+ * Retract around identity
+ * 		static T Retract(const Vector& v);
+ *
+ * Returns inverse retraction operation
+ * A default implementation of unretract(*this, lp) is available:
+ * 		Vector unretract(const T& lp) const;
+ *
+ * Unretract around identity
+ * 		static Vector Unretract(const T& p);
+ *
+ */
+
+
+#pragma once
+
+#include <string>
+#include <gtsam/base/Matrix.h>
+
+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 T>
+	class ManifoldConcept {
+	private:
+		/** concept checking function - implement the functions this demands */
+		static void concept_check(const T& t) {
+
+			/** assignment */
+			T t2 = t;
+
+			/**
+			 * 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));
+
+			/** expmap around identity */
+			T retract_identity_ret = T::Retract(gtsam::zero(dim_ret));
+
+			/**
+			 * Returns inverse retraction
+			 */
+			Vector unretract_ret = t.unretract(t2);
+
+			/** Unretract around identity */
+			Vector unretract_identity_ret = T::Unretract(t);
+		}
+	};
+
+} // namespace gtsam
+
+/**
+ * Macros for using the ManifoldConcept
+ *  - An instantiation for use inside unit tests
+ *  - A typedef for use inside generic algorithms
+ *
+ * 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_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>;
+#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;

gtsam/base/tests/testLieScalar.cpp

@@ -17,11 +17,14 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/Manifold.h>
 #include <gtsam/base/LieScalar.h>
 
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(LieScalar)
+GTSAM_CONCEPT_MANIFOLD_INST(LieScalar)
+GTSAM_CONCEPT_LIE_INST(LieScalar)
 
 const double tol=1e-9;
 

gtsam/base/tests/testLieVector.cpp

@@ -17,11 +17,14 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/Manifold.h>
 #include <gtsam/base/LieVector.h>
 
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(LieVector)
+GTSAM_CONCEPT_MANIFOLD_INST(LieVector)
+GTSAM_CONCEPT_LIE_INST(LieVector)
 
 /* ************************************************************************* */
 TEST( testLieVector, construction ) {