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

91 lines
2.9 KiB
C++
Raw Normal View History

Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
/**
* @file testBayesNetConditioner.cpp
* @brief Unit tests for BayesNetConditioner
* @author Frank Dellaert
**/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
Installing headers in subdirectories and converted include directives to match the subdirectory structure (also see email to frankcvs)
2010-08-20 01:23:19 +08:00
#include <gtsam/CppUnitLite/TestHarness.h>
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
#define GTSAM_MAGIC_KEY
Installing headers in subdirectories and converted include directives to match the subdirectory structure (also see email to frankcvs)
2010-08-20 01:23:19 +08:00
#include <gtsam/inference/Ordering.h>
#include <gtsam/linear/BayesNetPreconditioner.h>
#include <gtsam/linear/iterative-inl.h>
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
using namespace std;
using namespace gtsam;
Installing headers in subdirectories and converted include directives to match the subdirectory structure (also see email to frankcvs)
2010-08-20 01:23:19 +08:00
#include <gtsam/slam/smallExample.h>
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
using namespace example;
/* ************************************************************************* */
TEST( BayesNetPreconditioner, conjugateGradients )
{
// Build a planar graph
GaussianFactorGraph Ab;
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues xtrue;
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
// Eliminate the spanning tree to build a prior
Ordering ordering = planarOrdering(N);
GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues xbar = optimize(Rc1); // xbar = inv(R1)*c1
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
// Create BayesNet-preconditioned system
BayesNetPreconditioner system(Ab,Rc1);
// Create zero config y0 and perturbed config y1
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues y0;
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
Vector z2 = zero(2);
BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2);
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues y1 = y0;
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
y1["x2003"] = Vector_(2, 1.0, -1.0);
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues x1 = system.x(y1);
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
// Check gradient for y0
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues expectedGradient0;
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
expectedGradient0.insert("x1001", Vector_(2,-1000.,-1000.));
expectedGradient0.insert("x1002", Vector_(2, 0., -300.));
expectedGradient0.insert("x1003", Vector_(2, 0., -300.));
expectedGradient0.insert("x2001", Vector_(2, -100., 200.));
expectedGradient0.insert("x2002", Vector_(2, -100., 0.));
expectedGradient0.insert("x2003", Vector_(2, -100., -200.));
expectedGradient0.insert("x3001", Vector_(2, -100., 100.));
expectedGradient0.insert("x3002", Vector_(2, -100., 0.));
expectedGradient0.insert("x3003", Vector_(2, -100., -100.));
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues actualGradient0 = system.gradient(y0);
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
CHECK(assert_equal(expectedGradient0,actualGradient0));
#ifdef VECTORBTREE
CHECK(actualGradient0.cloned(y0));
#endif
// Solve using PCG
bool verbose = false;
double epsilon = 1e-6; // had to crank this down !!!
size_t maxIterations = 100;
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues actual_y = gtsam::conjugateGradients<BayesNetPreconditioner,
VectorValues, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
VectorValues actual_x = system.x(actual_y);
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
CHECK(assert_equal(xtrue,actual_x));
// Compare with non preconditioned version:
Changed linear config names to *Values, updated comments
2010-10-09 11:09:58 +08:00
VectorValues actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon,
Re-organization: moving all files. I re-organized into original cpp directory and 4 additional directories: base, geometry, slam, and tests. cpp will be further renamed/sub-divided and contains the core library. I wanted it not depend on geometry or slam, which necessitated moving complicated tests to a dedicated directory.
2010-07-12 15:16:31 +08:00
maxIterations);
CHECK(assert_equal(xtrue,actual2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */