From f79a9b8d3a04a11ad0b29b3002263933c9d2c729 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 15:04:36 -0800 Subject: [PATCH] Make two acceleration scenarios --- gtsam/navigation/tests/testScenarioRunner.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 8d01f5572..bf3e3836a 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -76,13 +76,17 @@ TEST(ScenarioRunner, Loop) { } /* ************************************************************************* */ -TEST(ScenarioRunner, Accelerating) { - // Set up body pointing towards y axis, and start at 10,20,0 with velocity - // going in X. The body itself has Z axis pointing down - const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - const Point3 P0(10, 20, 0); - const Vector3 V0(50, 0, 0); +namespace initial { +// Set up body pointing towards y axis, and start at 10,20,0 with velocity +// going in X. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace initial; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0); const AcceleratingScenario scenario(nRb, P0, V0, A); @@ -93,7 +97,26 @@ TEST(ScenarioRunner, Accelerating) { ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + cout << estimatedCov << endl << endl; + cout << runner.poseCovariance(pim) << endl; +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating) { + using namespace initial; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 3; // seconds + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); }