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

52
tests/testSubgraphPreconditioner.cpp
View File

@ -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,9 +44,7 @@ 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) {
@ -70,8 +67,7 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph ) TEST(SubgraphPreconditioner, planarGraph) {
{
// Check planar graph construction // Check planar graph construction
GaussianFactorGraph A; GaussianFactorGraph A;
VectorValues xtrue; VectorValues xtrue;
@ -87,8 +83,7 @@ TEST( SubgraphPreconditioner, planarGraph )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SubgraphPreconditioner, splitOffPlanarTree ) TEST(SubgraphPreconditioner, splitOffPlanarTree) {
{
// Build a planar graph // Build a planar graph
GaussianFactorGraph A; GaussianFactorGraph A;
VectorValues xtrue; VectorValues xtrue;
@ -107,8 +102,7 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SubgraphPreconditioner, system ) TEST(SubgraphPreconditioner, system) {
{
// Build a planar graph // Build a planar graph
GaussianFactorGraph Ab; GaussianFactorGraph Ab;
VectorValues xtrue; VectorValues xtrue;
@ -125,7 +119,8 @@ TEST( SubgraphPreconditioner, system )
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
@ -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);
@ -273,8 +267,7 @@ 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;
@ -286,11 +279,13 @@ TEST( SubgraphPreconditioner, conjugateGradients )
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 =
Ab1->eliminateSequential(); // 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
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
@ -312,5 +307,8 @@ TEST( SubgraphPreconditioner, conjugateGradients )
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */