diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index de753fca6..d06aab7d2 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -15,24 +15,23 @@ * @author Frank Dellaert **/ - #include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include #include #include +#include #include #include using namespace boost::assign; @@ -45,50 +44,46 @@ using namespace example; // define keys // Create key for simulated planar graph -Symbol key(int x, int y) { - return symbol_shorthand::X(1000*x+y); -} +Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarOrdering ) { +TEST(SubgraphPreconditioner, planarOrdering) { // Check canonical ordering Ordering expected, ordering = planarOrdering(3); expected += key(3, 3), key(2, 3), key(1, 3), key(3, 2), key(2, 2), key(1, 2), key(3, 1), key(2, 1), key(1, 1); - EXPECT(assert_equal(expected,ordering)); + EXPECT(assert_equal(expected, ordering)); } /* ************************************************************************* */ /** unnormalized error */ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - for(const GaussianFactor::shared_ptr& factor: fg) + for (const GaussianFactor::shared_ptr& factor : fg) total_error += factor->error(x); return total_error; } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarGraph ) - { +TEST(SubgraphPreconditioner, planarGraph) { // Check planar graph construction GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); - LONGS_EQUAL(13,A.size()); - LONGS_EQUAL(9,xtrue.size()); - DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue + LONGS_EQUAL(13, A.size()); + LONGS_EQUAL(9, xtrue.size()); + DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue // Check that xtrue is optimal GaussianBayesNet::shared_ptr R1 = A.eliminateSequential(); VectorValues actual = R1->optimize(); - EXPECT(assert_equal(xtrue,actual)); + EXPECT(assert_equal(xtrue, actual)); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, splitOffPlanarTree ) -{ +TEST(SubgraphPreconditioner, splitOffPlanarTree) { // Build a planar graph GaussianFactorGraph A; VectorValues xtrue; @@ -97,48 +92,48 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree ) // Get the spanning tree and constraints, and check their sizes GaussianFactorGraph::shared_ptr T, C; boost::tie(T, C) = splitOffPlanarTree(3, A); - LONGS_EQUAL(9,T->size()); - LONGS_EQUAL(4,C->size()); + LONGS_EQUAL(9, T->size()); + LONGS_EQUAL(4, C->size()); // Check that the tree can be solved to give the ground xtrue GaussianBayesNet::shared_ptr R1 = T->eliminateSequential(); VectorValues xbar = R1->optimize(); - EXPECT(assert_equal(xtrue,xbar)); + EXPECT(assert_equal(xtrue, xbar)); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, system ) -{ +TEST(SubgraphPreconditioner, system) { // 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) = planarGraph(N); // A*x-b // Get the spanning tree and remaining graph - GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); // Eliminate the spanning tree to build a prior const Ordering ord = planarOrdering(N); - auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1 - VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 + auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1 + VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + VectorValues::shared_ptr xbarShared( + new VectorValues(xbar)); // TODO: horrible const SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Get corresponding matrices for tests. Add dummy factors to Ab2 to make // sure it works with the ordering. - Ordering ordering = Rc1->ordering(); // not ord in general! - Ab2->add(key(1,1),Z_2x2, Z_2x1); - Ab2->add(key(1,2),Z_2x2, Z_2x1); - Ab2->add(key(1,3),Z_2x2, Z_2x1); + Ordering ordering = Rc1->ordering(); // not ord in general! + Ab2->add(key(1, 1), Z_2x2, Z_2x1); + Ab2->add(key(1, 2), Z_2x2, Z_2x1); + Ab2->add(key(1, 3), Z_2x2, Z_2x1); Matrix A, A1, A2; Vector b, b1, b2; - std::tie(A,b) = Ab.jacobian(ordering); - std::tie(A1,b1) = Ab1->jacobian(ordering); - std::tie(A2,b2) = Ab2->jacobian(ordering); + std::tie(A, b) = Ab.jacobian(ordering); + std::tie(A1, b1) = Ab1->jacobian(ordering); + std::tie(A2, b2) = Ab2->jacobian(ordering); Matrix R1 = Rc1->matrix(ordering).first; Matrix Abar(13 * 2, 9 * 2); Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2); @@ -146,14 +141,14 @@ TEST( SubgraphPreconditioner, system ) // Helper function to vectorize in correct order, which is the order in which // we eliminated the spanning tree. - auto vec = [ordering](const VectorValues& x) { return x.vector(ordering);}; + auto vec = [ordering](const VectorValues& x) { return x.vector(ordering); }; // Set up y0 as all zeros const VectorValues y0 = system.zero(); // y1 = perturbed y0 VectorValues y1 = system.zero(); - y1[key(3,3)] = Vector2(1.0, -1.0); + y1[key(3, 3)] = Vector2(1.0, -1.0); // Check backSubstituteTranspose works with R1 VectorValues actual = Rc1->backSubstituteTranspose(y1); @@ -169,22 +164,22 @@ TEST( SubgraphPreconditioner, system ) EXPECT(assert_equal(expected_x1, vec(x1))); // Check errors - DOUBLES_EQUAL(0,error(Ab,xbar),1e-9); - DOUBLES_EQUAL(0,system.error(y0),1e-9); - DOUBLES_EQUAL(2,error(Ab,x1),1e-9); - DOUBLES_EQUAL(2,system.error(y1),1e-9); + DOUBLES_EQUAL(0, error(Ab, xbar), 1e-9); + DOUBLES_EQUAL(0, system.error(y0), 1e-9); + DOUBLES_EQUAL(2, error(Ab, x1), 1e-9); + DOUBLES_EQUAL(2, system.error(y1), 1e-9); // Check that transposeMultiplyAdd <=> y += alpha * Abar' * e // We check for e1 =[1;0] and e2=[0;1] corresponding to T and C const double alpha = 0.5; - Errors e1,e2; - for (size_t i=0;i<13;i++) { - e1 += i<9 ? Vector2(1, 1) : Vector2(0, 0); - e2 += i>=9 ? Vector2(1, 1) : Vector2(0, 0); + Errors e1, e2; + for (size_t i = 0; i < 13; i++) { + e1 += i < 9 ? Vector2(1, 1) : Vector2(0, 0); + e2 += i >= 9 ? Vector2(1, 1) : Vector2(0, 0); } - Vector ee1(13*2), ee2(13*2); - ee1 << Vector::Ones(9*2), Vector::Zero(4*2); - ee2 << Vector::Zero(9*2), Vector::Ones(4*2); + Vector ee1(13 * 2), ee2(13 * 2); + ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2); + ee2 << Vector::Zero(9 * 2), Vector::Ones(4 * 2); // Check transposeMultiplyAdd for e1 VectorValues y = system.zero(); @@ -211,8 +206,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); static GaussianFactorGraph read(const string& name) { auto inputFile = findExampleDataFile(name); ifstream is(inputFile); - if (!is.is_open()) - throw runtime_error("Cannot find file " + inputFile); + if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); boost::archive::xml_iarchive in_archive(is); GaussianFactorGraph Ab; in_archive >> boost::serialization::make_nvp("graph", Ab); @@ -229,7 +223,7 @@ TEST(SubgraphSolver, Solves) { const auto Ab3 = read("randomGrid3D"); // For all graphs, test solve and solveTranspose - for (const auto& Ab : {Ab1,Ab2,Ab3}) { + for (const auto& Ab : {Ab1, Ab2, Ab3}) { // Call build, a non-const method needed to make solve work :-( KeyInfo keyInfo(Ab); std::map lambda; @@ -273,24 +267,25 @@ TEST(SubgraphSolver, Solves) { } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, conjugateGradients ) -{ +TEST(SubgraphPreconditioner, conjugateGradients) { // 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) = planarGraph(N); // A*x-b // Get the spanning tree - GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); // Eliminate the spanning tree to build a prior - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1->eliminateSequential(); // R1*x-c1 - VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 + SubgraphPreconditioner::sharedBayesNet Rc1 = + Ab1->eliminateSequential(); // R1*x-c1 + VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + VectorValues::shared_ptr xbarShared( + new VectorValues(xbar)); // TODO: horrible SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config y0 and perturbed config y1 @@ -308,9 +303,12 @@ TEST( SubgraphPreconditioner, conjugateGradients ) // Compare with non preconditioned version: VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); - EXPECT(assert_equal(xtrue,actual2,1e-4)); + EXPECT(assert_equal(xtrue, actual2, 1e-4)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */