Merge remote-tracking branch 'bitbucket/RSS_ImuFactor' into feature/scenarios

gtsam/navigation/ScenarioRunner.cpp

@@ -16,7 +16,6 @@
  */
 
 #include <gtsam/navigation/ScenarioRunner.h>
-#include <gtsam/linear/Sampler.h>
 
 #include <cmath>
 
@@ -26,24 +25,21 @@ static double intNoiseVar = 0.0000001;
 static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
 
 ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
-    double T, Sampler* gyroSampler, Sampler* accSampler) const {
+    double T, const imuBias::ConstantBias& estimatedBias,
-  // TODO(frank): allow non-zero
+    bool corrupted) const {
-  const imuBias::ConstantBias zeroBias;
   const bool use2ndOrderIntegration = true;
 
   ImuFactor::PreintegratedMeasurements pim(
-      zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance,
+      estimatedBias, accCovariance(), gyroCovariance(),
-      use2ndOrderIntegration);
+      kIntegrationErrorCovariance, use2ndOrderIntegration);
 
   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) {
-    Vector3 measuredOmega = measured_angular_velocity(t);
+    Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t);
-    if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
+    Vector3 measuredAcc =
-    Vector3 measuredAcc = measured_acceleration(t);
+        corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t);
-    if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
     pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
   }
 
@@ -51,28 +47,26 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
 }
 
 PoseVelocityBias ScenarioRunner::predict(
-    const ImuFactor::PreintegratedMeasurements& pim) const {
+    const ImuFactor::PreintegratedMeasurements& pim,
-  // TODO(frank): allow non-zero bias, omegaCoriolis
+    const imuBias::ConstantBias& estimatedBias) const {
-  const imuBias::ConstantBias zeroBias;
+  // 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), zeroBias,
+  return pim.predict(scenario_->pose(0), scenario_->velocity_n(0),
-                     gravity_n(), omegaCoriolis, use2ndOrderCoriolis);
+                     estimatedBias, gravity_n(), omegaCoriolis,
+                     use2ndOrderCoriolis);
 }
 
-Matrix6 ScenarioRunner::estimatePoseCovariance(double T, size_t N) const {
+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;
 
-  // Create two samplers for acceleration and omega noise
-  Sampler gyroSampler(gyroNoiseModel(), 10);
-  Sampler accSampler(accNoiseModel(), 29284);
-
   // Sample !
   Matrix samples(9, N);
   Vector6 sum = Vector6::Zero();
   for (size_t i = 0; i < N; i++) {
-    Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose;
+    Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose;
     Vector6 xi = sampled.localCoordinates(prediction);
     samples.col(i) = xi;
     sum += xi;

gtsam/navigation/ScenarioRunner.h

@@ -18,11 +18,10 @@
 #pragma once
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
+#include <gtsam/linear/Sampler.h>
 
 namespace gtsam {
 
-class Sampler;
-
 /*
  *  Simple class to test navigation scenarios.
  *  Takes a trajectory scenario as input, and can generate IMU measurements
@@ -30,27 +29,42 @@ class Sampler;
 class ScenarioRunner {
  public:
   ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
-                 double gyroSigma = 0.17, double accSigma = 0.01)
+                 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)) {}
+        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 simly measures angular velocity in body frame
+  // A gyro simply measures angular velocity in body frame
-  Vector3 measured_angular_velocity(double t) const {
+  Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
-    return scenario_->omega_b(t);
-  }
 
   // An accelerometer measures acceleration in body, but not gravity
-  Vector3 measured_acceleration(double t) const {
+  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 {
@@ -65,13 +79,15 @@ class ScenarioRunner {
   Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
 
   /// Integrate measurements for T seconds into a PIM
-  ImuFactor::PreintegratedMeasurements integrate(double T,
+  ImuFactor::PreintegratedMeasurements integrate(
-                                                 Sampler* gyroSampler = 0,
+      double T,
-                                                 Sampler* accSampler = 0) const;
+      const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
+      bool corrupted = false) const;
 
   /// Predict predict given a PIM
-  PoseVelocityBias predict(
+  PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim,
-      const ImuFactor::PreintegratedMeasurements& pim) const;
+                           const imuBias::ConstantBias& estimatedBias =
+                               imuBias::ConstantBias()) const;
 
   /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
   Matrix6 poseCovariance(
@@ -84,12 +100,18 @@ class ScenarioRunner {
   }
 
   /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
-  Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const;
+  Matrix6 estimatePoseCovariance(double T, size_t N = 1000,
+                                 const imuBias::ConstantBias& estimatedBias =
+                                     imuBias::ConstantBias()) const;
 
  private:
   const Scenario* scenario_;
-  double imuSampleTime_;
+  const double imuSampleTime_;
-  noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
+  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/testScenarioRunner.cpp

@@ -25,14 +25,20 @@ 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 kAccelerometerSigma = 0.1;
+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) {
-  const double v = 2;  // m/s
+  using namespace forward;
-  ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
-
-  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@@ -42,13 +48,24 @@ TEST(ScenarioRunner, Forward) {
   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, kAccelerometerSigma);
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@@ -65,7 +82,7 @@ TEST(ScenarioRunner, Loop) {
   const double v = 2, w = 6 * kDegree;
   ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
 
-  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
+  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@@ -85,22 +102,36 @@ const Vector3 V0(50, 0, 0);
 }
 
 /* ************************************************************************* */
-TEST(ScenarioRunner, Accelerating) {
+namespace accelerating {
-  using namespace initial;
+using namespace initial;
-  const double a = 0.2;  // m/s^2
+const double a = 0.2;  // m/s^2
-  const Vector3 A(0, a, 0);
+const Vector3 A(0, a, 0);
-  const AcceleratingScenario scenario(nRb, P0, V0, A);
+const AcceleratingScenario scenario(nRb, P0, V0, A);
 
-  const double T = 3;  // seconds
+const double T = 3;  // seconds
-  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
+}
+
+/* ************************************************************************* */
+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,10000);
+  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));
-  cout << estimatedCov << endl << endl;
-  cout << runner.poseCovariance(pim) << endl;
 }
 
 /* ************************************************************************* */
@@ -111,12 +142,12 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
   const AcceleratingScenario scenario(nRb, P0, V0, A, W);
 
   const double T = 3;  // seconds
-  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
+  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,10000);
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
 }