From 6f5e332a98e0c66fb73cd786c6d30441c0804f87 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 12 Jun 2019 14:21:08 -0400 Subject: [PATCH] Reduced time to run very slow testScenario by reducing sample count for estimating covariance. --- gtsam/navigation/ScenarioRunner.cpp | 1 + gtsam/navigation/tests/testScenarios.cpp | 57 ++++++++++++++++-------- 2 files changed, 40 insertions(+), 18 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index fcba92f60..3938ce86c 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -31,6 +31,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; PreintegratedImuMeasurements ScenarioRunner::integrate( double T, const Bias& estimatedBias, bool corrupted) const { + gttic_(integrate); PreintegratedImuMeasurements pim(p_, estimatedBias); const double dt = imuSampleTime(); diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index 2129d2d92..adac0fb53 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -15,6 +15,8 @@ * @author Frank Dellaert */ +// #define ENABLE_TIMING // uncomment for timing results + #include #include #include @@ -46,6 +48,7 @@ static boost::shared_ptr defaultParams() { /* ************************************************************************* */ TEST(ScenarioRunner, Spin) { + gttic(Spin); // angular velocity 6 degree/sec const double w = 6 * kDegree; const Vector3 W(0, 0, w), V(0, 0, 0); @@ -63,12 +66,12 @@ TEST(ScenarioRunner, Spin) { Matrix6 expected; expected << p->accelerometerCovariance / kDt, Z_3x3, // Z_3x3, p->gyroscopeCovariance / kDt; - Matrix6 actual = runner.estimateNoiseCovariance(10000); + Matrix6 actual = runner.estimateNoiseCovariance(1000); EXPECT(assert_equal(expected, actual, 1e-2)); #endif // Check calculated covariance against Monte Carlo estimate - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } @@ -79,6 +82,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0)); } /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { + gttic(Forward); using namespace forward; ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds @@ -86,24 +90,26 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, ForwardWithBias) { + gttic(ForwardWithBias); using namespace forward; ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { + gttic(Circle); // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); @@ -114,13 +120,14 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, Loop) { + gttic(Loop); // Forward velocity 2m/s // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * kDegree; @@ -132,7 +139,7 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } @@ -156,29 +163,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { + gttic(Accelerating); using namespace accelerating; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias) { + gttic(AcceleratingWithBias); using namespace accelerating; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating) { + gttic(AcceleratingAndRotating); using namespace initial; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -190,7 +200,7 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -215,29 +225,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating2) { + gttic(Accelerating2); using namespace accelerating2; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias2) { + gttic(AcceleratingWithBias2); using namespace accelerating2; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating2) { + gttic(AcceleratingAndRotating2); using namespace initial2; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -249,7 +262,7 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -275,29 +288,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating3) { + gttic(Accelerating3); using namespace accelerating3; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias3) { + gttic(AcceleratingWithBias3); using namespace accelerating3; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating3) { + gttic(AcceleratingAndRotating3); using namespace initial3; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -309,7 +325,7 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -336,29 +352,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating4) { + gttic(Accelerating4); using namespace accelerating4; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias4) { + gttic(AcceleratingWithBias4); using namespace accelerating4; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating4) { + gttic(AcceleratingAndRotating4); using namespace initial4; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -370,7 +389,7 @@ 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); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -379,7 +398,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { int main() { TestResult tr; auto result = TestRegistry::runAllTests(tr); +#ifdef ENABLE_TIMING tictoc_print_(); +#endif return result; } /* ************************************************************************* */