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