diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index aeeed1b9f..0a2a1788d 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -15,26 +15,27 @@ * @author Yong-Dian Jian **/ -#include - -#if 0 - #include -#include #include #include #include #include +#include #include #include +#include + #include #include using namespace boost::assign; using namespace std; using namespace gtsam; -using namespace example; + +static size_t N = 3; +static SubgraphSolverParameters kParameters; +static auto kOrdering = example::planarOrdering(N); /* ************************************************************************* */ /** unnormalized error */ @@ -45,20 +46,17 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { return total_error; } - /* ************************************************************************* */ TEST( SubgraphSolver, constructor1 ) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // The first constructor just takes a factor graph (and parameters) + // The first constructor just takes a factor graph (and kParameters) // and it will split the graph into A1 and A2, where A1 is a spanning tree - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab, parameters); + SubgraphSolver solver(Ab, kParameters, kOrdering); VectorValues optimized = solver.optimize(); // does PCG optimization DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -70,16 +68,15 @@ TEST( SubgraphSolver, constructor2 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering + // Get the spanning tree GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); - // The second constructor takes two factor graphs, - // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab1_, Ab2_, parameters); + // The second constructor takes two factor graphs, so the caller can specify + // the preconditioner (Ab1) and the constraints that are left out (Ab2) + SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -91,26 +88,22 @@ TEST( SubgraphSolver, constructor3 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering + // Get the spanning tree and corresponding kOrdering GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); - // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT - GaussianBayesNet::shared_ptr Rc1 = // - EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT + auto Rc1 = Ab1_.eliminateSequential(); // The third constructor allows the caller to pass an already solved preconditioner Rc1_ // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before - SubgraphSolverParameters parameters; - SubgraphSolver solver(Rc1, Ab2_, parameters); + SubgraphSolver solver(Rc1, Ab2_, kParameters); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */