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

Resurrected tests

release/4.3a0
Frank Dellaert 2019-04-04 01:17:47 -04:00
parent 6d938ce5cc
commit aaf2ff5689
1 changed files with 22 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
tests/testSubgraphSolver.cpp
View File

@ -15,26 +15,27 @@
* @author Yong-Dian Jian * @author Yong-Dian Jian
**/ **/
#include <CppUnitLite/TestHarness.h>
#if 0
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h> #include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> #include <boost/assign/std/list.hpp>
using namespace boost::assign; using namespace boost::assign;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example;
static size_t N = 3;
static SubgraphSolverParameters kParameters;
static auto kOrdering = example::planarOrdering(N);
/* ************************************************************************* */ /* ************************************************************************* */
/** unnormalized error */ /** unnormalized error */
@ -45,20 +46,17 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
return total_error; return total_error;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SubgraphSolver, constructor1 ) TEST( SubgraphSolver, constructor1 )
{ {
// Build a planar graph // Build a planar graph
GaussianFactorGraph Ab; GaussianFactorGraph Ab;
VectorValues xtrue; VectorValues xtrue;
size_t N = 3; boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// The first constructor just takes a factor graph (and parameters) // The first constructor just takes a factor graph (and kParameters)
// and it will split the graph into A1 and A2, where A1 is a spanning tree // and it will split the graph into A1 and A2, where A1 is a spanning tree
SubgraphSolverParameters parameters; SubgraphSolver solver(Ab, kParameters, kOrdering);
SubgraphSolver solver(Ab, parameters);
VectorValues optimized = solver.optimize(); // does PCG optimization VectorValues optimized = solver.optimize(); // does PCG optimization
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
} }
@ -70,16 +68,15 @@ TEST( SubgraphSolver, constructor2 )
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) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering // Get the spanning tree
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
// The second constructor takes two factor graphs, // The second constructor takes two factor graphs, so the caller can specify
// so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) // the preconditioner (Ab1) and the constraints that are left out (Ab2)
SubgraphSolverParameters parameters; SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering);
SubgraphSolver solver(Ab1_, Ab2_, parameters);
VectorValues optimized = solver.optimize(); VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
} }
@ -91,26 +88,22 @@ TEST( SubgraphSolver, constructor3 )
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) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering // Get the spanning tree and corresponding kOrdering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
GaussianBayesNet::shared_ptr Rc1 = // auto Rc1 = Ab1_.eliminateSequential();
EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
// The third constructor allows the caller to pass an already solved preconditioner Rc1_ // The third constructor allows the caller to pass an already solved preconditioner Rc1_
// as a Bayes net, in addition to the "loop closing constraints" Ab2, as before // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
SubgraphSolverParameters parameters; SubgraphSolver solver(Rc1, Ab2_, kParameters);
SubgraphSolver solver(Rc1, Ab2_, parameters);
VectorValues optimized = solver.optimize(); VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */