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

Gravity should be specified in NAV coordinates! Default Nav frame is assumed to be *Z down* for the old-style constructors.

release/4.3a0
dellaert 2015-07-24 13:22:32 +02:00
parent c6b68e6795
commit 323ed5220b
10 changed files with 114 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -148,7 +148,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedFRD();
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
@ -260,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity,
const CombinedPreintegratedMeasurements& 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,
@ -268,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor(
_PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->b_gravity = b_gravity;
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@ -279,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& b_gravity,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose;
vel_j = pvb.velocity;

20
gtsam/navigation/CombinedImuFactor.h
View File

@ -66,19 +66,19 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
/// See two named constructors below for good values of b_gravity in body frame
Params(const Vector3& b_gravity) :
PreintegrationBase::Params(b_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
/// See two named constructors below for good values of n_gravity in body frame
Params(const Vector3& n_gravity) :
PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInit(I_6x6) {
}
// Default Params for Front-Right-Down convention: b_gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, g));
}
// Default Params for Front-Left-Up convention: b_gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
@ -151,7 +151,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
Matrix preintMeasCov() const { return preintMeasCov_; }
/// deprecated constructor
/// NOTE(frank): assumes FRD convention, only second order integration supported
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
@ -263,7 +263,7 @@ public:
/// @deprecated constructor
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j, const CombinedPreintegratedMeasurements& pim,
const Vector3& b_gravity, const Vector3& omegaCoriolis,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
@ -271,7 +271,7 @@ public:
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& b_gravity, const Vector3& omegaCoriolis,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis = false);
private:

10
gtsam/navigation/ImuFactor.cpp
View File

@ -116,7 +116,7 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedFRD();
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
@ -171,14 +171,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& pim, const Vector3& b_gravity,
const PreintegratedMeasurements& 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());
p->b_gravity = b_gravity;
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@ -188,10 +188,10 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
//------------------------------------------------------------------------------
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& b_gravity,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose;
vel_j = pvb.velocity;

6
gtsam/navigation/ImuFactor.h
View File

@ -108,7 +108,7 @@ public:
Matrix preintMeasCov() const { return preintMeasCov_; }
/// @deprecated constructor
/// NOTE(frank): assumes FRD convention, only second order integration supported
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
@ -210,14 +210,14 @@ public:
/// @deprecated constructor
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& b_gravity, const Vector3& omegaCoriolis,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
/// @deprecated use PreintegrationBase::predict
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& b_gravity,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:

32
gtsam/navigation/NavState.cpp
View File

@ -248,7 +248,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
}
//------------------------------------------------------------------------------
Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
Vector9 NavState::correctPIM(const Vector9& pim, double dt,
const Vector3& n_gravity,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
const Rot3& nRb = R_;
@ -256,11 +257,12 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
const double dt2 = dt * dt;
Vector9 delta;
Matrix3 D_dP_Ri, D_dP_nv;
Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
dR(delta) = dR(pim);
dP(delta) = dP(pim)
+ dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0);
dV(delta) = dV(pim);
+ dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
+ (0.5 * dt2) * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
dV(delta) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
if (omegaCoriolis) {
delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
@ -272,8 +274,9 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
if (H1) {
if (!omegaCoriolis)
H1->setZero(); // if coriolis H1 is already initialized
D_t_R(H1) += dt * D_dP_Ri;
D_t_R(H1) += dt * D_dP_Ri1 + (0.5 * dt2) * D_dP_Ri2;
D_t_v(H1) += dt * D_dP_nv * Ri;
D_v_R(H1) += dt * D_dV_Ri;
}
if (H2) {
H2->setIdentity();
@ -283,24 +286,5 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
return delta;
}
//------------------------------------------------------------------------------
NavState NavState::predict(const Vector9& pim, double dt,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
Matrix9 D_delta_state, D_delta_pim;
Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis,
H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
Matrix9 D_predicted_state, D_predicted_delta;
NavState predicted = retract(delta, H1 ? &D_predicted_state : 0,
H1 || H2 ? &D_predicted_delta : 0);
// TODO(frank): the derivatives of retract are very sparse: inline below:
if (H1)
*H1 = D_predicted_state + D_predicted_delta * D_delta_state;
if (H2)
*H2 = D_predicted_delta * D_delta_pim;
return predicted;
}
//------------------------------------------------------------------------------
}/// namespace gtsam

14
gtsam/navigation/NavState.h
View File

@ -210,21 +210,13 @@ public:
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
OptionalJacobian<9, 9> H = boost::none) const;
/// Integrate a tangent vector forwards on tangent space, taking into account
/// Coriolis forces if omegaCoriolis is given.
Vector9 integrateTangent(const Vector9& pim, double dt,
/// Correct preintegrated tangent vector with our velocity and rotated gravity,
/// taking into account Coriolis forces if omegaCoriolis is given.
Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
/// Integrate a tangent vector forwards on tangent space, taking into account
/// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState
NavState predict(const Vector9& pim, double dt,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
/// @}
private:
/// @{
/// serialization

11
gtsam/navigation/PreintegrationBase.cpp
View File

@ -157,15 +157,10 @@ NavState PreintegrationBase::predict(const NavState& state_i,
// integrate on tangent space
Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_,
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
// Correct for gravity
double dt = deltaTij_, dt2 = dt * dt;
NavState::dP(xi) += 0.5 * p().b_gravity * dt2;
NavState::dV(xi) += p().b_gravity * dt;
// Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
@ -312,11 +307,11 @@ 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& b_gravity, const Vector3& omegaCoriolis,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// 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->b_gravity = b_gravity;
q->n_gravity = n_gravity;
q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q;

20
gtsam/navigation/PreintegrationBase.h
View File

@ -63,22 +63,22 @@ public:
/// (to compensate errors in Euler integration)
/// (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 b_gravity; ///< Gravity constant in body frame
Vector3 n_gravity; ///< Gravity constant in body frame
/// The Params constructor insists on the user passing in gravity in the *body* frame.
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
Params(const Vector3& b_gravity) :
Params(const Vector3& n_gravity) :
accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
false), b_gravity(b_gravity) {
false), n_gravity(n_gravity) {
}
// Default Params for Front-Right-Down convention: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, g));
}
// Default Params for Front-Left-Up convention: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
@ -94,7 +94,7 @@ public:
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(b_gravity);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
};
@ -201,7 +201,7 @@ public:
/// @deprecated predict
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& b_gravity,
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:

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

@ -36,7 +36,7 @@ using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
static const Vector3 kGravity(0, 0, 9.81);
static const Vector3 kGravityAlongNavZDown(0, 0, 9.81);
static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
@ -151,8 +151,7 @@ TEST(ImuFactor, StraightLine) {
imuBias::ConstantBias biasHat, bias;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::Params::MakeSharedFRD();
p->b_gravity = Vector3(0, 0, g); // FRD convention
PreintegratedImuMeasurements::Params::MakeSharedU(g); // Up convention!
PreintegratedImuMeasurements pim(p, biasHat);
for (size_t i = 0; i < 10; i++)
@ -171,7 +170,6 @@ TEST(ImuFactor, StraightLine) {
// Check prediction, note we move along x in body, along y in nav
NavState expected(nRb, Point3(10, 20 + a * exact, 0),
Velocity3(0, a * dt, 0));
GTSAM_PRINT(pim);
EXPECT(assert_equal(expected, pim.predict(state1, bias)));
}
@ -197,12 +195,10 @@ TEST(ImuFactor, PreintegratedMeasurements) {
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(
assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij())));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij())));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij())));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9);
// Integrate again
Vector3 expectedDeltaP2;
@ -216,39 +212,38 @@ TEST(ImuFactor, PreintegratedMeasurements) {
PreintegratedImuMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(
assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij())));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij())));
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij())));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9);
}
/* ************************************************************************* */
// Common linearization point and measurements for tests
namespace common {
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
NavState state1(x1, v1);
Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
NavState state2(x2, v2);
static const imuBias::ConstantBias biasHat, bias; // Bias
static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, 0));
static const Vector3 v1(Vector3(0.5, 0.0, 0.0));
static const NavState state1(x1, v1);
// Measurements
Vector3 measuredOmega(M_PI / 100, 0, 0);
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector();
double deltaT = 1.0;
static const double w = M_PI / 100;
static const Vector3 measuredOmega(w, 0, 0);
static const Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown);
static const double deltaT = 1.0;
static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0),
Point3(5.5, 1.0, 0));
static const Vector3 v2(Vector3(0.5, 0.0, 0.0));
static const NavState state2(x2, v2);
} // namespace common
/* ************************************************************************* */
TEST(ImuFactor, PreintegrationBaseMethods) {
using namespace common;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::Params::MakeSharedFRD();
PreintegratedImuMeasurements::Params::MakeSharedD();
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
p->accelerometerCovariance = kMeasuredAccCovariance;
@ -285,22 +280,21 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
/* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobians) {
using namespace common;
PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6));
EXPECT(assert_equal(state2, pim.predict(state1, bias)));
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kZeroOmegaCoriolis);
// Expected error
Vector errorExpected(9);
errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
Vector expectedError(9);
expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(
assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias),
1e-6));
assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, bias)));
Values values;
values.insert(X(1), x1);
@ -308,7 +302,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
values.insert(X(2), x2);
values.insert(V(2), v2);
values.insert(B(1), bias);
EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
// Make sure linearization is correct
double diffDelta = 1e-5;
@ -331,17 +325,17 @@ TEST(ImuFactor, ErrorAndJacobians) {
// Evaluate error with wrong values
Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
values.update(V(2), v2_wrong);
errorExpected << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901;
expectedError << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901;
EXPECT(
assert_equal(errorExpected,
factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));
EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
assert_equal(expectedError,
factor.evaluateError(x1, v1, x2, v2_wrong, bias)));
EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
// Make sure the whitening is done correctly
Matrix cov = pim.preintMeasCov();
Matrix R = RtR(cov.inverse());
Vector whitened = R * errorExpected;
EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6));
Vector whitened = R * expectedError;
EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values),1e-5));
// Make sure linearization is correct
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-5);
@ -359,7 +353,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
@ -370,7 +364,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis);
Values values;
@ -397,7 +391,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
@ -410,7 +404,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
// Create factor
Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
Values values;
@ -808,7 +802,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
@ -823,7 +817,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis);
Values values;
@ -858,13 +852,13 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kZeroOmegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravity,
PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravityAlongNavZDown,
kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity;
@ -893,14 +887,14 @@ TEST(ImuFactor, PredictRotation) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kZeroOmegaCoriolis);
// Predict
Pose3 x1, x2;
Vector3 v1 = Vector3(0, 0.0, 0.0);
Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis);
ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 0, 0;
@ -926,14 +920,14 @@ TEST(ImuFactor, PredictArbitrary) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kZeroOmegaCoriolis);
// Predict
Pose3 x1, x2;
Vector3 v1 = Vector3(0, 0.0, 0.0);
Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis);
ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis);
// Regression test for Imu Refactor
Rot3 expectedR( //

48
gtsam/navigation/tests/testNavState.cpp
View File

@ -149,55 +149,41 @@ boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
&NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
TEST(NavState, Coriolis) {
Matrix9 actualH;
Matrix9 aH;
// first-order
kState1.coriolis(dt, kOmegaCoriolis, false, actualH);
EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), actualH));
kState1.coriolis(dt, kOmegaCoriolis, false, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH));
// second-order
kState1.coriolis(dt, kOmegaCoriolis, true, actualH);
EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), actualH));
kState1.coriolis(dt, kOmegaCoriolis, true, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH));
}
TEST(NavState, Coriolis2) {
Matrix9 actualH;
Matrix9 aH;
NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
// first-order
state2.coriolis(dt, kOmegaCoriolis, false, actualH);
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), actualH));
state2.coriolis(dt, kOmegaCoriolis, false, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH));
// second-order
state2.coriolis(dt, kOmegaCoriolis, true, actualH);
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), actualH));
state2.coriolis(dt, kOmegaCoriolis, true, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
}
/* ************************************************************************* */
TEST(NavState, PredictXi) {
TEST(NavState, correctPIM) {
Vector9 xi;
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
double dt = 0.5;
Matrix9 actualH1, actualH2;
boost::function<Vector9(const NavState&, const Vector9&)> integrateTangent =
boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis,
Matrix9 aH1, aH2;
boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1));
EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2));
}
/* ************************************************************************* */
TEST(NavState, Predict) {
Vector9 xi;
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
double dt = 0.5;
Matrix9 actualH1, actualH2;
boost::function<NavState(const NavState&, const Vector9&)> predict =
boost::bind(&NavState::predict, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
kState1.predict(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
EXPECT(assert_equal(numericalDerivative21(predict, kState1, xi), actualH1));
EXPECT(assert_equal(numericalDerivative22(predict, kState1, xi), actualH2));
kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
}
/* ************************************************************************* */