Merge remote-tracking branch 'origin/feature/scenarios' into feature/ImuFactorPush2
Conflicts: gtsam/navigation/PreintegrationBase.h gtsam/navigation/tests/testImuFactor.cpprelease/4.3a0
Frank
commit
f37fe2613b
|
|
@ -298,7 +298,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
|||
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
||||
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
|
||||
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis) {
|
||||
const bool use2ndOrderCoriolis) const {
|
||||
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
|
||||
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
|
||||
q->n_gravity = n_gravity;
|
||||
|
|
|
|||
|
|
@ -100,8 +100,8 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
/// Parameters
|
||||
boost::shared_ptr<Params> p_;
|
||||
/// Parameters. Declared mutable only for deprecated predict method.
|
||||
mutable boost::shared_ptr<Params> p_;
|
||||
|
||||
/// Acceleration and gyro bias used for preintegration
|
||||
imuBias::ConstantBias biasHat_;
|
||||
|
|
@ -283,7 +283,7 @@ public:
|
|||
/// @deprecated predict
|
||||
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,92 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Scenario.h
|
||||
* @brief Simple class to test navigation scenarios
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Simple trajectory simulator.
|
||||
class Scenario {
|
||||
public:
|
||||
// Quantities a Scenario needs to specify:
|
||||
|
||||
virtual Pose3 pose(double t) const = 0;
|
||||
virtual Vector3 omega_b(double t) const = 0;
|
||||
virtual Vector3 velocity_n(double t) const = 0;
|
||||
virtual Vector3 acceleration_n(double t) const = 0;
|
||||
|
||||
// Derived quantities:
|
||||
|
||||
virtual Rot3 rotation(double t) const { return pose(t).rotation(); }
|
||||
|
||||
Vector3 velocity_b(double t) const {
|
||||
const Rot3 nRb = rotation(t);
|
||||
return nRb.transpose() * velocity_n(t);
|
||||
}
|
||||
|
||||
Vector3 acceleration_b(double t) const {
|
||||
const Rot3 nRb = rotation(t);
|
||||
return nRb.transpose() * acceleration_n(t);
|
||||
}
|
||||
};
|
||||
|
||||
/// Scenario with constant twist 3D trajectory.
|
||||
class ExpmapScenario : public Scenario {
|
||||
public:
|
||||
/// Construct scenario with constant twist [w,v]
|
||||
ExpmapScenario(const Vector3& w, const Vector3& v)
|
||||
: twist_((Vector6() << w, v).finished()),
|
||||
a_b_(twist_.head<3>().cross(twist_.tail<3>())) {}
|
||||
|
||||
Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); }
|
||||
Rot3 rotation(double t) const { return Rot3::Expmap(twist_.head<3>() * t); }
|
||||
Vector3 omega_b(double t) const { return twist_.head<3>(); }
|
||||
Vector3 velocity_n(double t) const {
|
||||
return rotation(t).matrix() * twist_.tail<3>();
|
||||
}
|
||||
Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; }
|
||||
|
||||
private:
|
||||
const Vector6 twist_;
|
||||
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
|
||||
};
|
||||
|
||||
/// Accelerating from an arbitrary initial state, with optional rotation
|
||||
class AcceleratingScenario : public Scenario {
|
||||
public:
|
||||
/// Construct scenario with constant acceleration in navigation frame and
|
||||
/// optional angular velocity in body: rotating while translating...
|
||||
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
|
||||
const Vector3& a_n,
|
||||
const Vector3& omega_b = Vector3::Zero())
|
||||
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
|
||||
|
||||
Pose3 pose(double t) const {
|
||||
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
|
||||
}
|
||||
Vector3 omega_b(double t) const { return omega_b_; }
|
||||
Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
|
||||
Vector3 acceleration_n(double t) const { return a_n_; }
|
||||
|
||||
private:
|
||||
const Rot3 nRb_;
|
||||
const Vector3 p0_, v0_, a_n_, omega_b_;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ScenarioRunner.h
|
||||
* @brief Simple class to test navigation scenarios
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static double intNoiseVar = 0.0000001;
|
||||
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);
|
||||
|
||||
const double dt = imuSampleTime();
|
||||
const size_t nrSteps = T / dt;
|
||||
double t = 0;
|
||||
for (size_t k = 0; k < nrSteps; k++, t += 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);
|
||||
}
|
||||
|
||||
return pim;
|
||||
}
|
||||
|
||||
PoseVelocityBias 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);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
// 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;
|
||||
Vector6 xi = sampled.localCoordinates(prediction);
|
||||
samples.col(i) = xi;
|
||||
sum += xi;
|
||||
}
|
||||
|
||||
// Compute MC covariance
|
||||
Vector6 sampleMean = sum / N;
|
||||
Matrix6 Q;
|
||||
Q.setZero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
Vector6 xi = samples.col(i);
|
||||
xi -= sampleMean;
|
||||
Q += xi * xi.transpose();
|
||||
}
|
||||
|
||||
return Q / (N - 1);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ScenarioRunner.h
|
||||
* @brief Simple class to test navigation scenarios
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/*
|
||||
* Simple class to test navigation scenarios.
|
||||
* Takes a trajectory scenario as input, and can generate IMU measurements
|
||||
*/
|
||||
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())
|
||||
: scenario_(scenario),
|
||||
imuSampleTime_(imuSampleTime),
|
||||
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
|
||||
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 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 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 {
|
||||
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,
|
||||
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
|
||||
bool corrupted = false) const;
|
||||
|
||||
/// Predict predict given a PIM
|
||||
PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim,
|
||||
const imuBias::ConstantBias& estimatedBias =
|
||||
imuBias::ConstantBias()) const;
|
||||
|
||||
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
|
||||
Matrix6 poseCovariance(
|
||||
const ImuFactor::PreintegratedMeasurements& pim) const {
|
||||
Matrix9 cov = pim.preintMeasCov();
|
||||
Matrix6 poseCov;
|
||||
poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), //
|
||||
cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3);
|
||||
return poseCov;
|
||||
}
|
||||
|
||||
/// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
|
||||
Matrix6 estimatePoseCovariance(double T, size_t N = 1000,
|
||||
const imuBias::ConstantBias& estimatedBias =
|
||||
imuBias::ConstantBias()) const;
|
||||
|
||||
private:
|
||||
const Scenario* scenario_;
|
||||
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
|
||||
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
|
|
@ -56,15 +57,17 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
|
|||
|
||||
// Define covariance matrices
|
||||
/* ************************************************************************* */
|
||||
double accNoiseVar = 0.01;
|
||||
double omegaNoiseVar = 0.03;
|
||||
double intNoiseVar = 0.0001;
|
||||
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
|
||||
const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
|
||||
const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
||||
const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
|
||||
const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
|
||||
int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10;
|
||||
static const double kGyroSigma = 0.02;
|
||||
static const double kAccelerometerSigma = 0.1;
|
||||
static double omegaNoiseVar = kGyroSigma * kGyroSigma;
|
||||
static double accNoiseVar = kAccelerometerSigma * kAccelerometerSigma;
|
||||
static double intNoiseVar = 0.0001;
|
||||
static const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
|
||||
static const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
|
||||
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
||||
static const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
|
||||
static const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
|
||||
static int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10;
|
||||
|
||||
// Auxiliary functions to test preintegrated Jacobians
|
||||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
||||
|
|
@ -118,114 +121,39 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
|
|||
} // namespace
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool MonteCarlo(const PreintegratedImuMeasurements& pim,
|
||||
const NavState& state, const imuBias::ConstantBias& bias, double dt,
|
||||
boost::optional<Pose3> body_P_sensor, const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, const Vector3& accNoiseVar,
|
||||
const Vector3& omegaNoiseVar, size_t N = 10000,
|
||||
size_t M = 1) {
|
||||
// Get mean prediction from "ground truth" measurements
|
||||
PreintegratedImuMeasurements pim1 = pim;
|
||||
for (size_t k = 0; k < M; k++)
|
||||
pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
|
||||
NavState prediction = pim1.predict(state, bias);
|
||||
Matrix9 actual = pim1.preintMeasCov();
|
||||
|
||||
// Do a Monte Carlo analysis to determine empirical density on the predicted state
|
||||
Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed);
|
||||
Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed);
|
||||
Matrix samples(9, N);
|
||||
Vector9 sum = Vector9::Zero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
PreintegratedImuMeasurements pim2 = pim;
|
||||
for (size_t k = 0; k < M; k++) {
|
||||
Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample();
|
||||
Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample();
|
||||
pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt,
|
||||
body_P_sensor);
|
||||
}
|
||||
NavState sampled = pim2.predict(state, bias);
|
||||
Vector9 xi = sampled.localCoordinates(prediction);
|
||||
samples.col(i) = xi;
|
||||
sum += xi;
|
||||
}
|
||||
|
||||
Vector9 sampleMean = sum / N;
|
||||
Matrix9 Q;
|
||||
Q.setZero();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
Vector9 xi = samples.col(i);
|
||||
xi -= sampleMean;
|
||||
Q += xi * (xi.transpose() / (N - 1));
|
||||
}
|
||||
|
||||
// Compare MonteCarlo value with actual (computed) value
|
||||
return assert_equal(Matrix(Q), actual, 1e-4);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, StraightLine) {
|
||||
// Set up IMU measurements
|
||||
static const double g = 10; // make gravity 10 to make this easy to check
|
||||
static const double v = 50.0, a = 0.2, dt = 3.0;
|
||||
const double dt22 = dt * dt / 2;
|
||||
TEST(ImuFactor, Accelerating) {
|
||||
const double a = 0.2, v=50;
|
||||
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
|
||||
// The body itself has Z axis pointing down
|
||||
static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
static const Point3 initial_position(10, 20, 0);
|
||||
static const Vector3 initial_velocity(v, 0, 0);
|
||||
static const NavState state1(nRb, initial_position, initial_velocity);
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 initial_position(10, 20, 0);
|
||||
const Vector3 initial_velocity(v, 0, 0);
|
||||
|
||||
// set up acceleration in X direction, no angular velocity.
|
||||
// Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z
|
||||
Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
|
||||
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
|
||||
Vector3(a, 0, 0));
|
||||
|
||||
// Create parameters assuming nav frame has Z up
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
|
||||
PreintegratedImuMeasurements::Params::MakeSharedU(g);
|
||||
p->accelerometerCovariance = kMeasuredAccCovariance;
|
||||
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
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));
|
||||
|
||||
// Check G1 and G2 derivatives of pim.update
|
||||
|
||||
// Now, preintegrate for 3 seconds, in 10 steps
|
||||
PreintegratedImuMeasurements pim(p, kZeroBiasHat);
|
||||
for (size_t i = 0; i < 10; i++)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
|
||||
|
||||
Matrix93 aG1,aG2;
|
||||
boost::function<NavState(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10,
|
||||
boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10,
|
||||
boost::none, boost::none, boost::none);
|
||||
pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
|
||||
|
||||
// Do Monte-Carlo analysis
|
||||
PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance,
|
||||
p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise
|
||||
EXPECT(
|
||||
MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc,
|
||||
measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
|
||||
|
||||
// Check integrated quantities in body frame: gravity measured by IMU is integrated!
|
||||
Vector3 b_deltaP(a * dt22, 0, -g * dt22);
|
||||
Vector3 b_deltaV(a * dt, 0, -g * dt);
|
||||
EXPECT(assert_equal(Rot3(), pim.deltaRij()));
|
||||
EXPECT(assert_equal(b_deltaP, pim.deltaPij()));
|
||||
EXPECT(assert_equal(b_deltaV, pim.deltaVij()));
|
||||
|
||||
// Check bias-corrected delta: also in body frame
|
||||
Vector9 expectedBC;
|
||||
expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV;
|
||||
EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias)));
|
||||
|
||||
// Check prediction in nav, note we move along x in body, along y in nav
|
||||
Point3 expected_position(10 + v * dt, 20 + a * dt22, 0);
|
||||
Velocity3 expected_velocity(v, a * dt, 0);
|
||||
NavState expected(nRb, expected_position, expected_velocity);
|
||||
EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias)));
|
||||
const Vector3 measuredAcc = runner.measured_acceleration(T);
|
||||
const Vector3 measuredOmega = runner.measured_angular_velocity(T);
|
||||
pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2);
|
||||
EXPECT(assert_equal(
|
||||
numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
|
||||
EXPECT(assert_equal(
|
||||
numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -615,12 +543,25 @@ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& mea
|
|||
}
|
||||
|
||||
TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
|
||||
Vector3 v1(Vector3(0.5, 0.0, 0.0));
|
||||
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
|
||||
Point3(5.5, 1.0, -50.0));
|
||||
Vector3 v2(Vector3(0.5, 0.0, 0.0));
|
||||
const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0));
|
||||
const Point3 initial_position(5.0, 1.0, -50.0);
|
||||
const Vector3 initial_velocity(0.5, 0.0, 0.0);
|
||||
|
||||
const double a = 0.2;
|
||||
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
|
||||
Vector3(a, 0, 0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
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));
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
Pose3 x1(nRb, initial_position);
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega;
|
||||
|
|
@ -659,42 +600,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
1e-6);
|
||||
EXPECT(assert_equal(expectedG2, G2, 1e-5));
|
||||
|
||||
#if 0
|
||||
/*
|
||||
* This code is to verify the quality of the generated samples
|
||||
* by checking if the covariance of the generated noises matches
|
||||
* with the input covariance, and visualizing the nonlinearity of
|
||||
* the sample set using the following matlab script:
|
||||
*
|
||||
noises = dlmread('noises.txt');
|
||||
cov(noises)
|
||||
|
||||
samples = dlmread('noises.txt');
|
||||
figure(1);
|
||||
for i=1:9
|
||||
subplot(3,3,i)
|
||||
hist(samples(:,i), 500)
|
||||
end
|
||||
*/
|
||||
size_t N = 100000;
|
||||
Matrix samples(9,N);
|
||||
Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284);
|
||||
Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10);
|
||||
ofstream samplesOut("preintSamples.txt");
|
||||
ofstream noiseOut("noises.txt");
|
||||
for (size_t i = 0; i<N; ++i) {
|
||||
Vector3 accNoise = sampleAccelerationNoise.sample();
|
||||
Vector3 omegaNoise = sampleOmegaNoise.sample();
|
||||
NavState sample = pim.updatedDeltaXij(measuredAcc+accNoise, measuredOmega+omegaNoise, dt);
|
||||
samples.col(i) = sample.localCoordinates(preint);
|
||||
samplesOut << samples.col(i).transpose() << endl;
|
||||
noiseOut << accNoise.transpose() << " " << omegaNoise.transpose() << endl;
|
||||
}
|
||||
samplesOut.close();
|
||||
noiseOut.close();
|
||||
#endif
|
||||
|
||||
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor,
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor,
|
||||
measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
|
||||
|
||||
|
||||
|
|
@ -710,6 +617,9 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
// Predict
|
||||
Pose3 actual_x2;
|
||||
Vector3 actual_v2;
|
||||
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
|
||||
Point3(5.5, 1.0, -50.0));
|
||||
Vector3 v2(Vector3(0.5, 0.0, 0.0));
|
||||
ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim,
|
||||
kGravityAlongNavZDown, kZeroOmegaCoriolis);
|
||||
|
||||
|
|
@ -800,6 +710,27 @@ TEST(ImuFactor, PredictRotation) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictArbitrary) {
|
||||
const double a = 0.2, v=50;
|
||||
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
|
||||
// The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 initial_position(10, 20, 0);
|
||||
const Vector3 initial_velocity(v, 0, 0);
|
||||
|
||||
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
|
||||
Vector3(a, 0, 0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
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));
|
||||
//////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
|
||||
|
||||
// Measurements
|
||||
|
|
|
|||
|
|
@ -0,0 +1,120 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testScenario.cpp
|
||||
* @brief Unit test Scenario class
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/bind.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
static const double degree = M_PI / 180.0;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, Forward) {
|
||||
const double v = 2; // m/s
|
||||
const Vector3 W(0, 0, 0), V(v, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
const double T = 15;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
const Pose3 T15 = scenario.pose(T);
|
||||
EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, Circle) {
|
||||
// Forward velocity 2m/s, angular velocity 6 degree/sec around Z
|
||||
const double v = 2, w = 6 * degree;
|
||||
const Vector3 W(0, 0, w), V(v, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
const double T = 15;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
// R = v/w, so test if circle is of right size
|
||||
const double R = v / w;
|
||||
const Pose3 T15 = scenario.pose(T);
|
||||
EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, Loop) {
|
||||
// Forward velocity 2m/s
|
||||
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
const double v = 2, w = 6 * degree;
|
||||
const Vector3 W(0, -w, 0), V(v, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
const double T = 30;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
// R = v/w, so test if loop crests at 2*R
|
||||
const double R = v / w;
|
||||
const Pose3 T30 = scenario.pose(30);
|
||||
EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, Accelerating) {
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
// going in X. The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 P0(10, 20, 0);
|
||||
const Vector3 V0(50, 0, 0);
|
||||
|
||||
const double a = 0.2; // m/s^2
|
||||
const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
|
||||
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||
|
||||
const double T = 3;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
|
||||
EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
|
||||
|
||||
{
|
||||
// Check acceleration in nav
|
||||
Matrix expected = numericalDerivative11<Vector3, double>(
|
||||
boost::bind(&Scenario::velocity_n, scenario, _1), T);
|
||||
EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
|
||||
}
|
||||
|
||||
const Pose3 T3 = scenario.pose(3);
|
||||
EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
|
||||
T3.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testScenarioRunner.cpp
|
||||
* @brief test ImuFacor with ScenarioRunner class
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
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 kAccelSigma = 0.1;
|
||||
|
||||
static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
|
||||
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace forward {
|
||||
const double v = 2; // m/s
|
||||
ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Forward) {
|
||||
using namespace forward;
|
||||
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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, kAccelSigma);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Loop) {
|
||||
// Forward velocity 2m/s
|
||||
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
||||
const double v = 2, w = 6 * kDegree;
|
||||
ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||
|
||||
ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace initial {
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
// going in X. The body itself has Z axis pointing down
|
||||
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
const Point3 P0(10, 20, 0);
|
||||
const Vector3 V0(50, 0, 0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, AcceleratingAndRotating) {
|
||||
using namespace initial;
|
||||
const double a = 0.2; // m/s^2
|
||||
const Vector3 A(0, a, 0), W(0, 0.1, 0);
|
||||
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||
|
||||
const double T = 3; // seconds
|
||||
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);
|
||||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue