Merge remote-tracking branch 'origin/feature/scenarios' into feature/ImuFactorPush2

Conflicts:
	gtsam/navigation/PreintegrationBase.h
	gtsam/navigation/tests/testImuFactor.cpp

gtsam/navigation/PreintegrationBase.cpp

@@ -298,7 +298,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
     const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
     const Vector3& n_gravity, const Vector3& omegaCoriolis,
-    const bool use2ndOrderCoriolis) {
+    const bool use2ndOrderCoriolis) const {
   // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
   boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
   q->n_gravity = n_gravity;

gtsam/navigation/PreintegrationBase.h

@@ -100,8 +100,8 @@ public:
 
 protected:
 
-  /// Parameters
+  /// Parameters. Declared mutable only for deprecated predict method.
-  boost::shared_ptr<Params> p_;
+  mutable boost::shared_ptr<Params> p_;
 
   /// Acceleration and gyro bias used for preintegration
   imuBias::ConstantBias biasHat_;
@@ -283,7 +283,7 @@ public:
   /// @deprecated predict
   PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
       const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const;
 
   /// @}
 

gtsam/navigation/Scenario.h

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    Scenario.h
+ * @brief   Simple class to test navigation scenarios
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/geometry/Pose3.h>
+
+namespace gtsam {
+
+/// Simple trajectory simulator.
+class Scenario {
+ public:
+  // Quantities a Scenario needs to specify:
+
+  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:
+
+  virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
+
+  Vector3 velocity_b(double t) const {
+    const Rot3 nRb = rotation(t);
+    return nRb.transpose() * velocity_n(t);
+  }
+
+  Vector3 acceleration_b(double t) const {
+    const Rot3 nRb = rotation(t);
+    return nRb.transpose() * acceleration_n(t);
+  }
+};
+
+/// 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)
+      : 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).matrix() * twist_.tail<3>();
+  }
+  Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
+
+ private:
+  const Vector6 twist_;
+  const Vector3 a_b_;  // constant centripetal acceleration in body = w_b * v_b
+};
+
+/// Accelerating from an arbitrary initial state, with optional rotation
+class AcceleratingScenario : public Scenario {
+ public:
+  /// Construct scenario with constant acceleration in navigation frame and
+  /// optional angular velocity in body: rotating while translating...
+  AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
+                       const Vector3& a_n,
+                       const Vector3& omega_b = Vector3::Zero())
+      : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
+
+  Pose3 pose(double t) const {
+    return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
+  }
+  Vector3 omega_b(double t) const { return omega_b_; }
+  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_, omega_b_;
+};
+
+}  // namespace gtsam

gtsam/navigation/ScenarioRunner.cpp

@@ -0,0 +1,88 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    ScenarioRunner.h
+ * @brief   Simple class to test navigation scenarios
+ * @author  Frank Dellaert
+ */
+
+#include <gtsam/navigation/ScenarioRunner.h>
+
+#include <cmath>
+
+namespace gtsam {
+
+static double intNoiseVar = 0.0000001;
+static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
+
+ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
+    double T, const imuBias::ConstantBias& estimatedBias,
+    bool corrupted) const {
+  const bool use2ndOrderIntegration = true;
+
+  ImuFactor::PreintegratedMeasurements pim(
+      estimatedBias, accCovariance(), gyroCovariance(),
+      kIntegrationErrorCovariance, use2ndOrderIntegration);
+
+  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 ? measured_omega_b(t) : actual_omega_b(t);
+    Vector3 measuredAcc =
+        corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t);
+    pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
+  }
+
+  return pim;
+}
+
+PoseVelocityBias ScenarioRunner::predict(
+    const ImuFactor::PreintegratedMeasurements& pim,
+    const imuBias::ConstantBias& estimatedBias) const {
+  // TODO(frank): allow non-zero  omegaCoriolis
+  const Vector3 omegaCoriolis = Vector3::Zero();
+  const bool use2ndOrderCoriolis = true;
+  return pim.predict(scenario_->pose(0), scenario_->velocity_n(0),
+                     estimatedBias, gravity_n(), omegaCoriolis,
+                     use2ndOrderCoriolis);
+}
+
+Matrix6 ScenarioRunner::estimatePoseCovariance(
+    double T, size_t N, const imuBias::ConstantBias& estimatedBias) const {
+  // Get predict prediction from ground truth measurements
+  Pose3 prediction = predict(integrate(T)).pose;
+
+  // Sample !
+  Matrix samples(9, N);
+  Vector6 sum = Vector6::Zero();
+  for (size_t i = 0; i < N; i++) {
+    Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose;
+    Vector6 xi = sampled.localCoordinates(prediction);
+    samples.col(i) = xi;
+    sum += xi;
+  }
+
+  // Compute MC covariance
+  Vector6 sampleMean = sum / N;
+  Matrix6 Q;
+  Q.setZero();
+  for (size_t i = 0; i < N; i++) {
+    Vector6 xi = samples.col(i);
+    xi -= sampleMean;
+    Q += xi * xi.transpose();
+  }
+
+  return Q / (N - 1);
+}
+
+}  // namespace gtsam

gtsam/navigation/ScenarioRunner.h

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    ScenarioRunner.h
+ * @brief   Simple class to test navigation scenarios
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/Scenario.h>
+#include <gtsam/linear/Sampler.h>
+
+namespace gtsam {
+
+/*
+ *  Simple class to test navigation scenarios.
+ *  Takes a trajectory scenario as input, and can generate IMU measurements
+ */
+class ScenarioRunner {
+ public:
+  ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
+                 double gyroSigma = 0.17, double accSigma = 0.01,
+                 const imuBias::ConstantBias& bias = imuBias::ConstantBias())
+      : scenario_(scenario),
+        imuSampleTime_(imuSampleTime),
+        gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
+        accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)),
+        bias_(bias),
+        // NOTE(duy): random seeds that work well:
+        gyroSampler_(gyroNoiseModel_, 10),
+        accSampler_(accNoiseModel_, 29284)
+
+  {}
+
+  // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
+  // also, uses g=10 for easy debugging
+  static Vector3 gravity_n() { return Vector3(0, 0, -10.0); }
+
+  // A gyro simply measures angular velocity in body frame
+  Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
+
+  // An accelerometer measures acceleration in body, but not gravity
+  Vector3 actual_specific_force_b(double t) const {
+    Rot3 bRn = scenario_->rotation(t).transpose();
+    return scenario_->acceleration_b(t) - bRn * gravity_n();
+  }
+
+  // versions corrupted by bias and noise
+  Vector3 measured_omega_b(double t) const {
+    return actual_omega_b(t) + bias_.gyroscope() +
+           gyroSampler_.sample() / std::sqrt(imuSampleTime_);
+  }
+  Vector3 measured_specific_force_b(double t) const {
+    return actual_specific_force_b(t) + bias_.accelerometer() +
+           accSampler_.sample() / std::sqrt(imuSampleTime_);
+  }
+
+  const double& imuSampleTime() const { return imuSampleTime_; }
+
+  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(
+      double T,
+      const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
+      bool corrupted = false) const;
+
+  /// Predict predict given a PIM
+  PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim,
+                           const imuBias::ConstantBias& estimatedBias =
+                               imuBias::ConstantBias()) const;
+
+  /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
+  Matrix6 poseCovariance(
+      const ImuFactor::PreintegratedMeasurements& pim) const {
+    Matrix9 cov = pim.preintMeasCov();
+    Matrix6 poseCov;
+    poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3),  //
+        cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3);
+    return poseCov;
+  }
+
+  /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
+  Matrix6 estimatePoseCovariance(double T, size_t N = 1000,
+                                 const imuBias::ConstantBias& estimatedBias =
+                                     imuBias::ConstantBias()) const;
+
+ private:
+  const Scenario* scenario_;
+  const double imuSampleTime_;
+  const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
+  const imuBias::ConstantBias bias_;
+
+  // Create two samplers for acceleration and omega noise
+  mutable Sampler gyroSampler_, accSampler_;
+};
+
+}  // namespace gtsam

gtsam/navigation/tests/testImuFactor.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/factorTesting.h>
@@ -56,15 +57,17 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
 
 // Define covariance matrices
 /* ************************************************************************* */
