Now using Params

gtsam/navigation/ScenarioRunner.cpp

@@ -27,11 +27,7 @@ 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(p_, estimatedBias);
-
-  ImuFactor::PreintegratedMeasurements pim(
-      estimatedBias, accCovariance(), gyroCovariance(),
-      kIntegrationErrorCovariance, use2ndOrderIntegration);
 
   const double dt = imuSampleTime();
   const size_t nrSteps = T / dt;
@@ -46,27 +42,23 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
   return pim;
 }
 
-PoseVelocityBias ScenarioRunner::predict(
+NavState ScenarioRunner::predict(
     const ImuFactor::PreintegratedMeasurements& pim,
     const imuBias::ConstantBias& estimatedBias) const {
-  // TODO(frank): allow non-zero  omegaCoriolis
+  const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
-  const Vector3 omegaCoriolis = Vector3::Zero();
+  return pim.predict(state_i, estimatedBias);
-  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;
+  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;
+    Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose();
     Vector6 xi = sampled.localCoordinates(prediction);
     samples.col(i) = xi;
     sum += xi;

gtsam/navigation/ScenarioRunner.h

@@ -28,25 +28,22 @@ namespace gtsam {
  */
 class ScenarioRunner {
  public:
-  ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
+  typedef boost::shared_ptr<ImuFactor::PreintegratedMeasurements::Params>
-                 double gyroSigma = 0.17, double accSigma = 0.01,
+      SharedParams;
-                 const imuBias::ConstantBias& bias = imuBias::ConstantBias(),
+  ScenarioRunner(const Scenario* scenario, const SharedParams& p,
-                 const Vector3& gravity_n = Vector3(0, 0, -10))
+                 double imuSampleTime = 1.0 / 100.0,
+                 const imuBias::ConstantBias& bias = imuBias::ConstantBias())
       : scenario_(scenario),
+        p_(p),
         imuSampleTime_(imuSampleTime),
-        gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
-        accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)),
         bias_(bias),
-        gravity_n_(gravity_n),
         // NOTE(duy): random seeds that work well:
-        gyroSampler_(gyroNoiseModel_, 10),
+        gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
-        accSampler_(accNoiseModel_, 29284)
+        accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
-
-  {}
 
   // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
   // also, uses g=10 for easy debugging
-  const Vector3& gravity_n() const { return gravity_n_; }
+  const Vector3& gravity_n() const { return p_->n_gravity; }
 
   // A gyro simply measures angular velocity in body frame
   Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
@@ -69,17 +66,6 @@ class ScenarioRunner {
 
   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,
@@ -87,7 +73,7 @@ class ScenarioRunner {
       bool corrupted = false) const;
 
   /// Predict predict given a PIM
-  PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim,
+  NavState predict(const ImuFactor::PreintegratedMeasurements& pim,
                    const imuBias::ConstantBias& estimatedBias =
                        imuBias::ConstantBias()) const;
 
@@ -107,12 +93,20 @@ class ScenarioRunner {
                                      imuBias::ConstantBias()) const;
 
  private:
+  // Convert covariance to diagonal noise model, if possible, otherwise throw
+  static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
+    bool smart = true;
+    auto model = noiseModel::Gaussian::Covariance(covariance, smart);
+    auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+    if (!diagonal)
+      throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
+    return diagonal;
+  }
+
   const Scenario* scenario_;
+  const SharedParams p_;
   const double imuSampleTime_;
-  // TODO(frank): unify with Params, use actual noisemodels there...
-  const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
   const imuBias::ConstantBias bias_;
-  const Vector3 gravity_n_;
 
   // Create two samplers for acceleration and omega noise
   mutable Sampler gyroSampler_, accSampler_;

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -30,6 +30,15 @@ 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);
 
+// Create default parameters with Z-down and above noise parameters
+static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
+  auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
+  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
+  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
+  p->integrationCovariance = 0.0000001 * I_3x3;
+  return p;
+}
+
 /* ************************************************************************* */
 namespace forward {
 const double v = 2;  // m/s
@@ -38,11 +47,11 @@ ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
 /* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
   using namespace forward;
-  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(scenario.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));
@@ -51,7 +60,7 @@ TEST(ScenarioRunner, Forward) {
 /* ************************************************************************* */
 TEST(ScenarioRunner, ForwardWithBias) {
   using namespace forward;
-  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
@@ -65,11 +74,11 @@ TEST(ScenarioRunner, Circle) {
   const double v = 2, w = 6 * kDegree;
   ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
 
-  ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
+  ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(scenario.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));
@@ -82,11 +91,11 @@ 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, kAccelSigma);
+  ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
   const double T = 0.1;  // seconds
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(scenario.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));
@@ -114,10 +123,10 @@ const double T = 3;  // seconds
 /* ************************************************************************* */
 TEST(ScenarioRunner, Accelerating) {
   using namespace accelerating;
-  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
+  ScenarioRunner runner(&scenario, defaultParams(), T / 10);
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(scenario.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), 0.1));
@@ -145,10 +154,10 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
   const AcceleratingScenario scenario(nRb, P0, V0, A, W);
 
   const double T = 3;  // seconds
-  ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
+  ScenarioRunner runner(&scenario, defaultParams(), T / 10);
 
   ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(scenario.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), 0.1));