ScenarioRunner.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#pragma once
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <gtsam/linear/Sampler.h>

namespace gtsam {

// 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;
}

/*
 *  Simple class to test navigation scenarios.
 *  Takes a trajectory scenario as input, and can generate IMU measurements
 */
class GTSAM_EXPORT ScenarioRunner {
 public:
  typedef imuBias::ConstantBias Bias;
  typedef boost::shared_ptr<PreintegrationParams> SharedParams;

 private:
  const Scenario& scenario_;
  const SharedParams p_;
  const double imuSampleTime_, sqrt_dt_;
  const Bias estimatedBias_;

  // Create two samplers for acceleration and omega noise
  Sampler gyroSampler_, accSampler_;

 public:
  ScenarioRunner(const Scenario& scenario, const SharedParams& p,
                 double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
      : scenario_(scenario),
        p_(p),
        imuSampleTime_(imuSampleTime),
        sqrt_dt_(std::sqrt(imuSampleTime)),
        estimatedBias_(bias),
        // NOTE(duy): random seeds that work well:
        gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
        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 p_->n_gravity; }

  // A gyro simply measures angular velocity in body frame
  Vector3 actualAngularVelocity(double t) const {
    return scenario_.omega_b(t);
  }

  // An accelerometer measures acceleration in body, but not gravity
  Vector3 actualSpecificForce(double t) const {
    Rot3 bRn(scenario_.rotation(t).transpose());
    return scenario_.acceleration_b(t) - bRn * gravity_n();
  }

  // versions corrupted by bias and noise
  Vector3 measuredAngularVelocity(double t) const {
    return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
           gyroSampler_.sample() / sqrt_dt_;
  }
  Vector3 measuredSpecificForce(double t) const {
    return actualSpecificForce(t) + estimatedBias_.accelerometer() +
           accSampler_.sample() / sqrt_dt_;
  }

  const double& imuSampleTime() const { return imuSampleTime_; }

  PreintegratedImuMeasurements integrate(double T,
                                         const Bias& estimatedBias = Bias(),
                                         bool corrupted = false) const;

  NavState predict(const PreintegratedImuMeasurements& pim,
                   const Bias& estimatedBias = Bias()) const;

  Matrix9 estimateCovariance(double T, size_t N = 1000,
                             const Bias& estimatedBias = Bias()) const;

  Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
};

}  // namespace gtsam