diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 496122419..3f030cdff 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -592,6 +592,76 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(genericStereoFactor3D)); } +/* ************************************************************************* */ +// Read from XML file +namespace { +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; +} +} // namespace + +/* ************************************************************************* */ +// Read from XML file +TEST(SubgraphSolver, Solves) { + // Create preconditioner + SubgraphPreconditioner system; + + // We test on three different graphs + const auto Ab1 = planarGraph(3).first; + const auto Ab2 = read("toy3D"); + const auto Ab3 = read("randomGrid3D"); + + // 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); + + // Create a perturbed (non-zero) RHS + const auto xbar = system.Rc1().optimize(); // merely for use in zero below + auto values_y = VectorValues::Zero(xbar); + auto it = values_y.begin(); + it->second.setConstant(100); + ++it; + it->second.setConstant(-100); + + // 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)); + } +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index eeba38b68..c5b4e42ec 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -29,10 +29,8 @@ #include -#include #include #include -#include #include using namespace boost::assign; @@ -197,75 +195,6 @@ TEST(SubgraphPreconditioner, system) { EXPECT(assert_equal(expected_g, vec(g))); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor") - -// 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; -} - -TEST(SubgraphSolver, Solves) { - // Create preconditioner - SubgraphPreconditioner system; - - // We test on three different graphs - const auto Ab1 = planarGraph(3).first; - const auto Ab2 = read("toy3D"); - const auto Ab3 = read("randomGrid3D"); - - // 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); - - // Create a perturbed (non-zero) RHS - const auto xbar = system.Rc1().optimize(); // merely for use in zero below - auto values_y = VectorValues::Zero(xbar); - auto it = values_y.begin(); - it->second.setConstant(100); - ++it; - it->second.setConstant(-100); - - // 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)); - } -} - /* ************************************************************************* */ TEST(SubgraphPreconditioner, conjugateGradients) { // Build a planar graph