From 1fed4fff8a4bfdf7526866b78025f21ba9b7c080 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 15 Oct 2011 19:56:14 +0000 Subject: [PATCH] Added unit test for JacobianFactor unwhitened_error and error_vector --- gtsam/linear/tests/testJacobianFactor.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 374261d30..8b287a491 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -70,6 +70,28 @@ TEST(JacobianFactor, constructor2) EXPECT(assert_equal(b, actualb)); } +/* ************************************************************************* */ +TEST(JacobianFactor, error) { + Vector b = Vector_(3, 1., 2., 3.); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,2.,2.,2.)); + std::list > terms; + terms.push_back(make_pair(_x0_, eye(3))); + terms.push_back(make_pair(_x1_, 2.*eye(3))); + const JacobianFactor jf(terms, b, noise); + + VectorValues values(2, 3); + values[0] = Vector_(3, 1.,2.,3.); + values[1] = Vector_(3, 4.,5.,6.); + + Vector expected_unwhitened = Vector_(3, 8., 10., 12.); + Vector actual_unwhitened = jf.unweighted_error(values); + EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); + + Vector expected_whitened = Vector_(3, 4., 5., 6.); + Vector actual_whitened = jf.error_vector(values); + EXPECT(assert_equal(expected_whitened, actual_whitened)); +} + /* ************************************************************************* */ #ifdef BROKEN TEST(JacobianFactor, operators )