diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 3938ce86c..3a447dcab 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include #include @@ -105,4 +105,61 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const { return Q / (N - 1); } +PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate( + double T, const Bias& estimatedBias, bool corrupted) const { + gttic_(integrate); + PreintegratedCombinedMeasurements pim(p_, estimatedBias); + + const double dt = imuSampleTime(); + const size_t nrSteps = T / dt; + double t = 0; + for (size_t k = 0; k < nrSteps; k++, t += dt) { + Vector3 measuredOmega = + corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t); + Vector3 measuredAcc = + corrupted ? measuredSpecificForce(t) : actualSpecificForce(t); + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + } + + return pim; +} + +NavState CombinedScenarioRunner::predict( + const PreintegratedCombinedMeasurements& pim, + const Bias& estimatedBias) const { + const NavState state_i(scenario().pose(0), scenario().velocity_n(0)); + return pim.predict(state_i, estimatedBias); +} + +Eigen::Matrix CombinedScenarioRunner::estimateCovariance( + double T, size_t N, const Bias& estimatedBias) const { + gttic_(estimateCovariance); + + // Get predict prediction from ground truth measurements + NavState prediction = predict(integrate(T)); + + // Sample ! + Matrix samples(15, N); + Vector15 sum = Vector15::Zero(); + for (size_t i = 0; i < N; i++) { + auto pim = integrate(T, estimatedBias, true); + NavState sampled = predict(pim); + Vector15 xi = Vector15::Zero(); + xi << sampled.localCoordinates(prediction), estimatedBias_.vector(); + samples.col(i) = xi; + sum += xi; + } + + // Compute MC covariance + Vector15 sampleMean = sum / N; + Eigen::Matrix Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector15 xi = samples.col(i) - sampleMean; + Q += xi * xi.transpose(); + } + + return Q / (N - 1); +} + } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 1577e36fe..cee5a54ab 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -16,9 +16,10 @@ */ #pragma once +#include +#include #include #include -#include namespace gtsam { @@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner { // also, uses g=10 for easy debugging const Vector3& gravity_n() const { return p_->n_gravity; } + const Scenario& scenario() const { return scenario_; } + // A gyro simply measures angular velocity in body frame - Vector3 actualAngularVelocity(double t) const { - return scenario_.omega_b(t); - } + Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); } // An accelerometer measures acceleration in body, but not gravity Vector3 actualSpecificForce(double t) const { @@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner { Matrix6 estimateNoiseCovariance(size_t N = 1000) const; }; +/* + * Simple class to test navigation scenarios with CombinedImuMeasurements. + * Takes a trajectory scenario as input, and can generate IMU measurements + */ +class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner { + public: + typedef boost::shared_ptr SharedParams; + + private: + const SharedParams p_; + const Bias estimatedBias_; + + public: + CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p, + double imuSampleTime = 1.0 / 100.0, + const Bias& bias = Bias()) + : ScenarioRunner(scenario, static_cast(p), + imuSampleTime, bias), + p_(p), + estimatedBias_(bias) {} + + /// Integrate measurements for T seconds into a PIM + PreintegratedCombinedMeasurements integrate( + double T, const Bias& estimatedBias = Bias(), + bool corrupted = false) const; + + /// Predict predict given a PIM + NavState predict(const PreintegratedCombinedMeasurements& pim, + const Bias& estimatedBias = Bias()) const; + + /// Compute a Monte Carlo estimate of the predict covariance using N samples + Eigen::Matrix estimateCovariance( + double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const; +}; + } // namespace gtsam