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

Merge remote-tracking branch 'bitbucket/RSS_ImuFactor' into feature/scenarios

release/4.3a0
Frank 2015-12-23 16:18:59 -08:00
parent 5869b8cb65 1245e899d6
commit 8e1041c56a
3 changed files with 105 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/navigation/ScenarioRunner.cpp
View File

@ -16,7 +16,6 @@
*/
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/linear/Sampler.h>
#include <cmath>
@ -26,24 +25,21 @@ static double intNoiseVar = 0.0000001;
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
double T, Sampler* gyroSampler, Sampler* accSampler) const {
// TODO(frank): allow non-zero
const imuBias::ConstantBias zeroBias;
double T, const imuBias::ConstantBias& estimatedBias,
bool corrupted) const {
const bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pim(
zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance,
use2ndOrderIntegration);
estimatedBias, accCovariance(), gyroCovariance(),
kIntegrationErrorCovariance, use2ndOrderIntegration);
const double dt = imuSampleTime();
const double sqrt_dt = std::sqrt(dt);
const size_t nrSteps = T / dt;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += dt) {
Vector3 measuredOmega = measured_angular_velocity(t);
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
Vector3 measuredAcc = measured_acceleration(t);
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t);
Vector3 measuredAcc =
corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t);
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
}
@ -51,28 +47,26 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
}
PoseVelocityBias ScenarioRunner::predict(
const ImuFactor::PreintegratedMeasurements& pim) const {
// TODO(frank): allow non-zero bias, omegaCoriolis
const imuBias::ConstantBias zeroBias;
const ImuFactor::PreintegratedMeasurements& pim,
const imuBias::ConstantBias& estimatedBias) const {
// TODO(frank): allow non-zero omegaCoriolis
const Vector3 omegaCoriolis = Vector3::Zero();
const bool use2ndOrderCoriolis = true;
return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias,
gravity_n(), omegaCoriolis, use2ndOrderCoriolis);
return pim.predict(scenario_->pose(0), scenario_->velocity_n(0),
estimatedBias, gravity_n(), omegaCoriolis,
use2ndOrderCoriolis);
}
Matrix6 ScenarioRunner::estimatePoseCovariance(double T, size_t N) const {
Matrix6 ScenarioRunner::estimatePoseCovariance(
double T, size_t N, const imuBias::ConstantBias& estimatedBias) const {
// Get predict prediction from ground truth measurements
Pose3 prediction = predict(integrate(T)).pose;
// Create two samplers for acceleration and omega noise
Sampler gyroSampler(gyroNoiseModel(), 10);
Sampler accSampler(accNoiseModel(), 29284);
// Sample !
Matrix samples(9, N);
Vector6 sum = Vector6::Zero();
for (size_t i = 0; i < N; i++) {
Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose;
Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose;
Vector6 xi = sampled.localCoordinates(prediction);
samples.col(i) = xi;
sum += xi;

56
gtsam/navigation/ScenarioRunner.h
View File

@ -18,11 +18,10 @@
#pragma once
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <gtsam/linear/Sampler.h>
namespace gtsam {
class Sampler;
/*
* Simple class to test navigation scenarios.
* Takes a trajectory scenario as input, and can generate IMU measurements
@ -30,27 +29,42 @@ class Sampler;
class ScenarioRunner {
public:
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())
: scenario_(scenario),
imuSampleTime_(imuSampleTime),
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)),
bias_(bias),
// NOTE(duy): random seeds that work well:
gyroSampler_(gyroNoiseModel_, 10),
accSampler_(accNoiseModel_, 29284)
{}
// NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
// also, uses g=10 for easy debugging
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);
}
// A gyro simply measures angular velocity in body frame
Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
// An accelerometer measures acceleration in body, but not gravity
Vector3 measured_acceleration(double t) const {
Vector3 actual_specific_force_b(double t) const {
Rot3 bRn = scenario_->rotation(t).transpose();
return scenario_->acceleration_b(t) - bRn * gravity_n();
}
// versions corrupted by bias and noise
Vector3 measured_omega_b(double t) const {
return actual_omega_b(t) + bias_.gyroscope() +
gyroSampler_.sample() / std::sqrt(imuSampleTime_);
}
Vector3 measured_specific_force_b(double t) const {
return actual_specific_force_b(t) + bias_.accelerometer() +
accSampler_.sample() / std::sqrt(imuSampleTime_);
}
const double& imuSampleTime() const { return imuSampleTime_; }
const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const {
@ -65,13 +79,15 @@ class ScenarioRunner {
Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
/// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate(double T,
Sampler* gyroSampler = 0,
Sampler* accSampler = 0) const;
ImuFactor::PreintegratedMeasurements integrate(
double T,
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
bool corrupted = false) const;
/// Predict predict given a PIM
PoseVelocityBias predict(
const ImuFactor::PreintegratedMeasurements& pim) const;
PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
Matrix6 poseCovariance(
@ -84,12 +100,18 @@ class ScenarioRunner {
}
/// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const;
Matrix6 estimatePoseCovariance(double T, size_t N = 1000,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
private:
const Scenario* scenario_;
double imuSampleTime_;
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
const double imuSampleTime_;
const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
const imuBias::ConstantBias bias_;
// Create two samplers for acceleration and omega noise
mutable Sampler gyroSampler_, accSampler_;
};
} // namespace gtsam

57
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -25,14 +25,20 @@ using namespace gtsam;
static const double kDegree = M_PI / 180.0;
static const double kDeltaT = 1e-2;
static const double kGyroSigma = 0.02;
static const double kAccelerometerSigma = 0.1;
static const double kAccelSigma = 0.1;
static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
/* ************************************************************************* */
TEST(ScenarioRunner, Forward) {
namespace forward {
const double v = 2; // m/s
ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
}
/* ************************************************************************* */
TEST(ScenarioRunner, Forward) {
using namespace forward;
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@ -42,13 +48,24 @@ TEST(ScenarioRunner, Forward) {
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
}
/* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) {
using namespace forward;
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
/* ************************************************************************* */
TEST(ScenarioRunner, Circle) {
// Forward velocity 2m/s, angular velocity 6 kDegree/sec
const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@ -65,7 +82,7 @@ TEST(ScenarioRunner, Loop) {
const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma);
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
@ -85,22 +102,36 @@ const Vector3 V0(50, 0, 0);
}
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
namespace accelerating {
using namespace initial;
const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0);
const AcceleratingScenario scenario(nRb, P0, V0, A);
const double T = 3; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
}
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
using namespace accelerating;
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
/* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias) {
using namespace accelerating;
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
kNonZeroBias);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
cout << estimatedCov << endl << endl;
cout << runner.poseCovariance(pim) << endl;
}
/* ************************************************************************* */
@ -111,12 +142,12 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 3; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}