220 lines
6.9 KiB
C++
220 lines
6.9 KiB
C++
/**
|
|
* @file testNonlinearOptimizer.cpp
|
|
* @brief Unit tests for NonlinearOptimizer class
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <iostream>
|
|
using namespace std;
|
|
|
|
#include <boost/assign/std/list.hpp> // for operator +=
|
|
using namespace boost::assign;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <boost/shared_ptr.hpp>
|
|
using namespace boost;
|
|
|
|
#define GTSAM_MAGIC_KEY
|
|
|
|
#include "Matrix.h"
|
|
#include "Ordering.h"
|
|
#include "smallExample.h"
|
|
#include "pose2SLAM.h"
|
|
#include "GaussianFactorGraph.h"
|
|
#include "NoiseModel.h"
|
|
|
|
// template definitions
|
|
#include "NonlinearFactorGraph-inl.h"
|
|
#include "NonlinearOptimizer-inl.h"
|
|
#include "SubgraphPreconditioner-inl.h"
|
|
|
|
using namespace gtsam;
|
|
using namespace example;
|
|
|
|
typedef NonlinearOptimizer<Graph,Config> Optimizer;
|
|
|
|
/* ************************************************************************* */
|
|
//TEST( NonlinearOptimizer, delta )
|
|
//{
|
|
// shared_ptr<Graph> fg(new Graph(
|
|
// createNonlinearFactorGraph()));
|
|
// Optimizer::shared_config initial = sharedNoisyConfig();
|
|
//
|
|
// // Expected configuration is the difference between the noisy config
|
|
// // and the ground-truth config. One step only because it's linear !
|
|
// VectorConfig expected;
|
|
// Vector dl1(2);
|
|
// dl1(0) = -0.1;
|
|
// dl1(1) = 0.1;
|
|
// expected.insert("l1", dl1);
|
|
// Vector dx1(2);
|
|
// dx1(0) = -0.1;
|
|
// dx1(1) = -0.1;
|
|
// expected.insert("x1", dx1);
|
|
// Vector dx2(2);
|
|
// dx2(0) = 0.1;
|
|
// dx2(1) = -0.2;
|
|
// expected.insert("x2", dx2);
|
|
//
|
|
// // Check one ordering
|
|
// shared_ptr<Ordering> ord1(new Ordering());
|
|
// *ord1 += "x2","l1","x1";
|
|
// Optimizer optimizer1(fg, ord1, initial);
|
|
// VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta();
|
|
// CHECK(assert_equal(actual1,expected));
|
|
//
|
|
// // Check another
|
|
// shared_ptr<Ordering> ord2(new Ordering());
|
|
// *ord2 += "x1","x2","l1";
|
|
// Optimizer optimizer2(fg, ord2, initial);
|
|
// VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta();
|
|
// CHECK(assert_equal(actual2,expected));
|
|
//
|
|
// // And yet another...
|
|
// shared_ptr<Ordering> ord3(new Ordering());
|
|
// *ord3 += "l1","x1","x2";
|
|
// Optimizer optimizer3(fg, ord3, initial);
|
|
// VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
|
|
// CHECK(assert_equal(actual3,expected));
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST( NonlinearOptimizer, iterateLM )
|
|
//{
|
|
// // really non-linear factor graph
|
|
// shared_ptr<Graph> fg(new Graph(
|
|
// createReallyNonlinearFactorGraph()));
|
|
//
|
|
// // config far from minimum
|
|
// Point2 x0(3,0);
|
|
// boost::shared_ptr<Config> config(new Config);
|
|
// config->insert(simulated2D::PoseKey(1), x0);
|
|
//
|
|
// // ordering
|
|
// shared_ptr<Ordering> ord(new Ordering());
|
|
// ord->push_back("x1");
|
|
//
|
|
// // create initial optimization state, with lambda=0
|
|
// Optimizer::shared_solver solver(new Factorization<
|
|
// Graph, Config> );
|
|
// Optimizer optimizer(fg, ord, config, solver, 0.);
|
|
//
|
|
// // normal iterate
|
|
// Optimizer iterated1 = optimizer.iterate();
|
|
//
|
|
// // LM iterate with lambda 0 should be the same
|
|
// Optimizer iterated2 = optimizer.iterateLM();
|
|
//
|
|
// // Try successive iterates. TODO: ugly pointers, better way ?
|
|
// Optimizer *pointer = new Optimizer(iterated2);
|
|
// for (int i=0;i<10;i++) {
|
|
// Optimizer* newOptimizer = new Optimizer(pointer->iterateLM());
|
|
// delete pointer;
|
|
// pointer = newOptimizer;
|
|
// }
|
|
// delete(pointer);
|
|
//
|
|
// CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9));
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST( NonlinearOptimizer, optimize )
|
|
//{
|
|
// shared_ptr<Graph> fg(new Graph(
|
|
// createReallyNonlinearFactorGraph()));
|
|
//
|
|
// // test error at minimum
|
|
// Point2 xstar(0,0);
|
|
// Config cstar;
|
|
// cstar.insert(simulated2D::PoseKey(1), xstar);
|
|
// DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
|
|
//
|
|
// // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
|
// Point2 x0(3,3);
|
|
// boost::shared_ptr<Config> c0(new Config);
|
|
// c0->insert(simulated2D::PoseKey(1), x0);
|
|
// DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
|
|
//
|
|
// // optimize parameters
|
|
// shared_ptr<Ordering> ord(new Ordering());
|
|
// ord->push_back("x1");
|
|
// double relativeThreshold = 1e-5;
|
|
// double absoluteThreshold = 1e-5;
|
|
//
|
|
// // initial optimization state is the same in both cases tested
|
|
// Optimizer optimizer(fg, ord, c0);
|
|
//
|
|
// // Gauss-Newton
|
|
// Optimizer actual1 = optimizer.gaussNewton(relativeThreshold,
|
|
// absoluteThreshold);
|
|
// DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3);
|
|
//
|
|
// // Levenberg-Marquardt
|
|
// Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold,
|
|
// absoluteThreshold, Optimizer::SILENT);
|
|
// DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3);
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST( NonlinearOptimizer, Factorization )
|
|
//{
|
|
// typedef NonlinearOptimizer<Pose2Graph, Pose2Config, GaussianFactorGraph, Factorization<Pose2Graph, Pose2Config> > Optimizer;
|
|
//
|
|
// boost::shared_ptr<Pose2Config> config(new Pose2Config);
|
|
// config->insert(1, Pose2(0.,0.,0.));
|
|
// config->insert(2, Pose2(1.5,0.,0.));
|
|
//
|
|
// boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
|
|
// graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
|
|
// graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
|
|
//
|
|
// boost::shared_ptr<Ordering> ordering(new Ordering);
|
|
// ordering->push_back(Pose2Config::Key(1));
|
|
// ordering->push_back(Pose2Config::Key(2));
|
|
//
|
|
// Optimizer optimizer(graph, ordering, config);
|
|
// Optimizer optimized = optimizer.iterateLM();
|
|
//
|
|
// Pose2Config expected;
|
|
// expected.insert(1, Pose2(0.,0.,0.));
|
|
// expected.insert(2, Pose2(1.,0.,0.));
|
|
// CHECK(assert_equal(expected, *optimized.config(), 1e-5));
|
|
//}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearOptimizer, SubgraphPCG )
|
|
{
|
|
typedef NonlinearOptimizer<Pose2Graph, Pose2Config, SubgraphPreconditioner, SubgraphPCG<Pose2Graph, Pose2Config> > Optimizer;
|
|
|
|
boost::shared_ptr<Pose2Config> config(new Pose2Config);
|
|
config->insert(1, Pose2(0.,0.,0.));
|
|
config->insert(2, Pose2(1.5,0.,0.));
|
|
|
|
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
|
|
graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
|
|
graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
|
|
|
|
boost::shared_ptr<Ordering> ordering(new Ordering);
|
|
ordering->push_back(Pose2Config::Key(1));
|
|
ordering->push_back(Pose2Config::Key(2));
|
|
|
|
double relativeThreshold = 1e-5;
|
|
double absoluteThreshold = 1e-5;
|
|
Optimizer::shared_solver solver(new SubgraphPCG<Pose2Graph, Pose2Config>(*graph, *config));
|
|
Optimizer optimizer(graph, ordering, config, solver);
|
|
Optimizer optimized = optimizer.gaussNewton(relativeThreshold, absoluteThreshold, Optimizer::SILENT);
|
|
|
|
Pose2Config expected;
|
|
expected.insert(1, Pose2(0.,0.,0.));
|
|
expected.insert(2, Pose2(1.,0.,0.));
|
|
CHECK(assert_equal(expected, *optimized.config(), 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|