12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed the tests - key was choosing right ordering

release/4.3a0
Frank Dellaert 2019-04-07 22:31:27 -04:00
parent 2de0957b2d
commit c1c2fd7008
1 changed files with 51 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
tests/testSubgraphPreconditioner.cpp
View File

@ -30,6 +30,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/archive/xml_iarchive.hpp> #include <boost/archive/xml_iarchive.hpp>
#include <boost/assign/std/list.hpp> #include <boost/assign/std/list.hpp>
#include <boost/serialization/export.hpp> #include <boost/serialization/export.hpp>
@ -203,88 +204,71 @@ TEST( SubgraphPreconditioner, system )
EXPECT(assert_equal(expected_g, vec(g))); 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<Key,Vector> 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"); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor");
TEST( SubgraphSolver, toy3D ) // Read from XML file
{ static GaussianFactorGraph read(const string& name) {
// Read from file auto inputFile = findExampleDataFile(name);
string inputFile = findExampleDataFile("randomGrid3D");
ifstream is(inputFile); ifstream is(inputFile);
if (!is.is_open()) if (!is.is_open())
throw runtime_error("Cannot find file " + inputFile); throw runtime_error("Cannot find file " + inputFile);
boost::archive::xml_iarchive in_archive(is); boost::archive::xml_iarchive in_archive(is);
GaussianFactorGraph Ab; GaussianFactorGraph Ab;
in_archive >> boost::serialization::make_nvp("graph", Ab); in_archive >> boost::serialization::make_nvp("graph", Ab);
return Ab;
}
// Create solver, leaving splitting to constructor TEST(SubgraphSolver, Solves) {
// Create preconditioner
SubgraphPreconditioner system; SubgraphPreconditioner system;
// Call build, a non-const method needed to make solve work :-(
KeyInfo keyInfo(Ab);
std::map<Key,Vector> lambda;
system.build(Ab, keyInfo, lambda);
// Solve the VectorValues way // We test on three different graphs
const auto xbar = system.Rc1()->optimize(); // merely for use in zero below const auto Ab1 = planarGraph(3).get<0>();
auto values_y = VectorValues::Zero(xbar); const auto Ab2 = read("toy3D");
const size_t n = values_y.size(); const auto Ab3 = read("randomGrid3D");
values_y[0] = Vector3(100, 200, -300);
values_y[n - 1] = Vector3(10, 20, -100);
auto values_x = system.Rc1()->backSubstitute(values_y);
// Check YD's sub-vector machinery // For all graphs, test solve and solveTranspose
const auto ordering = keyInfo.ordering(); for (const auto& Ab : {Ab1,Ab2,Ab3}) {
auto vector_y = values_y.vector(ordering); // Call build, a non-const method needed to make solve work :-(
for (size_t j = 0; j < n; j++) { KeyInfo keyInfo(Ab);
EXPECT(assert_equal(values_y[j], getSubvector(vector_y, keyInfo, {j}))); std::map<Key, Vector> lambda;
} system.build(Ab, keyInfo, lambda);
// Solve the matrix way, this really just checks BN::backSubstitute // Create a perturbed (non-zero) RHS
const Matrix R1 = system.Rc1()->matrix(ordering).first; const auto xbar = system.Rc1()->optimize(); // merely for use in zero below
auto vector_x = R1.inverse() * vector_y; auto values_y = VectorValues::Zero(xbar);
EXPECT(assert_equal(vector_x, values_x.vector(ordering))); values_y.begin()->second.setConstant(100);
(--values_y.end())->second.setConstant(-100);
// Test that 'solve' does implement x = R^{-1} y // Solve the VectorValues way
const size_t N = R1.cols(); auto values_x = system.Rc1()->backSubstitute(values_y);
EXPECT_LONGS_EQUAL(n * 3, N);
Vector solve_x = Vector::Zero(N); // Solve the matrix way, this really just checks BN::backSubstitute
system.solve(vector_y, solve_x); // This only works with Rc1 ordering, not with keyInfo !
EXPECT(assert_equal(vector_x, solve_x)); // TODO(frank): why does this not work with an arbitrary ordering?
for (size_t j = 0; j < n; j++) { const auto ord = system.Rc1()->ordering();
cout << j << endl; const Matrix R1 = system.Rc1()->matrix(ord).first;
EXPECT(assert_equal(values_x[j], getSubvector(vector_x, keyInfo, {j}), 1e-3)); 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));
} }
} }