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/linear/Sampler.h>
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.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 = std::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 std::shared_ptr<PreintegrationParams> SharedParams;
 
 protected:
  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; }
 
  const Scenario& scenario() const { return scenario_; }

  Vector3 actualAngularVelocity(double t) const;

  Vector3 actualSpecificForce(double t) const;
 
  // Angular velocity measured by gyroscope, corrupted by bias and noise
  Vector3 measuredAngularVelocity(double t) const;

  Vector3 measuredSpecificForce(double t) 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;
};
 
/*
 *  Simple class to test navigation scenarios with CombinedImuMeasurements.
 *  Takes a trajectory scenario as input, and can generate IMU measurements
 */
class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
 public:
  typedef std::shared_ptr<PreintegrationCombinedParams> SharedParams;
 
 private:
  const SharedParams p_;
  const Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
 public:
  CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
                         double imuSampleTime = 1.0 / 100.0,
                         const Bias& bias = Bias(),
                         const Eigen::Matrix<double, 15, 15>& preintMeasCov =
                             Eigen::Matrix<double, 15, 15>::Zero())
      : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
                       imuSampleTime, bias),
        p_(p),
        preintMeasCov_(preintMeasCov) {}

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

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

  Eigen::Matrix<double, 15, 15> estimateCovariance(
      double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
};
 
/*
 * Simple class to test navigation scenarios with PreintegratedAhrsMeasurements.
 * Takes a trajectory scenario as input, and can generate AHRS measurements.
 */
class GTSAM_EXPORT AhrsScenarioRunner : public ScenarioRunner {
 public:
  AhrsScenarioRunner(const Scenario& scenario, const SharedParams& p,
                     double imuSampleTime = 1.0 / 100.0,
                     const Bias& bias = Bias())
      : ScenarioRunner(scenario,
                       std::static_pointer_cast<PreintegrationParams>(p),
                       imuSampleTime, bias) {}

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

  Rot3 predict(const PreintegratedAhrsMeasurements& pim,
               const Bias& estimatedBias = Bias()) const;

  Matrix3 estimateCovariance(double T, size_t N = 1000,
                             const Bias& estimatedBias = Bias()) const;
};
 
}  // namespace gtsam