diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 3b0a763d8..643d44f3d 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -27,11 +27,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( double T, const imuBias::ConstantBias& estimatedBias, bool corrupted) const { - const bool use2ndOrderIntegration = true; - - ImuFactor::PreintegratedMeasurements pim( - estimatedBias, accCovariance(), gyroCovariance(), - kIntegrationErrorCovariance, use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias); const double dt = imuSampleTime(); const size_t nrSteps = T / dt; @@ -46,27 +42,23 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( return pim; } -PoseVelocityBias ScenarioRunner::predict( +NavState ScenarioRunner::predict( const ImuFactor::PreintegratedMeasurements& pim, const imuBias::ConstantBias& estimatedBias) const { - // TODO(frank): allow non-zero omegaCoriolis - const Vector3 omegaCoriolis = Vector3::Zero(); - const bool use2ndOrderCoriolis = true; - return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), - estimatedBias, gravity_n(), omegaCoriolis, - use2ndOrderCoriolis); + const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); + return pim.predict(state_i, estimatedBias); } Matrix6 ScenarioRunner::estimatePoseCovariance( double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { // Get predict prediction from ground truth measurements - Pose3 prediction = predict(integrate(T)).pose; + Pose3 prediction = predict(integrate(T)).pose(); // Sample ! Matrix samples(9, N); Vector6 sum = Vector6::Zero(); for (size_t i = 0; i < N; i++) { - Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose; + Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose(); Vector6 xi = sampled.localCoordinates(prediction); samples.col(i) = xi; sum += xi; diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index ae0c61797..33a456e87 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -28,25 +28,22 @@ namespace gtsam { */ class ScenarioRunner { public: - ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, - double gyroSigma = 0.17, double accSigma = 0.01, - const imuBias::ConstantBias& bias = imuBias::ConstantBias(), - const Vector3& gravity_n = Vector3(0, 0, -10)) + typedef boost::shared_ptr + SharedParams; + ScenarioRunner(const Scenario* scenario, const SharedParams& p, + double imuSampleTime = 1.0 / 100.0, + const imuBias::ConstantBias& bias = imuBias::ConstantBias()) : scenario_(scenario), + p_(p), imuSampleTime_(imuSampleTime), - gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), - accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)), bias_(bias), - gravity_n_(gravity_n), // NOTE(duy): random seeds that work well: - gyroSampler_(gyroNoiseModel_, 10), - accSampler_(accNoiseModel_, 29284) - - {} + gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), + accSampler_(Diagonal(p->accelerometerCovariance), 29284) {} // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) // also, uses g=10 for easy debugging - const Vector3& gravity_n() const { return gravity_n_; } + const Vector3& gravity_n() const { return p_->n_gravity; } // A gyro simply measures angular velocity in body frame Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); } @@ -69,17 +66,6 @@ class ScenarioRunner { const double& imuSampleTime() const { return imuSampleTime_; } - const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { - return gyroNoiseModel_; - } - - const noiseModel::Diagonal::shared_ptr& accNoiseModel() const { - return accNoiseModel_; - } - - Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } - Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - /// Integrate measurements for T seconds into a PIM ImuFactor::PreintegratedMeasurements integrate( double T, @@ -87,9 +73,9 @@ class ScenarioRunner { bool corrupted = false) const; /// Predict predict given a PIM - PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim, - const imuBias::ConstantBias& estimatedBias = - imuBias::ConstantBias()) const; + NavState predict(const ImuFactor::PreintegratedMeasurements& pim, + const imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately Matrix6 poseCovariance( @@ -107,12 +93,20 @@ class ScenarioRunner { imuBias::ConstantBias()) const; private: + // Convert covariance to diagonal noise model, if possible, otherwise throw + static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { + bool smart = true; + auto model = noiseModel::Gaussian::Covariance(covariance, smart); + auto diagonal = boost::dynamic_pointer_cast(model); + if (!diagonal) + throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); + return diagonal; + } + const Scenario* scenario_; + const SharedParams p_; const double imuSampleTime_; - // TODO(frank): unify with Params, use actual noisemodels there... - const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; const imuBias::ConstantBias bias_; - const Vector3 gravity_n_; // Create two samplers for acceleration and omega noise mutable Sampler gyroSampler_, accSampler_; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 6c07a32b0..b882f2597 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -30,6 +30,15 @@ static const double kAccelSigma = 0.1; static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr defaultParams() { + auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0000001 * I_3x3; + return p; +} + /* ************************************************************************* */ namespace forward { const double v = 2; // m/s @@ -38,11 +47,11 @@ ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { using namespace forward; - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); @@ -51,7 +60,7 @@ TEST(ScenarioRunner, Forward) { /* ************************************************************************* */ TEST(ScenarioRunner, ForwardWithBias) { using namespace forward; - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); @@ -65,11 +74,11 @@ TEST(ScenarioRunner, Circle) { const double v = 2, w = 6 * kDegree; ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); @@ -82,11 +91,11 @@ TEST(ScenarioRunner, Loop) { const double v = 2, w = 6 * kDegree; ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); @@ -114,10 +123,10 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { using namespace accelerating; - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); + ScenarioRunner runner(&scenario, defaultParams(), T / 10); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); @@ -145,10 +154,10 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 3; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); + ScenarioRunner runner(&scenario, defaultParams(), T / 10); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));