gtsam  4.1.0
gtsam
ScenarioRunner.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
21 #include <gtsam/linear/Sampler.h>
22 
23 namespace gtsam {
24 
25 // Convert covariance to diagonal noise model, if possible, otherwise throw
26 static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
27  bool smart = true;
28  auto model = noiseModel::Gaussian::Covariance(covariance, smart);
29  auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
30  if (!diagonal)
31  throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
32  return diagonal;
33 }
34 
35 /*
36  * Simple class to test navigation scenarios.
37  * Takes a trajectory scenario as input, and can generate IMU measurements
38  */
39 class GTSAM_EXPORT ScenarioRunner {
40  public:
42  typedef boost::shared_ptr<PreintegrationParams> SharedParams;
43 
44  private:
45  const Scenario& scenario_;
46  const SharedParams p_;
47  const double imuSampleTime_, sqrt_dt_;
48  const Bias estimatedBias_;
49 
50  // Create two samplers for acceleration and omega noise
51  Sampler gyroSampler_, accSampler_;
52 
53  public:
54  ScenarioRunner(const Scenario& scenario, const SharedParams& p,
55  double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
56  : scenario_(scenario),
57  p_(p),
58  imuSampleTime_(imuSampleTime),
59  sqrt_dt_(std::sqrt(imuSampleTime)),
60  estimatedBias_(bias),
61  // NOTE(duy): random seeds that work well:
62  gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
63  accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
64 
65  // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
66  // also, uses g=10 for easy debugging
67  const Vector3& gravity_n() const { return p_->n_gravity; }
68 
69  // A gyro simply measures angular velocity in body frame
70  Vector3 actualAngularVelocity(double t) const {
71  return scenario_.omega_b(t);
72  }
73 
74  // An accelerometer measures acceleration in body, but not gravity
75  Vector3 actualSpecificForce(double t) const {
76  Rot3 bRn(scenario_.rotation(t).transpose());
77  return scenario_.acceleration_b(t) - bRn * gravity_n();
78  }
79 
80  // versions corrupted by bias and noise
81  Vector3 measuredAngularVelocity(double t) const {
82  return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
83  gyroSampler_.sample() / sqrt_dt_;
84  }
85  Vector3 measuredSpecificForce(double t) const {
86  return actualSpecificForce(t) + estimatedBias_.accelerometer() +
87  accSampler_.sample() / sqrt_dt_;
88  }
89 
90  const double& imuSampleTime() const { return imuSampleTime_; }
91 
93  PreintegratedImuMeasurements integrate(double T,
94  const Bias& estimatedBias = Bias(),
95  bool corrupted = false) const;
96 
98  NavState predict(const PreintegratedImuMeasurements& pim,
99  const Bias& estimatedBias = Bias()) const;
100 
102  Matrix9 estimateCovariance(double T, size_t N = 1000,
103  const Bias& estimatedBias = Bias()) const;
104 
106  Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
107 };
108 
109 } // namespace gtsam
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
Sampler.h
sampling from a NoiseModel
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::Scenario
Simple trajectory simulator.
Definition: Scenario.h:25
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:71
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition: ImuBias.h:59
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition: Sampler.h:31
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:116
gtsam::Rot3
Definition: Rot3.h:59
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition: ImuBias.h:64
ImuFactor.h
gtsam::ScenarioRunner
Definition: ScenarioRunner.h:39
Scenario.h
Simple class to test navigation scenarios.
gtsam::Rot3::transpose
Matrix3 transpose() const
Return 3*3 transpose (inverse) rotation matrix.
Definition: Rot3M.cpp:144
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition: Sampler.cpp:51