Resurrected tests
							parent
							
								
									6d938ce5cc
								
							
						
					
					
						commit
						aaf2ff5689
					
				|  | @ -15,26 +15,27 @@ | |||
|  *  @author Yong-Dian Jian | ||||
|  **/ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #if 0 | ||||
| 
 | ||||
| #include <tests/smallExample.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/iterative.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/SubgraphSolver.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/inference/Ordering.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <boost/tuple/tuple.hpp> | ||||
| #include <boost/assign/std/list.hpp> | ||||
| using namespace boost::assign; | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| using namespace example; | ||||
| 
 | ||||
| static size_t N = 3; | ||||
| static SubgraphSolverParameters kParameters; | ||||
| static auto kOrdering = example::planarOrdering(N); | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| /** unnormalized error */ | ||||
|  | @ -45,20 +46,17 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { | |||
|   return total_error; | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( SubgraphSolver, constructor1 ) | ||||
| { | ||||
|   // Build a planar graph
 | ||||
|   GaussianFactorGraph Ab; | ||||
|   VectorValues xtrue; | ||||
|   size_t N = 3; | ||||
|   boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
 | ||||
|   boost::tie(Ab, xtrue) = example::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
 | ||||
|   SubgraphSolverParameters parameters; | ||||
|   SubgraphSolver solver(Ab, parameters); | ||||
|   SubgraphSolver solver(Ab, kParameters, kOrdering); | ||||
|   VectorValues optimized = solver.optimize(); // does PCG optimization
 | ||||
|   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); | ||||
| } | ||||
|  | @ -70,16 +68,15 @@ TEST( SubgraphSolver, constructor2 ) | |||
|   GaussianFactorGraph Ab; | ||||
|   VectorValues xtrue; | ||||
|   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
 | ||||
|   boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); | ||||
|   boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); | ||||
| 
 | ||||
|   // The second constructor takes two factor graphs,
 | ||||
|   // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2)
 | ||||
|   SubgraphSolverParameters parameters; | ||||
|   SubgraphSolver solver(Ab1_, Ab2_, parameters); | ||||
|   // The second constructor takes two factor graphs, so the caller can specify
 | ||||
|   // the preconditioner (Ab1) and the constraints that are left out (Ab2)
 | ||||
|   SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering); | ||||
|   VectorValues optimized = solver.optimize(); | ||||
|   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); | ||||
| } | ||||
|  | @ -91,26 +88,22 @@ TEST( SubgraphSolver, constructor3 ) | |||
|   GaussianFactorGraph Ab; | ||||
|   VectorValues xtrue; | ||||
|   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
 | ||||
|   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
 | ||||
|   GaussianBayesNet::shared_ptr Rc1 = //
 | ||||
|       EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR); | ||||
|   // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
 | ||||
|   auto Rc1 = Ab1_.eliminateSequential(); | ||||
| 
 | ||||
|   // 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
 | ||||
|   SubgraphSolverParameters parameters; | ||||
|   SubgraphSolver solver(Rc1, Ab2_, parameters); | ||||
|   SubgraphSolver solver(Rc1, Ab2_, kParameters); | ||||
|   VectorValues optimized = solver.optimize(); | ||||
|   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); | ||||
| } | ||||
| 
 | ||||
| #endif | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue