CombinedScenarioRunner

gtsam/navigation/ScenarioRunner.cpp

@@ -15,8 +15,8 @@
  * @author  Frank Dellaert
  */
 
-#include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/base/timing.h>
+#include <gtsam/navigation/ScenarioRunner.h>
 
 #include <boost/assign.hpp>
 #include <cmath>
@@ -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<double, 15, 15> 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<double, 15, 15> 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

gtsam/navigation/ScenarioRunner.h

@@ -16,9 +16,10 @@
  */
 
 #pragma once
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
-#include <gtsam/linear/Sampler.h>
 
 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 {
+  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 {
@@ -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<PreintegrationCombinedParams> 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<ScenarioRunner::SharedParams>(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<double, 15, 15> estimateCovariance(
+      double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
+};
+
 }  // namespace gtsam