Implemented a rank deficient constraint test in testLinearFactor: note that it is possible to reorder elimination to make a system solvable or rank-deficient

cpp/testLinearFactor.cpp

@@ -658,51 +658,81 @@ TEST ( LinearFactor, constraint_eliminate1 )
 	CHECK(assert_equal(expCG, *actualCG));
 }
 
-// This test fails due to multiple constraints on a node
 /* ************************************************************************* */
-//TEST ( LinearFactor, constraint_eliminate2 )
+TEST ( LinearFactor, constraint_eliminate2 )
-//{
+{
-//	// Construct a linear constraint
+	// Construct a linear constraint
-//	// RHS
+	// RHS
-//	Vector b(2); b(0)=3.0; b(1)=4.0;
+	Vector b(2); b(0)=3.0; b(1)=4.0;
-//
-//	// A1 - invertible
-//	Matrix A1(2,2);
-//	A1(0,0) = 1.0 ; A1(0,1) = 2.0;
-//	A1(1,0) = 2.0 ; A1(1,1) = 1.0;
-//
-//	// A2 - not invertible - solve will throw an exception
-//	Matrix A2(2,2);
-//	A2(0,0) = 1.0 ; A2(0,1) = 2.0;
-//	A2(1,0) = 2.0 ; A2(1,1) = 4.0;
-//
-//	LinearFactor lc("x", A1, "y", A2, b, 0.0);
-//
-//	Vector y = Vector_(2, 1.0, 2.0);
-//
-//	VectorConfig fg1;
-//	fg1.insert("y", y);
-//
-//	Vector expected = Vector_(2, -3.3333, 0.6667);
-//
-//	// eliminate x for basic check
-//
-//	ConditionalGaussian::shared_ptr actualCG;
-//	LinearFactor::shared_ptr actualLF;
-//	boost::tie(actualCG, actualLF) = lc.eliminate("x");
-//	CHECK(assert_equal(expected, actualCG->solve(fg1), 1e-4));
 
-//	// eliminate y to test thrown error
+	// A1 - invertible
-//	VectorConfig fg2;
+	Matrix A1(2,2);
-//	fg2.insert("x", expected);
+	A1(0,0) = 1.0 ; A1(0,1) = 2.0;
-//	actual = lc.eliminate("y");
+	A1(1,0) = 2.0 ; A1(1,1) = 1.0;
-//	try {
+
-//		Vector output = actual->solve(fg2);
+	// A2 - not invertible
-//		CHECK(false);
+	Matrix A2(2,2);
-//	} catch (...) {
+	A2(0,0) = 1.0 ; A2(0,1) = 2.0;
-//		CHECK(true);
+	A2(1,0) = 2.0 ; A2(1,1) = 4.0;
-//	}
+
-//}
+	LinearFactor lc("x", A1, "y", A2, b, 0.0);
+
+	// eliminate x and verify results
+	ConditionalGaussian::shared_ptr actualCG;
+	LinearFactor::shared_ptr actualLF;
+	boost::tie(actualCG, actualLF) = lc.eliminate("x");
+
+	// LF should be null
+	LinearFactor expectedLF;
+	CHECK(assert_equal(*actualLF, expectedLF));
+
+	// verify CG
+	Matrix R = Matrix_(2, 2,
+			1.0,    2.0,
+			0.0,    1.0);
+	Matrix S = Matrix_(2,2,
+			1.0,    2.0,
+			0.0,    0.0);
+	Vector d = Vector_(2, 3.0, 0.6666);
+	ConditionalGaussian expectedCG("x", d, R, "y", S, zero(2));
+	CHECK(assert_equal(expectedCG, *actualCG, 1e-4));
+}
+
+/* ************************************************************************* */
+TEST ( LinearFactor, constraint_eliminate3 )
+{
+	// This test shows that ordering matters if there are non-invertable
+	// blocks, as this example can be eliminated if x is first, but not
+	// if y is first.
+
+	// Construct a linear constraint
+	// RHS
+	Vector b(2); b(0)=3.0; b(1)=4.0;
+
+	// A1 - invertible
+	Matrix A1(2,2);
+	A1(0,0) = 1.0 ; A1(0,1) = 2.0;
+	A1(1,0) = 2.0 ; A1(1,1) = 1.0;
+
+	// A2 - not invertible
+	Matrix A2(2,2);
+	A2(0,0) = 1.0 ; A2(0,1) = 2.0;
+	A2(1,0) = 2.0 ; A2(1,1) = 4.0;
+
+	LinearFactor lc("x", A1, "y", A2, b, 0.0);
+
+	// eliminate y from original graph
+	// NOTE: this will throw an exception, as
+	// the leading matrix is rank deficient
+	ConditionalGaussian::shared_ptr actualCG;
+	LinearFactor::shared_ptr actualLF;
+	try {
+		boost::tie(actualCG, actualLF) = lc.eliminate("y");
+		CHECK(false);
+	} catch (domain_error) {
+		CHECK(true);
+	}
+}
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

cpp/testLinearFactorGraph.cpp

@@ -605,7 +605,6 @@ TEST( LinearFactorGraph, constrained_simple )
 	CHECK(assert_equal(actual, expected));
 }
 
-// These tests require multiple constraints on a single node and will fail
 /* ************************************************************************* */
 TEST( LinearFactorGraph, constrained_single )
 {
@@ -639,7 +638,7 @@ TEST( LinearFactorGraph, constrained_single2 )
 }
 
 /* ************************************************************************* */
-TEST( LinearFactorGraph, constrained_multi )
+TEST( LinearFactorGraph, constrained_multi1 )
 {
 	// get a graph with a constraint in it
 	LinearFactorGraph fg = createMultiConstraintGraph();
@@ -654,6 +653,22 @@ TEST( LinearFactorGraph, constrained_multi )
 	CHECK(assert_equal(actual, expected));
 }
 
+/* ************************************************************************* */
+TEST( LinearFactorGraph, constrained_multi2 )
+{
+	// get a graph with a constraint in it
+	LinearFactorGraph fg = createMultiConstraintGraph();
+
+	// eliminate and solve
+	Ordering ord;
+	ord += "z", "x", "y";
+	VectorConfig actual = fg.optimize(ord);
+
+	// verify
+	VectorConfig expected = createMultiConstraintConfig();
+	CHECK(assert_equal(actual, expected));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */