12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Allow different convention

release/4.3a0
Frank 2015-12-24 16:01:21 -08:00
parent f37fe2613b
commit b9281b9ea6
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/navigation/ScenarioRunner.h
View File

@ -30,12 +30,14 @@ class ScenarioRunner {
public: public:
ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
double gyroSigma = 0.17, double accSigma = 0.01, double gyroSigma = 0.17, double accSigma = 0.01,
const imuBias::ConstantBias& bias = imuBias::ConstantBias()) const imuBias::ConstantBias& bias = imuBias::ConstantBias(),
const Vector3& gravity_n = Vector3(0, 0, -10))
: scenario_(scenario), : scenario_(scenario),
imuSampleTime_(imuSampleTime), imuSampleTime_(imuSampleTime),
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)), accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)),
bias_(bias), bias_(bias),
gravity_n_(gravity_n),
// NOTE(duy): random seeds that work well: // NOTE(duy): random seeds that work well:
gyroSampler_(gyroNoiseModel_, 10), gyroSampler_(gyroNoiseModel_, 10),
accSampler_(accNoiseModel_, 29284) accSampler_(accNoiseModel_, 29284)
@ -44,7 +46,7 @@ class ScenarioRunner {
// NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
// also, uses g=10 for easy debugging // also, uses g=10 for easy debugging
static Vector3 gravity_n() { return Vector3(0, 0, -10.0); } const Vector3& gravity_n() const { return gravity_n_; }
// A gyro simply measures angular velocity in body frame // A gyro simply measures angular velocity in body frame
Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); } Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
@ -107,8 +109,10 @@ class ScenarioRunner {
private: private:
const Scenario* scenario_; const Scenario* scenario_;
const double imuSampleTime_; const double imuSampleTime_;
// TODO(frank): unify with Params, use actual noisemodels there...
const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
const imuBias::ConstantBias bias_; const imuBias::ConstantBias bias_;
const Vector3 gravity_n_;
// Create two samplers for acceleration and omega noise // Create two samplers for acceleration and omega noise
mutable Sampler gyroSampler_, accSampler_; mutable Sampler gyroSampler_, accSampler_;