From ecaf415d1eaf2e647ba417acee262a67fdc34180 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Apr 2019 18:45:16 -0400 Subject: [PATCH] Better tests on backSubstituteTranspose --- gtsam/linear/tests/testGaussianBayesNet.cpp | 28 +++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 13601844c..3bc5f3371 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -152,6 +152,34 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = smallBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) +{ + // x=R'*y, expected=inv(R')*x + // 2 = 1 2 + // 5 1 1 3 + VectorValues + x = map_list_of + (_x_, Vector1::Constant(2)) + (_y_, Vector1::Constant(5)), + expected = map_list_of + (_x_, Vector1::Constant(4)) + (_y_, Vector1::Constant(9)); + + VectorValues actual = noisyBayesNet.backSubstituteTranspose(x); + EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = noisyBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); } /* ************************************************************************* */