58 lines
1.8 KiB
C++
58 lines
1.8 KiB
C++
/**
|
|
* @file testIterative.cpp
|
|
* @brief Unit tests for iterative methods
|
|
* @author Frank Dellaert
|
|
**/
|
|
|
|
#include <boost/assign/std/list.hpp> // for operator +=
|
|
using namespace boost::assign;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Ordering.h"
|
|
#include "iterative.h"
|
|
#include "smallExample.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( Iterative, gradientDescent )
|
|
{
|
|
// Expected solution
|
|
Ordering ord;
|
|
ord += "l1","x1","x2";
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
VectorConfig expected = fg.optimize(ord); // destructive
|
|
|
|
// Do gradient descent
|
|
GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
|
VectorConfig zero = createZeroDelta();
|
|
VectorConfig actual = fg2.gradientDescent(zero);
|
|
CHECK(assert_equal(expected,actual,1e-2));
|
|
|
|
// Do conjugate gradient descent
|
|
//VectorConfig actual2 = fg2.conjugateGradientDescent(zero);
|
|
VectorConfig actual2 = conjugateGradientDescent(fg2,zero);
|
|
CHECK(assert_equal(expected,actual2,1e-2));
|
|
|
|
// Do conjugate gradient descent, Matrix version
|
|
Matrix A;Vector b;
|
|
boost::tie(A,b) = fg2.matrix(ord);
|
|
// print(A,"A");
|
|
// print(b,"b");
|
|
Vector x0 = gtsam::zero(6);
|
|
Vector actualX = conjugateGradientDescent(A,b,x0);
|
|
Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2);
|
|
CHECK(assert_equal(expectedX,actualX,1e-9));
|
|
|
|
// Do conjugate gradient descent, System version
|
|
System Ab = make_pair(A,b);
|
|
Vector actualX2 = conjugateGradientDescent(Ab,x0);
|
|
CHECK(assert_equal(expectedX,actualX2,1e-9));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|