diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index d1cf71eb9..3b0a763d8 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -16,7 +16,6 @@ */ #include -#include #include @@ -26,24 +25,21 @@ static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( - double T, Sampler* gyroSampler, Sampler* accSampler) const { - // TODO(frank): allow non-zero - const imuBias::ConstantBias zeroBias; + double T, const imuBias::ConstantBias& estimatedBias, + bool corrupted) const { const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( - zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance, - use2ndOrderIntegration); + estimatedBias, accCovariance(), gyroCovariance(), + kIntegrationErrorCovariance, use2ndOrderIntegration); const double dt = imuSampleTime(); - const double sqrt_dt = std::sqrt(dt); const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = measured_angular_velocity(t); - if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = measured_acceleration(t); - if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; + Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t); + Vector3 measuredAcc = + corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t); pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -51,28 +47,26 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( } PoseVelocityBias ScenarioRunner::predict( - const ImuFactor::PreintegratedMeasurements& pim) const { - // TODO(frank): allow non-zero bias, omegaCoriolis - const imuBias::ConstantBias zeroBias; + 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), zeroBias, - gravity_n(), omegaCoriolis, use2ndOrderCoriolis); + return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), + estimatedBias, gravity_n(), omegaCoriolis, + use2ndOrderCoriolis); } -Matrix6 ScenarioRunner::estimatePoseCovariance(double T, size_t N) const { +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; - // Create two samplers for acceleration and omega noise - Sampler gyroSampler(gyroNoiseModel(), 10); - Sampler accSampler(accNoiseModel(), 29284); - // Sample ! Matrix samples(9, N); Vector6 sum = Vector6::Zero(); for (size_t i = 0; i < N; i++) { - Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).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 1cb717292..d32d7b836 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -18,11 +18,10 @@ #pragma once #include #include +#include namespace gtsam { -class Sampler; - /* * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements @@ -30,27 +29,42 @@ class Sampler; class ScenarioRunner { public: ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, - double gyroSigma = 0.17, double accSigma = 0.01) + double gyroSigma = 0.17, double accSigma = 0.01, + const imuBias::ConstantBias& bias = imuBias::ConstantBias()) : scenario_(scenario), imuSampleTime_(imuSampleTime), gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), - accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)), + bias_(bias), + // NOTE(duy): random seeds that work well: + gyroSampler_(gyroNoiseModel_, 10), + accSampler_(accNoiseModel_, 29284) + + {} // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) // also, uses g=10 for easy debugging static Vector3 gravity_n() { return Vector3(0, 0, -10.0); } - // A gyro simly measures angular velocity in body frame - Vector3 measured_angular_velocity(double t) const { - return scenario_->omega_b(t); - } + // A gyro simply measures angular velocity in body frame + Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); } // An accelerometer measures acceleration in body, but not gravity - Vector3 measured_acceleration(double t) const { + Vector3 actual_specific_force_b(double t) const { Rot3 bRn = scenario_->rotation(t).transpose(); return scenario_->acceleration_b(t) - bRn * gravity_n(); } + // versions corrupted by bias and noise + Vector3 measured_omega_b(double t) const { + return actual_omega_b(t) + bias_.gyroscope() + + gyroSampler_.sample() / std::sqrt(imuSampleTime_); + } + Vector3 measured_specific_force_b(double t) const { + return actual_specific_force_b(t) + bias_.accelerometer() + + accSampler_.sample() / std::sqrt(imuSampleTime_); + } + const double& imuSampleTime() const { return imuSampleTime_; } const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { @@ -65,13 +79,15 @@ class ScenarioRunner { Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate(double T, - Sampler* gyroSampler = 0, - Sampler* accSampler = 0) const; + ImuFactor::PreintegratedMeasurements integrate( + double T, + const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), + bool corrupted = false) const; /// Predict predict given a PIM - PoseVelocityBias predict( - const ImuFactor::PreintegratedMeasurements& pim) const; + PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim, + const imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately Matrix6 poseCovariance( @@ -84,12 +100,18 @@ class ScenarioRunner { } /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples - Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const; + Matrix6 estimatePoseCovariance(double T, size_t N = 1000, + const imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; private: const Scenario* scenario_; - double imuSampleTime_; - noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; + const double imuSampleTime_; + const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; + const imuBias::ConstantBias bias_; + + // Create two samplers for acceleration and omega noise + mutable Sampler gyroSampler_, accSampler_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index bf3e3836a..019d60f91 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -25,14 +25,20 @@ using namespace gtsam; static const double kDegree = M_PI / 180.0; static const double kDeltaT = 1e-2; static const double kGyroSigma = 0.02; -static const double kAccelerometerSigma = 0.1; +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); +/* ************************************************************************* */ +namespace forward { +const double v = 2; // m/s +ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +} /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { - const double v = 2; // m/s - ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); - - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); + using namespace forward; + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -42,13 +48,24 @@ TEST(ScenarioRunner, Forward) { EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } +/* ************************************************************************* */ +TEST(ScenarioRunner, ForwardWithBias) { + using namespace forward; + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + const double T = 0.1; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec const double v = 2, w = 6 * kDegree; ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -65,7 +82,7 @@ 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, kAccelerometerSigma); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -85,22 +102,36 @@ 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); +namespace 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); - const double T = 3; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace accelerating; + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias) { + using namespace accelerating; + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, + kNonZeroBias); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); - cout << estimatedCov << endl << endl; - cout << runner.poseCovariance(pim) << endl; } /* ************************************************************************* */ @@ -111,12 +142,12 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 3; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); }