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

Fixed all navigation tests that were still using deprecated methods/types

release/4.3a0
Frank Dellaert 2016-01-17 14:44:03 -08:00
parent 7c66ea1323
commit 43520265aa
9 changed files with 117 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/CombinedImuFactor.h
View File

@ -278,10 +278,10 @@ public:
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const;
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
/// @deprecated typename
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
/// @deprecated constructor
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j, const CombinedPreintegratedMeasurements& pim,

15
gtsam/navigation/ImuFactor.cpp
View File

@ -29,7 +29,7 @@ namespace gtsam {
using namespace std;
//------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements
// Inner class PreintegratedImuMeasurements
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s);
@ -156,14 +156,15 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
}
//------------------------------------------------------------------------------
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& pim, const Vector3& n_gravity,
const PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), _PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
PreintegratedMeasurements::Params>(pim.p());
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
PreintegratedImuMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
@ -171,11 +172,9 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
@ -184,4 +183,6 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
vel_j = pvb.velocity;
}
#endif
//------------------------------------------------------------------------------
} // namespace gtsam

2
gtsam/navigation/ImuFactor.h
View File

@ -223,6 +223,7 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
/// @deprecated typename
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
@ -239,6 +240,7 @@ public:
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
#endif
private:

6
gtsam/navigation/ScenarioRunner.cpp
View File

@ -25,10 +25,10 @@ namespace gtsam {
static double intNoiseVar = 0.0000001;
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
PreintegratedImuMeasurements ScenarioRunner::integrate(
double T, const imuBias::ConstantBias& estimatedBias,
bool corrupted) const {
ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias);
PreintegratedImuMeasurements pim(p_, estimatedBias);
const double dt = imuSampleTime();
const size_t nrSteps = T / dt;
@ -45,7 +45,7 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate(
}
NavState ScenarioRunner::predict(
const ImuFactor::PreintegratedMeasurements& pim,
const PreintegratedImuMeasurements& pim,
const imuBias::ConstantBias& estimatedBias) const {
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
return pim.predict(state_i, estimatedBias);

10
gtsam/navigation/ScenarioRunner.h
View File

@ -28,8 +28,7 @@ namespace gtsam {
*/
class ScenarioRunner {
public:
typedef boost::shared_ptr<ImuFactor::PreintegratedMeasurements::Params>
SharedParams;
typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const imuBias::ConstantBias& bias = imuBias::ConstantBias())
@ -69,19 +68,18 @@ class ScenarioRunner {
const double& imuSampleTime() const { return imuSampleTime_; }
/// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate(
PreintegratedImuMeasurements integrate(
double T,
const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(),
bool corrupted = false) const;
/// Predict predict given a PIM
NavState predict(const ImuFactor::PreintegratedMeasurements& pim,
NavState predict(const PreintegratedImuMeasurements& pim,
const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const;
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
Matrix6 poseCovariance(
const ImuFactor::PreintegratedMeasurements& pim) const {
Matrix6 poseCovariance(const PreintegratedImuMeasurements& pim) const {
Matrix9 cov = pim.preintMeasCov();
Matrix6 poseCov;
poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), //

76
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -44,10 +44,10 @@ namespace {
// Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3,
PreintegratedCombinedMeasurements result(bias, I_3x3,
I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
@ -94,12 +94,13 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
double deltaT = 0.5;
double tol = 1e-6;
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3);
PreintegratedImuMeasurements expected1(p, bias);
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6);
PreintegratedCombinedMeasurements actual1(p, bias);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -119,38 +120,33 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
Point3(5.5, 1.0, -50.0));
Vector3 v2(0.5, 0.0, 0.0);
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
p->omegaCoriolis = Vector3(0,0.1,0.1);
PreintegratedImuMeasurements pim(
p, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
// Measurements
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
Vector3 measuredAcc =
x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
double tol = 1e-6;
ImuFactor::PreintegratedMeasurements pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
I_3x3, I_3x3, I_3x3);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6);
PreintegratedCombinedMeasurements combined_pim(p,
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity,
omegaCoriolis);
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim);
noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
combined_pim, gravity, omegaCoriolis);
combined_pim);
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
@ -197,7 +193,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
}
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements pim =
PreintegratedCombinedMeasurements pim =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
@ -236,19 +232,16 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
// Measurements
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 1.1, -9.81;
double deltaT = 0.001;
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
PreintegratedCombinedMeasurements pim(p, bias);
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -256,48 +249,39 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
// Create factor
noiseModel::Gaussian::shared_ptr combinedmodel =
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
gravity, omegaCoriolis);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity,
omegaCoriolis);
NavState actual = pim.predict(NavState(), bias);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
EXPECT(assert_equal(expectedPose, actual.pose()));
EXPECT(
assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
PreintegratedCombinedMeasurements pim(p, bias);
Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81;
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
double tol = 1e-4;
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
gravity, omegaCoriolis);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// Predict
Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
NavState actual = pim.predict(NavState(x, v), bias);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol));
EXPECT(assert_equal(expectedPose, actual.pose(), tol));
}
/* ************************************************************************* */

