141 lines
3.7 KiB
C++
141 lines
3.7 KiB
C++
/**
|
|
* @file testNonlinearOptimizer.cpp
|
|
* @brief Unit tests for NonlinearOptimizer class
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <iostream>
|
|
using namespace std;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Matrix.h"
|
|
#include "smallExample.h"
|
|
// template definitions
|
|
#include "NonlinearOptimizer-inl.h"
|
|
|
|
using namespace gtsam;
|
|
|
|
typedef NonlinearOptimizer<NonlinearFactorGraph,FGConfig> Optimizer;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, delta )
|
|
{
|
|
NonlinearFactorGraph fg = 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 !
|
|
FGConfig 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
|
|
Ordering ord1;
|
|
ord1.push_back("x2");
|
|
ord1.push_back("l1");
|
|
ord1.push_back("x1");
|
|
Optimizer optimizer1(fg, ord1, initial);
|
|
FGConfig actual1 = optimizer1.delta();
|
|
CHECK(assert_equal(actual1,expected));
|
|
|
|
// Check another
|
|
Ordering ord2;
|
|
ord2.push_back("x1");
|
|
ord2.push_back("x2");
|
|
ord2.push_back("l1");
|
|
Optimizer optimizer2(fg, ord2, initial);
|
|
FGConfig actual2 = optimizer2.delta();
|
|
CHECK(assert_equal(actual2,expected));
|
|
|
|
// And yet another...
|
|
Ordering ord3;
|
|
ord3.push_back("l1");
|
|
ord3.push_back("x1");
|
|
ord3.push_back("x2");
|
|
Optimizer optimizer3(fg, ord3, initial);
|
|
FGConfig actual3 = optimizer3.delta();
|
|
CHECK(assert_equal(actual3,expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, iterateLM )
|
|
{
|
|
// really non-linear factor graph
|
|
NonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
|
|
|
|
// config far from minimum
|
|
Vector x0 = Vector_(1, 3.0);
|
|
boost::shared_ptr<FGConfig> config(new FGConfig);
|
|
config->insert("x", x0);
|
|
|
|
// ordering
|
|
Ordering ord;
|
|
ord.push_back("x");
|
|
|
|
// create initial optimization state, with lambda=0
|
|
Optimizer optimizer(fg, ord, config, 0);
|
|
|
|
// normal iterate
|
|
Optimizer iterated1 = optimizer.iterate();
|
|
|
|
// LM iterate with lambda 0 should be the same
|
|
Optimizer iterated2 = optimizer.iterateLM();
|
|
|
|
CHECK(assert_equal(*(iterated1.config()), *(iterated2.config()), 1e-9));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, optimize )
|
|
{
|
|
NonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
|
|
|
|
// test error at minimum
|
|
Vector xstar = Vector_(1, 0.0);
|
|
FGConfig cstar;
|
|
cstar.insert("x", xstar);
|
|
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
|
|
|
|
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
|
Vector x0 = Vector_(1, 3.0);
|
|
boost::shared_ptr<FGConfig> c0(new FGConfig);
|
|
c0->insert("x", x0);
|
|
DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3);
|
|
|
|
// optimize parameters
|
|
Ordering ord;
|
|
ord.push_back("x");
|
|
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);
|
|
CHECK(assert_equal(*(actual1.config()),cstar));
|
|
|
|
// Levenberg-Marquardt
|
|
Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold,
|
|
absoluteThreshold, Optimizer::SILENT);
|
|
CHECK(assert_equal(*(actual2.config()),cstar));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|