786 lines
23 KiB
C++
786 lines
23 KiB
C++
/**
|
|
* @file testGaussianFactorGraph.cpp
|
|
* @brief Unit tests for Linear Factor Graph
|
|
* @author Christian Potthast
|
|
**/
|
|
|
|
#include <string.h>
|
|
#include <iostream>
|
|
using namespace std;
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <boost/assign/std/list.hpp> // for operator +=
|
|
using namespace boost::assign;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Matrix.h"
|
|
#include "Ordering.h"
|
|
#include "smallExample.h"
|
|
#include "GaussianBayesNet.h"
|
|
#include "numericalDerivative.h"
|
|
#include "inference-inl.h" // needed for eliminate and marginals
|
|
|
|
using namespace gtsam;
|
|
|
|
double tol=1e-4;
|
|
|
|
/* ************************************************************************* */
|
|
/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, equals ){
|
|
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
|
CHECK(fg.equals(fg2));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, error )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
VectorConfig cfg = createZeroDelta();
|
|
|
|
// note the error is the same as in testNonlinearFactorGraph as a
|
|
// zero delta config in the linear graph is equivalent to noisy in
|
|
// non-linear, which is really linear under the hood
|
|
double actual = fg.error(cfg);
|
|
DOUBLES_EQUAL( 5.625, actual, 1e-9 );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/* unit test for find seperator */
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, find_separator )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
set<string> separator = fg.find_separator("x2");
|
|
set<string> expected;
|
|
expected.insert("x1");
|
|
expected.insert("l1");
|
|
|
|
CHECK(separator.size()==expected.size());
|
|
set<string>::iterator it1 = separator.begin(), it2 = expected.begin();
|
|
for(; it1!=separator.end(); it1++, it2++)
|
|
CHECK(*it1 == *it2);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, combine_factors_x1 )
|
|
{
|
|
// create a small example for a linear factor graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// create sigmas
|
|
double sigma1 = 0.1;
|
|
double sigma2 = 0.1;
|
|
double sigma3 = 0.2;
|
|
Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3);
|
|
|
|
// combine all factors
|
|
GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
|
|
|
|
// the expected linear factor
|
|
Matrix Al1 = Matrix_(6,2,
|
|
0., 0.,
|
|
0., 0.,
|
|
0., 0.,
|
|
0., 0.,
|
|
1., 0.,
|
|
0., 1.
|
|
);
|
|
|
|
Matrix Ax1 = Matrix_(6,2,
|
|
1., 0.,
|
|
0.00, 1.,
|
|
-1., 0.,
|
|
0.00,-1.,
|
|
-1., 0.,
|
|
00., -1.
|
|
);
|
|
|
|
Matrix Ax2 = Matrix_(6,2,
|
|
0., 0.,
|
|
0., 0.,
|
|
1., 0.,
|
|
+0.,1.,
|
|
0., 0.,
|
|
0., 0.
|
|
);
|
|
|
|
// the expected RHS vector
|
|
Vector b(6);
|
|
b(0) = -1*sigma1;
|
|
b(1) = -1*sigma1;
|
|
b(2) = 2*sigma2;
|
|
b(3) = -1*sigma2;
|
|
b(4) = 0*sigma3;
|
|
b(5) = 1*sigma3;
|
|
|
|
vector<pair<string, Matrix> > meas;
|
|
meas.push_back(make_pair("l1", Al1));
|
|
meas.push_back(make_pair("x1", Ax1));
|
|
meas.push_back(make_pair("x2", Ax2));
|
|
GaussianFactor expected(meas, b, sigmas);
|
|
//GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
|
|
|
|
// check if the two factors are the same
|
|
CHECK(assert_equal(expected,*actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, combine_factors_x2 )
|
|
{
|
|
// create a small example for a linear factor graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// determine sigmas
|
|
double sigma1 = 0.1;
|
|
double sigma2 = 0.2;
|
|
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
|
|
|
|
// combine all factors
|
|
GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2");
|
|
|
|
// the expected linear factor
|
|
Matrix Al1 = Matrix_(4,2,
|
|
// l1
|
|
0., 0.,
|
|
0., 0.,
|
|
1., 0.,
|
|
0., 1.
|
|
);
|
|
|
|
Matrix Ax1 = Matrix_(4,2,
|
|
// x1
|
|
-1., 0., // f2
|
|
0.00,-1., // f2
|
|
0.00, 0., // f4
|
|
0.00, 0. // f4
|
|
);
|
|
|
|
Matrix Ax2 = Matrix_(4,2,
|
|
// x2
|
|
1., 0.,
|
|
+0.,1.,
|
|
-1., 0.,
|
|
+0.,-1.
|
|
);
|
|
|
|
// the expected RHS vector
|
|
Vector b(4);
|
|
b(0) = 2*sigma1;
|
|
b(1) = -1*sigma1;
|
|
b(2) = -1*sigma2;
|
|
b(3) = 1.5*sigma2;
|
|
|
|
vector<pair<string, Matrix> > meas;
|
|
meas.push_back(make_pair("l1", Al1));
|
|
meas.push_back(make_pair("x1", Ax1));
|
|
meas.push_back(make_pair("x2", Ax2));
|
|
GaussianFactor expected(meas, b, sigmas);
|
|
|
|
// check if the two factors are the same
|
|
CHECK(assert_equal(expected,*actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( GaussianFactorGraph, eliminateOne_x1 )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
GaussianConditional::shared_ptr actual = fg.eliminateOne("x1");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
|
|
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15);
|
|
GaussianConditional expected("x1",d,R11,"l1",S12,"x2",S13,sigma);
|
|
|
|
CHECK(assert_equal(expected,*actual,tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( GaussianFactorGraph, eliminateOne_x2 )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
GaussianConditional::shared_ptr actual = fg.eliminateOne("x2");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I;
|
|
Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427);
|
|
GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma);
|
|
|
|
CHECK(assert_equal(expected,*actual,tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, eliminateOne_l1 )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
GaussianConditional::shared_ptr actual = fg.eliminateOne("l1");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I;
|
|
Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421);
|
|
GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma);
|
|
|
|
CHECK(assert_equal(expected,*actual,tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, eliminateAll )
|
|
{
|
|
// create expected Chordal bayes Net
|
|
Matrix I = eye(2);
|
|
|
|
Vector d1 = Vector_(2, -0.1,-0.1);
|
|
GaussianBayesNet expected = simpleGaussian("x1",d1,0.1);
|
|
|
|
Vector d2 = Vector_(2, 0.0, 0.2), sigma2 = repeat(2,0.149071);
|
|
push_front(expected,"l1",d2, I,"x1", (-1)*I,sigma2);
|
|
|
|
Vector d3 = Vector_(2, 0.2, -0.14), sigma3 = repeat(2,0.0894427);
|
|
push_front(expected,"x2",d3, I,"l1", (-0.2)*I, "x1", (-0.8)*I, sigma3);
|
|
|
|
// Check one ordering
|
|
GaussianFactorGraph fg1 = createGaussianFactorGraph();
|
|
Ordering ordering;
|
|
ordering += "x2","l1","x1";
|
|
GaussianBayesNet actual = fg1.eliminate(ordering);
|
|
CHECK(assert_equal(expected,actual,tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, add_priors )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
GaussianFactorGraph actual = fg.add_priors(3);
|
|
GaussianFactorGraph expected = createGaussianFactorGraph();
|
|
Matrix A = eye(2);
|
|
Vector b = zero(2);
|
|
double sigma = 3.0;
|
|
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma)));
|
|
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma)));
|
|
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
|
|
CHECK(assert_equal(expected,actual)); // Fails
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, copying )
|
|
{
|
|
// Create a graph
|
|
GaussianFactorGraph actual = createGaussianFactorGraph();
|
|
|
|
// Copy the graph !
|
|
GaussianFactorGraph copy = actual;
|
|
|
|
// now eliminate the copy
|
|
Ordering ord1;
|
|
ord1 += "x2","l1","x1";
|
|
GaussianBayesNet actual1 = copy.eliminate(ord1);
|
|
|
|
// Create the same graph, but not by copying
|
|
GaussianFactorGraph expected = createGaussianFactorGraph();
|
|
|
|
// and check that original is still the same graph
|
|
CHECK(assert_equal(expected,actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, matrix )
|
|
{
|
|
// Create a graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// render with a given ordering
|
|
Ordering ord;
|
|
ord += "x2","l1","x1";
|
|
|
|
Matrix A; Vector b;
|
|
boost::tie(A,b) = fg.matrix(ord);
|
|
|
|
Matrix A1 = Matrix_(2*4,3*2,
|
|
+0., 0., 0., 0., 10., 0., // unary factor on x1 (prior)
|
|
+0., 0., 0., 0., 0., 10.,
|
|
10., 0., 0., 0.,-10., 0., // binary factor on x2,x1 (odometry)
|
|
+0., 10., 0., 0., 0.,-10.,
|
|
+0., 0., 5., 0., -5., 0., // binary factor on l1,x1 (z1)
|
|
+0., 0., 0., 5., 0., -5.,
|
|
-5., 0., 5., 0., 0., 0., // binary factor on x2,l1 (z2)
|
|
+0., -5., 0., 5., 0., 0.
|
|
);
|
|
Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5);
|
|
|
|
EQUALITY(A,A1);
|
|
CHECK(b==b1);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, sparse )
|
|
{
|
|
// create a small linear factor graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// render with a given ordering
|
|
Ordering ord;
|
|
ord += "x2","l1","x1";
|
|
|
|
Matrix ijs = fg.sparse(ord);
|
|
|
|
EQUALITY(Matrix_(3, 14,
|
|
// f(x1) f(x2,x1) f(l1,x1) f(x2,l1)
|
|
+1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8., 7., 8.,
|
|
+5., 6., 5., 6., 1., 2., 3.,4., 5., 6., 3., 4., 1., 2.,
|
|
10.,10., -10.,-10., 10., 10., 5.,5.,-5.,-5., 5., 5.,-5.,-5.), ijs);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// render with a given ordering
|
|
Ordering ord;
|
|
ord += "x2","l1","x1";
|
|
GaussianBayesNet CBN = fg.eliminate(ord);
|
|
|
|
// True GaussianFactorGraph
|
|
GaussianFactorGraph fg2(CBN);
|
|
GaussianBayesNet CBN2 = fg2.eliminate(ord);
|
|
CHECK(assert_equal(CBN,CBN2));
|
|
|
|
// Base FactorGraph only
|
|
FactorGraph<GaussianFactor> fg3(CBN);
|
|
GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,GaussianConditional>(fg3,ord);
|
|
CHECK(assert_equal(CBN,CBN3));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, GET_ORDERING)
|
|
{
|
|
Ordering expected;
|
|
expected += "l1","x1","x2";
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
Ordering actual = fg.getOrdering();
|
|
CHECK(assert_equal(expected,actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, OPTIMIZE )
|
|
{
|
|
// create a graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// create an ordering
|
|
Ordering ord = fg.getOrdering();
|
|
|
|
// optimize the graph
|
|
VectorConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
VectorConfig expected = createCorrectDelta();
|
|
|
|
CHECK(assert_equal(expected,actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, COMBINE_GRAPHS_INPLACE)
|
|
{
|
|
// create a test graph
|
|
GaussianFactorGraph fg1 = createGaussianFactorGraph();
|
|
|
|
// create another factor graph
|
|
GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
|
|
|
// get sizes
|
|
int size1 = fg1.size();
|
|
int size2 = fg2.size();
|
|
|
|
// combine them
|
|
fg1.combine(fg2);
|
|
|
|
CHECK(size1+size2 == fg1.size());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, COMBINE_GRAPHS)
|
|
{
|
|
// create a test graph
|
|
GaussianFactorGraph fg1 = createGaussianFactorGraph();
|
|
|
|
// create another factor graph
|
|
GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
|
|
|
// get sizes
|
|
int size1 = fg1.size();
|
|
int size2 = fg2.size();
|
|
|
|
// combine them
|
|
GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);
|
|
|
|
CHECK(size1+size2 == fg3.size());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// print a vector of ints if needed for debugging
|
|
void print(vector<int> v) {
|
|
for (int k = 0; k < v.size(); k++)
|
|
cout << v[k] << " ";
|
|
cout << endl;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, factor_lookup)
|
|
{
|
|
// create a test graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// ask for all factor indices connected to x1
|
|
list<int> x1_factors = fg.factors("x1");
|
|
int x1_indices[] = { 0, 1, 2 };
|
|
list<int> x1_expected(x1_indices, x1_indices + 3);
|
|
CHECK(x1_factors==x1_expected);
|
|
|
|
// ask for all factor indices connected to x2
|
|
list<int> x2_factors = fg.factors("x2");
|
|
int x2_indices[] = { 1, 3 };
|
|
list<int> x2_expected(x2_indices, x2_indices + 2);
|
|
CHECK(x2_factors==x2_expected);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, findAndRemoveFactors )
|
|
{
|
|
// create the graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// We expect to remove these three factors: 0, 1, 2
|
|
GaussianFactor::shared_ptr f0 = fg[0];
|
|
GaussianFactor::shared_ptr f1 = fg[1];
|
|
GaussianFactor::shared_ptr f2 = fg[2];
|
|
|
|
// call the function
|
|
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
|
|
|
|
// Check the factors
|
|
CHECK(f0==factors[0]);
|
|
CHECK(f1==factors[1]);
|
|
CHECK(f2==factors[2]);
|
|
|
|
// CHECK if the factors are deleted from the factor graph
|
|
LONGS_EQUAL(1,fg.nrFactors());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, findAndRemoveFactors_twice )
|
|
{
|
|
// create the graph
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// We expect to remove these three factors: 0, 1, 2
|
|
GaussianFactor::shared_ptr f0 = fg[0];
|
|
GaussianFactor::shared_ptr f1 = fg[1];
|
|
GaussianFactor::shared_ptr f2 = fg[2];
|
|
|
|
// call the function
|
|
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
|
|
|
|
// Check the factors
|
|
CHECK(f0==factors[0]);
|
|
CHECK(f1==factors[1]);
|
|
CHECK(f2==factors[2]);
|
|
|
|
factors = fg.findAndRemoveFactors("x1");
|
|
CHECK(factors.size() == 0);
|
|
|
|
// CHECK if the factors are deleted from the factor graph
|
|
LONGS_EQUAL(1,fg.nrFactors());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(GaussianFactorGraph, createSmoother)
|
|
{
|
|
GaussianFactorGraph fg1 = createSmoother(2);
|
|
LONGS_EQUAL(3,fg1.size());
|
|
GaussianFactorGraph fg2 = createSmoother(3);
|
|
LONGS_EQUAL(5,fg2.size());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, variables )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
Dimensions expected;
|
|
insert(expected)("l1", 2)("x1", 2)("x2", 2);
|
|
Dimensions actual = fg.dimensions();
|
|
CHECK(expected==actual);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, keys )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
Ordering expected;
|
|
expected += "l1","x1","x2";
|
|
CHECK(assert_equal(expected,fg.keys()));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, involves )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
CHECK(fg.involves("l1"));
|
|
CHECK(fg.involves("x1"));
|
|
CHECK(fg.involves("x2"));
|
|
CHECK(!fg.involves("x3"));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
double error(const VectorConfig& x) {
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
return fg.error(x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, gradient )
|
|
{
|
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
|
|
|
// Construct expected gradient
|
|
VectorConfig expected;
|
|
|
|
// 2*f(x) = 100*(x1+c["x1"])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
|
|
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
|
|
expected.insert("l1",Vector_(2, 5.0,-12.5));
|
|
expected.insert("x1",Vector_(2, 30.0, 5.0));
|
|
expected.insert("x2",Vector_(2,-25.0, 17.5));
|
|
|
|
// Check the gradient at delta=0
|
|
VectorConfig zero = createZeroDelta();
|
|
VectorConfig actual = fg.gradient(zero);
|
|
CHECK(assert_equal(expected,actual));
|
|
|
|
// Check it numerically for good measure
|
|
Vector numerical_g = numericalGradient<VectorConfig>(error,zero,0.001);
|
|
CHECK(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g));
|
|
|
|
// Check the gradient at the solution (should be zero)
|
|
Ordering ord;
|
|
ord += "x2","l1","x1";
|
|
GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
|
VectorConfig solution = fg2.optimize(ord); // destructive
|
|
VectorConfig actual2 = fg.gradient(solution);
|
|
CHECK(assert_equal(zero,actual2));
|
|
}
|
|
|
|
/* ************************************************************************* *
|
|
TEST( GaussianFactorGraph, multiplication )
|
|
{
|
|
GaussianFactorGraph A = createGaussianFactorGraph();
|
|
VectorConfig x = createConfig();
|
|
ErrorConfig actual = A * x;
|
|
CHECK(assert_equal(expected,actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
typedef pair<Matrix,Vector> System;
|
|
|
|
/**
|
|
* gradient of objective function 0.5*|Ax-b|^2 at x = A'*(Ax-b)
|
|
*/
|
|
Vector gradient(const System& Ab, const Vector& x) {
|
|
const Matrix& A = Ab.first;
|
|
const Vector& b = Ab.second;
|
|
return A ^ (A * x - b);
|
|
}
|
|
|
|
/**
|
|
* Apply operator A
|
|
*/
|
|
Vector operator*(const System& Ab, const Vector& x) {
|
|
const Matrix& A = Ab.first;
|
|
return A * x;
|
|
}
|
|
|
|
/**
|
|
* Apply operator A^T
|
|
*/
|
|
Vector operator^(const System& Ab, const Vector& x) {
|
|
const Matrix& A = Ab.first;
|
|
return A ^ x;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Method of conjugate gradients (CG)
|
|
// "System" class S needs gradient(S,v), e=S*v, v=S^e
|
|
// "Vector" class V needs dot(v,v), -v, v+v, s*v
|
|
// "Vector" class E needs dot(v,v)
|
|
template <class S, class V, class E>
|
|
Vector conjugateGradientDescent(const S& Ab, V x, double threshold = 1e-9) {
|
|
|
|
// Start with g0 = A'*(A*x0-b), d0 = - g0
|
|
// i.e., first step is in direction of negative gradient
|
|
V g = gradient(Ab, x);
|
|
V d = -g;
|
|
double prev_dotg = dot(g, g);
|
|
|
|
// loop max n times
|
|
size_t n = x.size();
|
|
for (int k = 1; k <= n; k++) {
|
|
|
|
// calculate optimal step-size
|
|
E Ad = Ab * d;
|
|
double alpha = -dot(d, g) / dot(Ad, Ad);
|
|
|
|
// do step in new search direction
|
|
x = x + alpha * d;
|
|
if (k == n) break;
|
|
|
|
// update gradient
|
|
g = g + alpha * (Ab ^ Ad);
|
|
|
|
// check for convergence
|
|
double dotg = dot(g, g);
|
|
if (dotg < threshold) break;
|
|
|
|
// calculate new search direction
|
|
double beta = dotg / prev_dotg;
|
|
prev_dotg = dotg;
|
|
d = -g + beta * d;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Method of conjugate gradients (CG)
|
|
Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
|
|
const Vector& x, double threshold = 1e-9) {
|
|
System Ab = make_pair(A, b);
|
|
return conjugateGradientDescent<System, Vector, Vector> (Ab, x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, 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,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<System,Vector,Vector>(Ab,x0);
|
|
CHECK(assert_equal(expectedX,actualX2,1e-9));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Tests ported from ConstrainedGaussianFactorGraph
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, constrained_simple )
|
|
{
|
|
// get a graph with a constraint in it
|
|
GaussianFactorGraph fg = createSimpleConstraintGraph();
|
|
|
|
// eliminate and solve
|
|
Ordering ord;
|
|
ord += "x", "y";
|
|
VectorConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
VectorConfig expected = createSimpleConstraintConfig();
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, constrained_single )
|
|
{
|
|
// get a graph with a constraint in it
|
|
GaussianFactorGraph fg = createSingleConstraintGraph();
|
|
|
|
// eliminate and solve
|
|
Ordering ord;
|
|
ord += "x", "y";
|
|
VectorConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
VectorConfig expected = createSingleConstraintConfig();
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, constrained_single2 )
|
|
{
|
|
// get a graph with a constraint in it
|
|
GaussianFactorGraph fg = createSingleConstraintGraph();
|
|
|
|
// eliminate and solve
|
|
Ordering ord;
|
|
ord += "y", "x";
|
|
VectorConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
VectorConfig expected = createSingleConstraintConfig();
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, constrained_multi1 )
|
|
{
|
|
// get a graph with a constraint in it
|
|
GaussianFactorGraph fg = createMultiConstraintGraph();
|
|
|
|
// eliminate and solve
|
|
Ordering ord;
|
|
ord += "x", "y", "z";
|
|
VectorConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
VectorConfig expected = createMultiConstraintConfig();
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( GaussianFactorGraph, constrained_multi2 )
|
|
{
|
|
// get a graph with a constraint in it
|
|
GaussianFactorGraph fg = createMultiConstraintGraph();
|
|
|
|
// eliminate and solve
|
|
Ordering ord;
|
|
ord += "z", "x", "y";
|
|
VectorConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
VectorConfig expected = createMultiConstraintConfig();
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|