Removed deprecated unit tests from testGaussianFactorGraphB and moved SPCG-specific ones to testGraph and testJacobianFactorGraph
Richard Roberts
parent
61b6c72363
commit
b6150bd27e
|
|
@ -0,0 +1,92 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testJacobianFactorGraph.cpp
|
||||
* @brief Unit tests for JacobianFactorGraph
|
||||
* @author Yong Dian Jian
|
||||
**/
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( GaussianFactorGraph, gradient )
|
||||
//{
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// // Construct expected gradient
|
||||
// VectorValues expected;
|
||||
//
|
||||
// // 2*f(x) = 100*(x1+c[X(1)])^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(L(1),Vector_(2, 5.0,-12.5));
|
||||
// expected.insert(X(1),Vector_(2, 30.0, 5.0));
|
||||
// expected.insert(X(2),Vector_(2,-25.0, 17.5));
|
||||
//
|
||||
// // Check the gradient at delta=0
|
||||
// VectorValues zero = createZeroDelta();
|
||||
// VectorValues actual = fg.gradient(zero);
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//
|
||||
// // Check it numerically for good measure
|
||||
// Vector numerical_g = numericalGradient<VectorValues>(error,zero,0.001);
|
||||
// EXPECT(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 += X(2),L(1),X(1);
|
||||
// GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
||||
// VectorValues solution = fg2.optimize(ord); // destructive
|
||||
// VectorValues actual2 = fg.gradient(solution);
|
||||
// EXPECT(assert_equal(zero,actual2));
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( GaussianFactorGraph, transposeMultiplication )
|
||||
//{
|
||||
// // create an ordering
|
||||
// Ordering ord; ord += X(2),L(1),X(1);
|
||||
//
|
||||
// GaussianFactorGraph A = createGaussianFactorGraph(ord);
|
||||
// Errors e;
|
||||
// e += Vector_(2, 0.0, 0.0);
|
||||
// e += Vector_(2,15.0, 0.0);
|
||||
// e += Vector_(2, 0.0,-5.0);
|
||||
// e += Vector_(2,-7.5,-5.0);
|
||||
//
|
||||
// VectorValues expected = createZeroDelta(ord), actual = A ^ e;
|
||||
// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
|
||||
// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
|
||||
// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( GaussianFactorGraph, rhs )
|
||||
//{
|
||||
// // create an ordering
|
||||
// Ordering ord; ord += X(2),L(1),X(1);
|
||||
//
|
||||
// GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
|
||||
// Errors expected = createZeroDelta(ord), actual = Ab.rhs();
|
||||
// expected.push_back(Vector_(2,-1.0,-1.0));
|
||||
// expected.push_back(Vector_(2, 2.0,-1.0));
|
||||
// expected.push_back(Vector_(2, 0.0, 1.0));
|
||||
// expected.push_back(Vector_(2,-1.0, 1.5));
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -56,143 +56,17 @@ TEST( GaussianFactorGraph, equals ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST( GaussianFactorGraph, error ) {
|
||||
// Ordering ordering; ordering += X(1),X(2),L(1);
|
||||
// FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
|
||||
// VectorValues cfg = createZeroDelta(ordering);
|
||||
//
|
||||
// // 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 );
|
||||
//}
|
||||
TEST( GaussianFactorGraph, error ) {
|
||||
Ordering ordering; ordering += X(1),X(2),L(1);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
||||
VectorValues cfg = createZeroDelta(ordering);
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* unit test for find seperator */
|
||||
/* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, find_separator )
|
||||
//{
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// set<Symbol> separator = fg.find_separator(X(2));
|
||||
// set<Symbol> expected;
|
||||
// expected.insert(X(1));
|
||||
// expected.insert(L(1));
|
||||
//
|
||||
// EXPECT(separator.size()==expected.size());
|
||||
// set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin();
|
||||
// for(; it1!=separator.end(); it1++, it2++)
|
||||
// EXPECT(*it1 == *it2);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, combine_factors_x1 )
|
||||
//{
|
||||
// // create a small example for a linear factor graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// // combine all factors
|
||||
// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
|
||||
//
|
||||
// // the expected linear factor
|
||||
// Matrix Al1 = Matrix_(6,2,
|
||||
// 0., 0.,
|
||||
// 0., 0.,
|
||||
// 0., 0.,
|
||||
// 0., 0.,
|
||||
// 5., 0.,
|
||||
// 0., 5.
|
||||
// );
|
||||
//
|
||||
// Matrix Ax1 = Matrix_(6,2,
|
||||
// 10., 0.,
|
||||
// 0., 10.,
|
||||
// -10., 0.,
|
||||
// 0.,-10.,
|
||||
// -5., 0.,
|
||||
// 0.,-5.
|
||||
// );
|
||||
//
|
||||
// Matrix Ax2 = Matrix_(6,2,
|
||||
// 0., 0.,
|
||||
// 0., 0.,
|
||||
// 10., 0.,
|
||||
// 0., 10.,
|
||||
// 0., 0.,
|
||||
// 0., 0.
|
||||
// );
|
||||
//
|
||||
// // the expected RHS vector
|
||||
// Vector b(6);
|
||||
// b(0) = -1;
|
||||
// b(1) = -1;
|
||||
// b(2) = 2;
|
||||
// b(3) = -1;
|
||||
// b(4) = 0;
|
||||
// b(5) = 1;
|
||||
//
|
||||
// vector<pair<Symbol, Matrix> > meas;
|
||||
// meas.push_back(make_pair(L(1), Al1));
|
||||
// meas.push_back(make_pair(X(1), Ax1));
|
||||
// meas.push_back(make_pair(X(2), Ax2));
|
||||
// GaussianFactor expected(meas, b, ones(6));
|
||||
// //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b);
|
||||
//
|
||||
// // check if the two factors are the same
|
||||
// EXPECT(assert_equal(expected,*actual));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, combine_factors_x2 )
|
||||
//{
|
||||
// // create a small example for a linear factor graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// // combine all factors
|
||||
// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2));
|
||||
//
|
||||
// // the expected linear factor
|
||||
// Matrix Al1 = Matrix_(4,2,
|
||||
// // l1
|
||||
// 0., 0.,
|
||||
// 0., 0.,
|
||||
// 5., 0.,
|
||||
// 0., 5.
|
||||
// );
|
||||
//
|
||||
// Matrix Ax1 = Matrix_(4,2,
|
||||
// // x1
|
||||
// -10., 0., // f2
|
||||
// 0.,-10., // f2
|
||||
// 0., 0., // f4
|
||||
// 0., 0. // f4
|
||||
// );
|
||||
//
|
||||
// Matrix Ax2 = Matrix_(4,2,
|
||||
// // x2
|
||||
// 10., 0.,
|
||||
// 0., 10.,
|
||||
// -5., 0.,
|
||||
// 0.,-5.
|
||||
// );
|
||||
//
|
||||
// // the expected RHS vector
|
||||
// Vector b(4);
|
||||
// b(0) = 2;
|
||||
// b(1) = -1;
|
||||
// b(2) = -1;
|
||||
// b(3) = 1.5;
|
||||
//
|
||||
// vector<pair<Symbol, Matrix> > meas;
|
||||
// meas.push_back(make_pair(L(1), Al1));
|
||||
// meas.push_back(make_pair(X(1), Ax1));
|
||||
// meas.push_back(make_pair(X(2), Ax2));
|
||||
// GaussianFactor expected(meas, b, ones(4));
|
||||
//
|
||||
// // check if the two factors are the same
|
||||
// EXPECT(assert_equal(expected,*actual));
|
||||
//}
|
||||
// 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 );
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, eliminateOne_x1 )
|
||||
|
|
@ -336,47 +210,6 @@ TEST( GaussianFactorGraph, eliminateAll )
|
|||
EXPECT(assert_equal(expected,actualQR,tol));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( GaussianFactorGraph, eliminateAll_fast )
|
||||
//{
|
||||
// // create expected Chordal bayes Net
|
||||
// Matrix I = eye(2);
|
||||
//
|
||||
// Vector d1 = Vector_(2, -0.1,-0.1);
|
||||
// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1);
|
||||
//
|
||||
// double sig1 = 0.149071;
|
||||
// Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
|
||||
// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2);
|
||||
//
|
||||
// double sig2 = 0.0894427;
|
||||
// Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
|
||||
// push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3);
|
||||
//
|
||||
// // Check one ordering
|
||||
// GaussianFactorGraph fg1 = createGaussianFactorGraph();
|
||||
// Ordering ordering;
|
||||
// ordering += X(2),L(1),X(1);
|
||||
// GaussianBayesNet actual = fg1.eliminate(ordering, false);
|
||||
// EXPECT(assert_equal(expected,actual,tol));
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( GaussianFactorGraph, add_priors )
|
||||
//{
|
||||
// Ordering ordering; ordering += L(1),X(1),X(2);
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
||||
// GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
|
||||
// GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
|
||||
// Matrix A = eye(2);
|
||||
// Vector b = zero(2);
|
||||
// SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0);
|
||||
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma)));
|
||||
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma)));
|
||||
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma)));
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, copying )
|
||||
{
|
||||
|
|
@ -397,46 +230,6 @@ TEST( GaussianFactorGraph, copying )
|
|||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, matrix )
|
||||
//{
|
||||
// // render with a given ordering
|
||||
// Ordering ord;
|
||||
// ord += X(2),L(1),X(1);
|
||||
//
|
||||
// // Create a graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
||||
//
|
||||
// Matrix A; Vector b;
|
||||
// boost::tie(A,b) = fg.matrix();
|
||||
//
|
||||
// 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);
|
||||
// EXPECT(b==b1);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, sizeOfA )
|
||||
//{
|
||||
// // create a small linear factor graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// pair<size_t, size_t> mn = fg.sizeOfA();
|
||||
// EXPECT(8 == mn.first);
|
||||
// EXPECT(6 == mn.second);
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
|
||||
{
|
||||
|
|
@ -451,11 +244,6 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
|
|||
GaussianFactorGraph fg2(CBN);
|
||||
GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
|
||||
EXPECT(assert_equal(CBN,CBN2));
|
||||
|
||||
// Base FactorGraph only
|
||||
// FactorGraph<GaussianFactor> fg3(CBN);
|
||||
// GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,GaussianConditional>(fg3,ord);
|
||||
// EXPECT(assert_equal(CBN,CBN3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -469,16 +257,6 @@ TEST( GaussianFactorGraph, getOrdering)
|
|||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
// SL-FIX TEST( GaussianFactorGraph, getOrdering2)
|
||||
//{
|
||||
// Ordering expected;
|
||||
// expected += L(1),X(1);
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
// set<Symbol> interested; interested += L(1),X(1);
|
||||
// Ordering actual = fg.getOrdering(interested);
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, optimize_Cholesky )
|
||||
{
|
||||
|
|
@ -515,24 +293,6 @@ TEST( GaussianFactorGraph, optimize_QR )
|
|||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas )
|
||||
//{
|
||||
// // create an ordering
|
||||
// Ordering ord; ord += X(2),L(1),X(1);
|
||||
//
|
||||
// // create a graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph(ord);
|
||||
//
|
||||
// // optimize the graph
|
||||
// VectorValues actual = fg.optimizeMultiFrontals(ord);
|
||||
//
|
||||
// // verify
|
||||
// VectorValues expected = createCorrectDelta();
|
||||
//
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, combine)
|
||||
{
|
||||
|
|
@ -585,48 +345,6 @@ void print(vector<int> v) {
|
|||
cout << endl;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, factor_lookup)
|
||||
//{
|
||||
// // create a test graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// // ask for all factor indices connected to x1
|
||||
// list<size_t> x1_factors = fg.factors(X(1));
|
||||
// size_t x1_indices[] = { 0, 1, 2 };
|
||||
// list<size_t> x1_expected(x1_indices, x1_indices + 3);
|
||||
// EXPECT(x1_factors==x1_expected);
|
||||
//
|
||||
// // ask for all factor indices connected to x2
|
||||
// list<size_t> x2_factors = fg.factors(X(2));
|
||||
// size_t x2_indices[] = { 1, 3 };
|
||||
// list<size_t> x2_expected(x2_indices, x2_indices + 2);
|
||||
// EXPECT(x2_factors==x2_expected);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? 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(X(1));
|
||||
//
|
||||
// // Check the factors
|
||||
// EXPECT(f0==factors[0]);
|
||||
// EXPECT(f1==factors[1]);
|
||||
// EXPECT(f2==factors[2]);
|
||||
//
|
||||
// // EXPECT if the factors are deleted from the factor graph
|
||||
// LONGS_EQUAL(1,fg.nrFactors());
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, createSmoother)
|
||||
{
|
||||
|
|
@ -636,35 +354,6 @@ TEST(GaussianFactorGraph, createSmoother)
|
|||
LONGS_EQUAL(5,fg2.size());
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, variables )
|
||||
//{
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
// Dimensions expected;
|
||||
// insert(expected)(L(1), 2)(X(1), 2)(X(2), 2);
|
||||
// Dimensions actual = fg.dimensions();
|
||||
// EXPECT(expected==actual);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, keys )
|
||||
//{
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
// Ordering expected;
|
||||
// expected += L(1),X(1),X(2);
|
||||
// EXPECT(assert_equal(expected,fg.keys()));
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, involves )
|
||||
//{
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
// EXPECT(fg.involves(L(1)));
|
||||
// EXPECT(fg.involves(X(1)));
|
||||
// EXPECT(fg.involves(X(2)));
|
||||
// EXPECT(!fg.involves(X(3)));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double error(const VectorValues& x) {
|
||||
// create an ordering
|
||||
|
|
@ -674,38 +363,6 @@ double error(const VectorValues& x) {
|
|||
return fg.error(x);
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, gradient )
|
||||
//{
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// // Construct expected gradient
|
||||
// VectorValues expected;
|
||||
//
|
||||
// // 2*f(x) = 100*(x1+c[X(1)])^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(L(1),Vector_(2, 5.0,-12.5));
|
||||
// expected.insert(X(1),Vector_(2, 30.0, 5.0));
|
||||
// expected.insert(X(2),Vector_(2,-25.0, 17.5));
|
||||
//
|
||||
// // Check the gradient at delta=0
|
||||
// VectorValues zero = createZeroDelta();
|
||||
// VectorValues actual = fg.gradient(zero);
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//
|
||||
// // Check it numerically for good measure
|
||||
// Vector numerical_g = numericalGradient<VectorValues>(error,zero,0.001);
|
||||
// EXPECT(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 += X(2),L(1),X(1);
|
||||
// GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
||||
// VectorValues solution = fg2.optimize(ord); // destructive
|
||||
// VectorValues actual2 = fg.gradient(solution);
|
||||
// EXPECT(assert_equal(zero,actual2));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, multiplication )
|
||||
{
|
||||
|
|
@ -723,41 +380,6 @@ TEST( GaussianFactorGraph, multiplication )
|
|||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication )
|
||||
//{
|
||||
// // create an ordering
|
||||
// Ordering ord; ord += X(2),L(1),X(1);
|
||||
//
|
||||
// GaussianFactorGraph A = createGaussianFactorGraph(ord);
|
||||
// Errors e;
|
||||
// e += Vector_(2, 0.0, 0.0);
|
||||
// e += Vector_(2,15.0, 0.0);
|
||||
// e += Vector_(2, 0.0,-5.0);
|
||||
// e += Vector_(2,-7.5,-5.0);
|
||||
//
|
||||
// VectorValues expected = createZeroDelta(ord), actual = A ^ e;
|
||||
// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
|
||||
// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
|
||||
// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-NEEDED? TEST( GaussianFactorGraph, rhs )
|
||||
//{
|
||||
// // create an ordering
|
||||
// Ordering ord; ord += X(2),L(1),X(1);
|
||||
//
|
||||
// GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
|
||||
// Errors expected = createZeroDelta(ord), actual = Ab.rhs();
|
||||
// expected.push_back(Vector_(2,-1.0,-1.0));
|
||||
// expected.push_back(Vector_(2, 2.0,-1.0));
|
||||
// expected.push_back(Vector_(2, 0.0, 1.0));
|
||||
// expected.push_back(Vector_(2,-1.0, 1.5));
|
||||
// EXPECT(assert_equal(expected,actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
|
||||
TEST( GaussianFactorGraph, elimination )
|
||||
|
|
@ -824,22 +446,6 @@ TEST( GaussianFactorGraph, constrained_single )
|
|||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//SL-FIX TEST( GaussianFactorGraph, constrained_single2 )
|
||||
//{
|
||||
// // get a graph with a constraint in it
|
||||
// GaussianFactorGraph fg = createSingleConstraintGraph();
|
||||
//
|
||||
// // eliminate and solve
|
||||
// Ordering ord;
|
||||
// ord += "yk, x";
|
||||
// VectorValues actual = fg.optimize(ord);
|
||||
//
|
||||
// // verify
|
||||
// VectorValues expected = createSingleConstraintValues();
|
||||
// EXPECT(assert_equal(expected, actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, constrained_multi1 )
|
||||
{
|
||||
|
|
@ -855,68 +461,9 @@ TEST( GaussianFactorGraph, constrained_multi1 )
|
|||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//SL-FIX TEST( GaussianFactorGraph, constrained_multi2 )
|
||||
//{
|
||||
// // get a graph with a constraint in it
|
||||
// GaussianFactorGraph fg = createMultiConstraintGraph();
|
||||
//
|
||||
// // eliminate and solve
|
||||
// Ordering ord;
|
||||
// ord += "zk, xk, y";
|
||||
// VectorValues actual = fg.optimize(ord);
|
||||
//
|
||||
// // verify
|
||||
// VectorValues expected = createMultiConstraintValues();
|
||||
// EXPECT(assert_equal(expected, actual));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
|
||||
|
||||
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
|
||||
//{
|
||||
// GaussianFactorGraph g;
|
||||
// Matrix I = eye(2);
|
||||
// Vector b = Vector_(0, 0, 0);
|
||||
// g.add(X(1), I, X(2), I, b, model);
|
||||
// g.add(X(1), I, X(3), I, b, model);
|
||||
// g.add(X(1), I, X(4), I, b, model);
|
||||
// g.add(X(2), I, X(3), I, b, model);
|
||||
// g.add(X(2), I, X(4), I, b, model);
|
||||
// g.add(X(3), I, X(4), I, b, model);
|
||||
//
|
||||
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
|
||||
// EXPECT(tree[X(1)].compare(X(1))==0);
|
||||
// EXPECT(tree[X(2)].compare(X(1))==0);
|
||||
// EXPECT(tree[X(3)].compare(X(1))==0);
|
||||
// EXPECT(tree[X(4)].compare(X(1))==0);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, split )
|
||||
//{
|
||||
// GaussianFactorGraph g;
|
||||
// Matrix I = eye(2);
|
||||
// Vector b = Vector_(0, 0, 0);
|
||||
// g.add(X(1), I, X(2), I, b, model);
|
||||
// g.add(X(1), I, X(3), I, b, model);
|
||||
// g.add(X(1), I, X(4), I, b, model);
|
||||
// g.add(X(2), I, X(3), I, b, model);
|
||||
// g.add(X(2), I, X(4), I, b, model);
|
||||
//
|
||||
// PredecessorMap<string> tree;
|
||||
// tree[X(1)] = X(1);
|
||||
// tree[X(2)] = X(1);
|
||||
// tree[X(3)] = X(1);
|
||||
// tree[X(4)] = X(1);
|
||||
//
|
||||
// GaussianFactorGraph Ab1, Ab2;
|
||||
// g.split<string, GaussianFactor>(tree, Ab1, Ab2);
|
||||
// LONGS_EQUAL(3, Ab1.size());
|
||||
// LONGS_EQUAL(2, Ab2.size());
|
||||
//}
|
||||
static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, replace)
|
||||
|
|
@ -935,91 +482,18 @@ TEST(GaussianFactorGraph, replace)
|
|||
|
||||
GaussianFactorGraph actual;
|
||||
actual.push_back(f1);
|
||||
// actual.checkGraphConsistency();
|
||||
actual.push_back(f2);
|
||||
// actual.checkGraphConsistency();
|
||||
actual.push_back(f3);
|
||||
// actual.checkGraphConsistency();
|
||||
actual.replace(0, f4);
|
||||
// actual.checkGraphConsistency();
|
||||
|
||||
GaussianFactorGraph expected;
|
||||
expected.push_back(f4);
|
||||
// actual.checkGraphConsistency();
|
||||
expected.push_back(f2);
|
||||
// actual.checkGraphConsistency();
|
||||
expected.push_back(f3);
|
||||
// actual.checkGraphConsistency();
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST ( GaussianFactorGraph, combine_matrix ) {
|
||||
// // create a small linear factor graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
// Dimensions dimensions = fg.dimensions();
|
||||
//
|
||||
// // get two factors from it and insert the factors into a vector
|
||||
// vector<GaussianFactor::shared_ptr> lfg;
|
||||
// lfg.push_back(fg[4 - 1]);
|
||||
// lfg.push_back(fg[2 - 1]);
|
||||
//
|
||||
// // combine in a factor
|
||||
// Matrix Ab; SharedDiagonal noise;
|
||||
// Ordering order; order += X(2), L(1), X(1);
|
||||
// boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions);
|
||||
//
|
||||
// // the expected augmented matrix
|
||||
// Matrix expAb = Matrix_(4, 7,
|
||||
// -5., 0., 5., 0., 0., 0.,-1.0,
|
||||
// +0., -5., 0., 5., 0., 0., 1.5,
|
||||
// 10., 0., 0., 0.,-10., 0., 2.0,
|
||||
// +0., 10., 0., 0., 0.,-10.,-1.0);
|
||||
//
|
||||
// // expected noise model
|
||||
// SharedDiagonal expModel = noiseModel::Unit::Create(4);
|
||||
//
|
||||
// EXPECT(assert_equal(expAb, Ab));
|
||||
// EXPECT(assert_equal(*expModel, *noise));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/*
|
||||
* x2 x1 x3 b
|
||||
* 1 1 1 1 1 0 1
|
||||
* 1 1 1 -> 1 1 1
|
||||
* 1 1 1 1
|
||||
*/
|
||||
// SL-NEEDED? TEST ( GaussianFactorGraph, eliminateFrontals ) {
|
||||
// typedef GaussianFactorGraph::sharedFactor Factor;
|
||||
// SharedDiagonal model(Vector_(1, 0.5));
|
||||
// GaussianFactorGraph fg;
|
||||
// Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
// Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
// Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model));
|
||||
// fg.push_back(factor1);
|
||||
// fg.push_back(factor2);
|
||||
// fg.push_back(factor3);
|
||||
//
|
||||
// Ordering frontals; frontals += X(2), X(1);
|
||||
// GaussianBayesNet bn = fg.eliminateFrontals(frontals);
|
||||
//
|
||||
// GaussianBayesNet bn_expected;
|
||||
// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.),
|
||||
// X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
|
||||
// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.),
|
||||
// X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
|
||||
// bn_expected.push_back(conditional1);
|
||||
// bn_expected.push_back(conditional2);
|
||||
// EXPECT(assert_equal(bn_expected, bn));
|
||||
//
|
||||
// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
|
||||
// GaussianFactorGraph fg_expected;
|
||||
// fg_expected.push_back(factor_expected);
|
||||
// EXPECT(assert_equal(fg_expected, fg));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, createSmoother2)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -105,6 +105,49 @@ TEST( Graph, composePoses )
|
|||
CHECK(assert_equal(expected, *actual));
|
||||
}
|
||||
|
||||
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
|
||||
//{
|
||||
// GaussianFactorGraph g;
|
||||
// Matrix I = eye(2);
|
||||
// Vector b = Vector_(0, 0, 0);
|
||||
// g.add(X(1), I, X(2), I, b, model);
|
||||
// g.add(X(1), I, X(3), I, b, model);
|
||||
// g.add(X(1), I, X(4), I, b, model);
|
||||
// g.add(X(2), I, X(3), I, b, model);
|
||||
// g.add(X(2), I, X(4), I, b, model);
|
||||
// g.add(X(3), I, X(4), I, b, model);
|
||||
//
|
||||
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
|
||||
// EXPECT(tree[X(1)].compare(X(1))==0);
|
||||
// EXPECT(tree[X(2)].compare(X(1))==0);
|
||||
// EXPECT(tree[X(3)].compare(X(1))==0);
|
||||
// EXPECT(tree[X(4)].compare(X(1))==0);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// SL-FIX TEST( GaussianFactorGraph, split )
|
||||
//{
|
||||
// GaussianFactorGraph g;
|
||||
// Matrix I = eye(2);
|
||||
// Vector b = Vector_(0, 0, 0);
|
||||
// g.add(X(1), I, X(2), I, b, model);
|
||||
// g.add(X(1), I, X(3), I, b, model);
|
||||
// g.add(X(1), I, X(4), I, b, model);
|
||||
// g.add(X(2), I, X(3), I, b, model);
|
||||
// g.add(X(2), I, X(4), I, b, model);
|
||||
//
|
||||
// PredecessorMap<string> tree;
|
||||
// tree[X(1)] = X(1);
|
||||
// tree[X(2)] = X(1);
|
||||
// tree[X(3)] = X(1);
|
||||
// tree[X(4)] = X(1);
|
||||
//
|
||||
// GaussianFactorGraph Ab1, Ab2;
|
||||
// g.split<string, GaussianFactor>(tree, Ab1, Ab2);
|
||||
// LONGS_EQUAL(3, Ab1.size());
|
||||
// LONGS_EQUAL(2, Ab2.size());
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue