GaussianConditionalUnordered unit tests
Richard Roberts
parent
c954b87441
commit
255b79a62b
|
|
@ -0,0 +1,326 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 testGaussianConditional.cpp
|
||||||
|
* @brief Unit tests for Conditional gaussian
|
||||||
|
* @author Christian Potthast
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
|
|
||||||
|
#include <gtsam/base/Matrix.h>
|
||||||
|
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||||
|
#include <gtsam/inference/Key.h>
|
||||||
|
#include <gtsam/linear/JacobianFactorUnordered.h>
|
||||||
|
#include <gtsam/linear/GaussianConditionalUnordered.h>
|
||||||
|
#include <gtsam/linear/GaussianBayesNetUnordered.h>
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <sstream>
|
||||||
|
#include <vector>
|
||||||
|
#include <boost/assign/std/list.hpp>
|
||||||
|
#include <boost/assign/std/vector.hpp>
|
||||||
|
#include <boost/assign/list_inserter.hpp>
|
||||||
|
#include <boost/make_shared.hpp>
|
||||||
|
|
||||||
|
using namespace gtsam;
|
||||||
|
using namespace std;
|
||||||
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
static const double tol = 1e-5;
|
||||||
|
|
||||||
|
static Matrix R = Matrix_(2,2,
|
||||||
|
-12.1244, -5.1962,
|
||||||
|
0., 4.6904);
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(GaussianConditional, constructor)
|
||||||
|
{
|
||||||
|
Matrix S1 = Matrix_(2,2,
|
||||||
|
-5.2786, -8.6603,
|
||||||
|
5.0254, 5.5432);
|
||||||
|
Matrix S2 = Matrix_(2,2,
|
||||||
|
-10.5573, -5.9385,
|
||||||
|
5.5737, 3.0153);
|
||||||
|
Matrix S3 = Matrix_(2,2,
|
||||||
|
-11.3820, -7.2581,
|
||||||
|
-3.0153, -3.5635);
|
||||||
|
|
||||||
|
Vector d = Vector_(2, 1.0, 2.0);
|
||||||
|
SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0));
|
||||||
|
|
||||||
|
list<pair<Key, Matrix> > terms;
|
||||||
|
terms +=
|
||||||
|
make_pair(3, S1),
|
||||||
|
make_pair(5, S2),
|
||||||
|
make_pair(7, S3);
|
||||||
|
|
||||||
|
GaussianConditionalUnordered actual(1, d, R, terms, s);
|
||||||
|
|
||||||
|
GaussianConditionalUnordered::const_iterator it = actual.beginFrontals();
|
||||||
|
EXPECT(assert_equal(Key(1), *it));
|
||||||
|
EXPECT(assert_equal(R, actual.get_R()));
|
||||||
|
++ it;
|
||||||
|
EXPECT(it == actual.endFrontals());
|
||||||
|
|
||||||
|
it = actual.beginParents();
|
||||||
|
EXPECT(assert_equal(Key(3), *it));
|
||||||
|
EXPECT(assert_equal(S1, actual.get_S(it)));
|
||||||
|
|
||||||
|
++ it;
|
||||||
|
EXPECT(assert_equal(Key(5), *it));
|
||||||
|
EXPECT(assert_equal(S2, actual.get_S(it)));
|
||||||
|
|
||||||
|
++ it;
|
||||||
|
EXPECT(assert_equal(Key(7), *it));
|
||||||
|
EXPECT(assert_equal(S3, actual.get_S(it)));
|
||||||
|
|
||||||
|
++it;
|
||||||
|
EXPECT(it == actual.endParents());
|
||||||
|
|
||||||
|
EXPECT(assert_equal(d, actual.get_d()));
|
||||||
|
EXPECT(assert_equal(*s, *actual.get_model()));
|
||||||
|
|
||||||
|
// test copy constructor
|
||||||
|
GaussianConditionalUnordered copied(actual);
|
||||||
|
EXPECT(assert_equal(d, copied.get_d()));
|
||||||
|
EXPECT(assert_equal(*s, *copied.get_model()));
|
||||||
|
EXPECT(assert_equal(R, copied.get_R()));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, equals )
|
||||||
|
{
|
||||||
|
// create a conditional gaussian node
|
||||||
|
Matrix A1(2,2);
|
||||||
|
A1(0,0) = 1 ; A1(1,0) = 2;
|
||||||
|
A1(0,1) = 3 ; A1(1,1) = 4;
|
||||||
|
|
||||||
|
Matrix A2(2,2);
|
||||||
|
A2(0,0) = 6 ; A2(1,0) = 0.2;
|
||||||
|
A2(0,1) = 8 ; A2(1,1) = 0.4;
|
||||||
|
|
||||||
|
Matrix R(2,2);
|
||||||
|
R(0,0) = 0.1 ; R(1,0) = 0.3;
|
||||||
|
R(0,1) = 0.0 ; R(1,1) = 0.34;
|
||||||
|
|
||||||
|
Vector tau(2);
|
||||||
|
tau(0) = 1.0;
|
||||||
|
tau(1) = 0.34;
|
||||||
|
SharedDiagonal model = noiseModel::Diagonal::Sigmas(tau);
|
||||||
|
|
||||||
|
Vector d(2);
|
||||||
|
d(0) = 0.2; d(1) = 0.5;
|
||||||
|
|
||||||
|
GaussianConditionalUnordered
|
||||||
|
expected(1, d, R, 2, A1, 10, A2, model),
|
||||||
|
actual(1, d, R, 2, A1, 10, A2, model);
|
||||||
|
|
||||||
|
EXPECT( expected.equals(actual) );
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, solve )
|
||||||
|
{
|
||||||
|
//expected solution
|
||||||
|
Vector expectedX(2);
|
||||||
|
expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15;
|
||||||
|
|
||||||
|
// create a conditional Gaussian node
|
||||||
|
Matrix R = Matrix_(2,2, 1., 0.,
|
||||||
|
0., 1.);
|
||||||
|
|
||||||
|
Matrix A1 = Matrix_(2,2, 1., 2.,
|
||||||
|
3., 4.);
|
||||||
|
|
||||||
|
Matrix A2 = Matrix_(2,2, 5., 6.,
|
||||||
|
7., 8.);
|
||||||
|
|
||||||
|
Vector d(2);
|
||||||
|
d(0) = 20.0; d(1) = 40.0;
|
||||||
|
|
||||||
|
GaussianConditionalUnordered cg(1, d, R, 2, A1, 10, A2);
|
||||||
|
|
||||||
|
Vector sx1(2);
|
||||||
|
sx1(0) = 1.0; sx1(1) = 1.0;
|
||||||
|
|
||||||
|
Vector sl1(2);
|
||||||
|
sl1(0) = 1.0; sl1(1) = 1.0;
|
||||||
|
|
||||||
|
VectorValuesUnordered expected = map_list_of
|
||||||
|
(1, expectedX)
|
||||||
|
(2, sx1)
|
||||||
|
(10, sl1);
|
||||||
|
|
||||||
|
VectorValuesUnordered solution = map_list_of
|
||||||
|
(2, sx1) // parents
|
||||||
|
(10, sl1);
|
||||||
|
solution.insert(cg.solve(solution));
|
||||||
|
|
||||||
|
EXPECT(assert_equal(expected, solution, tol));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, solve_simple )
|
||||||
|
{
|
||||||
|
// 2 variables, frontal has dim=4
|
||||||
|
VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4);
|
||||||
|
blockMatrix.matrix() <<
|
||||||
|
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
|
||||||
|
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
|
||||||
|
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
|
||||||
|
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
|
||||||
|
|
||||||
|
// solve system as a non-multifrontal version first
|
||||||
|
GaussianConditionalUnordered cg(list_of(1)(2), 1, blockMatrix);
|
||||||
|
|
||||||
|
// partial solution
|
||||||
|
Vector sx1 = Vector_(2, 9.0, 10.0);
|
||||||
|
|
||||||
|
// elimination order: 1, 2
|
||||||
|
VectorValuesUnordered actual = map_list_of
|
||||||
|
(2, sx1); // parent
|
||||||
|
|
||||||
|
VectorValuesUnordered expected = map_list_of
|
||||||
|
(2, sx1)
|
||||||
|
(1, Vector_(4, -3.1,-3.4,-11.9,-13.2));
|
||||||
|
|
||||||
|
// verify indices/size
|
||||||
|
EXPECT_LONGS_EQUAL(2, cg.size());
|
||||||
|
EXPECT_LONGS_EQUAL(4, cg.rows());
|
||||||
|
|
||||||
|
// solve and verify
|
||||||
|
actual.insert(cg.solve(actual));
|
||||||
|
EXPECT(assert_equal(expected, actual, tol));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, solve_multifrontal )
|
||||||
|
{
|
||||||
|
// create full system, 3 variables, 2 frontals, all 2 dim
|
||||||
|
VerticalBlockMatrix blockMatrix(list_of(2)(2)(2)(1), 4);
|
||||||
|
blockMatrix.matrix() <<
|
||||||
|
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
|
||||||
|
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
|
||||||
|
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
|
||||||
|
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
|
||||||
|
|
||||||
|
// 3 variables, all dim=2
|
||||||
|
GaussianConditionalUnordered cg(list_of(1)(2)(10), 2, blockMatrix);
|
||||||
|
|
||||||
|
EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d()));
|
||||||
|
|
||||||
|
// partial solution
|
||||||
|
Vector sl1 = Vector_(2, 9.0, 10.0);
|
||||||
|
|
||||||
|
// elimination order; _x_, _x1_, _l1_
|
||||||
|
VectorValuesUnordered actual = map_list_of
|
||||||
|
(10, sl1); // parent
|
||||||
|
|
||||||
|
VectorValuesUnordered expected = map_list_of
|
||||||
|
(1, Vector_(2, -3.1,-3.4))
|
||||||
|
(2, Vector_(2, -11.9,-13.2))
|
||||||
|
(10, sl1);
|
||||||
|
|
||||||
|
// verify indices/size
|
||||||
|
EXPECT_LONGS_EQUAL(3, cg.size());
|
||||||
|
EXPECT_LONGS_EQUAL(4, cg.rows());
|
||||||
|
|
||||||
|
// solve and verify
|
||||||
|
actual.insert(cg.solve(actual));
|
||||||
|
EXPECT(assert_equal(expected, actual, tol));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, solveTranspose ) {
|
||||||
|
/** create small Chordal Bayes Net x <- y
|
||||||
|
* x y d
|
||||||
|
* 1 1 9
|
||||||
|
* 1 5
|
||||||
|
*/
|
||||||
|
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
|
||||||
|
Matrix R22 = Matrix_(1, 1, 1.0);
|
||||||
|
Vector d1(1), d2(1);
|
||||||
|
d1(0) = 9;
|
||||||
|
d2(0) = 5;
|
||||||
|
|
||||||
|
// define nodes and specify in reverse topological sort (i.e. parents last)
|
||||||
|
GaussianBayesNetUnordered cbn = list_of
|
||||||
|
(GaussianConditionalUnordered(1, d1, R11, 2, S12))
|
||||||
|
(GaussianConditionalUnordered(1, d2, R22));
|
||||||
|
|
||||||
|
// x=R'*y, y=inv(R')*x
|
||||||
|
// 2 = 1 2
|
||||||
|
// 5 1 1 3
|
||||||
|
|
||||||
|
VectorValuesUnordered
|
||||||
|
x = map_list_of
|
||||||
|
(1, Vector_(1,2.))
|
||||||
|
(2, Vector_(1,5.)),
|
||||||
|
y = map_list_of
|
||||||
|
(1, Vector_(1,2.))
|
||||||
|
(2, Vector_(1,3.));
|
||||||
|
|
||||||
|
// test functional version
|
||||||
|
VectorValuesUnordered actual = cbn.backSubstituteTranspose(x);
|
||||||
|
CHECK(assert_equal(y, actual));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, information ) {
|
||||||
|
|
||||||
|
// Create R matrix
|
||||||
|
Matrix R; R <<
|
||||||
|
1, 2, 3, 4,
|
||||||
|
0, 5, 6, 7,
|
||||||
|
0, 0, 8, 9,
|
||||||
|
0, 0, 0, 10;
|
||||||
|
|
||||||
|
// Create conditional
|
||||||
|
GaussianConditionalUnordered conditional(0, Vector::Zero(4), R);
|
||||||
|
|
||||||
|
// Expected information matrix (using permuted R)
|
||||||
|
Matrix IExpected = R.transpose() * R;
|
||||||
|
|
||||||
|
// Actual information matrix (conditional should permute R)
|
||||||
|
Matrix IActual = conditional.information();
|
||||||
|
EXPECT(assert_equal(IExpected, IActual));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( GaussianConditional, isGaussianFactor ) {
|
||||||
|
|
||||||
|
// Create R matrix
|
||||||
|
Matrix R; R <<
|
||||||
|
1, 2, 3, 4,
|
||||||
|
0, 5, 6, 7,
|
||||||
|
0, 0, 8, 9,
|
||||||
|
0, 0, 0, 10;
|
||||||
|
|
||||||
|
// Create a conditional
|
||||||
|
GaussianConditionalUnordered conditional(0, Vector::Zero(4), R);
|
||||||
|
|
||||||
|
// Expected information matrix computed by conditional
|
||||||
|
Matrix IExpected = conditional.information();
|
||||||
|
|
||||||
|
// Expected information matrix computed by a factor
|
||||||
|
JacobianFactorUnordered jf = conditional;
|
||||||
|
Matrix IActual = jf.information();
|
||||||
|
|
||||||
|
EXPECT(assert_equal(IExpected, IActual));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue