From 110240aa4f2d6d22b307c8b12ce67ff87f18783f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 20:10:46 -0400 Subject: [PATCH] Resurrected lots of elimination tests --- tests/testGaussianFactorGraphB.cpp | 250 +++++++++++------------------ 1 file changed, 93 insertions(+), 157 deletions(-) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 879ebc1c1..1bad1c412 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -169,241 +169,178 @@ TEST(GaussianFactorGraph, eliminateOne_l1_fast) { EXPECT(assert_equal(expected, *actual, tol)); } -#if 0 +// /* ************************************************************************* */ +// TEST( GaussianFactorGraph, eliminateAll ) +// { +// // create expected Chordal bayes Net +// Matrix I = I_2x2; + +// Ordering ordering; +// ordering += X(2),L(1),X(1); + +// Vector d1 = Vector2(-0.1,-0.1); +// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1); + +// double sig1 = 0.149071; +// Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2); +// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2); + +// double sig2 = 0.0894427; +// Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2); +// push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3); + +// // Check one ordering +// GaussianFactorGraph fg1 = createGaussianFactorGraph(); +// GaussianBayesNet actual = *fg1.eliminateSequential(); +// EXPECT(assert_equal(expected,actual,tol)); + +// GaussianBayesNet actualQR = *fg1.eliminateSequential(, true); +// EXPECT(assert_equal(expected,actualQR,tol)); +// } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateAll ) -{ - // create expected Chordal bayes Net - Matrix I = I_2x2; - - Ordering ordering; - ordering += X(2),L(1),X(1); - - Vector d1 = Vector2(-0.1,-0.1); - GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1); - - double sig1 = 0.149071; - Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2); - push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2); - - double sig2 = 0.0894427; - Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2); - push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3); - - // Check one ordering - GaussianFactorGraph fg1 = createGaussianFactorGraph(); - GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate(); - EXPECT(assert_equal(expected,actual,tol)); - - GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate(); - EXPECT(assert_equal(expected,actualQR,tol)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, copying ) -{ +TEST(GaussianFactorGraph, copying) { // Create a graph - Ordering ordering; ordering += X(2),L(1),X(1); GaussianFactorGraph actual = createGaussianFactorGraph(); // Copy the graph ! GaussianFactorGraph copy = actual; // now eliminate the copy - GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate(); + GaussianBayesNet actual1 = *copy.eliminateSequential(); // Create the same graph, but not by copying GaussianFactorGraph expected = createGaussianFactorGraph(); // and check that original is still the same graph - EXPECT(assert_equal(expected,actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) -{ - Ordering ord; - ord += X(2),L(1),X(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ord); +TEST(GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet) { + GaussianFactorGraph fg = createGaussianFactorGraph(); // render with a given ordering - GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate(); + GaussianBayesNet CBN = *fg.eliminateSequential(); // True GaussianFactorGraph GaussianFactorGraph fg2(CBN); - GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); - EXPECT(assert_equal(CBN,CBN2)); + GaussianBayesNet CBN2 = *fg2.eliminateSequential(); + EXPECT(assert_equal(CBN, CBN2)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, getOrdering) -{ - Ordering original; original += L(1),X(1),X(2); - FactorGraph symbolic(createGaussianFactorGraph(original)); - Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); - Ordering actual = original; actual.permuteInPlace(perm); - Ordering expected; expected += L(1),X(2),X(1); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, optimize_Cholesky ) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - +TEST(GaussianFactorGraph, optimize_Cholesky) { // create a graph - GaussianFactorGraph fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = *GaussianSequentialSolver(fg, false).optimize(); + VectorValues actual = fg.optimize(boost::none, EliminateCholesky); // verify - VectorValues expected = createCorrectDelta(ord); - - EXPECT(assert_equal(expected,actual)); + VectorValues expected = createCorrectDelta(); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST( GaussianFactorGraph, optimize_QR ) { - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - // create a graph - GaussianFactorGraph fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = *GaussianSequentialSolver(fg, true).optimize(); + VectorValues actual = fg.optimize(boost::none, EliminateQR); // verify - VectorValues expected = createCorrectDelta(ord); - + VectorValues expected = createCorrectDelta(); EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, combine) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - +TEST(GaussianFactorGraph, combine) { // create a test graph - GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg1 = createGaussianFactorGraph(); // create another factor graph - GaussianFactorGraph fg2 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg2 = createGaussianFactorGraph(); // get sizes size_t size1 = fg1.size(); size_t size2 = fg2.size(); // combine them - fg1.combine(fg2); + fg1.push_back(fg2); - EXPECT(size1+size2 == fg1.size()); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, combine2) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - - // create a test graph - GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); - - // create another factor graph - GaussianFactorGraph fg2 = createGaussianFactorGraph(ord); - - // get sizes - size_t size1 = fg1.size(); - size_t size2 = fg2.size(); - - // combine them - GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2); - - EXPECT(size1+size2 == fg3.size()); + EXPECT(size1 + size2 == fg1.size()); } /* ************************************************************************* */ // print a vector of ints if needed for debugging void print(vector v) { - for (size_t k = 0; k < v.size(); k++) - cout << v[k] << " "; + for (size_t k = 0; k < v.size(); k++) cout << v[k] << " "; cout << endl; } /* ************************************************************************* */ -TEST(GaussianFactorGraph, createSmoother) -{ - GaussianFactorGraph fg1 = createSmoother(2).first; - LONGS_EQUAL(3,fg1.size()); - GaussianFactorGraph fg2 = createSmoother(3).first; - LONGS_EQUAL(5,fg2.size()); +TEST(GaussianFactorGraph, createSmoother) { + GaussianFactorGraph fg1 = createSmoother(2); + LONGS_EQUAL(3, fg1.size()); + GaussianFactorGraph fg2 = createSmoother(3); + LONGS_EQUAL(5, fg2.size()); } /* ************************************************************************* */ double error(const VectorValues& x) { - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - - GaussianFactorGraph fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(); return fg.error(x); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplication ) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - - GaussianFactorGraph A = createGaussianFactorGraph(ord); - VectorValues x = createCorrectDelta(ord); +TEST(GaussianFactorGraph, multiplication) { + GaussianFactorGraph A = createGaussianFactorGraph(); + VectorValues x = createCorrectDelta(); Errors actual = A * x; Errors expected; - expected += Vector2(-1.0,-1.0); - expected += Vector2(2.0,-1.0); + expected += Vector2(-1.0, -1.0); + expected += Vector2(2.0, -1.0); expected += Vector2(0.0, 1.0); expected += Vector2(-1.0, 1.5); - EXPECT(assert_equal(expected,actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ // Extra test on elimination prompted by Michael's email to Frank 1/4/2010 -TEST( GaussianFactorGraph, elimination ) -{ - Ordering ord; - ord += X(1), X(2); +TEST(GaussianFactorGraph, elimination) { // Create Gaussian Factor Graph GaussianFactorGraph fg; - Matrix Ap = I_2x2, An = I_2x2 * -1; + Matrix Ap = I_1x1, An = I_1x1 * -1; Vector b = (Vector(1) << 0.0).finished(); - SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); - fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; - fg += ord[X(1)], Ap, b, sigma; - fg += ord[X(2)], Ap, b, sigma; + SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1, 2.0); + fg += JacobianFactor(X(1), An, X(2), Ap, b, sigma); + fg += JacobianFactor(X(1), Ap, b, sigma); + fg += JacobianFactor(X(2), Ap, b, sigma); // Eliminate - GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); - - // Check sigma - EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5); + Ordering ordering; + ordering += X(1), X(2); + GaussianBayesNet bayesNet = *fg.eliminateSequential(); // Check matrix - Matrix R;Vector d; - boost::tie(R,d) = matrix(bayesNet); - Matrix expected = (Matrix(2, 2) << - 0.707107, -0.353553, - 0.0, 0.612372).finished(); - Matrix expected2 = (Matrix(2, 2) << - 0.707107, -0.353553, - 0.0, -0.612372).finished(); - EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6)); + Matrix R; + Vector d; + boost::tie(R, d) = bayesNet.matrix(); + Matrix expected = + (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished(); + Matrix expected2 = + (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372).finished(); + EXPECT(assert_equal(expected, R, 1e-6)); + EXPECT(equal_with_abs_tol(expected, R, 1e-6) || + equal_with_abs_tol(expected2, R, 1e-6)); } - /* ************************************************************************* */ +#if 0 + +/* ************************************************************************* */ // Tests ported from ConstrainedGaussianFactorGraph /* ************************************************************************* */ TEST( GaussianFactorGraph, constrained_simple ) @@ -412,9 +349,8 @@ TEST( GaussianFactorGraph, constrained_simple ) GaussianFactorGraph fg = createSimpleConstraintGraph(); EXPECT(hasConstraints(fg)); - // eliminate and solve - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *fg.eliminateSequential().optimize(); // verify VectorValues expected = createSimpleConstraintValues(); @@ -429,7 +365,7 @@ TEST( GaussianFactorGraph, constrained_single ) EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *fg.eliminateSequential().optimize(); // verify VectorValues expected = createSingleConstraintValues(); @@ -444,7 +380,7 @@ TEST( GaussianFactorGraph, constrained_multi1 ) EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *fg.eliminateSequential().optimize(); // verify VectorValues expected = createMultiConstraintValues(); @@ -462,13 +398,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise)); + X(1), I_3x3, X(2), I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise)); + X(2), I_3x3, X(3), I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise)); + X(3), I_3x3, X(4), I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise)); + X(5), I_3x3, X(6), I_3x3, Z_3x1, noise)); GaussianFactorGraph actual; actual.push_back(f1); @@ -497,9 +433,9 @@ TEST(GaussianFactorGraph, createSmoother2) vector x3var; x3var.push_back(ordering[X(3)]); vector x1var; x1var.push_back(X(1)); GaussianBayesNet p_x3 = *GaussianSequentialSolver( - *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); + *fg2.eliminateSequential().jointFactorGraph(x3var)); GaussianBayesNet p_x1 = *GaussianSequentialSolver( - *GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate(); + *fg2.eliminateSequential().jointFactorGraph(x1var)); CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry }