Added planar graph with easy subtree

cpp/smallExample.cpp

@@ -31,7 +31,8 @@ typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
 	/* ************************************************************************* */
 	boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() {
 		// Create
-	boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(new ExampleNonlinearFactorGraph);
+		boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(
+				new ExampleNonlinearFactorGraph);
 
 		// prior on x1
 		double sigma1 = 0.1;
@@ -41,19 +42,25 @@ boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph(
 
 		// odometry between x1 and x2
 		double sigma2 = 0.1;
-	Vector z2(2); z2(0) = 1.5 ; z2(1) = 0;
+		Vector z2(2);
+		z2(0) = 1.5;
+		z2(1) = 0;
 		shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2"));
 		nlfg->push_back(f2);
 
 		// measurement between x1 and l1
 		double sigma3 = 0.2;
-	Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
+		Vector z3(2);
+		z3(0) = 0.;
+		z3(1) = -1.;
 		shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
 		nlfg->push_back(f3);
 
 		// measurement between x2 and l1
 		double sigma4 = 0.2;
-	Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
+		Vector z4(2);
+		z4(0) = -1.5;
+		z4(1) = -1.;
 		shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
 		nlfg->push_back(f4);
 
@@ -84,7 +91,9 @@ boost::shared_ptr<const VectorConfig> sharedNoisyConfig() {
 	}
 
 	/* ************************************************************************* */
-VectorConfig createNoisyConfig() { return *sharedNoisyConfig();}
+	VectorConfig createNoisyConfig() {
+		return *sharedNoisyConfig();
+	}
 
 	/* ************************************************************************* */
 	VectorConfig createCorrectDelta() {
@@ -105,8 +114,7 @@ VectorConfig createZeroDelta() {
 	}
 
 	/* ************************************************************************* */
-GaussianFactorGraph createGaussianFactorGraph()
+	GaussianFactorGraph createGaussianFactorGraph() {
-{
 		Matrix I = eye(2);
 		VectorConfig c = createNoisyConfig();
 
@@ -142,18 +150,18 @@ GaussianFactorGraph createGaussianFactorGraph()
 	 * 1 1 9
 	 *   1 5
 	 */
-GaussianBayesNet createSmallGaussianBayesNet()
+	GaussianBayesNet createSmallGaussianBayesNet() {
-{
 		Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
 		Matrix R22 = Matrix_(1, 1, 1.0);
 		Vector d1(1), d2(1);
-  d1(0) = 9; d2(0) = 5;
+		d1(0) = 9;
-  Vector tau(1); tau(0) = 1.0;
+		d2(0) = 5;
+		Vector tau(1);
+		tau(0) = 1.0;
 
 		// define nodes and specify in reverse topological sort (i.e. parents last)
-  GaussianConditional::shared_ptr
+		GaussianConditional::shared_ptr Px_y(new GaussianConditional("x", d1, R11,
-    Px_y(new GaussianConditional("x",d1,R11,"y",S12,tau)),
+				"y", S12, tau)), Py(new GaussianConditional("y", d2, R22, tau));
-    Py(new GaussianConditional("y",d2,R22,tau));
 		GaussianBayesNet cbn;
 		cbn.push_back(Px_y);
 		cbn.push_back(Py);
@@ -168,21 +176,23 @@ namespace smallOptimize {
 		Vector h(const Vector& v) {
 			double x = v(0);
 			return Vector_(2, cos(x), sin(x));
-  };
+		}
+		;
 		Matrix H(const Vector& v) {
 			double x = v(0);
 			return Matrix_(2, 1, -sin(x), cos(x));
-  };
+		}
+		;
 	}
 
 	/* ************************************************************************* */
-boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph()
+	boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
-{
+		boost::shared_ptr<ExampleNonlinearFactorGraph> fg(
-	boost::shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph);
+				new ExampleNonlinearFactorGraph);
 		Vector z = Vector_(2, 1.0, 0.0);
 		double sigma = 0.1;
-  boost::shared_ptr<NonlinearFactor1> 
+		boost::shared_ptr<NonlinearFactor1> factor(new NonlinearFactor1(z, sigma,
-    factor(new NonlinearFactor1(z,sigma,&smallOptimize::h,"x",&smallOptimize::H));
+				&smallOptimize::h, "x", &smallOptimize::H));
 		fg->push_back(factor);
 		return fg;
 	}
@@ -212,7 +222,8 @@ pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
 			// odometry between x_t and x_{t-1}
 			Vector odo = Vector_(2, 1.0, 0.0);
 			string key = symbol('x', t);
-		shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), key));
+			shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
+					key));
 			nlfg.push_back(odometry);
 
 			// measurement on x_t is like perfect GPS
@@ -255,8 +266,8 @@ GaussianFactorGraph createSimpleConstraintGraph() {
 		Matrix Ax1 = eye(2);
 		Matrix Ay1 = eye(2) * -1;
 		Vector b2 = Vector_(2, 0.0, 0.0);
-	GaussianFactor::shared_ptr f2(
+		GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
-			new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0));
+				0.0));
 
 		// construct the graph
 		GaussianFactorGraph fg;
@@ -291,12 +302,14 @@ GaussianFactorGraph createSingleConstraintGraph() {
 		// |1 2||x_1| + |10 0||y_1| = |1|
 		// |2 1||x_2|   |0 10||y_2|   |2|
 		Matrix Ax1(2, 2);
-	Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
+		Ax1(0, 0) = 1.0;
-	Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
+		Ax1(0, 1) = 2.0;
+		Ax1(1, 0) = 2.0;
+		Ax1(1, 1) = 1.0;
 		Matrix Ay1 = eye(2) * 10;
 		Vector b2 = Vector_(2, 1.0, 2.0);
-	GaussianFactor::shared_ptr f2(
+		GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
-			new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0));
+				0.0));
 
 		// construct the graph
 		GaussianFactorGraph fg;
@@ -324,29 +337,41 @@ GaussianFactorGraph createMultiConstraintGraph() {
 
 		// constraint 1
 		Matrix A11(2, 2);
-	A11(0,0) = 1.0 ; A11(0,1) = 2.0;
+		A11(0, 0) = 1.0;
-	A11(1,0) = 2.0 ; A11(1,1) = 1.0;
+		A11(0, 1) = 2.0;
+		A11(1, 0) = 2.0;
+		A11(1, 1) = 1.0;
 
 		Matrix A12(2, 2);
-	A12(0,0) = 10.0 ; A12(0,1) = 0.0;
+		A12(0, 0) = 10.0;
-	A12(1,0) = 0.0 ; A12(1,1) = 10.0;
+		A12(0, 1) = 0.0;
+		A12(1, 0) = 0.0;
+		A12(1, 1) = 10.0;
 
 		Vector b1(2);
-	b1(0) = 1.0; b1(1) = 2.0;
+		b1(0) = 1.0;
-	GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1, 0.0));
+		b1(1) = 2.0;
+		GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1,
+				0.0));
 
 		// constraint 2
 		Matrix A21(2, 2);
-	A21(0,0) =  3.0 ; A21(0,1) =  4.0;
+		A21(0, 0) = 3.0;
-	A21(1,0) = -1.0 ; A21(1,1) = -2.0;
+		A21(0, 1) = 4.0;
+		A21(1, 0) = -1.0;
+		A21(1, 1) = -2.0;
 
 		Matrix A22(2, 2);
-	A22(0,0) = 1.0 ; A22(0,1) = 1.0;
+		A22(0, 0) = 1.0;
-	A22(1,0) = 1.0 ; A22(1,1) = 2.0;
+		A22(0, 1) = 1.0;
+		A22(1, 0) = 1.0;
+		A22(1, 1) = 2.0;
 
 		Vector b2(2);
-	b2(0) = 3.0; b2(1) = 4.0;
+		b2(0) = 3.0;
-	GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2, 0.0));
+		b2(1) = 4.0;
+		GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2,
+				0.0));
 
 		// construct the graph
 		GaussianFactorGraph fg;
@@ -468,4 +493,91 @@ VectorConfig createMultiConstraintConfig() {
 	//	return cbn;
 	//}
 
+	/* ************************************************************************* */
+	// Create key for simulated planar graph
+	string key(int x, int y) {
+		stringstream ss;
+		ss << "x" << x << y;
+		return ss.str();
+	}
+
+	/* ************************************************************************* */
+	pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N) {
+
+		// create empty graph
+		NonlinearFactorGraph<VectorConfig> nlfg;
+
+		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
+		double sigma0 = 1e-3;
+		shared constraint(new Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
+		nlfg.push_back(constraint);
+
+		double sigma = 0.01;
+
+		// Create horizontal constraints, 1...N*(N-1)
+		Vector z1 = Vector_(2, 1.0, 0.0); // move right
+		for (size_t x = 1; x < N; x++)
+			for (size_t y = 1; y <= N; y++) {
+				shared f(new Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
+				nlfg.push_back(f);
+			}
+
+		// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
+		Vector z2 = Vector_(2, 0.0, 1.0); // move up
+		for (size_t x = 1; x <= N; x++)
+			for (size_t y = 1; y < N; y++) {
+				shared f(new Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
+				nlfg.push_back(f);
+			}
+
+		// Create linearization and ground xtrue config
+		VectorConfig zeros, xtrue;
+		for (size_t x = 1; x <= N; x++)
+			for (size_t y = 1; y <= N; y++) {
+				zeros.add(key(x, y), zero(2));
+				xtrue.add(key(x, y), Vector_(2, (double) x, double(y)));
+			}
+
+		// linearize around zero
+		GaussianFactorGraph A = nlfg.linearize(zeros);
+
+		return make_pair(A, xtrue);
+	}
+
+	/* ************************************************************************* */
+	Ordering planarOrdering(size_t N) {
+		Ordering ordering;
+		for (size_t y = N; y >= 1; y--)
+			for (size_t x = N; x >= 1; x--)
+				ordering.push_back(key(x, y));
+		return ordering;
+	}
+
+	/* ************************************************************************* */
+	pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
+			const GaussianFactorGraph& original) {
+		GaussianFactorGraph T, C;
+
+		// Add the x11 constraint to the tree
+		T.push_back(original[0]);
+
+		// Add all horizontal constraints to the tree
+		size_t i = 1;
+		for (size_t x = 1; x < N; x++)
+			for (size_t y = 1; y <= N; y++, i++)
+				T.push_back(original[i]);
+
+		// Add first vertical column of constraints to T, others to C
+		for (size_t x = 1; x <= N; x++)
+			for (size_t y = 1; y < N; y++, i++)
+				if (x == 1)
+					T.push_back(original[i]);
+				else
+					C.push_back(original[i]);
+
+		return make_pair(T, C);
+	}
+
+/* ************************************************************************* */
+
 } // namespace gtsam

cpp/smallExample.h

@@ -130,4 +130,38 @@ namespace gtsam {
 	 */
 //	ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig>
 //		createConstrainedNonlinearFactorGraph();
-}
+
+	/* ******************************************************* */
+	// Planar graph with easy subtree for SubgraphPreconditioner
+	/* ******************************************************* */
+
+	/*
+	 * Create factor graph with N^2 nodes, for example for N=3
+	 *  x13-x23-x33
+	 *   |   |   |
+	 *  x12-x22-x32
+	 *   |   |   |
+	 * -x11-x21-x31
+	 * with x11 clamped at (1,1), and others related by 2D odometry.
+	 */
+	std::pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N);
+
+	/*
+	 * Create canonical ordering for planar graph that also works for tree
+	 * With x11 the root, e.g. for N=3
+	 * x33 x23 x13 x32 x22 x12 x31 x21 x11
+	 */
+	Ordering planarOrdering(size_t N);
+
+	/*
+	 * Split graph into tree and loop closing constraints, e.g., with N=3
+	 *  x13-x23-x33
+	 *   |
+	 *  x12-x22-x32
+	 *   |
+	 * -x11-x21-x31
+	 */
+	std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
+			const GaussianFactorGraph& original);
+
+} // gtsam