From c1c2fd7008ccb8ffc42b7c7604d2ffac4cfc63a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 7 Apr 2019 22:31:27 -0400 Subject: [PATCH] Fixed the tests - key was choosing right ordering --- tests/testSubgraphPreconditioner.cpp | 118 ++++++++++++--------------- 1 file changed, 51 insertions(+), 67 deletions(-) diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 1ffa9db93..de753fca6 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -30,6 +30,7 @@ #include +#include #include #include #include @@ -203,88 +204,71 @@ TEST( SubgraphPreconditioner, system ) EXPECT(assert_equal(expected_g, vec(g))); } -/* ************************************************************************* */ - // Test raw vector interface -TEST( SubgraphPreconditioner, RawVectorAPI ) -{ - // Build a planar graph - GaussianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - SubgraphPreconditioner system; - - // Call build, a non-const method needed to make solve work :-( - KeyInfo keyInfo(Ab); - std::map lambda; - system.build(Ab, keyInfo, lambda); - const auto ordering = keyInfo.ordering(); // build changed R1 ! - const Matrix R1 = system.Rc1()->matrix(ordering).first; - - // Test that 'solve' does implement x = R^{-1} y - Vector vector_y = Vector::Zero(18), solve_x(18), solveT_x(18); - vector_y.head(2) << 100, -100; - system.solve(vector_y, solve_x); - EXPECT(assert_equal(R1.inverse() * vector_y, solve_x)); - - // I can't get test below to pass! - // Test that transposeSolve does implement x = R^{-T} y - system.transposeSolve(vector_y, solveT_x); - EXPECT(assert_equal(R1.transpose().inverse() * vector_y, solveT_x)); -} - /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); -TEST( SubgraphSolver, toy3D ) -{ - // Read from file - string inputFile = findExampleDataFile("randomGrid3D"); +// Read from XML file +static GaussianFactorGraph read(const string& name) { + auto inputFile = findExampleDataFile(name); ifstream is(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); + return Ab; +} - // Create solver, leaving splitting to constructor +TEST(SubgraphSolver, Solves) { + // Create preconditioner SubgraphPreconditioner system; - - // Call build, a non-const method needed to make solve work :-( - KeyInfo keyInfo(Ab); - std::map lambda; - system.build(Ab, keyInfo, lambda); - // Solve the VectorValues way - const auto xbar = system.Rc1()->optimize(); // merely for use in zero below - auto values_y = VectorValues::Zero(xbar); - const size_t n = values_y.size(); - values_y[0] = Vector3(100, 200, -300); - values_y[n - 1] = Vector3(10, 20, -100); - auto values_x = system.Rc1()->backSubstitute(values_y); + // We test on three different graphs + const auto Ab1 = planarGraph(3).get<0>(); + const auto Ab2 = read("toy3D"); + const auto Ab3 = read("randomGrid3D"); - // Check YD's sub-vector machinery - const auto ordering = keyInfo.ordering(); - auto vector_y = values_y.vector(ordering); - for (size_t j = 0; j < n; j++) { - EXPECT(assert_equal(values_y[j], getSubvector(vector_y, keyInfo, {j}))); - } + // For all graphs, test solve and solveTranspose + for (const auto& Ab : {Ab1,Ab2,Ab3}) { + // Call build, a non-const method needed to make solve work :-( + KeyInfo keyInfo(Ab); + std::map lambda; + system.build(Ab, keyInfo, lambda); - // Solve the matrix way, this really just checks BN::backSubstitute - const Matrix R1 = system.Rc1()->matrix(ordering).first; - auto vector_x = R1.inverse() * vector_y; - EXPECT(assert_equal(vector_x, values_x.vector(ordering))); + // Create a perturbed (non-zero) RHS + const auto xbar = system.Rc1()->optimize(); // merely for use in zero below + auto values_y = VectorValues::Zero(xbar); + values_y.begin()->second.setConstant(100); + (--values_y.end())->second.setConstant(-100); - // Test that 'solve' does implement x = R^{-1} y - const size_t N = R1.cols(); - EXPECT_LONGS_EQUAL(n * 3, N); - Vector solve_x = Vector::Zero(N); - system.solve(vector_y, solve_x); - EXPECT(assert_equal(vector_x, solve_x)); - for (size_t j = 0; j < n; j++) { - cout << j << endl; - EXPECT(assert_equal(values_x[j], getSubvector(vector_x, keyInfo, {j}), 1e-3)); + // Solve the VectorValues way + auto values_x = system.Rc1()->backSubstitute(values_y); + + // Solve the matrix way, this really just checks BN::backSubstitute + // This only works with Rc1 ordering, not with keyInfo ! + // TODO(frank): why does this not work with an arbitrary ordering? + const auto ord = system.Rc1()->ordering(); + const Matrix R1 = system.Rc1()->matrix(ord).first; + auto ord_y = values_y.vector(ord); + auto vector_x = R1.inverse() * ord_y; + EXPECT(assert_equal(vector_x, values_x.vector(ord))); + + // Test that 'solve' does implement x = R^{-1} y + // We do this by asserting it gives same answer as backSubstitute + // Only works with keyInfo ordering: + const auto ordering = keyInfo.ordering(); + auto vector_y = values_y.vector(ordering); + const size_t N = R1.cols(); + Vector solve_x = Vector::Zero(N); + system.solve(vector_y, solve_x); + EXPECT(assert_equal(values_x.vector(ordering), solve_x)); + + // Test that transposeSolve does implement x = R^{-T} y + // We do this by asserting it gives same answer as backSubstituteTranspose + auto values_x2 = system.Rc1()->backSubstituteTranspose(values_y); + Vector solveT_x = Vector::Zero(N); + system.transposeSolve(vector_y, solveT_x); + EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); } }