diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 8872d536d..ce5631e21 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -16,12 +16,13 @@ */ #pragma once +#include #include namespace gtsam { /** - * Simple class with constant twist 3D trajectory. + * Simple IMU simulator with constant twist 3D trajectory. * It is also assumed that gravity is magically counteracted and has no effect * on trajectory. Hence, a simulated IMU yields the actual body angular * velocity, and negative G acceleration plus the acceleration created by the @@ -31,8 +32,12 @@ class Scenario { public: /// Construct scenario with constant twist [w,v] Scenario(const Vector3& w, const Vector3& v, - double imuSampleTime = 1.0 / 100.0) - : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {} + double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, + double accSigma = 0.01) + : twist_((Vector6() << w, v).finished()), + imuSampleTime_(imuSampleTime), + gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} const double& imuSampleTime() const { return imuSampleTime_; } @@ -40,6 +45,17 @@ class Scenario { // also, uses g=10 for easy debugging Vector3 gravity() const { return Vector3(0, 0, -10.0); } + 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(); } + Vector3 angularVelocityInBody() const { return twist_.head<3>(); } Vector3 linearVelocityInBody() const { return twist_.tail<3>(); } @@ -76,6 +92,7 @@ class Scenario { private: Vector6 twist_; double imuSampleTime_; + noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 45ec74978..67b9d0b0c 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -16,59 +16,100 @@ */ #pragma once +#include #include #include -#include +#include namespace gtsam { -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +static double intNoiseVar = 0.0001; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /// Simple class to test navigation scenarios class ScenarioRunner { public: ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} - // Integrate measurements for T seconds - ImuFactor::PreintegratedMeasurements integrate(double T) { + /// Integrate measurements for T seconds into a PIM + ImuFactor::PreintegratedMeasurements integrate( + double T, boost::optional gyroSampler = boost::none, + boost::optional accSampler = boost::none) { // TODO(frank): allow non-zero const imuBias::ConstantBias zeroBias; const bool use2ndOrderCoriolis = true; - ImuFactor::PreintegratedMeasurements result( - zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, + ImuFactor::PreintegratedMeasurements pim( + zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), kIntegrationErrorCovariance, use2ndOrderCoriolis); - const Vector3 measuredOmega = scenario_.angularVelocityInBody(); - const double deltaT = scenario_.imuSampleTime(); - const size_t nrSteps = T / deltaT; + const double dt = scenario_.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 += deltaT) { - const Vector3 measuredAcc = scenario_.accelerationInBody(t); - result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (size_t k = 0; k < nrSteps; k++, t += dt) { + Vector3 measuredOmega = scenario_.angularVelocityInBody(); + if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; + Vector3 measuredAcc = scenario_.accelerationInBody(t); + if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } - return result; + return pim; } - // Predict mean - Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) { - // TODO(frank): allow non-standard + /// Predict predict given a PIM + PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim) { + // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); const Vector3 vel_i = scenario_.velocity(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - const PoseVelocityBias prediction = - integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), - omegaCoriolis, use2ndOrderCoriolis); - return prediction.pose; + return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + omegaCoriolis, use2ndOrderCoriolis); + } + + /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately + Matrix6 poseCovariance(const ImuFactor::PreintegratedMeasurements& pim) { + Matrix9 cov = pim.preintMeasCov(); // _ position rotation + Matrix6 poseCov; + poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // + cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3); + return poseCov; + } + + /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples + Matrix6 estimatePoseCovariance(double T, size_t N = 1000) { + // Get predict prediction from ground truth measurements + Pose3 prediction = predict(integrate(T)).pose; + + // Create two samplers for acceleration and omega noise + Sampler gyroSampler(scenario_.gyroNoiseModel(), 29285); + Sampler accSampler(scenario_.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; + Vector6 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + // Compute MC covariance + Vector6 sampleMean = sum / N; + Matrix6 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector6 xi = samples.col(i); + xi -= sampleMean; + Q += xi * (xi.transpose() / (N - 1)); + } + + return Q; } private: @@ -76,4 +117,3 @@ class ScenarioRunner { }; } // namespace gtsam - diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index fc92bc416..c38b9a20a 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -27,12 +27,15 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.1, 0.00001); ScenarioRunner runner(forward); const double T = 1; // seconds - ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9)); + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9)); } /* ************************************************************************* */ @@ -43,8 +46,9 @@ TEST(ScenarioRunner, Circle) { ScenarioRunner runner(circle); const double T = 15; // seconds - ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1)); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); } /* ************************************************************************* */ @@ -56,8 +60,8 @@ TEST(ScenarioRunner, Loop) { ScenarioRunner runner(loop); const double T = 30; // seconds - ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1)); + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); } /* ************************************************************************* */