Moved testIterative.cpp back to GTSAM, made the first test compile/run
Frank Dellaert
parent
b6150bd27e
commit
35d188e36d
|
|
@ -0,0 +1,194 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testIterative.cpp
|
||||
* @brief Unit tests for iterative methods
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
//#include <gtsam/linear/VectorValues.h>
|
||||
//#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
//#include <gtsam/inference/FactorGraph-inl.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace example;
|
||||
using symbol_shorthand::X; // to create pose keys
|
||||
using symbol_shorthand::L; // to create landmark keys
|
||||
|
||||
static bool verbose = false;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Iterative, steepestDescent )
|
||||
{
|
||||
// Create factor graph
|
||||
Ordering ord;
|
||||
ord += L(1), X(1), X(2);
|
||||
JacobianFactorGraph fg = createGaussianFactorGraph(ord);
|
||||
|
||||
// eliminate and solve
|
||||
VectorValues expected = *GaussianSequentialSolver(fg).optimize();
|
||||
|
||||
// Do gradient descent
|
||||
VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally?
|
||||
ConjugateGradientParameters parameters;
|
||||
// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY;
|
||||
VectorValues actual = steepestDescent(fg, zero, parameters);
|
||||
CHECK(assert_equal(expected,actual,1e-2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Iterative, conjugateGradientDescent )
|
||||
{
|
||||
// // Expected solution
|
||||
// Ordering ord;
|
||||
// ord += L(1), X(1), X(2);
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
// VectorValues expected = fg.optimize(ord); // destructive
|
||||
//
|
||||
// // create graph and get matrices
|
||||
// GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
||||
// Matrix A;
|
||||
// Vector b;
|
||||
// Vector x0 = gtsam::zero(6);
|
||||
// boost::tie(A, b) = fg2.matrix(ord);
|
||||
// Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2);
|
||||
//
|
||||
// // Do conjugate gradient descent, System version
|
||||
// System Ab(A, b);
|
||||
// Vector actualX = conjugateGradientDescent(Ab, x0, verbose);
|
||||
// CHECK(assert_equal(expectedX,actualX,1e-9));
|
||||
//
|
||||
// // Do conjugate gradient descent, Matrix version
|
||||
// Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose);
|
||||
// CHECK(assert_equal(expectedX,actualX2,1e-9));
|
||||
//
|
||||
// // Do conjugate gradient descent on factor graph
|
||||
// VectorValues zero = createZeroDelta();
|
||||
// VectorValues actual = conjugateGradientDescent(fg2, zero, verbose);
|
||||
// CHECK(assert_equal(expected,actual,1e-2));
|
||||
//
|
||||
// // Test method
|
||||
// VectorValues actual2 = fg2.conjugateGradientDescent(zero, verbose);
|
||||
// CHECK(assert_equal(expected,actual2,1e-2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/*TEST( Iterative, conjugateGradientDescent_hard_constraint )
|
||||
{
|
||||
typedef Pose2Values::Key Key;
|
||||
|
||||
Pose2Values config;
|
||||
config.insert(1, Pose2(0.,0.,0.));
|
||||
config.insert(2, Pose2(1.5,0.,0.));
|
||||
|
||||
Pose2Graph graph;
|
||||
Matrix cov = eye(3);
|
||||
graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov)));
|
||||
graph.addHardConstraint(1, config[1]);
|
||||
|
||||
VectorValues zeros;
|
||||
zeros.insert(X(1),zero(3));
|
||||
zeros.insert(X(2),zero(3));
|
||||
|
||||
GaussianFactorGraph fg = graph.linearize(config);
|
||||
VectorValues actual = conjugateGradientDescent(fg, zeros, true, 1e-3, 1e-5, 10);
|
||||
|
||||
VectorValues expected;
|
||||
expected.insert(X(1), zero(3));
|
||||
expected.insert(X(2), Vector_(-0.5,0.,0.));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}*/
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Iterative, conjugateGradientDescent_soft_constraint )
|
||||
{
|
||||
// Pose2Values config;
|
||||
// config.insert(1, Pose2(0.,0.,0.));
|
||||
// config.insert(2, Pose2(1.5,0.,0.));
|
||||
//
|
||||
// Pose2Graph graph;
|
||||
// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
|
||||
// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
|
||||
//
|
||||
// VectorValues zeros;
|
||||
// zeros.insert(X(1),zero(3));
|
||||
// zeros.insert(X(2),zero(3));
|
||||
//
|
||||
// boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);
|
||||
// VectorValues actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100);
|
||||
//
|
||||
// VectorValues expected;
|
||||
// expected.insert(X(1), zero(3));
|
||||
// expected.insert(X(2), Vector_(3,-0.5,0.,0.));
|
||||
// CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Iterative, subgraphPCG )
|
||||
{
|
||||
// typedef Pose2Values::Key Key;
|
||||
//
|
||||
// Pose2Values theta_bar;
|
||||
// theta_bar.insert(1, Pose2(0.,0.,0.));
|
||||
// theta_bar.insert(2, Pose2(1.5,0.,0.));
|
||||
//
|
||||
// Pose2Graph graph;
|
||||
// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
|
||||
// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
|
||||
//
|
||||
// // generate spanning tree and create ordering
|
||||
// PredecessorMap<Key> tree = graph.findMinimumSpanningTree<Key, Pose2Factor>();
|
||||
// list<Key> keys = predecessorMap2Keys(tree);
|
||||
// list<Symbol> symbols;
|
||||
// symbols.resize(keys.size());
|
||||
// std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol<Key>);
|
||||
// Ordering ordering(symbols);
|
||||
//
|
||||
// Key root = keys.back();
|
||||
// Pose2Graph T, C;
|
||||
// graph.split<Key, Pose2Factor>(tree, T, C);
|
||||
//
|
||||
// // build the subgraph PCG system
|
||||
// boost::shared_ptr<GaussianFactorGraph> Ab1_ = T.linearize(theta_bar);
|
||||
// SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar);
|
||||
// SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar);
|
||||
// SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering);
|
||||
// SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1);
|
||||
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar);
|
||||
//
|
||||
// VectorValues zeros = VectorValues::zero(*xbar);
|
||||
//
|
||||
// // Solve the subgraph PCG
|
||||
// VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
|
||||
// Errors> (system, zeros, verbose, 1e-5, 1e-5, 100);
|
||||
// VectorValues actual = system.x(ybar);
|
||||
//
|
||||
// VectorValues expected;
|
||||
// expected.insert(X(1), zero(3));
|
||||
// expected.insert(X(2), Vector_(3, -0.5, 0., 0.));
|
||||
// CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue