Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.

base/LieVector.h

@@ -6,22 +6,69 @@
 
 #pragma once
 
+#include <gtsam/base/Testable.h>
 #include <gtsam/base/Lie.h>
+#include <gtsam/base/Vector.h>
 
 namespace gtsam {
-/** temporarily using the old system */
 
-// The rest of the file makes double and Vector behave as a Lie type (with + as compose)
+	/**
+	 * LieVector is a wrapper around vector to allow it to be a Lie type
+	 */
+	struct LieVector : public Vector, public Lie<LieVector>, Testable<LieVector> {
 
-// Vector group operations
+		/** default constructor - should be unnecessary */
-inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
+		LieVector() {}
-inline Vector inverse(const Vector& p) { return -p;}
-inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
 
-// Vector is a trivial Lie group
+		/** initialize from a normal vector */
-template<> inline Vector expmap(const Vector& d) { return d;}
+		LieVector(const Vector& v) : Vector(v) {}
-template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
-template<> inline Vector logmap(const Vector& p) { return p;}
-template<> inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
 
+		/** get the underlying vector */
+		inline Vector vector() const {
+			return static_cast<Vector>(*this);
+		}
+
+		/** print @param s optional string naming the object */
+		inline void print(const std::string& name="") const {
+			gtsam::print(vector(), name);
+		}
+
+		/** equality up to tolerance */
+		inline bool equals(const LieVector& expected, double tol=1e-5) const {
+			return gtsam::equal(vector(), expected.vector(), tol);
+		}
+
+	    /**
+	     * Returns dimensionality of the tangent space
+	     */
+	    inline size_t dim() const { return this->size(); }
+
+	    /**
+	     * Returns Exponential map update of T
+	     * Default implementation calls global binary function
+	     */
+	    LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
+
+	    /** expmap around identity */
+	    static LieVector Expmap(const Vector& v) { return LieVector(v); }
+
+	    /**
+	     * Returns Log map
+	     * Default Implementation calls global binary function
+	     */
+	    Vector logmap(const LieVector& lp) const { return *this - lp; }
+
+	    /** Logmap around identity - just returns with default cast back */
+	    static Vector Logmap(const LieVector& p) { return p; }
+
+	    /** compose with another object */
+	    inline LieVector compose(const LieVector& p) const {
+	    	return LieVector(vector() + p);
+	    }
+
+	    /** invert the object and yield a new one */
+	    inline LieVector inverse() const {
+	    	return LieVector(-1.0 * vector());
+	    }
+	};
 } // \namespace gtsam

base/Makefile.am

@@ -27,6 +27,7 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h
 
 # Lie Groups
 headers += Lie.h Lie-inl.h LieVector.h
+check_PROGRAMS += tests/testLieVector
 
 # Data structures
 headers += BTree.h DSF.h

base/numericalDerivative.h

@@ -12,6 +12,7 @@
 #include <boost/bind.hpp>
 
 #include <gtsam/base/Lie.h>
+#include <gtsam/base/LieVector.h>
 #include <gtsam/base/Matrix.h>
 
 //#define LINEARIZE_AT_IDENTITY
@@ -40,6 +41,11 @@ namespace gtsam {
 	 * 		http://www.boost.org/doc/libs/1_43_0/libs/bind/bind.html
 	 */
 
+
+	/** global functions for converting to a LieVector for use with numericalDerivative */
+	LieVector makeLieVector(const Vector& v) { return LieVector(v); }
+	LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); }
+
 	/**
 	 * Numerically compute gradient of scalar function
 	 * Class X is the input argument
@@ -89,31 +95,32 @@ namespace gtsam {
 		return H;
 	}
 
+	/** use a raw C++ function pointer */
 	template<class Y, class X>
 	Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
 		return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
 	}
 
-	/** pseudo-template specialization for double Y values */
+	/** remapping for double valued functions */
 	template<class X>
 	Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
-		double hx = h(x);
+		return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
-		double factor = 1.0/(2.0*delta);
-		const size_t m = 1, n = dim(x);
-		Vector d(n,0.0);
-		Matrix H = zeros(m,n);
-		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; Vector hxplus = Vector_(1, h(expmap(x,d))-hx);
-			d(j) -= 2*delta; Vector hxmin  = Vector_(1, h(expmap(x,d))-hx);
-			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
-		}
-		return H;
 	}
 
 	template<class X>
 	Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
-		return numericalDerivative11<X>(boost::bind(h, _1), x, delta);
+		return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
+	}
+
+	/** remapping for vector valued functions */
+	template<class X>
+	Matrix numericalDerivative11(boost::function<Vector(const X&)> h, const X& x, double delta=1e-5) {
+		return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
+	}
+
+	template<class X>
+	Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) {
+		return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
 	}
 
 	/**
@@ -142,6 +149,7 @@ namespace gtsam {
 		return H;
 	}
 
+	/** use a raw C++ function pointer */
 	template<class Y, class X1, class X2>
 	inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
 			const X1& x1, const X2& x2, double delta=1e-5) {
@@ -152,24 +160,30 @@ namespace gtsam {
 	template<class X1, class X2>
 	Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
 			const X1& x1, const X2& x2, double delta=1e-5) {
-		double hx = h(x1,x2);
+		return numericalDerivative21<LieVector,X1,X2>(
-		double factor = 1.0/(2.0*delta);
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
-		const size_t m = 1, n = dim(x1);
-		Vector d(n,0.0);
-		Matrix H = zeros(m,n);
-		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; Vector hxplus = Vector_(1, h(expmap(x1,d),x2)-hx);
-			d(j) -= 2*delta; Vector hxmin  = Vector_(1, h(expmap(x1,d),x2)-hx);
-			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
-		}
-		return H;
 	}
 
 	template<class X1, class X2>
-	inline Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
+	Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
 			const X1& x1, const X2& x2, double delta=1e-5) {
-		return numericalDerivative21<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
+		return numericalDerivative21<LieVector,X1,X2>(
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
+	}
+
+	/** pseudo-partial template specialization for vector return values */
+	template<class X1, class X2>
+	Matrix numericalDerivative21(boost::function<Vector(const X1&, const X2&)> h,
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		return numericalDerivative21<LieVector,X1,X2>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
+	}
+
+	template<class X1, class X2>
+	inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&),
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		return numericalDerivative21<LieVector,X1,X2>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
 	}
 
 	/**
@@ -199,33 +213,42 @@ namespace gtsam {
 		}
 		return H;
 	}
+
+	/** use a raw C++ function pointer */
 	template<class Y, class X1, class X2>
 	inline Matrix numericalDerivative22
 	(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
 		return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
 	}
 
-	/** pseudo-specialization for double Y values */
+	/** pseudo-partial template specialization for double return values */
 	template<class X1, class X2>
 	Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
 			const X1& x1, const X2& x2, double delta=1e-5) {
-		double hx = h(x1,x2);
+		return numericalDerivative22<LieVector,X1,X2>(
-		double factor = 1.0/(2.0*delta);
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
-		const size_t m = 1, n = dim(x2);
-		Vector d(n,0.0);
-		Matrix H = zeros(m,n);
-		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; Vector hxplus = Vector_(1,h(x1,expmap(x2,d))-hx);
-			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(x1,expmap(x2,d))-hx);
-			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
-		}
-		return H;
 	}
+
 	template<class X1, class X2>
 	inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
 			const X1& x1, const X2& x2, double delta=1e-5) {
-		return numericalDerivative22<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
+		return numericalDerivative22<LieVector,X1,X2>(
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
+	}
+
+	/** pseudo-partial template specialization for vector return values */
+	template<class X1, class X2>
+	Matrix numericalDerivative22(boost::function<Vector(const X1&, const X2&)> h,
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		return numericalDerivative22<LieVector,X1,X2>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
+	}
+
+	template<class X1, class X2>
+	inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&),
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		return numericalDerivative22<LieVector,X1,X2>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
 	}
 
 	/**
@@ -263,7 +286,46 @@ namespace gtsam {
 		return numericalDerivative31<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
 	}
 
-	// arg 2
+	/** pseudo-partial template specialization for double return values */
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative31(boost::function<double(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative31<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative31<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	/** pseudo-partial template specialization for vector return values */
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative31(boost::function<Vector(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative31<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative31<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	/**
+	 * Compute numerical derivative in argument 2 of ternary function
+	 * @param h ternary function yielding m-vector
+	 * @param x1 n-dimensional first argument value
+	 * @param x2 second argument value
+	 * @param x3 third argument value
+	 * @param delta increment for numerical derivative
+	 * @return m*n Jacobian computed via central differencing
+	 * All classes Y,X1,X2,X3 need dim, expmap, logmap
+	 */
 	template<class Y, class X1, class X2, class X3>
 	Matrix numericalDerivative32
 	(boost::function<Y(const X1&, const X2&, const X3&)> h,
@@ -289,7 +351,46 @@ namespace gtsam {
 		return numericalDerivative32<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
 	}
 
-	// arg 3
+	/** pseudo-partial template specialization for double return values */
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative32(boost::function<double(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative32<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative32<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	/** pseudo-partial template specialization for vector return values */
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative32(boost::function<Vector(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative32<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative32<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
+	}
+
+	/**
+	 * Compute numerical derivative in argument 3 of ternary function
+	 * @param h ternary function yielding m-vector
+	 * @param x1 n-dimensional first argument value
+	 * @param x2 second argument value
+	 * @param x3 third argument value
+	 * @param delta increment for numerical derivative
+	 * @return m*n Jacobian computed via central differencing
+	 * All classes Y,X1,X2,X3 need dim, expmap, logmap
+	 */
 	template<class Y, class X1, class X2, class X3>
 	Matrix numericalDerivative33
 	(boost::function<Y(const X1&, const X2&, const X3&)> h,
@@ -315,82 +416,34 @@ namespace gtsam {
 		return numericalDerivative33<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
 	}
 
-
+	/** pseudo-partial template specialization for double return values */
-	/**
-	 * specializations for double outputs
-	 */
 	template<class X1, class X2, class X3>
-	Matrix numericalDerivative31
+	Matrix numericalDerivative33(boost::function<double(const X1&, const X2&, const X3&)> h,
-	(boost::function<double(const X1&, const X2&, const X3&)> h,
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
-		double hx = h(x1,x2,x3);
+		return numericalDerivative33<LieVector,X1,X2,X3>(
-		double factor = 1.0/(2.0*delta);
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
-		const size_t m = 1, n = dim(x1);
-		Vector d(n,0.0);
-		Matrix H = zeros(m,n);
-		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; Vector hxplus = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
-			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
-			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
-		}
-		return H;
-	}
-	template<class X1, class X2, class X3>
-	inline Matrix numericalDerivative31
-	(double (*h)(const X1&, const X2&, const X3&),
-			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
-		return numericalDerivative31<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
 	}
 
-	// arg 2
 	template<class X1, class X2, class X3>
-	Matrix numericalDerivative32
+	inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&),
-	(boost::function<double(const X1&, const X2&, const X3&)> h,
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
-		double hx = h(x1,x2,x3);
+		return numericalDerivative33<LieVector,X1,X2,X3>(
-		double factor = 1.0/(2.0*delta);
+				boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
-		const size_t m = 1, n = dim(x2);
-		Vector d(n,0.0);
-		Matrix H = zeros(m,n);
-		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; Vector hxplus = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
-			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
-			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
-		}
-		return H;
-	}
-	template<class X1, class X2, class X3>
-	inline Matrix numericalDerivative32
-	(double (*h)(const X1&, const X2&, const X3&),
-			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
-		return numericalDerivative32<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
 	}
 
-	// arg 3
+	/** pseudo-partial template specialization for vector return values */
 	template<class X1, class X2, class X3>
-	Matrix numericalDerivative33
+	Matrix numericalDerivative33(boost::function<Vector(const X1&, const X2&, const X3&)> h,
-	(boost::function<double(const X1&, const X2&, const X3&)> h,
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
-		double hx = h(x1,x2,x3);
+		return numericalDerivative33<LieVector,X1,X2,X3>(
-		double factor = 1.0/(2.0*delta);
+				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
-		const size_t m = 1, n = dim(x3);
-		Vector d(n,0.0);
-		Matrix H = zeros(m,n);
-		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; Vector hxplus = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
-			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
-			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
-		}
-		return H;
 	}
+
 	template<class X1, class X2, class X3>
-	inline Matrix numericalDerivative33
+	inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&),
-	(double (*h)(const X1&, const X2&, const X3&),
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
-		return numericalDerivative33<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
+		return numericalDerivative33<LieVector,X1,X2,X3>(
+				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
 	}
 
 }

base/tests/testLieVector.cpp

@@ -0,0 +1,26 @@
+/**
+ * @file testLieVector.cpp
+ * @author Alex Cunningham
+ */
+
+#include <gtsam/CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/LieVector.h>
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( testLieVector, construction ) {
+	Vector v = Vector_(3, 1.0, 2.0, 3.0);
+	LieVector lie1(v), lie2(v);
+
+	EXPECT(lie1.dim() == 3);
+	EXPECT(assert_equal(v, lie1.vector()));
+	EXPECT(assert_equal(lie1, lie2));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+
+

geometry/tests/testPose2.cpp

@@ -439,8 +439,8 @@ TEST( Pose2, bearing )
 }
 
 /* ************************************************************************* */
-double range_proxy(const Pose2& pose, const Point2& point) {
+LieVector range_proxy(const Pose2& pose, const Point2& point) {
-	return pose.range(point);
+	return LieVector(Vector_(1, pose.range(point)));
 }
 TEST( Pose2, range )
 {

nonlinear/tests/testLieConfig.cpp

@@ -18,7 +18,7 @@ using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
-typedef TypedSymbol<Vector, 'v'> VecKey;
+typedef TypedSymbol<LieVector, 'v'> VecKey;
 typedef LieConfig<VecKey> Config;
 
 VecKey key1(1), key2(2), key3(3), key4(4);
@@ -211,7 +211,7 @@ TEST(LieConfig, exists_)
 	config0.insert(key1, Vector_(1, 1.));
 	config0.insert(key2, Vector_(1, 2.));
 
-	boost::optional<Vector> v = config0.exists_(key1);
+	boost::optional<LieVector> v = config0.exists_(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
 }
 
@@ -237,7 +237,7 @@ TEST(LieConfig, update)
 /* ************************************************************************* */
 TEST(LieConfig, dummy_initialization)
 {
-	typedef TypedSymbol<Vector, 'z'> Key;
+	typedef TypedSymbol<LieVector, 'z'> Key;
 	typedef LieConfig<Key> Config1;
 
 	Config1 init1;

slam/Simulated3D.cpp

@@ -5,54 +5,35 @@
 **/
 
 #include <gtsam/slam/Simulated3D.h>
+#include <gtsam/nonlinear/LieConfig-inl.h>
+#include <gtsam/nonlinear/TupleConfig-inl.h>
 
 namespace gtsam {
+
+using namespace simulated3D;
+INSTANTIATE_LIE_CONFIG(PointKey)
+INSTANTIATE_LIE_CONFIG(PoseKey)
+INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig)
+
 namespace simulated3D {
 
-Vector prior (const Vector& x)
+Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
-{
+	if (H) *H = eye(3);
 	return x;
 }
 
-Matrix Dprior(const Vector& x)
+Point3 odo(const Point3& x1, const Point3& x2,
-{
+		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-	Matrix H = eye((int) x.size());
+	if (H1) *H1 = -1 * eye(3);
-	return H;
+	if (H2) *H2 = eye(3);
-}
-
-Vector odo(const Vector& x1, const Vector& x2)
-{
 	return x2 - x1;
 }
 
-Matrix Dodo1(const Vector& x1, const Vector& x2)
+Point3 mea(const Point3& x,  const Point3& l,
-{
+		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-	Matrix H = -1 * eye((int) x1.size());
+	if (H1) *H1 = -1 * eye(3);
-	return H;
+	if (H2) *H2 = eye(3);
-}
-
-Matrix Dodo2(const Vector& x1, const Vector& x2)
-{
-	Matrix H = eye((int) x1.size());
-	return H;
-}
-
-
-Vector mea(const Vector& x,  const Vector& l)
-{
 	return l - x;
 }
 
-Matrix Dmea1(const Vector& x, const Vector& l)
-{
-	Matrix H = -1 * eye((int) x.size());
-	return H;
-}
-
-Matrix Dmea2(const Vector& x, const Vector& l)
-{
-	Matrix H = eye((int) x.size());
-	return H;
-}
-
 }} // namespace gtsam::simulated3D

slam/Simulated3D.h

@@ -9,73 +9,72 @@
 #pragma once
 
 #include <gtsam/base/Matrix.h>
-#include <gtsam/base/LieVector.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/linear/VectorConfig.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/TupleConfig.h>
 
 // \namespace
 
 namespace gtsam {
 namespace simulated3D {
 
-	typedef VectorConfig VectorConfig;
+	typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
+	typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
 
-	typedef gtsam::TypedSymbol<Vector, 'x'> PoseKey;
+	typedef LieConfig<PoseKey> PoseConfig;
-	typedef gtsam::TypedSymbol<Vector, 'l'> PointKey;
+	typedef LieConfig<PointKey> PointConfig;
+	typedef TupleConfig2<PoseConfig, PointConfig> Config;
 
 	/**
 	 * Prior on a single pose
 	 */
-	Vector prior(const Vector& x);
+	Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
-	Matrix Dprior(const Vector& x);
 
 	/**
 	 * odometry between two poses
 	 */
-	Vector odo(const Vector& x1, const Vector& x2);
+	Point3 odo(const Point3& x1, const Point3& x2,
-	Matrix Dodo1(const Vector& x1, const Vector& x2);
+			boost::optional<Matrix&> H1 = boost::none,
-	Matrix Dodo2(const Vector& x1, const Vector& x2);
+			boost::optional<Matrix&> H2 = boost::none);
 
 	/**
 	 *  measurement between landmark and pose
 	 */
-	Vector mea(const Vector& x, const Vector& l);
+	Point3 mea(const Point3& x, const Point3& l,
-	Matrix Dmea1(const Vector& x, const Vector& l);
+			boost::optional<Matrix&> H1 = boost::none,
-	Matrix Dmea2(const Vector& x, const Vector& l);
+			boost::optional<Matrix&> H2 = boost::none);
 
-	struct Point2Prior3D: public NonlinearFactor1<VectorConfig, PoseKey> {
+	struct PointPrior3D: public NonlinearFactor1<Config, PoseKey> {
 
-		Vector z_;
+		Point3 z_;
 
-		Point2Prior3D(const Vector& z,
+		PointPrior3D(const Point3& z,
 					const SharedGaussian& model, const PoseKey& j) :
-				NonlinearFactor1<VectorConfig, PoseKey> (model, j), z_(z) {
+				NonlinearFactor1<Config, PoseKey> (model, j), z_(z) {
 			}
 
-		Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
+		Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
 				boost::none) {
-			if (H) *H = Dprior(x);
+			return (prior(x, H) - z_).vector();
-			return prior(x) - z_;
 		}
 	};
 
-	struct Simulated3DMeasurement: public NonlinearFactor2<VectorConfig,
+	struct Simulated3DMeasurement: public NonlinearFactor2<Config,
 			PoseKey, PointKey> {
 
-		Vector z_;
+		Point3 z_;
 
-		Simulated3DMeasurement(const Vector& z,
+		Simulated3DMeasurement(const Point3& z,
 					const SharedGaussian& model, PoseKey& j1, PointKey j2) :
-				NonlinearFactor2<VectorConfig, PoseKey, PointKey> (
+				NonlinearFactor2<Config, PoseKey, PointKey> (
 								model, j1, j2), z_(z) {
 			}
 
-		Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
+		Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
 				Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
-			if (H1) *H1 = Dmea1(x1, x2);
+			return (mea(x1, x2, H1, H2) - z_).vector();
-			if (H2) *H2 = Dmea2(x1, x2);
-			return mea(x1, x2) - z_;
 		}
 	};
 

slam/TransformConstraint.h

@@ -49,21 +49,12 @@ public:
 				boost::optional<Matrix&> Dforeign = boost::none,
 				boost::optional<Matrix&> Dtrans = boost::none,
 				boost::optional<Matrix&> Dlocal = boost::none) const  {
-		if (Dforeign) {
+		Point newlocal = trans.transform_from(foreign, Dtrans, Dforeign);
-			Matrix Af;
-			trans.transform_from(foreign, boost::none, Af);
-			*Dforeign = Af;
-		}
-		if (Dtrans) {
-			Matrix At;
-			trans.transform_from(foreign, At, boost::none);
-			*Dtrans = At;
-		}
 		if (Dlocal) {
 			Point dummy;
 			*Dlocal = -1* eye(dummy.dim());
 		}
-		return gtsam::logmap(gtsam::between<Point>(local, trans.transform_from(foreign)));
+		return gtsam::logmap(gtsam::between<Point>(local, newlocal));
 	}
 };
 } // \namespace gtsam

slam/tests/testPose2Factor.cpp

@@ -8,6 +8,7 @@
 
 #define GTSAM_MAGIC_KEY
 
+#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/slam/pose2SLAM.h>
 
@@ -87,8 +88,8 @@ TEST( Pose2Factor, rhs )
 /* ************************************************************************* */
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
 // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
-Vector h(const Pose2& p1,const Pose2& p2) {
+LieVector h(const Pose2& p1,const Pose2& p2) {
-	return covariance->whiten(factor.evaluateError(p1,p2));
+	return LieVector(covariance->whiten(factor.evaluateError(p1,p2)));
 }
 
 /* ************************************************************************* */

slam/tests/testPose2Prior.cpp

@@ -8,6 +8,7 @@
 
 #define GTSAM_MAGIC_KEY
 
+#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/slam/pose2SLAM.h>
 
@@ -56,8 +57,8 @@ static Pose2Prior factor(1,prior, sigmas);
 /* ************************************************************************* */
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
 // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
-Vector h(const Pose2& p1) {
+LieVector h(const Pose2& p1) {
-	return sigmas->whiten(factor.evaluateError(p1));
+	return LieVector(sigmas->whiten(factor.evaluateError(p1)));
 }
 
 /* ************************************************************************* */

slam/tests/testSimulated3D.cpp

@@ -14,38 +14,35 @@
 using namespace gtsam;
 using namespace simulated3D;
 
+/* ************************************************************************* */
+TEST( simulated3D, Config )
+{
+	Config actual;
+	actual.insert(PointKey(1),Point3(1,1,1));
+	actual.insert(PoseKey(2),Point3(2,2,2));
+	EXPECT(assert_equal(actual,actual,1e-9));
+}
+
 /* ************************************************************************* */
 TEST( simulated3D, Dprior )
 {
-	Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
+	Point3 x(1,-9, 7);
-	Vector v = logmap(x1);
+	Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(prior, _1, boost::none),x);
-	Matrix numerical = numericalDerivative11(prior,v);
+	Matrix computed;
-	Matrix computed = Dprior(v);
+	prior(x,computed);
-	CHECK(assert_equal(numerical,computed,1e-9));
+	EXPECT(assert_equal(numerical,computed,1e-9));
 }
 
 /* ************************************************************************* */
-TEST( simulated3D, DOdo1 )
+TEST( simulated3D, DOdo )
 {
-	Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
+	Point3 x1(1,-9,7),x2(-5,6,7);
-	Vector v1 = logmap(x1);
+	Matrix H1,H2;
-	Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0));
+	odo(x1,x2,H1,H2);
-	Vector v2 = logmap(x2);
+	Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2);
-	Matrix numerical = numericalDerivative21(odo,v1,v2);
+	EXPECT(assert_equal(A1,H1,1e-9));
-	Matrix computed = Dodo1(v1,v2);
+	Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2);
-	CHECK(assert_equal(numerical,computed,1e-9));
+	EXPECT(assert_equal(A2,H2,1e-9));
-}
-
-/* ************************************************************************* */
-TEST( simulated3D, DOdo2 )
-{
-	Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
-	Vector v1 = logmap(x1);
-	Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0));
-	Vector v2 = logmap(x2);
-	Matrix numerical = numericalDerivative22(odo,v1,v2);
-	Matrix computed = Dodo2(v1,v2);
-	CHECK(assert_equal(numerical,computed,1e-9));
 }
 
 

tests/Makefile.am

@@ -18,7 +18,7 @@ check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
 check_PROGRAMS += testTransformConstraint testLinearApproxFactor
 
 # experimental
-#check_PROGRAMS += testFusionTupleConfig
+#check_PROGRAMS += testFusionTupleConfig # Doesn't work on Macs
 
 if USE_LDL
 check_PROGRAMS += testConstraintOptimizer 

tests/testTransformConstraint.cpp

@@ -62,6 +62,10 @@ TEST( TransformConstraint, equals ) {
 }
 
 /* ************************************************************************* */
+LieVector evaluateError_(const PointTransformConstraint& c,
+		const Point2& global, const Pose2& trans, const Point2& local) {
+	return LieVector(c.evaluateError(global, trans, local));
+}
 TEST( TransformConstraint, jacobians ) {
 
 	// from examples below
@@ -73,14 +77,14 @@ TEST( TransformConstraint, jacobians ) {
 	tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
 
 	Matrix numericalDT, numericalDL, numericalDF;
-	numericalDF = numericalDerivative31<Vector, Point2, Pose2, Point2>(
+	numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
-			boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
+			boost::bind(evaluateError_, tc, _1, _2, _3),
 			global, trans, local, 1e-5);
-	numericalDT = numericalDerivative32<Vector, Point2, Pose2, Point2>(
+	numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
-			boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
+			boost::bind(evaluateError_, tc, _1, _2, _3),
 			global, trans, local, 1e-5);
-	numericalDL = numericalDerivative33<Vector, Point2, Pose2, Point2>(
+	numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
-			boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
+			boost::bind(evaluateError_, tc, _1, _2, _3),
 			global, trans, local, 1e-5);
 
 	CHECK(assert_equal(numericalDF, actualDF));
@@ -105,14 +109,14 @@ TEST( TransformConstraint, jacobians_zero ) {
 	tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
 
 	Matrix numericalDT, numericalDL, numericalDF;
-	numericalDF = numericalDerivative31<Vector, Point2, Pose2, Point2>(
+	numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
-			boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
+			boost::bind(evaluateError_, tc, _1, _2, _3),
 			global, trans, local, 1e-5);
-	numericalDT = numericalDerivative32<Vector, Point2, Pose2, Point2>(
+	numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
-			boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
+			boost::bind(evaluateError_, tc, _1, _2, _3),
 			global, trans, local, 1e-5);
-	numericalDL = numericalDerivative33<Vector, Point2, Pose2, Point2>(
+	numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
-			boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
+			boost::bind(evaluateError_, tc, _1, _2, _3),
 			global, trans, local, 1e-5);
 
 	CHECK(assert_equal(numericalDF, actualDF));