From 84eb953ca33760491148fbceb337c5a20f1f4b90 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 17 May 2019 10:36:18 -0400 Subject: [PATCH] deprecated pointer constructor --- gtsam/navigation/ScenarioRunner.cpp | 2 +- gtsam/navigation/ScenarioRunner.h | 19 +++++++++---- gtsam/navigation/tests/testScenarios.cpp | 34 ++++++++++++------------ 3 files changed, 32 insertions(+), 23 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 79f3f42cc..fcba92f60 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -49,7 +49,7 @@ PreintegratedImuMeasurements ScenarioRunner::integrate( NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim, const Bias& estimatedBias) const { - const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); + const NavState state_i(scenario_.pose(0), scenario_.velocity_n(0)); return pim.predict(state_i, estimatedBias); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 537785e06..aa544542d 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -42,7 +42,7 @@ class ScenarioRunner { typedef boost::shared_ptr SharedParams; private: - const Scenario* scenario_; + const Scenario& scenario_; const SharedParams p_; const double imuSampleTime_, sqrt_dt_; const Bias estimatedBias_; @@ -51,7 +51,7 @@ class ScenarioRunner { mutable Sampler gyroSampler_, accSampler_; public: - ScenarioRunner(const Scenario* scenario, const SharedParams& p, + ScenarioRunner(const Scenario& scenario, const SharedParams& p, double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) : scenario_(scenario), p_(p), @@ -68,13 +68,13 @@ class ScenarioRunner { // A gyro simply measures angular velocity in body frame Vector3 actualAngularVelocity(double t) const { - return scenario_->omega_b(t); + return scenario_.omega_b(t); } // An accelerometer measures acceleration in body, but not gravity Vector3 actualSpecificForce(double t) const { - Rot3 bRn(scenario_->rotation(t).transpose()); - return scenario_->acceleration_b(t) - bRn * gravity_n(); + Rot3 bRn(scenario_.rotation(t).transpose()); + return scenario_.acceleration_b(t) - bRn * gravity_n(); } // versions corrupted by bias and noise @@ -104,6 +104,15 @@ class ScenarioRunner { /// Estimate covariance of sampled noise for sanity-check Matrix6 estimateNoiseCovariance(size_t N = 1000) const; + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + ScenarioRunner(const Scenario* scenario, const SharedParams& p, + double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) + : ScenarioRunner(*scenario, p, imuSampleTime, bias) {} + /// @} +#endif }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index 5097e9e66..e790c45ba 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -52,7 +52,7 @@ TEST(ScenarioRunner, Spin) { const ConstantTwistScenario scenario(W, V); auto p = defaultParams(); - ScenarioRunner runner(&scenario, p, kDt); + ScenarioRunner runner(scenario, p, kDt); const double T = 2 * kDt; // seconds auto pim = runner.integrate(T); @@ -80,7 +80,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0)); /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -94,7 +94,7 @@ TEST(ScenarioRunner, Forward) { /* ************************************************************************* */ TEST(ScenarioRunner, ForwardWithBias) { using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T, kNonZeroBias); @@ -108,7 +108,7 @@ TEST(ScenarioRunner, Circle) { const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -126,7 +126,7 @@ TEST(ScenarioRunner, Loop) { const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -157,7 +157,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -170,7 +170,7 @@ TEST(ScenarioRunner, Accelerating) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias) { using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -185,7 +185,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -216,7 +216,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating2) { using namespace accelerating2; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -229,7 +229,7 @@ TEST(ScenarioRunner, Accelerating2) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias2) { using namespace accelerating2; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -244,7 +244,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -276,7 +276,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating3) { using namespace accelerating3; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -289,7 +289,7 @@ TEST(ScenarioRunner, Accelerating3) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias3) { using namespace accelerating3; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -304,7 +304,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -337,7 +337,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating4) { using namespace accelerating4; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -350,7 +350,7 @@ TEST(ScenarioRunner, Accelerating4) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias4) { using namespace accelerating4; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -365,7 +365,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));