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

Now using Params

release/4.3a0
Frank Dellaert 2015-12-27 18:56:13 -08:00
parent 05df0ca0cc
commit 7834ac08df
3 changed files with 49 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/navigation/ScenarioRunner.cpp
View File

@ -27,11 +27,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
double T, const imuBias::ConstantBias& estimatedBias,
bool corrupted) const {
const bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pim(
estimatedBias, accCovariance(), gyroCovariance(),
kIntegrationErrorCovariance, use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias);
const double dt = imuSampleTime();
const size_t nrSteps = T / dt;
@ -46,27 +42,23 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
return pim;
}
PoseVelocityBias ScenarioRunner::predict(
NavState ScenarioRunner::predict(
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),
estimatedBias, gravity_n(), omegaCoriolis,
use2ndOrderCoriolis);
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
return pim.predict(state_i, estimatedBias);
}
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;
Pose3 prediction = predict(integrate(T)).pose();
// Sample !
Matrix samples(9, N);
Vector6 sum = Vector6::Zero();
for (size_t i = 0; i < N; i++) {
Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose;
Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose();
Vector6 xi = sampled.localCoordinates(prediction);
samples.col(i) = xi;
sum += xi;

52
gtsam/navigation/ScenarioRunner.h
View File

@ -28,25 +28,22 @@ namespace gtsam {
*/
class ScenarioRunner {
public:
ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0,
double gyroSigma = 0.17, double accSigma = 0.01,
const imuBias::ConstantBias& bias = imuBias::ConstantBias(),
const Vector3& gravity_n = Vector3(0, 0, -10))
typedef boost::shared_ptr<ImuFactor::PreintegratedMeasurements::Params>
SharedParams;
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const imuBias::ConstantBias& bias = imuBias::ConstantBias())
: scenario_(scenario),
p_(p),
imuSampleTime_(imuSampleTime),
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)),
bias_(bias),
gravity_n_(gravity_n),
// NOTE(duy): random seeds that work well:
gyroSampler_(gyroNoiseModel_, 10),
accSampler_(accNoiseModel_, 29284)
{}
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 gravity_n_; }
const Vector3& gravity_n() const { return p_->n_gravity; }
// A gyro simply measures angular velocity in body frame
Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); }
@ -69,17 +66,6 @@ class ScenarioRunner {
const double& imuSampleTime() const { return imuSampleTime_; }
const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const {
return gyroNoiseModel_;
}
const noiseModel::Diagonal::shared_ptr& accNoiseModel() const {
return accNoiseModel_;
}
Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
/// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate(
double T,
@ -87,9 +73,9 @@ class ScenarioRunner {
bool corrupted = false) const;
/// Predict predict given a PIM
PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
NavState predict(const ImuFactor::PreintegratedMeasurements& pim,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
Matrix6 poseCovariance(
@ -107,12 +93,20 @@ class ScenarioRunner {
imuBias::ConstantBias()) const;
private:
// 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;
}
const Scenario* scenario_;
const SharedParams p_;
const double imuSampleTime_;
// TODO(frank): unify with Params, use actual noisemodels there...
const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
const imuBias::ConstantBias bias_;
const Vector3 gravity_n_;
// Create two samplers for acceleration and omega noise
mutable Sampler gyroSampler_, accSampler_;

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

@ -30,6 +30,15 @@ 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);
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0000001 * I_3x3;
return p;
}
/* ************************************************************************* */
namespace forward {
const double v = 2; // m/s
@ -38,11 +47,11 @@ ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
/* ************************************************************************* */
TEST(ScenarioRunner, Forward) {
using namespace forward;
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
@ -51,7 +60,7 @@ TEST(ScenarioRunner, Forward) {
/* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) {
using namespace forward;
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
@ -65,11 +74,11 @@ TEST(ScenarioRunner, Circle) {
const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1));
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
@ -82,11 +91,11 @@ 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, kAccelSigma);
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1));
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
@ -114,10 +123,10 @@ const double T = 3; // seconds
/* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) {
using namespace accelerating;
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
@ -145,10 +154,10 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 3; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma);
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));