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:
48  const double imuSampleTime_, sqrt_dt_;
50 
51  // Create two samplers for acceleration and omega noise
52  Sampler gyroSampler_, accSampler_;
53 
54  public:
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:
122 
123  public:
125  double imuSampleTime = 1.0 / 100.0,
126  const Bias& bias = Bias(),
127  const Eigen::Matrix<double, 15, 15>& preintMeasCov =
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
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
Definition: ImuBias.h:71
gtsam::ScenarioRunner::measuredSpecificForce
Vector3 measuredSpecificForce(double t) const
Definition: ScenarioRunner.h:86
Scenario.h
Simple class to test navigation scenarios.
gtsam::CombinedScenarioRunner::preintMeasCov_
const Eigen::Matrix< double, 15, 15 > preintMeasCov_
Definition: ScenarioRunner.h:121
gtsam::ScenarioRunner::scenario
const Scenario & scenario() const
Definition: ScenarioRunner.h:70
gtsam::ScenarioRunner::gravity_n
const Vector3 & gravity_n() const
Definition: ScenarioRunner.h:68
gtsam::ScenarioRunner
Definition: ScenarioRunner.h:40
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
gtsam::ScenarioRunner::SharedParams
std::shared_ptr< PreintegrationParams > SharedParams
Definition: ScenarioRunner.h:43
gtsam::NavState
Definition: NavState.h:34
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::ScenarioRunner::gyroSampler_
Sampler gyroSampler_
Definition: ScenarioRunner.h:52
gtsam::Scenario::rotation
Rot3 rotation(double t) const
Definition: Scenario.h:39
diagonal
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
gtsam::ScenarioRunner::actualAngularVelocity
Vector3 actualAngularVelocity(double t) const
Definition: ScenarioRunner.h:73
gtsam::CombinedScenarioRunner::SharedParams
std::shared_ptr< PreintegrationCombinedParams > SharedParams
Definition: ScenarioRunner.h:116
gtsam::CombinedScenarioRunner
Definition: ScenarioRunner.h:114
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
Definition: ImuBias.h:66
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:72
gtsam::CombinedScenarioRunner::CombinedScenarioRunner
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())
Definition: ScenarioRunner.h:124
gtsam::ScenarioRunner::p_
const SharedParams p_
Definition: ScenarioRunner.h:47
forward::scenario
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
gtsam::ScenarioRunner::sqrt_dt_
const double sqrt_dt_
Definition: ScenarioRunner.h:48
Eigen::Triplet
A small structure to hold a non zero as a triplet (i,j,value).
Definition: SparseUtil.h:162
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::ScenarioRunner::estimatedBias_
const Bias estimatedBias_
Definition: ScenarioRunner.h:49
gtsam::ScenarioRunner::ScenarioRunner
ScenarioRunner(const Scenario &scenario, const SharedParams &p, double imuSampleTime=1.0/100.0, const Bias &bias=Bias())
Definition: ScenarioRunner.h:55
gtsam::ScenarioRunner::imuSampleTime
const double & imuSampleTime() const
Definition: ScenarioRunner.h:91
gtsam::CombinedScenarioRunner::estimatedBias_
const Bias estimatedBias_
Definition: ScenarioRunner.h:120
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition: Sampler.cpp:59
gtsam
traits
Definition: chartTesting.h:28
Bias
imuBias::ConstantBias Bias
Definition: testImuBias.cpp:27
std
Definition: BFloat16.h:88
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::ScenarioRunner::actualSpecificForce
Vector3 actualSpecificForce(double t) const
Definition: ScenarioRunner.h:76
gtsam::Scenario::acceleration_b
Vector3 acceleration_b(double t) const
Definition: Scenario.h:47
Eigen::Matrix< double, 9, 9 >
CombinedImuFactor.h
N
#define N
Definition: igam.h:9
gtsam::ScenarioRunner::Bias
imuBias::ConstantBias Bias
Definition: ScenarioRunner.h:42
Sampler.h
sampling from a NoiseModel
gtsam::CombinedScenarioRunner::p_
const SharedParams p_
Definition: ScenarioRunner.h:119
gtsam::PreintegratedCombinedMeasurements
Definition: CombinedImuFactor.h:66
align_3::t
Point2 t(10, 10)
gtsam::ScenarioRunner::scenario_
const Scenario & scenario_
Definition: ScenarioRunner.h:46
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::ScenarioRunner::measuredAngularVelocity
Vector3 measuredAngularVelocity(double t) const
Definition: ScenarioRunner.h:82
gtsam::Sampler
Definition: Sampler.h:31
gtsam::Scenario
Simple trajectory simulator.
Definition: Scenario.h:25
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:114
ImuFactor.h


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:05:01