Resurrected lots of elimination tests
Frank Dellaert
parent
464804d8f5
commit
110240aa4f
|
|
@ -169,241 +169,178 @@ TEST(GaussianFactorGraph, eliminateOne_l1_fast) {
|
|||
EXPECT(assert_equal(expected, *actual, tol));
|
||||
}
|
||||
|
||||
#if 0
|
||||
// /* ************************************************************************* */
|
||||
// TEST( GaussianFactorGraph, eliminateAll )
|
||||
// {
|
||||
// // create expected Chordal bayes Net
|
||||
// Matrix I = I_2x2;
|
||||
|
||||
// Ordering ordering;
|
||||
// ordering += X(2),L(1),X(1);
|
||||
|
||||
// Vector d1 = Vector2(-0.1,-0.1);
|
||||
// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1);
|
||||
|
||||
// double sig1 = 0.149071;
|
||||
// Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2);
|
||||
// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2);
|
||||
|
||||
// double sig2 = 0.0894427;
|
||||
// Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::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();
|
||||
// GaussianBayesNet actual = *fg1.eliminateSequential();
|
||||
// EXPECT(assert_equal(expected,actual,tol));
|
||||
|
||||
// GaussianBayesNet actualQR = *fg1.eliminateSequential(, true);
|
||||
// EXPECT(assert_equal(expected,actualQR,tol));
|
||||
// }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, eliminateAll )
|
||||
{
|
||||
// create expected Chordal bayes Net
|
||||
Matrix I = I_2x2;
|
||||
|
||||
Ordering ordering;
|
||||
ordering += X(2),L(1),X(1);
|
||||
|
||||
Vector d1 = Vector2(-0.1,-0.1);
|
||||
GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1);
|
||||
|
||||
double sig1 = 0.149071;
|
||||
Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2);
|
||||
push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2);
|
||||
|
||||
double sig2 = 0.0894427;
|
||||
Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::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();
|
||||
GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
|
||||
EXPECT(assert_equal(expected,actual,tol));
|
||||
|
||||
GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
|
||||
EXPECT(assert_equal(expected,actualQR,tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, copying )
|
||||
{
|
||||
TEST(GaussianFactorGraph, copying) {
|
||||
// Create a graph
|
||||
Ordering ordering; ordering += X(2),L(1),X(1);
|
||||
GaussianFactorGraph actual = createGaussianFactorGraph();
|
||||
|
||||
// Copy the graph !
|
||||
GaussianFactorGraph copy = actual;
|
||||
|
||||
// now eliminate the copy
|
||||
GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate();
|
||||
GaussianBayesNet actual1 = *copy.eliminateSequential();
|
||||
|
||||
// Create the same graph, but not by copying
|
||||
GaussianFactorGraph expected = createGaussianFactorGraph();
|
||||
|
||||
// and check that original is still the same graph
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
|
||||
{
|
||||
Ordering ord;
|
||||
ord += X(2),L(1),X(1);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
|
||||
TEST(GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet) {
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
|
||||
// render with a given ordering
|
||||
GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate();
|
||||
GaussianBayesNet CBN = *fg.eliminateSequential();
|
||||
|
||||
// True GaussianFactorGraph
|
||||
GaussianFactorGraph fg2(CBN);
|
||||
GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
|
||||
EXPECT(assert_equal(CBN,CBN2));
|
||||
GaussianBayesNet CBN2 = *fg2.eliminateSequential();
|
||||
EXPECT(assert_equal(CBN, CBN2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, getOrdering)
|
||||
{
|
||||
Ordering original; original += L(1),X(1),X(2);
|
||||
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
|
||||
Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
|
||||
Ordering actual = original; actual.permuteInPlace(perm);
|
||||
Ordering expected; expected += L(1),X(2),X(1);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, optimize_Cholesky )
|
||||
{
|
||||
// create an ordering
|
||||
Ordering ord; ord += X(2),L(1),X(1);
|
||||
|
||||
TEST(GaussianFactorGraph, optimize_Cholesky) {
|
||||
// create a graph
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
|
||||
// optimize the graph
|
||||
VectorValues actual = *GaussianSequentialSolver(fg, false).optimize();
|
||||
VectorValues actual = fg.optimize(boost::none, EliminateCholesky);
|
||||
|
||||
// verify
|
||||
VectorValues expected = createCorrectDelta(ord);
|
||||
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
VectorValues expected = createCorrectDelta();
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, optimize_QR )
|
||||
{
|
||||
// create an ordering
|
||||
Ordering ord; ord += X(2),L(1),X(1);
|
||||
|
||||
// create a graph
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
|
||||
// optimize the graph
|
||||
VectorValues actual = *GaussianSequentialSolver(fg, true).optimize();
|
||||
VectorValues actual = fg.optimize(boost::none, EliminateQR);
|
||||
|
||||
// verify
|
||||
VectorValues expected = createCorrectDelta(ord);
|
||||
|
||||
VectorValues expected = createCorrectDelta();
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, combine)
|
||||
{
|
||||
// create an ordering
|
||||
Ordering ord; ord += X(2),L(1),X(1);
|
||||
|
||||
TEST(GaussianFactorGraph, combine) {
|
||||
// create a test graph
|
||||
GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
|
||||
GaussianFactorGraph fg1 = createGaussianFactorGraph();
|
||||
|
||||
// create another factor graph
|
||||
GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);
|
||||
GaussianFactorGraph fg2 = createGaussianFactorGraph();
|
||||
|
||||
// get sizes
|
||||
size_t size1 = fg1.size();
|
||||
size_t size2 = fg2.size();
|
||||
|
||||
// combine them
|
||||
fg1.combine(fg2);
|
||||
fg1.push_back(fg2);
|
||||
|
||||
EXPECT(size1+size2 == fg1.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, combine2)
|
||||
{
|
||||
// create an ordering
|
||||
Ordering ord; ord += X(2),L(1),X(1);
|
||||
|
||||
// create a test graph
|
||||
GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
|
||||
|
||||
// create another factor graph
|
||||
GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);
|
||||
|
||||
// get sizes
|
||||
size_t size1 = fg1.size();
|
||||
size_t size2 = fg2.size();
|
||||
|
||||
// combine them
|
||||
GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);
|
||||
|
||||
EXPECT(size1+size2 == fg3.size());
|
||||
EXPECT(size1 + size2 == fg1.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// print a vector of ints if needed for debugging
|
||||
void print(vector<int> v) {
|
||||
for (size_t k = 0; k < v.size(); k++)
|
||||
cout << v[k] << " ";
|
||||
for (size_t k = 0; k < v.size(); k++) cout << v[k] << " ";
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, createSmoother)
|
||||
{
|
||||
GaussianFactorGraph fg1 = createSmoother(2).first;
|
||||
LONGS_EQUAL(3,fg1.size());
|
||||
GaussianFactorGraph fg2 = createSmoother(3).first;
|
||||
LONGS_EQUAL(5,fg2.size());
|
||||
TEST(GaussianFactorGraph, createSmoother) {
|
||||
GaussianFactorGraph fg1 = createSmoother(2);
|
||||
LONGS_EQUAL(3, fg1.size());
|
||||
GaussianFactorGraph fg2 = createSmoother(3);
|
||||
LONGS_EQUAL(5, fg2.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double error(const VectorValues& x) {
|
||||
// create an ordering
|
||||
Ordering ord; ord += X(2),L(1),X(1);
|
||||
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
return fg.error(x);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, multiplication )
|
||||
{
|
||||
// create an ordering
|
||||
Ordering ord; ord += X(2),L(1),X(1);
|
||||
|
||||
GaussianFactorGraph A = createGaussianFactorGraph(ord);
|
||||
VectorValues x = createCorrectDelta(ord);
|
||||
TEST(GaussianFactorGraph, multiplication) {
|
||||
GaussianFactorGraph A = createGaussianFactorGraph();
|
||||
VectorValues x = createCorrectDelta();
|
||||
Errors actual = A * x;
|
||||
Errors expected;
|
||||
expected += Vector2(-1.0,-1.0);
|
||||
expected += Vector2(2.0,-1.0);
|
||||
expected += Vector2(-1.0, -1.0);
|
||||
expected += Vector2(2.0, -1.0);
|
||||
expected += Vector2(0.0, 1.0);
|
||||
expected += Vector2(-1.0, 1.5);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
|
||||
TEST( GaussianFactorGraph, elimination )
|
||||
{
|
||||
Ordering ord;
|
||||
ord += X(1), X(2);
|
||||
TEST(GaussianFactorGraph, elimination) {
|
||||
// Create Gaussian Factor Graph
|
||||
GaussianFactorGraph fg;
|
||||
Matrix Ap = I_2x2, An = I_2x2 * -1;
|
||||
Matrix Ap = I_1x1, An = I_1x1 * -1;
|
||||
Vector b = (Vector(1) << 0.0).finished();
|
||||
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0);
|
||||
fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma;
|
||||
fg += ord[X(1)], Ap, b, sigma;
|
||||
fg += ord[X(2)], Ap, b, sigma;
|
||||
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1, 2.0);
|
||||
fg += JacobianFactor(X(1), An, X(2), Ap, b, sigma);
|
||||
fg += JacobianFactor(X(1), Ap, b, sigma);
|
||||
fg += JacobianFactor(X(2), Ap, b, sigma);
|
||||
|
||||
// Eliminate
|
||||
GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
|
||||
|
||||
// Check sigma
|
||||
EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5);
|
||||
Ordering ordering;
|
||||
ordering += X(1), X(2);
|
||||
GaussianBayesNet bayesNet = *fg.eliminateSequential();
|
||||
|
||||
// Check matrix
|
||||
Matrix R;Vector d;
|
||||
boost::tie(R,d) = matrix(bayesNet);
|
||||
Matrix expected = (Matrix(2, 2) <<
|
||||
0.707107, -0.353553,
|
||||
0.0, 0.612372).finished();
|
||||
Matrix expected2 = (Matrix(2, 2) <<
|
||||
0.707107, -0.353553,
|
||||
0.0, -0.612372).finished();
|
||||
EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6));
|
||||
Matrix R;
|
||||
Vector d;
|
||||
boost::tie(R, d) = bayesNet.matrix();
|
||||
Matrix expected =
|
||||
(Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished();
|
||||
Matrix expected2 =
|
||||
(Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372).finished();
|
||||
EXPECT(assert_equal(expected, R, 1e-6));
|
||||
EXPECT(equal_with_abs_tol(expected, R, 1e-6) ||
|
||||
equal_with_abs_tol(expected2, R, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#if 0
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Tests ported from ConstrainedGaussianFactorGraph
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, constrained_simple )
|
||||
|
|
@ -412,9 +349,8 @@ TEST( GaussianFactorGraph, constrained_simple )
|
|||
GaussianFactorGraph fg = createSimpleConstraintGraph();
|
||||
EXPECT(hasConstraints(fg));
|
||||
|
||||
|
||||
// eliminate and solve
|
||||
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
|
||||
VectorValues actual = *fg.eliminateSequential().optimize();
|
||||
|
||||
// verify
|
||||
VectorValues expected = createSimpleConstraintValues();
|
||||
|
|
@ -429,7 +365,7 @@ TEST( GaussianFactorGraph, constrained_single )
|
|||
EXPECT(hasConstraints(fg));
|
||||
|
||||
// eliminate and solve
|
||||
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
|
||||
VectorValues actual = *fg.eliminateSequential().optimize();
|
||||
|
||||
// verify
|
||||
VectorValues expected = createSingleConstraintValues();
|
||||
|
|
@ -444,7 +380,7 @@ TEST( GaussianFactorGraph, constrained_multi1 )
|
|||
EXPECT(hasConstraints(fg));
|
||||
|
||||
// eliminate and solve
|
||||
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
|
||||
VectorValues actual = *fg.eliminateSequential().optimize();
|
||||
|
||||
// verify
|
||||
VectorValues expected = createMultiConstraintValues();
|
||||
|
|
@ -462,13 +398,13 @@ TEST(GaussianFactorGraph, replace)
|
|||
SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
|
||||
|
||||
GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
|
||||
ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise));
|
||||
X(1), I_3x3, X(2), I_3x3, Z_3x1, noise));
|
||||
GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
|
||||
ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise));
|
||||
X(2), I_3x3, X(3), I_3x3, Z_3x1, noise));
|
||||
GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
|
||||
ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise));
|
||||
X(3), I_3x3, X(4), I_3x3, Z_3x1, noise));
|
||||
GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
|
||||
ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise));
|
||||
X(5), I_3x3, X(6), I_3x3, Z_3x1, noise));
|
||||
|
||||
GaussianFactorGraph actual;
|
||||
actual.push_back(f1);
|
||||
|
|
@ -497,9 +433,9 @@ TEST(GaussianFactorGraph, createSmoother2)
|
|||
vector<Index> x3var; x3var.push_back(ordering[X(3)]);
|
||||
vector<Index> x1var; x1var.push_back(X(1));
|
||||
GaussianBayesNet p_x3 = *GaussianSequentialSolver(
|
||||
*GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
|
||||
*fg2.eliminateSequential().jointFactorGraph(x3var));
|
||||
GaussianBayesNet p_x1 = *GaussianSequentialSolver(
|
||||
*GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate();
|
||||
*fg2.eliminateSequential().jointFactorGraph(x1var));
|
||||
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue