Loading...
Searching...
No Matches
ScenarioRunner.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/Sampler.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
namespace gtsam {
// Convert covariance to diagonal noise model, if possible, otherwise throw
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
bool smart = true;
auto model = noiseModel::Gaussian::Covariance(covariance, smart);
auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonal)
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
return diagonal;
}
/*
* Simple class to test navigation scenarios.
* Takes a trajectory scenario as input, and can generate IMU measurements
*/
class GTSAM_EXPORT ScenarioRunner {
public:
typedef imuBias::ConstantBias Bias;
typedef boost::shared_ptr<PreintegrationParams> SharedParams;
private:
const Scenario& scenario_;
const SharedParams p_;
const double imuSampleTime_, sqrt_dt_;
const Bias estimatedBias_;
// Create two samplers for acceleration and omega noise
Sampler gyroSampler_, accSampler_;
public:
ScenarioRunner(const Scenario& scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
: scenario_(scenario),
p_(p),
imuSampleTime_(imuSampleTime),
sqrt_dt_(std::sqrt(imuSampleTime)),
estimatedBias_(bias),
// NOTE(duy): random seeds that work well:
gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
// NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
// also, uses g=10 for easy debugging
const Vector3& gravity_n() const { return p_->n_gravity; }
const Scenario& scenario() const { return scenario_; }
// A gyro simply measures angular velocity in body frame
Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
// An accelerometer measures acceleration in body, but not gravity
Vector3 actualSpecificForce(double t) const {
Rot3 bRn(scenario_.rotation(t).transpose());
return scenario_.acceleration_b(t) - bRn * gravity_n();
}
// versions corrupted by bias and noise
Vector3 measuredAngularVelocity(double t) const {
return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
gyroSampler_.sample() / sqrt_dt_;
}
Vector3 measuredSpecificForce(double t) const {
return actualSpecificForce(t) + estimatedBias_.accelerometer() +
accSampler_.sample() / sqrt_dt_;
}
const double& imuSampleTime() const { return imuSampleTime_; }
PreintegratedImuMeasurements integrate(double T,
const Bias& estimatedBias = Bias(),
bool corrupted = false) const;
NavState predict(const PreintegratedImuMeasurements& pim,
const Bias& estimatedBias = Bias()) const;
Matrix9 estimateCovariance(double T, size_t N = 1000,
const Bias& estimatedBias = Bias()) const;
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
};
/*
* Simple class to test navigation scenarios with CombinedImuMeasurements.
* Takes a trajectory scenario as input, and can generate IMU measurements
*/
class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
public:
typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
private:
const SharedParams p_;
const Bias estimatedBias_;
public:
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const Bias& bias = Bias())
: ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
imuSampleTime, bias),
p_(p),
estimatedBias_(bias) {}
PreintegratedCombinedMeasurements integrate(
double T, const Bias& estimatedBias = Bias(),
bool corrupted = false) const;
NavState predict(const PreintegratedCombinedMeasurements& pim,
const Bias& estimatedBias = Bias()) const;
Eigen::Matrix<double, 15, 15> estimateCovariance(
double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
};
} // namespace gtsam
Sampler.h
sampling from a NoiseModel
CombinedImuFactor.h
ImuFactor.h
Scenario.h
Simple class to test navigation scenarios.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
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:117
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
gtsam::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::PreintegratedImuMeasurements
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
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::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
gtsam::ScenarioRunner::estimateNoiseCovariance
Matrix6 estimateNoiseCovariance(size_t N=1000) const
Estimate covariance of sampled noise for sanity-check.
Definition ScenarioRunner.cpp:85
gtsam::ScenarioRunner::integrate
PreintegratedImuMeasurements integrate(double T, const Bias &estimatedBias=Bias(), bool corrupted=false) const
Integrate measurements for T seconds into a PIM.
Definition ScenarioRunner.cpp:30
gtsam::ScenarioRunner::estimateCovariance
Matrix9 estimateCovariance(double T, size_t N=1000, const Bias &estimatedBias=Bias()) const
Compute a Monte Carlo estimate of the predict covariance using N samples.
Definition ScenarioRunner.cpp:55
gtsam::ScenarioRunner::predict
NavState predict(const PreintegratedImuMeasurements &pim, const Bias &estimatedBias=Bias()) const
Predict predict given a PIM.
Definition ScenarioRunner.cpp:49
gtsam::CombinedScenarioRunner::estimateCovariance
Eigen::Matrix< double, 15, 15 > estimateCovariance(double T, size_t N=1000, const Bias &estimatedBias=Bias()) const
Compute a Monte Carlo estimate of the predict covariance using N samples.
Definition ScenarioRunner.cpp:132
gtsam::CombinedScenarioRunner::integrate
PreintegratedCombinedMeasurements integrate(double T, const Bias &estimatedBias=Bias(), bool corrupted=false) const
Integrate measurements for T seconds into a PIM.
Definition ScenarioRunner.cpp:106
gtsam::CombinedScenarioRunner::predict
NavState predict(const PreintegratedCombinedMeasurements &pim, const Bias &estimatedBias=Bias()) const
Predict predict given a PIM.
Definition ScenarioRunner.cpp:125