Resurrected lots of elimination tests

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 )
+TEST(GaussianFactorGraph, copying) {
-{
-  // 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 )
-{
   // 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 )
+TEST(GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet) {
-{
+  GaussianFactorGraph fg = createGaussianFactorGraph();
-  Ordering ord;
-  ord += X(2),L(1),X(1);
-  GaussianFactorGraph fg = createGaussianFactorGraph(ord);
 
   // render with a given ordering
-  GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate();
+  GaussianBayesNet CBN = *fg.eliminateSequential();
 
   // True GaussianFactorGraph
   GaussianFactorGraph fg2(CBN);
-  GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
+  GaussianBayesNet CBN2 = *fg2.eliminateSequential();
-  EXPECT(assert_equal(CBN,CBN2));
+  EXPECT(assert_equal(CBN, CBN2));
 }
 
 /* ************************************************************************* */
-TEST( GaussianFactorGraph, getOrdering)
+TEST(GaussianFactorGraph, optimize_Cholesky) {
-{
-  Ordering original; original += L(1),X(1),X(2);
-  FactorGraph<IndexFactor> 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);
-
   // 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);
+  VectorValues expected = createCorrectDelta();
-
+  EXPECT(assert_equal(expected, actual));
-  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)
+TEST(GaussianFactorGraph, combine) {
-{
-  // create an ordering
-  Ordering ord; ord += X(2),L(1),X(1);
-
   // 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());
+  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());
 }
 
 /* ************************************************************************* */
 // print a vector of ints if needed for debugging
 void print(vector<int> v) {
-  for (size_t k = 0; k < v.size(); k++)
+  for (size_t k = 0; k < v.size(); k++) cout << v[k] << " ";
-    cout << v[k] << " ";
   cout << endl;
 }
 
 /* ************************************************************************* */
-TEST(GaussianFactorGraph, createSmoother)
+TEST(GaussianFactorGraph, createSmoother) {
-{
+  GaussianFactorGraph fg1 = createSmoother(2);
-  GaussianFactorGraph fg1 = createSmoother(2).first;
+  LONGS_EQUAL(3, fg1.size());
-  LONGS_EQUAL(3,fg1.size());
+  GaussianFactorGraph fg2 = createSmoother(3);
-  GaussianFactorGraph fg2 = createSmoother(3).first;
+  LONGS_EQUAL(5, fg2.size());
-  LONGS_EQUAL(5,fg2.size());
 }
 
 /* ************************************************************************* */
 double error(const VectorValues& x) {
-  // create an ordering
+  GaussianFactorGraph fg = createGaussianFactorGraph();
-  Ordering ord; ord += X(2),L(1),X(1);
-
-  GaussianFactorGraph fg = createGaussianFactorGraph(ord);
   return fg.error(x);
 }
 
 /* ************************************************************************* */
-TEST( GaussianFactorGraph, multiplication )
+TEST(GaussianFactorGraph, multiplication) {
-{
+  GaussianFactorGraph A = createGaussianFactorGraph();
-  // create an ordering
+  VectorValues x = createCorrectDelta();
-  Ordering ord; ord += X(2),L(1),X(1);
-
-  GaussianFactorGraph A = createGaussianFactorGraph(ord);
-  VectorValues x = createCorrectDelta(ord);
   Errors actual = A * x;
   Errors expected;
-  expected += Vector2(-1.0,-1.0);
+  expected += Vector2(-1.0, -1.0);
-  expected += Vector2(2.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 )
+TEST(GaussianFactorGraph, elimination) {
-{
-  Ordering ord;
-  ord += X(1), X(2);
   // 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);
+  SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1, 2.0);
-  fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma;
+  fg += JacobianFactor(X(1), An, X(2), Ap, b, sigma);
-  fg += ord[X(1)], Ap, b, sigma;
+  fg += JacobianFactor(X(1), Ap, b, sigma);
-  fg += ord[X(2)], Ap, b, sigma;
+  fg += JacobianFactor(X(2), Ap, b, sigma);
 
   // Eliminate
-  GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
+  Ordering ordering;
-
+  ordering += X(1), X(2);
-  // Check sigma
+  GaussianBayesNet bayesNet = *fg.eliminateSequential();
-  EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5);
 
   // Check matrix
-  Matrix R;Vector d;
+  Matrix R;
-  boost::tie(R,d) = matrix(bayesNet);
+  Vector d;
-  Matrix expected = (Matrix(2, 2) <<
+  boost::tie(R, d) = bayesNet.matrix();
-      0.707107,  -0.353553,
+  Matrix expected =
-      0.0,   0.612372).finished();
+      (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished();
-  Matrix expected2 = (Matrix(2, 2) <<
+  Matrix expected2 =
-      0.707107,  -0.353553,
+      (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372).finished();
-      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));
+  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<Index> x3var; x3var.push_back(ordering[X(3)]);
   vector<Index> 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
 }