AcceleratingScenario + some refactoring (v and a specified in nav frame)

gtsam/navigation/Scenario.h

@@ -54,25 +54,25 @@ class Scenario {
   Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
   Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
 
-  /// Pose of body in nav frame at time t
+  // Quantities a Scenario needs to specify:
-  virtual Pose3 pose(double t) const = 0;
+
+  virtual Pose3 pose(double t) const = 0;
+  virtual Vector3 omega_b(double t) const = 0;
+  virtual Vector3 velocity_n(double t) const = 0;
+  virtual Vector3 acceleration_n(double t) const = 0;
+
+  // Derived quantities:
 
-  /// Rotation of body in nav frame at time t
   virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
 
-  virtual Vector3 angularVelocityInBody(double t) const = 0;
+  Vector3 velocity_b(double t) const {
-  virtual Vector3 linearVelocityInBody(double t) const = 0;
+    const Rot3 nRb = rotation(t);
-  virtual Vector3 accelerationInBody(double t) const = 0;
+    return nRb.transpose() * velocity_n(t);
-
-  /// Velocity in nav frame at time t
-  Vector3 velocity(double t) const {
-    return rotation(t) * linearVelocityInBody(t);
   }
 
-  // acceleration in nav frame
+  Vector3 acceleration_b(double t) const {
-  // TODO(frank): correct for rotating frames?
+    const Rot3 nRb = rotation(t);
-  Vector3 acceleration(double t) const {
+    return nRb.transpose() * acceleration_n(t);
-    return rotation(t) * accelerationInBody(t);
   }
 
  private:
@@ -80,9 +80,7 @@ class Scenario {
   noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
 };
 
-/**
+/// Scenario with constant twist 3D trajectory.
- * Simple IMU simulator with constant twist 3D trajectory.
- */
 class ExpmapScenario : public Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
@@ -91,21 +89,40 @@ class ExpmapScenario : public Scenario {
                  double accSigma = 0.01)
       : Scenario(imuSampleTime, gyroSigma, accSigma),
         twist_((Vector6() << w, v).finished()),
-        centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {}
+        a_b_(twist_.head<3>().cross(twist_.tail<3>())) {}
 
   Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
-
+  Vector3 omega_b(double t) const { return twist_.head<3>(); }
-  Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); }
+  Vector3 velocity_n(double t) const { return rotation(t) * twist_.tail<3>(); }
-  Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); }
+  Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
-
-  Vector3 accelerationInBody(double t) const {
-    const Rot3 nRb = rotation(t);
-    return centripetalAcceleration_ - nRb.transpose() * gravity();
-  }
 
  private:
   const Vector6 twist_;
-  const Vector3 centripetalAcceleration_;
+  const Vector3 a_b_;  // constant centripetal acceleration in body = w_b * v_b
+};
+
+/// Accelerating from an arbitrary initial state
+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,
+                       double imuSampleTime = 1.0 / 100.0,
+                       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(); }
+  Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
+  Vector3 acceleration_n(double t) const { return a_n_; }
+
+ private:
+  const Rot3 nRb_;
+  const Vector3 p0_, v0_, a_n_;
 };
 
 }  // namespace gtsam

gtsam/navigation/ScenarioRunner.h

@@ -48,9 +48,9 @@ class ScenarioRunner {
     const size_t nrSteps = T / dt;
     double t = 0;
     for (size_t k = 0; k < nrSteps; k++, t += dt) {
-      Vector3 measuredOmega = scenario_->angularVelocityInBody(t);
+      Vector3 measuredOmega = scenario_->omega_b(t);
       if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
-      Vector3 measuredAcc = scenario_->accelerationInBody(t);
+      Vector3 measuredAcc = scenario_->acceleration_b(t);
       if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
       pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
     }
@@ -64,7 +64,7 @@ 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_n(0);
     const Vector3 omegaCoriolis = Vector3::Zero();
     const bool use2ndOrderCoriolis = true;
     return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(),

gtsam/navigation/tests/testScenario.cpp

@@ -27,9 +27,15 @@ static const double degree = M_PI / 180.0;
 /* ************************************************************************* */
 TEST(Scenario, Forward) {
   const double v = 2;  // m/s
-  ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0));
+  const Vector3 W(0, 0, 0), V(v, 0, 0);
+  const ExpmapScenario scenario(W, V);
 
-  const Pose3 T15 = forward.pose(15);
+  const double T = 15;
+  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
+  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
+  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
+
+  const Pose3 T15 = scenario.pose(T);
   EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
   EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
 }
@@ -38,11 +44,17 @@ TEST(Scenario, Forward) {
 TEST(Scenario, Circle) {
   // Forward velocity 2m/s, angular velocity 6 degree/sec around Z
   const double v = 2, w = 6 * degree;
-  ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0));
+  const Vector3 W(0, 0, w), V(v, 0, 0);
+  const ExpmapScenario scenario(W, V);
+
+  const double T = 15;
+  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
+  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
+  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
 
   // R = v/w, so test if circle is of right size
   const double R = v / w;
-  const Pose3 T15 = circle.pose(15);
+  const Pose3 T15 = scenario.pose(T);
   EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
   EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
 }
@@ -52,15 +64,44 @@ 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;
-  ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0));
+  const Vector3 W(0, -w, 0), V(v, 0, 0);
+  const ExpmapScenario scenario(W, V);
+
+  const double T = 30;
+  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
+  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
+  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
 
   // R = v/w, so test if loop crests at 2*R
   const double R = v / w;
-  const Pose3 T30 = loop.pose(30);
+  const Pose3 T30 = scenario.pose(30);
   EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9));
   EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
 }
 
+/* ************************************************************************* */
+TEST(Scenario, Accelerating) {
+  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
+  // going in X. 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 P0(10, 20, 0);
+  const Vector3 V0(50, 0, 0);
+
+  const double a_b = 0.2;  // m/s^2
+  const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0));
+
+  const double T = 3;
+  const Vector3 A = nRb * Vector3(a_b, 0, 0);
+  EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9));
+  EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
+  EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
+
+  const Pose3 T3 = scenario.pose(3);
+  EXPECT(assert_equal(nRb, T3.rotation(), 1e-9));
+  EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(),
+                      1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -78,6 +78,30 @@ TEST(ScenarioRunner, Loop) {
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(ScenarioRunner, Accelerating) {
+  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
+  // going in X
+  // 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 Vector3 initial_velocity(50, 0, 0);
+
+  const double a = 0.2, dt = 3.0;
+  AcceleratingScenario scenario(nRb, initial_velocity, initial_velocity,
+                                Vector3(a, 0, 0), dt / 10, kGyroSigma,
+                                kAccelerometerSigma);
+
+  ScenarioRunner runner(&scenario);
+  const double T = 3;  // seconds
+
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  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));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;