From 9a26f8508e3516e3ca25f7037d9d69914241efe7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Dec 2015 17:26:15 -0800 Subject: [PATCH] Compare diagonals as well for easy debugging --- gtsam/navigation/tests/testScenarioRunner.cpp | 57 ++++++++++++------- 1 file changed, 35 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 4671882f3..a987245d0 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -39,6 +39,8 @@ static boost::shared_ptr defaultParams() { return p; } +#define EXPECT_NEAR(a, b, c) EXPECT(assert_equal(Vector(a), Vector(b), c)); + /* ************************************************************************* */ TEST(ScenarioRunner, Spin) { // angular velocity 6 kDegree/sec @@ -87,8 +89,9 @@ TEST(ScenarioRunner, Forward) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ @@ -115,8 +118,9 @@ TEST(ScenarioRunner, Circle) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ @@ -132,8 +136,9 @@ TEST(ScenarioRunner, Loop) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ @@ -161,8 +166,9 @@ TEST(ScenarioRunner, Accelerating) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -192,8 +198,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -222,8 +229,9 @@ TEST(ScenarioRunner, Accelerating2) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -253,8 +261,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -284,8 +293,9 @@ TEST(ScenarioRunner, Accelerating3) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -315,8 +325,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -347,8 +358,9 @@ TEST(ScenarioRunner, Accelerating4) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -378,8 +390,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix9 estimatedCov = runner.estimateCovariance(T); - // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */