gtsam 4.2.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
23
24namespace gtsam {
25
26// Convert covariance to diagonal noise model, if possible, otherwise throw
27static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
28 bool smart = true;
29 auto model = noiseModel::Gaussian::Covariance(covariance, smart);
30 auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
31 if (!diagonal)
32 throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
33 return diagonal;
34}
35
36/*
37 * Simple class to test navigation scenarios.
38 * Takes a trajectory scenario as input, and can generate IMU measurements
39 */
40class GTSAM_EXPORT ScenarioRunner {
41 public:
43 typedef boost::shared_ptr<PreintegrationParams> SharedParams;
44
45 private:
46 const Scenario& scenario_;
47 const SharedParams p_;
48 const double imuSampleTime_, sqrt_dt_;
49 const Bias estimatedBias_;
50
51 // Create two samplers for acceleration and omega noise
52 Sampler gyroSampler_, accSampler_;
53
54 public:
55 ScenarioRunner(const Scenario& scenario, const SharedParams& p,
56 double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
57 : scenario_(scenario),
58 p_(p),
59 imuSampleTime_(imuSampleTime),
60 sqrt_dt_(std::sqrt(imuSampleTime)),
61 estimatedBias_(bias),
62 // NOTE(duy): random seeds that work well:
63 gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
64 accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
65
66 // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
67 // also, uses g=10 for easy debugging
68 const Vector3& gravity_n() const { return p_->n_gravity; }
69
70 const Scenario& scenario() const { return scenario_; }
71
72 // A gyro simply measures angular velocity in body frame
73 Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
74
75 // An accelerometer measures acceleration in body, but not gravity
76 Vector3 actualSpecificForce(double t) const {
77 Rot3 bRn(scenario_.rotation(t).transpose());
78 return scenario_.acceleration_b(t) - bRn * gravity_n();
79 }
80
81 // versions corrupted by bias and noise
82 Vector3 measuredAngularVelocity(double t) const {
83 return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
84 gyroSampler_.sample() / sqrt_dt_;
85 }
86 Vector3 measuredSpecificForce(double t) const {
87 return actualSpecificForce(t) + estimatedBias_.accelerometer() +
88 accSampler_.sample() / sqrt_dt_;
89 }
90
91 const double& imuSampleTime() const { return imuSampleTime_; }
92
94 PreintegratedImuMeasurements integrate(double T,
95 const Bias& estimatedBias = Bias(),
96 bool corrupted = false) const;
97
99 NavState predict(const PreintegratedImuMeasurements& pim,
100 const Bias& estimatedBias = Bias()) const;
101
103 Matrix9 estimateCovariance(double T, size_t N = 1000,
104 const Bias& estimatedBias = Bias()) const;
105
107 Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
108};
109
110/*
111 * Simple class to test navigation scenarios with CombinedImuMeasurements.
112 * Takes a trajectory scenario as input, and can generate IMU measurements
113 */
114class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
115 public:
116 typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
117
118 private:
119 const SharedParams p_;
120 const Bias estimatedBias_;
121
122 public:
123 CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
124 double imuSampleTime = 1.0 / 100.0,
125 const Bias& bias = Bias())
126 : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
127 imuSampleTime, bias),
128 p_(p),
129 estimatedBias_(bias) {}
130
133 double T, const Bias& estimatedBias = Bias(),
134 bool corrupted = false) const;
135
138 const Bias& estimatedBias = Bias()) const;
139
141 Eigen::Matrix<double, 15, 15> estimateCovariance(
142 double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
143};
144
145} // namespace gtsam
sampling from a NoiseModel
Simple class to test navigation scenarios.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Matrix3 transpose() const
Return 3*3 transpose (inverse) rotation matrix.
Definition: Rot3M.cpp:144
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:117
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition: Sampler.h:31
Vector sample() const
sample from distribution
Definition: Sampler.cpp:59
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition: CombinedImuFactor.h:129
Definition: ImuBias.h:30
const Vector3 & gyroscope() const
get gyroscope bias
Definition: ImuBias.h:69
const Vector3 & accelerometer() const
get accelerometer bias
Definition: ImuBias.h:64
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition: ImuFactor.h:72
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
Simple trajectory simulator.
Definition: Scenario.h:25
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
Definition: ScenarioRunner.h:40
Definition: ScenarioRunner.h:114