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
19 #include <gtsam/linear/Sampler.h>
23 
24 namespace gtsam {
25 
26 // Convert covariance to diagonal noise model, if possible, otherwise throw
28  bool smart = true;
29  auto model = noiseModel::Gaussian::Covariance(covariance, smart);
30  auto diagonal = std::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  */
40 class GTSAM_EXPORT ScenarioRunner {
41  public:
43  typedef std::shared_ptr<PreintegrationParams> SharedParams;
44 
45  private:
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
83  return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
84  gyroSampler_.sample() / sqrt_dt_;
85  }
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  */
114 class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
115  public:
116  typedef std::shared_ptr<PreintegrationCombinedParams> SharedParams;
117 
118  private:
119  const SharedParams p_;
120  const Bias estimatedBias_;
122 
123  public:
124  CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
125  double imuSampleTime = 1.0 / 100.0,
126  const Bias& bias = Bias(),
127  const Eigen::Matrix<double, 15, 15>& preintMeasCov =
129  : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
130  imuSampleTime, bias),
131  p_(p),
132  estimatedBias_(bias),
133  preintMeasCov_(preintMeasCov) {}
134 
137  double T, const Bias& estimatedBias = Bias(),
138  bool corrupted = false) const;
139 
141  NavState predict(const PreintegratedCombinedMeasurements& pim,
142  const Bias& estimatedBias = Bias()) const;
143 
145  Eigen::Matrix<double, 15, 15> estimateCovariance(
146  double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
147 };
148 
149 } // namespace gtsam
Simple class to test navigation scenarios.
Vector3 actualAngularVelocity(double t) const
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
Vector sample() const
sample from distribution
Definition: Sampler.cpp:59
imuBias::ConstantBias Bias
ScenarioRunner(const Scenario &scenario, const SharedParams &p, double imuSampleTime=1.0/100.0, const Bias &bias=Bias())
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:114
Eigen::Vector3d Vector3
Definition: Vector.h:43
const Eigen::Matrix< double, 15, 15 > preintMeasCov_
noiseModel::Diagonal::shared_ptr model
Vector3 actualSpecificForce(double t) const
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Definition: BFloat16.h:88
const double & imuSampleTime() const
#define N
Definition: gksort.c:12
Rot3 rotation(double t) const
Definition: Scenario.h:39
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Vector3 measuredAngularVelocity(double t) const
const Vector3 & accelerometer() const
Definition: ImuBias.h:66
const Vector3 & gravity_n() const
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())
const Scenario & scenario_
std::shared_ptr< PreintegrationCombinedParams > SharedParams
sampling from a NoiseModel
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
Simple trajectory simulator.
Definition: Scenario.h:25
traits
Definition: chartTesting.h:28
imuBias::ConstantBias Bias
Definition: testImuBias.cpp:27
const SharedParams p_
const Vector3 & gyroscope() const
Definition: ImuBias.h:71
float * p
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
const Scenario & scenario() const
Matrix3 transpose() const
Definition: Rot3M.cpp:143
Vector3 acceleration_b(double t) const
Definition: Scenario.h:47
std::shared_ptr< PreintegrationParams > SharedParams
Point2 t(10, 10)
Vector3 measuredSpecificForce(double t) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:36