128
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -135,11 +135,10 @@ TEST(ImuFactor, Accelerating) {
Vector3(a, 0, 0));
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma,
kZeroBias, kGravityAlongNavZDown);
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
PreintegratedImuMeasurements 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));
@ -149,8 +148,8 @@ TEST(ImuFactor, Accelerating) {
boost::function<NavState(const Vector3&, const Vector3&)> f =
boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10,
boost::none, boost::none, boost::none);
const Vector3 measuredAcc = runner.actual_specific_force_b(T);
const Vector3 measuredOmega = runner.actual_omega_b(T);
const Vector3 measuredAcc = runner.actualSpecificForce(T);
const Vector3 measuredOmega = runner.actualAngularVelocity(T);
pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2);
EXPECT(assert_equal(
numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
@ -332,8 +331,11 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
auto p = defaultParams();
p->omegaCoriolis = kNonZeroOmegaCoriolis;
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
PreintegratedImuMeasurements pim(defaultParams(), biasHat);
PreintegratedImuMeasurements pim(p, biasHat);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Make sure of biasCorrectedDelta
@ -345,8 +347,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
EXPECT(assert_equal(expectedH, actualH));
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
Values values;
values.insert(X(1), x1);
@ -376,14 +377,17 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
PreintegratedImuMeasurements pim(defaultParams(),
auto p = defaultParams();
p->omegaCoriolis = kNonZeroOmegaCoriolis;
p->use2ndOrderCoriolis = true;
PreintegratedImuMeasurements pim(p,
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis, boost::none, use2ndOrderCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
Values values;
values.insert(X(1), x1);
@ -472,8 +476,6 @@ TEST(ImuFactor, fistOrderExponential) {
/* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
@ -544,11 +546,16 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
const AcceleratingScenario scenario(nRb, p1, v1, a,
Vector3(0, 0, M_PI / 10.0 + 0.3));
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma,
kZeroBias, kGravityAlongNavZDown);
auto p = defaultParams();
p->body_P_sensor =Pose3(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01));
p->omegaCoriolis = kNonZeroOmegaCoriolis;
// ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, p, T / 10);
// PreintegratedImuMeasurements pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -558,18 +565,13 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
Pose3 x1(nRb, p1);
// Measurements
Vector3 measuredOmega = runner.actual_omega_b(0);
Vector3 measuredAcc = runner.actual_specific_force_b(0);
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01));
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
Vector3 measuredOmega = runner.actualAngularVelocity(0);
Vector3 measuredAcc = runner.actualSpecificForce(0);
// Get mean prediction from "ground truth" measurements
const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
PreintegratedImuMeasurements pim(biasHat, accNoiseVar2.asDiagonal(),
omegaNoiseVar2.asDiagonal(), Z_3x3, true); // MonteCarlo does not sample integration noise
pim.set_body_P_sensor(body_P_sensor);
const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
PreintegratedImuMeasurements pim(p, biasHat);
// Check updatedDeltaXij derivatives
Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero();
@ -601,12 +603,11 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// integrate at least twice to get position information
// otherwise factor cov noise from preint_cov is not positive definite
pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
@ -690,10 +691,9 @@ TEST(ImuFactor, PredictArbitrary) {
Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma,
kZeroBias, kGravityAlongNavZDown);
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
//
// ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
// PreintegratedImuMeasurements pim = runner.integrate(T);
// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
//
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -703,12 +703,12 @@ TEST(ImuFactor, PredictArbitrary) {
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
// Measurements
Vector3 measuredOmega = runner.actual_omega_b(0);
Vector3 measuredAcc = runner.actual_specific_force_b(0);
Vector3 measuredOmega = runner.actualAngularVelocity(0);
Vector3 measuredAcc = runner.actualSpecificForce(0);
auto p = defaultParams();
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
ImuFactor::PreintegratedMeasurements pim(p, biasHat);
PreintegratedImuMeasurements pim(p, biasHat);
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
// Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
@ -738,10 +738,12 @@ TEST(ImuFactor, PredictArbitrary) {
TEST(ImuFactor, bodyPSensorNoBias) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
auto p = defaultParams();
p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
// Measurements
Vector3 n_gravity(0, 0, -kGravity); // z-up nav frame
Vector3 omegaCoriolis(0, 0, 0);
// Sensor frame is z-down
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10);
// Acc measurement is acceleration of sensor in the sensor frame, when stationary,
@ -749,28 +751,22 @@ TEST(ImuFactor, bodyPSensorNoBias) {
Vector3 s_accMeas(0, 0, -kGravity);
double dt = 0.001;
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true);
PreintegratedImuMeasurements pim(p, bias);
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor);
pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
// Predict
Pose3 x1;
Vector3 v1(0, 0, 0);
PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity,
omegaCoriolis);
NavState actual = pim.predict(NavState(), bias);
Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
EXPECT(assert_equal(expectedPose, actual.pose()));
Vector3 expectedVelocity(0, 0, 0);
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
}
/* ************************************************************************* */
@ -791,9 +787,6 @@ TEST(ImuFactor, bodyPSensorWithBias) {
SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
// Measurements
Vector3 n_gravity(0, 0, -kGravity);
Vector3 omegaCoriolis(0, 0, 0);
// Sensor frame is z-down
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
Vector3 measuredOmega(0, 0.01, 0);
@ -801,11 +794,12 @@ TEST(ImuFactor, bodyPSensorWithBias) {
// table exerts an equal and opposite force w.r.t gravity
Vector3 measuredAcc(0, 0, -kGravity);
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
Matrix3 accCov = 1e-7 * I_3x3;
Matrix3 gyroCov = 1e-8 * I_3x3;
Matrix3 integrationCov = 1e-9 * I_3x3;
auto p = defaultParams();
p->n_gravity = Vector3(0, 0, -kGravity);
p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3());
p->accelerometerCovariance = 1e-7 * I_3x3;
p->gyroscopeCovariance = 1e-8 * I_3x3;
p->integrationCovariance = 1e-9 * I_3x3;
double deltaT = 0.005;
// Specify noise values on priors
@ -842,17 +836,13 @@ TEST(ImuFactor, bodyPSensorWithBias) {
// Now add IMU factors and bias noise models
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
for (int i = 1; i < numFactors; i++) {
ImuFactor::PreintegratedMeasurements pim =
ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov,
integrationCov, true);
PreintegratedImuMeasurements pim =
PreintegratedImuMeasurements(p, priorBias);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
body_P_sensor);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factors
graph.add(
ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity,
omegaCoriolis));
graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim));
graph.add(BetweenFactor<Bias>(B(i - 1), B(i), zeroBias, biasNoiseModel));
values.insert(X(i), Pose3());

3
gtsam/navigation/tests/testPoseVelocityBias.cpp
View File

@ -25,6 +25,8 @@
using namespace std;
using namespace gtsam;
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
Matrix3 R1 = pvb1.pose.rotation().matrix();
@ -55,6 +57,7 @@ TEST(PoseVelocityBias, error) {
expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3;
EXPECT(assert_equal(expected, actual, 1e-9));
}
#endif
/* ************************************************************************************************/
int main() {

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

@ -51,7 +51,7 @@ TEST(ScenarioRunner, Forward) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
PreintegratedImuMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -64,7 +64,7 @@ TEST(ScenarioRunner, ForwardWithBias) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias);
PreintegratedImuMeasurements pim = runner.integrate(T, kNonZeroBias);
Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
}
@ -78,7 +78,7 @@ TEST(ScenarioRunner, Circle) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
PreintegratedImuMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -95,7 +95,7 @@ TEST(ScenarioRunner, Loop) {
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
const double T = 0.1; // seconds
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
PreintegratedImuMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -126,7 +126,7 @@ TEST(ScenarioRunner, Accelerating) {
using namespace accelerating;
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
PreintegratedImuMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
@ -140,7 +140,7 @@ TEST(ScenarioRunner, Accelerating) {
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias);
//
// ImuFactor::PreintegratedMeasurements pim = runner.integrate(T,
// PreintegratedImuMeasurements pim = runner.integrate(T,
// kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
// kNonZeroBias);
@ -157,7 +157,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
const double T = 3; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
PreintegratedImuMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);