Implemented a rank deficient constraint test in testLinearFactor: note that it is possible to reorder elimination to make a system solvable or rank-deficient
Alex Cunningham
parent
ab65dfb4dd
commit
8c695a778c
|
|
@ -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 )
|
||||
//{
|
||||
// // 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 - 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));
|
||||
TEST ( LinearFactor, constraint_eliminate2 )
|
||||
{
|
||||
// Construct a linear constraint
|
||||
// RHS
|
||||
Vector b(2); b(0)=3.0; b(1)=4.0;
|
||||
|
||||
// // eliminate y to test thrown error
|
||||
// VectorConfig fg2;
|
||||
// fg2.insert("x", expected);
|
||||
// actual = lc.eliminate("y");
|
||||
// try {
|
||||
// Vector output = actual->solve(fg2);
|
||||
// CHECK(false);
|
||||
// } catch (...) {
|
||||
// CHECK(true);
|
||||
// }
|
||||
//}
|
||||
// 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 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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue