Moved all noise/sampling of IMU to ScenarioRunner

gtsam/navigation/Scenario.h

@@ -21,39 +21,9 @@
 
 namespace gtsam {
 
-/**
+/// Simple trajectory simulator.
- * Simple IMU simulator.
- * 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
- * rotating body frame.
- */
 class Scenario {
  public:
-  /// Construct scenario with constant twist [w,v]
-  Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
-           double accSigma = 0.01)
-      : imuSampleTime_(imuSampleTime),
-        gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
-        accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
-
-  const double& imuSampleTime() const { return imuSampleTime_; }
-
-  // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
-  // 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(); }
-
   // Quantities a Scenario needs to specify:
 
   virtual Pose3 pose(double t) const = 0;
@@ -74,24 +44,18 @@ class Scenario {
     const Rot3 nRb = rotation(t);
     return nRb.transpose() * acceleration_n(t);
   }
-
- private:
-  double imuSampleTime_;
-  noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
 };
 
 /// Scenario with constant twist 3D trajectory.
 class ExpmapScenario : public Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
-  ExpmapScenario(const Vector3& w, const Vector3& v,
+  ExpmapScenario(const Vector3& w, const Vector3& v)
-                 double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
+      : twist_((Vector6() << w, v).finished()),
-                 double accSigma = 0.01)
-      : Scenario(imuSampleTime, gyroSigma, accSigma),
-        twist_((Vector6() << w, v).finished()),
         a_b_(twist_.head<3>().cross(twist_.tail<3>())) {}
 
   Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
+  Rot3 rotation(double t) const { return Rot3::Expmap(twist_.head<3>() * t); }
   Vector3 omega_b(double t) const { return twist_.head<3>(); }
   Vector3 velocity_n(double t) const { return rotation(t) * twist_.tail<3>(); }
   Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
@@ -106,14 +70,8 @@ class AcceleratingScenario : public Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
   AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
-                       const Vector3& accelerationInBody,
+                       const Vector3& accelerationInBody)
-                       double imuSampleTime = 1.0 / 100.0,
+      : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {}
-                       double gyroSigma = 0.17, double accSigma = 0.01)
-      : Scenario(imuSampleTime, gyroSigma, accSigma),
-        nRb_(nRb),
-        p0_(p0.vector()),
-        v0_(v0),
-        a_n_(nRb_ * accelerationInBody) {}
 
   Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); }
   Vector3 omega_b(double t) const { return Vector3::Zero(); }

gtsam/navigation/ScenarioRunner.h

@@ -30,7 +30,29 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
 /// Simple class to test navigation scenarios
 class ScenarioRunner {
  public:
-  ScenarioRunner(const Scenario* scenario) : scenario_(scenario) {}
+  ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
+                 double gyroSigma = 0.17, double accSigma = 0.01)
+      : scenario_(scenario),
+        imuSampleTime_(imuSampleTime),
+        gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
+        accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
+
+  const double& imuSampleTime() const { return imuSampleTime_; }
+
+  // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
+  // also, uses g=10 for easy debugging
+  Vector3 gravity_n() 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(); }
 
   /// Integrate measurements for T seconds into a PIM
   ImuFactor::PreintegratedMeasurements integrate(
@@ -40,17 +62,18 @@ class ScenarioRunner {
     const bool use2ndOrderIntegration = true;
 
     ImuFactor::PreintegratedMeasurements pim(
-        zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(),
+        zeroBias, accCovariance(), gyroCovariance(),
         kIntegrationErrorCovariance, use2ndOrderIntegration);
 
-    const double dt = scenario_->imuSampleTime();
+    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) {
+      Rot3 bRn = scenario_->rotation(t).transpose();
       Vector3 measuredOmega = scenario_->omega_b(t);
       if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
-      Vector3 measuredAcc = scenario_->acceleration_b(t);
+      Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n();
       if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
       pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
     }
@@ -63,12 +86,10 @@ class ScenarioRunner {
       const ImuFactor::PreintegratedMeasurements& pim) const {
     // TODO(frank): allow non-zero bias, omegaCoriolis
     const imuBias::ConstantBias zeroBias;
-    const Pose3 pose_i = Pose3::identity();
-    const Vector3 vel_i = scenario_->velocity_n(0);
     const Vector3 omegaCoriolis = Vector3::Zero();
     const bool use2ndOrderCoriolis = true;
-    return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(),
+    return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias,
-                       omegaCoriolis, use2ndOrderCoriolis);
+                       gravity_n(), omegaCoriolis, use2ndOrderCoriolis);
   }
 
   /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
@@ -87,8 +108,8 @@ class ScenarioRunner {
     Pose3 prediction = predict(integrate(T)).pose;
 
     // Create two samplers for acceleration and omega noise
-    Sampler gyroSampler(scenario_->gyroNoiseModel(), 10);
+    Sampler gyroSampler(gyroNoiseModel(), 10);
-    Sampler accSampler(scenario_->accNoiseModel(), 29284);
+    Sampler accSampler(accNoiseModel(), 29284);
 
     // Sample !
     Matrix samples(9, N);
@@ -115,6 +136,8 @@ class ScenarioRunner {
 
  private:
   const Scenario* scenario_;
+  double imuSampleTime_;
+  noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
 };
 
 }  // namespace gtsam

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -30,10 +30,9 @@ static const double kAccelerometerSigma = 0.1;
 /* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
   const double v = 2;  // m/s
-  ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT,
+  ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
-                          kGyroSigma, kAccelerometerSigma);
 
-  ScenarioRunner runner(&scenario);
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@@ -47,10 +46,9 @@ TEST(ScenarioRunner, Forward) {
 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), kDeltaT,
+  ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
-                          kGyroSigma, kAccelerometerSigma);
 
-  ScenarioRunner runner(&scenario);
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@@ -65,10 +63,9 @@ TEST(ScenarioRunner, Loop) {
   // Forward velocity 2m/s
   // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
   const double v = 2, w = 6 * kDegree;
-  ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT,
+  ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
-                          kGyroSigma, kAccelerometerSigma);
 
-  ScenarioRunner runner(&scenario);
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@@ -81,19 +78,16 @@ TEST(ScenarioRunner, Loop) {
 /* ************************************************************************* */
 TEST(ScenarioRunner, Accelerating) {
   // Set up body pointing towards y axis, and start at 10,20,0 with velocity
-  // going in X
+  // going in X. The body itself has Z axis pointing down
-  // The body itself has Z axis pointing down
   const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
-  const Point3 initial_position(10, 20, 0);
+  const Point3 P0(10, 20, 0);
-  const Vector3 initial_velocity(50, 0, 0);
+  const Vector3 V0(50, 0, 0);
 
-  const double a = 0.2, dt = 3.0;
+  const double a_b = 0.2;  // m/s^2
-  AcceleratingScenario scenario(nRb, initial_velocity, initial_velocity,
+  const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
-                                Vector3(a, 0, 0), dt / 10, kGyroSigma,
-                                kAccelerometerSigma);
 
-  ScenarioRunner runner(&scenario);
   const double T = 3;  // seconds
+  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));