-double accNoiseVar = 0.01;
+static const double kGyroSigma = 0.02;
-double omegaNoiseVar = 0.03;
+static const double kAccelerometerSigma = 0.1;
-double intNoiseVar = 0.0001;
+static double omegaNoiseVar = kGyroSigma * kGyroSigma;
-const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
+static double accNoiseVar = kAccelerometerSigma * kAccelerometerSigma;
-const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
+static double intNoiseVar = 0.0001;
-const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
+static const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
-const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
+static const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
-const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
+static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
-int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10;
+static const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
+static const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
+static int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10;
 
 // Auxiliary functions to test preintegrated Jacobians
 // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
@@ -118,114 +121,39 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
 } // namespace
 
 /* ************************************************************************* */
-bool MonteCarlo(const PreintegratedImuMeasurements& pim,
+TEST(ImuFactor, Accelerating) {
-    const NavState& state, const imuBias::ConstantBias& bias, double dt,
+  const double a = 0.2, v=50;
-    boost::optional<Pose3> body_P_sensor, const Vector3& measuredAcc,
-    const Vector3& measuredOmega, const Vector3& accNoiseVar,
-    const Vector3& omegaNoiseVar, size_t N = 10000,
-    size_t M = 1) {
-  // Get mean prediction from "ground truth" measurements
-  PreintegratedImuMeasurements pim1 = pim;
-  for (size_t k = 0; k < M; k++)
-    pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
-  NavState prediction = pim1.predict(state, bias);
-  Matrix9 actual = pim1.preintMeasCov();
-
-  // Do a Monte Carlo analysis to determine empirical density on the predicted state
-  Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed);
-  Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed);
-  Matrix samples(9, N);
-  Vector9 sum = Vector9::Zero();
-  for (size_t i = 0; i < N; i++) {
-    PreintegratedImuMeasurements pim2 = pim;
-    for (size_t k = 0; k < M; k++) {
-      Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample();
-      Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample();
-      pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt,
-          body_P_sensor);
-    }
-    NavState sampled = pim2.predict(state, bias);
-    Vector9 xi = sampled.localCoordinates(prediction);
-    samples.col(i) = xi;
-    sum += xi;
-  }
-
-  Vector9 sampleMean = sum / N;
-  Matrix9 Q;
-  Q.setZero();
-  for (size_t i = 0; i < N; i++) {
-    Vector9 xi = samples.col(i);
-    xi -= sampleMean;
-    Q += xi * (xi.transpose() / (N - 1));
-  }
-
-  // Compare MonteCarlo value with actual (computed) value
-  return assert_equal(Matrix(Q), actual, 1e-4);
-}
-
-/* ************************************************************************* */
-TEST(ImuFactor, StraightLine) {
-  // Set up IMU measurements
-  static const double g = 10; // make gravity 10 to make this easy to check
-  static const double v = 50.0, a = 0.2, dt = 3.0;
-  const double dt22 = dt * dt / 2;
 
   // 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
-  static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
+  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
-  static const Point3 initial_position(10, 20, 0);
+  const Point3 initial_position(10, 20, 0);
-  static const Vector3 initial_velocity(v, 0, 0);
+  const Vector3 initial_velocity(v, 0, 0);
-  static const NavState state1(nRb, initial_position, initial_velocity);
 
-  // set up acceleration in X direction, no angular velocity.
+  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
-  // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z
+                                      Vector3(a, 0, 0));
-  Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
 
-  // Create parameters assuming nav frame has Z up
+  const double T = 3.0;  // seconds
-  boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
+  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
-      PreintegratedImuMeasurements::Params::MakeSharedU(g);
+
-  p->accelerometerCovariance = kMeasuredAccCovariance;
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  p->gyroscopeCovariance = kMeasuredOmegaCovariance;
+  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
+
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
+  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
 
   // Check G1 and G2 derivatives of pim.update
-
-  // Now, preintegrate for 3 seconds, in 10 steps
-  PreintegratedImuMeasurements pim(p, kZeroBiasHat);
-  for (size_t i = 0; i < 10; i++)
-    pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
-
   Matrix93 aG1,aG2;
   boost::function<NavState(const Vector3&, const Vector3&)> f =
-      boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10,
+      boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10,
           boost::none, boost::none, boost::none);
-  pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2);
+  const Vector3 measuredAcc = runner.measured_acceleration(T);
-  EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
+  const Vector3 measuredOmega = runner.measured_angular_velocity(T);
-  EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
+  pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2);
-
+  EXPECT(assert_equal(
-  // Do Monte-Carlo analysis
+      numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
-  PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance,
+  EXPECT(assert_equal(
-      p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise
+      numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
-  EXPECT(
-      MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc,
-          measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
-
-  // Check integrated quantities in body frame: gravity measured by IMU is integrated!
-  Vector3 b_deltaP(a * dt22, 0, -g * dt22);
-  Vector3 b_deltaV(a * dt, 0, -g * dt);
-  EXPECT(assert_equal(Rot3(), pim.deltaRij()));
-  EXPECT(assert_equal(b_deltaP, pim.deltaPij()));
-  EXPECT(assert_equal(b_deltaV, pim.deltaVij()));
-
-  // Check bias-corrected delta: also in body frame
-  Vector9 expectedBC;
-  expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV;
-  EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias)));
-
-  // Check prediction in nav, note we move along x in body, along y in nav
-  Point3 expected_position(10 + v * dt, 20 + a * dt22, 0);
-  Velocity3 expected_velocity(v, a * dt, 0);
-  NavState expected(nRb, expected_position, expected_velocity);
-  EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias)));
 }
 
 /* ************************************************************************* */
@@ -615,12 +543,25 @@ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& mea
 }
 
 TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
-  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
+  const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0));
-  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
+  const Point3 initial_position(5.0, 1.0, -50.0);
-  Vector3 v1(Vector3(0.5, 0.0, 0.0));
+  const Vector3 initial_velocity(0.5, 0.0, 0.0);
-  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
+
-      Point3(5.5, 1.0, -50.0));
+  const double a = 0.2;
-  Vector3 v2(Vector3(0.5, 0.0, 0.0));
+  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
+                                      Vector3(a, 0, 0));
+
+  const double T = 3.0;  // 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));
+
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
+  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
+
+  ///////////////////////////////////////////////////////////////////////////////////////////
+  Pose3 x1(nRb, initial_position);
 
   // Measurements
   Vector3 measuredOmega;
@@ -659,42 +600,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
       1e-6);
   EXPECT(assert_equal(expectedG2, G2, 1e-5));
 
-#if 0
+  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
-  /*
+  EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor,
-   * This code is to verify the quality of the generated samples
-   * by checking if the covariance of the generated noises matches
-   * with the input covariance, and visualizing the nonlinearity of
-   * the sample set using the following matlab script:
-   *
-      noises = dlmread('noises.txt');
-      cov(noises)
-
-      samples = dlmread('noises.txt');
-      figure(1);
-      for i=1:9
-          subplot(3,3,i)
-          hist(samples(:,i), 500)
-      end
-   */
-  size_t N = 100000;
-  Matrix samples(9,N);
-  Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284);
-  Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10);
-  ofstream samplesOut("preintSamples.txt");
-  ofstream noiseOut("noises.txt");
-  for (size_t i = 0; i<N; ++i) {
-    Vector3 accNoise = sampleAccelerationNoise.sample();
-    Vector3 omegaNoise = sampleOmegaNoise.sample();
-    NavState sample = pim.updatedDeltaXij(measuredAcc+accNoise, measuredOmega+omegaNoise, dt);
-    samples.col(i) = sample.localCoordinates(preint);
-    samplesOut << samples.col(i).transpose() << endl;
-    noiseOut << accNoise.transpose() << " " << omegaNoise.transpose() << endl;
-  }
-  samplesOut.close();
-  noiseOut.close();
-#endif
-
-  EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor,
       measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
 
 
@@ -710,6 +617,9 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   // Predict
   Pose3 actual_x2;
   Vector3 actual_v2;
+  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
+      Point3(5.5, 1.0, -50.0));
+  Vector3 v2(Vector3(0.5, 0.0, 0.0));
   ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim,
       kGravityAlongNavZDown, kZeroOmegaCoriolis);
 
