Split off Scenario abstract base class

gtsam/navigation/Scenario.h

@@ -22,7 +22,7 @@
 namespace gtsam {
 
 /**
- * Simple IMU simulator with constant twist 3D trajectory.
+ * 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
@@ -31,11 +31,9 @@ namespace gtsam {
 class Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
-  Scenario(const Vector3& w, const Vector3& v,
+  Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
-           double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
            double accSigma = 0.01)
-      : twist_((Vector6() << w, v).finished()),
+      : imuSampleTime_(imuSampleTime),
-        imuSampleTime_(imuSampleTime),
         gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
         accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
 
@@ -56,43 +54,58 @@ class Scenario {
   Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
   Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
 
-  Vector3 angularVelocityInBody() const { return twist_.head<3>(); }
+  /// Pose of body in nav frame at time t
-  Vector3 linearVelocityInBody() const { return twist_.tail<3>(); }
+  virtual Pose3 pose(double t) const = 0;
 
   /// Rotation of body in nav frame at time t
-  Rot3 rotAtTime(double t) const {
+  virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
-    return Rot3::Expmap(angularVelocityInBody() * t);
-  }
 
-  /// Pose of body in nav frame at time t
+  virtual Vector3 angularVelocityInBody(double t) const = 0;
-  Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
+  virtual Vector3 linearVelocityInBody(double t) const = 0;
+  virtual Vector3 accelerationInBody(double t) const = 0;
 
   /// Velocity in nav frame at time t
   Vector3 velocity(double t) const {
-    const Rot3 nRb = rotAtTime(t);
+    return rotation(t) * linearVelocityInBody(t);
-    return nRb * linearVelocityInBody();
   }
 
   // acceleration in nav frame
+  // TODO(frank): correct for rotating frames?
   Vector3 acceleration(double t) const {
-    const Vector3 centripetalAcceleration =
+    return rotation(t) * accelerationInBody(t);
-        angularVelocityInBody().cross(linearVelocityInBody());
-    const Rot3 nRb = rotAtTime(t);
-    return nRb * centripetalAcceleration - gravity();
-  }
-
-  // acceleration in body frame frame
-  Vector3 accelerationInBody(double t) const {
-    const Vector3 centripetalAcceleration =
-        angularVelocityInBody().cross(linearVelocityInBody());
-    const Rot3 nRb = rotAtTime(t);
-    return centripetalAcceleration - nRb.transpose() * gravity();
   }
 
  private:
-  Vector6 twist_;
   double imuSampleTime_;
   noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
 };
 
+/**
+ * Simple IMU simulator with constant twist 3D trajectory.
+ */
+class ExpmapScenario : public Scenario {
+ public:
+  /// Construct scenario with constant twist [w,v]
+  ExpmapScenario(const Vector3& w, const Vector3& v,
+                 double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
+                 double accSigma = 0.01)
+      : Scenario(imuSampleTime, gyroSigma, accSigma),
+        twist_((Vector6() << w, v).finished()),
+        centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {}
+
+  Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
+
+  Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); }
+  Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); }
+
+  Vector3 accelerationInBody(double t) const {
+    const Rot3 nRb = rotation(t);
+    return centripetalAcceleration_ - nRb.transpose() * gravity();
+  }
+
+ private:
+  const Vector6 twist_;
+  const Vector3 centripetalAcceleration_;
+};
+
 }  // namespace gtsam

gtsam/navigation/ScenarioRunner.h

@@ -30,7 +30,7 @@ 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) : scenario_(scenario) {}
 
   /// Integrate measurements for T seconds into a PIM
   ImuFactor::PreintegratedMeasurements integrate(
@@ -40,17 +40,17 @@ class ScenarioRunner {
     const bool use2ndOrderIntegration = true;
 
     ImuFactor::PreintegratedMeasurements pim(
-        zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
+        zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(),
         kIntegrationErrorCovariance, use2ndOrderIntegration);
 
-    const double dt = scenario_.imuSampleTime();
+    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 += dt) {
-      Vector3 measuredOmega = scenario_.angularVelocityInBody();
+      Vector3 measuredOmega = scenario_->angularVelocityInBody(t);
       if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
-      Vector3 measuredAcc = scenario_.accelerationInBody(t);
+      Vector3 measuredAcc = scenario_->accelerationInBody(t);
       if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
       pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
     }
@@ -64,10 +64,10 @@ class ScenarioRunner {
     // 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 vel_i = scenario_->velocity(0);
     const Vector3 omegaCoriolis = Vector3::Zero();
     const bool use2ndOrderCoriolis = true;
-    return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
+    return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(),
                        omegaCoriolis, use2ndOrderCoriolis);
   }
 
@@ -87,8 +87,8 @@ class ScenarioRunner {
     Pose3 prediction = predict(integrate(T)).pose;
 
     // Create two samplers for acceleration and omega noise
-    Sampler gyroSampler(scenario_.gyroNoiseModel(), 10);
+    Sampler gyroSampler(scenario_->gyroNoiseModel(), 10);
-    Sampler accSampler(scenario_.accNoiseModel(), 29284);
+    Sampler accSampler(scenario_->accNoiseModel(), 29284);
 
     // Sample !
     Matrix samples(9, N);
@@ -114,7 +114,7 @@ class ScenarioRunner {
   }
 
  private:
-  Scenario scenario_;
+  const Scenario* scenario_;
 };
 
 }  // namespace gtsam

gtsam/navigation/tests/testScenario.cpp

@@ -27,7 +27,7 @@ static const double degree = M_PI / 180.0;
 /* ************************************************************************* */
 TEST(Scenario, Forward) {
   const double v = 2;  // m/s
-  Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
+  ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0));
 
   const Pose3 T15 = forward.pose(15);
   EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
@@ -38,7 +38,7 @@ TEST(Scenario, Forward) {
 TEST(Scenario, Circle) {
   // Forward velocity 2m/s, angular velocity 6 degree/sec around Z
   const double v = 2, w = 6 * degree;
-  Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
+  ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
 
   // R = v/w, so test if circle is of right size
   const double R = v / w;
@@ -52,7 +52,7 @@ TEST(Scenario, Loop) {
   // Forward velocity 2m/s
   // Pitch up with angular velocity 6 degree/sec (negative in FLU)
   const double v = 2, w = 6 * degree;
-  Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
+  ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
 
   // R = v/w, so test if loop crests at 2*R
   const double R = v / w;

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -30,14 +30,14 @@ static const double kAccelerometerSigma = 0.1;
 /* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
   const double v = 2;  // m/s
-  Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, kGyroSigma,
+  ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT,
-                   kAccelerometerSigma);
+                          kGyroSigma, kAccelerometerSigma);
 
-  ScenarioRunner runner(forward);
+  ScenarioRunner runner(&scenario);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9));
+  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
@@ -47,14 +47,14 @@ TEST(ScenarioRunner, Forward) {
 TEST(ScenarioRunner, Circle) {
   // Forward velocity 2m/s, angular velocity 6 kDegree/sec
   const double v = 2, w = 6 * kDegree;
-  Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, kGyroSigma,
+  ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT,
-                  kAccelerometerSigma);
+                          kGyroSigma, kAccelerometerSigma);
 
-  ScenarioRunner runner(circle);
+  ScenarioRunner runner(&scenario);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1));
+  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
@@ -65,14 +65,14 @@ 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;
-  Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, kGyroSigma,
+  ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT,
-                kAccelerometerSigma);
+                          kGyroSigma, kAccelerometerSigma);
 
-  ScenarioRunner runner(loop);
+  ScenarioRunner runner(&scenario);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
+  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1));
 
   Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));