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

Merge branch 'RSS_ImuFactor' into feature/scenarios

release/4.3a0
Frank Dellaert 2015-12-23 12:48:17 -08:00
parent 630c2a7a18 9eb7e38cb8
commit bf82b58782
2 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/ScenarioRunner.cpp
View File

@ -40,10 +40,9 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
const size_t nrSteps = T / dt; const size_t nrSteps = T / dt;
double t = 0; double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += dt) { for (size_t k = 0; k < nrSteps; k++, t += dt) {
Rot3 bRn = scenario_->rotation(t).transpose(); Vector3 measuredOmega = measured_angular_velocity(t);
Vector3 measuredOmega = scenario_->omega_b(t);
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n(); Vector3 measuredAcc = measured_acceleration(t);
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
pim.integrateMeasurement(measuredAcc, measuredOmega, dt); pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
} }

24
gtsam/navigation/ScenarioRunner.h
View File

@ -23,7 +23,10 @@ namespace gtsam {
class Sampler; class Sampler;
/// Simple class to test navigation scenarios /*
* Simple class to test navigation scenarios.
* Takes a trajectory scenario as input, and can generate IMU measurements
*/
class ScenarioRunner { class ScenarioRunner {
public: public:
ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
@ -33,11 +36,22 @@ class ScenarioRunner {
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
const double& imuSampleTime() const { return imuSampleTime_; }
// 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
Vector3 gravity_n() const { return Vector3(0, 0, -10.0); } static Vector3 gravity_n() { return Vector3(0, 0, -10.0); }
// A gyro simly measures angular velocity in body frame
Vector3 measured_angular_velocity(double t) const {
return scenario_->omega_b(t);
}
// An accelerometer measures acceleration in body, but not gravity
Vector3 measured_acceleration(double t) const {
Rot3 bRn = scenario_->rotation(t).transpose();
return scenario_->acceleration_b(t) - bRn * gravity_n();
}
const double& imuSampleTime() const { return imuSampleTime_; }
const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const {
return gyroNoiseModel_; return gyroNoiseModel_;
@ -70,7 +84,7 @@ class ScenarioRunner {
} }
/// Compute a Monte Carlo estimate of the PIM pose covariance using N samples /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
Matrix6 estimatePoseCovariance(double T, size_t N = 100) const; Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const;
private: private:
const Scenario* scenario_; const Scenario* scenario_;