@@ -800,6 +710,27 @@ TEST(ImuFactor, PredictRotation) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, PredictArbitrary) {
+  const double a = 0.2, v=50;
+
+  // 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(v, 0, 0);
+
+  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
+                                      Vector3(a, 0, 0));
+
+  const double T = 3.0;  // 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));
+
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
+  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
+  //////////////////////////////////////////////////////////////////////////////////
+
   imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
 
   // Measurements

gtsam/navigation/tests/testScenario.cpp

@@ -0,0 +1,120 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testScenario.cpp
+ * @brief   Unit test Scenario class
+ * @author  Frank Dellaert
+ */
+
+#include <gtsam/navigation/Scenario.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <boost/bind.hpp>
+#include <cmath>
+
+using namespace std;
+using namespace gtsam;
+
+static const double degree = M_PI / 180.0;
+
+/* ************************************************************************* */
+TEST(Scenario, Forward) {
+  const double v = 2;  // m/s
+  const Vector3 W(0, 0, 0), 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));
+
+  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));
+}
+
+/* ************************************************************************* */
+TEST(Scenario, Circle) {
+  // Forward velocity 2m/s, angular velocity 6 degree/sec around Z
+  const double v = 2, w = 6 * degree;
+  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 = 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));
+}
+
+/* ************************************************************************* */
+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;
+  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 = 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 = 0.2;  // m/s^2
+  const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
+  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
+
+  const double T = 3;
+  EXPECT(assert_equal(W, 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));
+
+  {
+  // Check acceleration in nav
+  Matrix expected = numericalDerivative11<Vector3, double>(
+      boost::bind(&Scenario::velocity_n, scenario, _1), T);
+  EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
+  }
+
+  const Pose3 T3 = scenario.pose(3);
+  EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
+  EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
+                      T3.translation(), 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -0,0 +1,159 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testScenarioRunner.cpp
+ * @brief   test ImuFacor with ScenarioRunner class
+ * @author  Frank Dellaert
+ */
+
+#include <gtsam/navigation/ScenarioRunner.h>
+#include <CppUnitLite/TestHarness.h>
+#include <cmath>
+
+using namespace std;
+using namespace gtsam;
+
+static const double kDegree = M_PI / 180.0;
+static const double kDeltaT = 1e-2;
+static const double kGyroSigma = 0.02;
+static const double kAccelSigma = 0.1;
+
+static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
+static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
+
+/* ************************************************************************* */
+namespace forward {
+const double v = 2;  // m/s
+ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
+}
+/* ************************************************************************* */
+TEST(ScenarioRunner, Forward) {
+  using namespace forward;
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  const double T = 0.1;  // 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));
+}
+
+/* ************************************************************************* */
+TEST(ScenarioRunner, ForwardWithBias) {
+  using namespace forward;
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  const double T = 0.1;  // seconds
+
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
+  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
+}
+
+/* ************************************************************************* */
+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));
+
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  const double T = 0.1;  // seconds
+
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  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));
+}
+
+/* ************************************************************************* */
+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));
+
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  const double T = 0.1;  // seconds
+
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  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));
+}
+
+/* ************************************************************************* */
+namespace initial {
+// 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);
+}
+
+/* ************************************************************************* */
+namespace accelerating {
+using namespace initial;
+const double a = 0.2;  // m/s^2
+const Vector3 A(0, a, 0);
+const AcceleratingScenario scenario(nRb, P0, V0, A);
+
+const double T = 3;  // seconds
+}
+
+/* ************************************************************************* */
+TEST(ScenarioRunner, Accelerating) {
+  using namespace accelerating;
+  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
+
+  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), 0.1));
+}
+
+/* ************************************************************************* */
+TEST(ScenarioRunner, AcceleratingWithBias) {
+  using namespace accelerating;
+  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
+                        kNonZeroBias);
+
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
+  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
+}
+
+/* ************************************************************************* */
+TEST(ScenarioRunner, AcceleratingAndRotating) {
+  using namespace initial;
+  const double a = 0.2;  // m/s^2
+  const Vector3 A(0, a, 0), W(0, 0.1, 0);
+  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
+
+  const double T = 3;  // seconds
+  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
+
+  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), 0.1));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */