diff --git a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp new file mode 100644 index 000000000..1cf26ade1 --- /dev/null +++ b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp @@ -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 +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +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 > 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);} +/* ************************************************************************* */