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

Cleaned up formatting

release/4.3a0
Frank Dellaert 2019-04-07 22:33:58 -04:00
parent c1c2fd7008
commit a7227cab43
1 changed files with 65 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
tests/testSubgraphPreconditioner.cpp
View File

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