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

No more second-order integration flag

release/4.3a0
dellaert 2015-07-23 17:00:07 +02:00
parent 0bb73bae9e
commit c6b68e6795
10 changed files with 250 additions and 272 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -145,15 +145,16 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
const bool use2ndOrderIntegration) { const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat; biasHat_ = biasHat;
boost::shared_ptr<Params> p = boost::make_shared<Params>(); boost::shared_ptr<Params> p = Params::MakeSharedFRD();
p->gyroscopeCovariance = measuredOmegaCovariance; p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance; p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance; p->integrationCovariance = integrationErrorCovariance;
p->biasAccCovariance = biasAccCovariance; p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = biasOmegaCovariance; p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInit = biasAccOmegaInit; p->biasAccOmegaInit = biasAccOmegaInit;
p->use2ndOrderIntegration = use2ndOrderIntegration;
p_ = p; p_ = p;
resetIntegration(); resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
@ -259,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor( CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& pim, const Vector3& gravity, const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor, const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis) const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
@ -267,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor(
_PIM_(pim) { _PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p = boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p()); boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->gravity = gravity; p->b_gravity = b_gravity;
p->omegaCoriolis = omegaCoriolis; p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor; p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis; p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@ -278,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim, CombinedPreintegratedMeasurements& pim,
const Vector3& gravity, const Vector3& b_gravity,
const Vector3& omegaCoriolis, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) { const bool use2ndOrderCoriolis) {
// use deprecated predict // use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity, PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
omegaCoriolis, use2ndOrderCoriolis); omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose; pose_j = pvb.pose;
vel_j = pvb.velocity; vel_j = pvb.velocity;

26
gtsam/navigation/CombinedImuFactor.h
View File

@ -66,9 +66,26 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
Params():biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {} /// 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(
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) {
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) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
private: private:
/// Default constructor makes unitialized params struct
Params() {}
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
@ -134,12 +151,13 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
Matrix preintMeasCov() const { return preintMeasCov_; } Matrix preintMeasCov() const { return preintMeasCov_; }
/// deprecated constructor /// deprecated constructor
/// NOTE(frank): assumes FRD convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = false); const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
private: private:
/// Serialization function /// Serialization function
@ -245,7 +263,7 @@ public:
/// @deprecated constructor /// @deprecated constructor
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j, const CombinedPreintegratedMeasurements& pim, Key bias_j, const CombinedPreintegratedMeasurements& pim,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& b_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none, const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false); const bool use2ndOrderCoriolis = false);
@ -253,7 +271,7 @@ public:
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim, CombinedPreintegratedMeasurements& pim,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& b_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis = false); const bool use2ndOrderCoriolis = false);
private: private:

88
gtsam/navigation/ImuFactor.cpp
View File

@ -38,8 +38,8 @@ void PreintegratedImuMeasurements::print(const string& s) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegratedImuMeasurements::equals( bool PreintegratedImuMeasurements::equals(
const PreintegratedImuMeasurements& other, double tol) const { const PreintegratedImuMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) && return PreintegrationBase::equals(other, tol)
equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -51,8 +51,7 @@ void PreintegratedImuMeasurements::resetIntegration() {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurement( void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
@ -60,16 +59,17 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// rotation increment computed from the current rotation rate measurement // rotation increment computed from the current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT; const Vector3 integratedOmega = correctedOmega * deltaT;
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
// rotation increment computed from the current rotation rate measurement // rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega);
// Update Jacobians // Update Jacobians
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
// Update preintegrated measurements (also get Jacobian) // Update preintegrated measurements (also get Jacobian)
const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
// first order covariance propagation: // first order covariance propagation:
@ -88,14 +88,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
* p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT; * p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc; Matrix3 F_pos_noiseacc;
if (p().use2ndOrderIntegration) { F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc * p().accelerometerCovariance * F_pos_noiseacc.transpose();
* p().accelerometerCovariance * F_pos_noiseacc.transpose(); Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance
Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose(); // has 1/deltaT * dRij.transpose(); // has 1/deltaT
preintMeasCov_.block<3, 3>(0, 3) += temp; preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
}
// F_test and G_test are given as output for testing purposes and are not needed by the factor // F_test and G_test are given as output for testing purposes and are not needed by the factor
if (F_test) { if (F_test) {
@ -103,13 +102,10 @@ void PreintegratedImuMeasurements::integrateMeasurement(
} }
if (G_test) { if (G_test) {
// This in only for testing & documentation, while the actual computation is done block-wise // This in only for testing & documentation, while the actual computation is done block-wise
if (!p().use2ndOrderIntegration)
F_pos_noiseacc = Z_3x3;
// intNoise accNoise omegaNoise // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, dRij * deltaT, Z_3x3, // vel Z_3x3, dRij * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
} }
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -117,12 +113,13 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) { const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat; biasHat_ = biasHat;
boost::shared_ptr<Params> p = boost::make_shared<Params>(); boost::shared_ptr<Params> p = Params::MakeSharedFRD();
p->gyroscopeCovariance = measuredOmegaCovariance; p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance; p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance; p->integrationCovariance = integrationErrorCovariance;
p->use2ndOrderIntegration = use2ndOrderIntegration;
p_ = p; p_ = p;
resetIntegration(); resetIntegration();
} }
@ -131,10 +128,10 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
// ImuFactor methods // ImuFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedImuMeasurements& pim) const PreintegratedImuMeasurements& pim) :
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), pose_j, vel_j, bias), _PIM_(pim) {
_PIM_(pim) {} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@ -151,7 +148,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
Base::print(""); Base::print("");
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
// Print standard deviations on covariance only // Print standard deviations on covariance only
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose()
<< endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -168,21 +166,19 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3, boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const { boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1, H2, H3, H4, H5); H1, H2, H3, H4, H5);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& pim, const PreintegratedMeasurements& pim, const Vector3& b_gravity,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const boost::optional<Pose3>& body_P_sensor, const bool use2ndOrderCoriolis) :
const bool use2ndOrderCoriolis) Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), _PIM_(pim) {
pose_j, vel_j, bias), boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
_PIM_(pim) { PreintegratedMeasurements::Params>(pim.p());
boost::shared_ptr<PreintegratedMeasurements::Params> p = p->b_gravity = b_gravity;
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->gravity = gravity;
p->omegaCoriolis = omegaCoriolis; p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor; p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis; p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@ -191,16 +187,14 @@ 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, void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const imuBias::ConstantBias& bias_i, PreintegratedMeasurements& pim, const Vector3& b_gravity,
PreintegratedMeasurements& pim, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
const Vector3& gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// use deprecated predict // use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity, PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
omegaCoriolis, use2ndOrderCoriolis); omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose; pose_j = pvb.pose;
vel_j = pvb.velocity; vel_j = pvb.velocity;
} }
} // namespace gtsam } // namespace gtsam

7
gtsam/navigation/ImuFactor.h
View File

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

39
gtsam/navigation/NavState.cpp
View File

@ -248,25 +248,19 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 NavState::predictXi(const Vector9& pim, double dt, Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
const Vector3& gravity, const boost::optional<Vector3>& omegaCoriolis, const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
OptionalJacobian<9, 9> H2) const {
const Rot3& nRb = R_; const Rot3& nRb = R_;
const Velocity3& n_v = v_; // derivative is Ri ! const Velocity3& n_v = v_; // derivative is Ri !
const double dt2 = dt * dt; const double dt2 = dt * dt;
Vector9 delta; Vector9 delta;
Matrix3 D_dP_Ri, D_dP_dP, D_dV_Ri, D_dV_dV; Matrix3 D_dP_Ri, D_dP_nv;
dR(delta) = dR(pim); dR(delta) = dR(pim);
// TODO(frank): dP(delta) = dP(pim)
// - why do we integrate n_v here ? + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0);
// - the dP and dV should be in body ! How come semi-retract works out ?? dV(delta) = dV(pim);
// - should we rename t_ to p_? if not, we should rename dP do dT
dP(delta) = nRb.rotate(dP(pim), H1 ? &D_dP_Ri : 0, H2 ? &D_dP_dP : 0)
+ n_v * dt + 0.5 * gravity * dt2;
dV(delta) = nRb.rotate(dV(pim), H1 ? &D_dV_Ri : 0, H2 ? &D_dV_dV : 0)
+ gravity * dt;
if (omegaCoriolis) { if (omegaCoriolis) {
delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
@ -278,15 +272,11 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt,
if (H1) { if (H1) {
if (!omegaCoriolis) if (!omegaCoriolis)
H1->setZero(); // if coriolis H1 is already initialized H1->setZero(); // if coriolis H1 is already initialized
D_t_R(H1) += D_dP_Ri; D_t_R(H1) += dt * D_dP_Ri;
D_t_v(H1) += I_3x3 * dt * Ri; D_t_v(H1) += dt * D_dP_nv * Ri;
D_v_R(H1) += D_dV_Ri;
} }
if (H2) { if (H2) {
H2->setZero(); H2->setIdentity();
D_R_R(H2) << I_3x3;
D_t_t(H2) << D_dP_dP;
D_v_v(H2) << D_dV_dV;
} }
} }
@ -294,13 +284,12 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::predict(const Vector9& pim, double dt, NavState NavState::predict(const Vector9& pim, double dt,
const Vector3& gravity, const boost::optional<Vector3>& omegaCoriolis, const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
OptionalJacobian<9, 9> H2) const {
Matrix9 D_delta_state, D_delta_pim; Matrix9 D_delta_state, D_delta_pim;
Vector9 delta = predictXi(pim, dt, gravity, omegaCoriolis, Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis,
use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0); H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
Matrix9 D_predicted_state, D_predicted_delta; Matrix9 D_predicted_state, D_predicted_delta;
NavState predicted = retract(delta, H1 ? &D_predicted_state : 0, NavState predicted = retract(delta, H1 ? &D_predicted_state : 0,

15
gtsam/navigation/NavState.h
View File

@ -33,6 +33,9 @@ typedef Vector3 Velocity3;
*/ */
class NavState: public LieGroup<NavState, 9> { class NavState: public LieGroup<NavState, 9> {
private: private:
// TODO(frank):
// - should we rename t_ to p_? if not, we should rename dP do dT
Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
Point3 t_; ///< position n_t, in nav frame Point3 t_; ///< position n_t, in nav frame
Velocity3 v_; ///< velocity n_v in nav frame Velocity3 v_; ///< velocity n_v in nav frame
@ -207,16 +210,16 @@ public:
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
OptionalJacobian<9, 9> H = boost::none) const; OptionalJacobian<9, 9> H = boost::none) const;
/// Combine preintegrated measurements, in the form of a tangent space vector, /// Integrate a tangent vector forwards on tangent space, taking into account
/// with gravity, velocity, and Coriolis forces into a tangent space update. /// Coriolis forces if omegaCoriolis is given.
Vector9 predictXi(const Vector9& pim, double dt, const Vector3& gravity, Vector9 integrateTangent(const Vector9& pim, double dt,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis = const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none, false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const; OptionalJacobian<9, 9> H2 = boost::none) const;
/// Combine preintegrated measurements, in the form of a tangent space vector, /// Integrate a tangent vector forwards on tangent space, taking into account
/// with gravity, velocity, and Coriolis forces into a new state. /// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState
NavState predict(const Vector9& pim, double dt, const Vector3& gravity, NavState predict(const Vector9& pim, double dt,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis = const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none, false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const; OptionalJacobian<9, 9> H2 = boost::none) const;

49
gtsam/navigation/PreintegrationBase.cpp
View File

@ -64,14 +64,11 @@ void PreintegrationBase::updatePreintegratedMeasurements(
OptionalJacobian<9, 9> F) { OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij_.matrix(); // expensive const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Vector3 temp = dRij * correctedAcc * deltaT; const Vector3 j_acc = dRij * correctedAcc; // acceleration in current frame
if (!p().use2ndOrderIntegration) { double dt22 = 0.5 * deltaT * deltaT;
deltaPij_ += deltaVij_ * deltaT; deltaPij_ += deltaVij_ * deltaT + dt22 * j_acc;
} else { deltaVij_ += deltaT * j_acc;
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
}
deltaVij_ += temp;
Matrix3 R_i, F_angles_angles; Matrix3 R_i, F_angles_angles;
if (F) if (F)
@ -81,10 +78,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
if (F) { if (F) {
const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles; Matrix3 F_pos_angles;
if (p().use2ndOrderIntegration) F_pos_angles = 0.5 * F_vel_angles * deltaT;
F_pos_angles = 0.5 * F_vel_angles * deltaT;
else
F_pos_angles = Z_3x3;
// pos vel angle // pos vel angle
*F << // *F << //
@ -101,13 +95,8 @@ void PreintegrationBase::updatePreintegratedJacobians(
const Matrix3 dRij = deltaRij_.matrix(); // expensive const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
* delRdelBiasOmega_; * delRdelBiasOmega_;
if (!p().use2ndOrderIntegration) { delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
} else {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
}
delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp; delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
@ -135,10 +124,9 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta( Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_; const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_deltaRij_bias; Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope());
Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij(
biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
Vector9 xi; Vector9 xi;
Matrix3 D_dR_deltaRij; Matrix3 D_dR_deltaRij;
@ -147,9 +135,10 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
+ delPdelBiasOmega_ * biasIncr.gyroscope(); + delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer() NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope(); + delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) { if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
D_dR_bias << Z_3x3, D_dR_deltaRij * D_deltaRij_bias; D_dR_bias << Z_3x3, D_dR_deltaRij * delRdelBiasOmega_;
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias; (*H) << D_dR_bias, D_dP_bias, D_dV_bias;
@ -161,13 +150,23 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
NavState PreintegrationBase::predict(const NavState& state_i, NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const { OptionalJacobian<9, 6> H2) const {
// correct for bias
Matrix96 D_biasCorrected_bias; Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = biasCorrectedDelta(bias_i, Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0); H2 ? &D_biasCorrected_bias : 0);
// integrate on tangent space
Matrix9 D_delta_state, D_delta_biasCorrected; Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity, Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_,
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 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; Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1) if (H1)
@ -313,11 +312,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& b_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) { const bool use2ndOrderCoriolis) {
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
boost::shared_ptr<Params> q = boost::make_shared<Params>(p()); boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
q->gravity = gravity; q->b_gravity = b_gravity;
q->omegaCoriolis = omegaCoriolis; q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis; q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q; p_ = q;

154
gtsam/navigation/PreintegrationBase.h
View File

@ -24,6 +24,7 @@
#include <gtsam/navigation/PreintegratedRotation.h> #include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/navigation/NavState.h> #include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
@ -32,11 +33,16 @@ struct PoseVelocityBias {
Pose3 pose; Pose3 pose;
Vector3 velocity; Vector3 velocity;
imuBias::ConstantBias bias; imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
: pose(_pose), velocity(_velocity), bias(_bias) {} const imuBias::ConstantBias _bias) :
PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) pose(_pose), velocity(_velocity), bias(_bias) {
: pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {} }
NavState navState() const { return NavState(pose,velocity);} PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) :
pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {
}
NavState navState() const {
return NavState(pose, velocity);
}
}; };
/** /**
@ -45,29 +51,41 @@ struct PoseVelocityBias {
* It includes the definitions of the preintegrated variables and the methods * It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them. * to access, print, and compare them.
*/ */
class PreintegrationBase : public PreintegratedRotation { class PreintegrationBase: public PreintegratedRotation {
public: public:
/// Parameters for pre-integration: /// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor /// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params : PreintegratedRotation::Params { struct Params: PreintegratedRotation::Params {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
/// (to compensate errors in Euler integration) /// (to compensate errors in Euler integration)
bool use2ndOrderIntegration; ///< Controls the order of 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) /// (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 bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 gravity; ///< Gravity constant Vector3 b_gravity; ///< Gravity constant in body frame
Params() /// The Params constructor insists on the user passing in gravity in the *body* frame.
: accelerometerCovariance(I_3x3), /// For convenience, two commonly used conventions are provided by named constructors below
integrationCovariance(I_3x3), Params(const Vector3& b_gravity) :
use2ndOrderIntegration(false), accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
use2ndOrderCoriolis(false), false), b_gravity(b_gravity) {
gravity(0, 0, 9.8) {} }
// Default Params for Front-Right-Down convention: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedFRD(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) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
protected:
/// Default constructor for serialization only: uninitialized!
Params();
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -75,29 +93,29 @@ class PreintegrationBase : public PreintegratedRotation {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance); ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderIntegration);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(gravity); ar & BOOST_SERIALIZATION_NVP(b_gravity);
} }
}; };
protected: protected:
/// Acceleration and gyro bias used for preintegration /// Acceleration and gyro bias used for preintegration
imuBias::ConstantBias biasHat_; imuBias::ConstantBias biasHat_;
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
/// Default constructor for serialization /// Default constructor for serialization
PreintegrationBase() {} PreintegrationBase() {
}
public: public:
/** /**
* Constructor, initializes the variables in the base class * Constructor, initializes the variables in the base class
@ -105,27 +123,45 @@ class PreintegrationBase : public PreintegratedRotation {
* @param p Parameters, typically fixed in a single application * @param p Parameters, typically fixed in a single application
*/ */
PreintegrationBase(const boost::shared_ptr<const Params>& p, PreintegrationBase(const boost::shared_ptr<const Params>& p,
const imuBias::ConstantBias& biasHat) const imuBias::ConstantBias& biasHat) :
: PreintegratedRotation(p), biasHat_(biasHat) { PreintegratedRotation(p), biasHat_(biasHat) {
resetIntegration(); resetIntegration();
} }
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(); void resetIntegration();
const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);} const Params& p() const {
return *boost::static_pointer_cast<const Params>(p_);
}
/// getters /// getters
const imuBias::ConstantBias& biasHat() const { return biasHat_; } const imuBias::ConstantBias& biasHat() const {
const Vector3& deltaPij() const { return deltaPij_; } return biasHat_;
const Vector3& deltaVij() const { return deltaVij_; } }
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } const Vector3& deltaPij() const {
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; } return deltaPij_;
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; } }
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } const Vector3& deltaVij() const {
return deltaVij_;
}
const Matrix3& delPdelBiasAcc() const {
return delPdelBiasAcc_;
}
const Matrix3& delPdelBiasOmega() const {
return delPdelBiasOmega_;
}
const Matrix3& delVdelBiasAcc() const {
return delVdelBiasAcc_;
}
const Matrix3& delVdelBiasOmega() const {
return delVdelBiasOmega_;
}
// Exposed for MATLAB // Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); } Vector6 biasHatVector() const {
return biasHat_.vector();
}
/// print /// print
void print(const std::string& s) const; void print(const std::string& s) const;
@ -134,18 +170,16 @@ class PreintegrationBase : public PreintegratedRotation {
bool equals(const PreintegrationBase& other, double tol) const; bool equals(const PreintegrationBase& other, double tol) const;
/// Update preintegrated measurements /// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const double deltaT, OptionalJacobian<9, 9> F); const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F);
/// Update Jacobians to be used during preintegration /// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc, void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT);
double deltaT);
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, const Vector3& measuredOmega, Vector3* correctedAcc,
Vector3* correctedAcc, Vector3* correctedOmega);
Vector3* correctedOmega);
/// Given the estimate of the bias, return a NavState tangent vector /// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far /// summarizing the preintegrated IMU measurements so far
@ -154,23 +188,23 @@ class PreintegrationBase : public PreintegratedRotation {
/// Predict state at time j /// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
boost::none) const;
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const Pose3& pose_j, const Vector3& vel_j,
OptionalJacobian<9, 6> H1 = boost::none, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
OptionalJacobian<9, 3> H2 = boost::none, boost::none, OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
OptionalJacobian<9, 3> H4 = boost::none, boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
OptionalJacobian<9, 6> H5 = boost::none) const;
/// @deprecated predict /// @deprecated predict
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, const imuBias::ConstantBias& bias_i, const Vector3& b_gravity,
const bool use2ndOrderCoriolis = false); const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -187,4 +221,4 @@ class PreintegrationBase : public PreintegratedRotation {
} }
}; };
} /// namespace gtsam } /// namespace gtsam

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

@ -57,15 +57,11 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, Vector updatePreintegratedPosVel(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const Vector3& correctedAcc, const Vector3& correctedOmega,
const double deltaT, const bool use2ndOrderIntegration_) { const double deltaT) {
Matrix3 dRij = deltaRij_old.matrix(); Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
Vector3 deltaPij_new; Vector3 deltaPij_new;
if (!use2ndOrderIntegration_) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
} else {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp; Vector3 deltaVij_new = deltaVij_old + temp;
Vector result(6); Vector result(6);
@ -93,11 +89,9 @@ const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
/* ************************************************************************* */ /* ************************************************************************* */
PreintegratedImuMeasurements evaluatePreintegratedMeasurements( PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const bool use2ndOrderIntegration = false) {
PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
use2ndOrderIntegration);
list<Vector3>::const_iterator itAcc = measuredAccs.begin(); list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -142,27 +136,22 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
} // namespace } // namespace
/* ************************************************************************* */ /* ************************************************************************* */
namespace straight { TEST(ImuFactor, StraightLine) {
// Set up IMU measurements // Set up IMU measurements
static const double g = 10; // make this easy to check static const double g = 10; // make this easy to check
static const double a = 0.2, dt = 3.0; static const double a = 0.2, dt = 3.0;
const double exact = dt * dt / 2; const double exact = dt * dt / 2;
Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
// Set up body pointing towards y axis, and start at 10,20,0 with zero velocity // Set up body pointing towards y axis, and start at 10,20,0 with zero velocity
// TODO(frank): clean up Rot3 mess // TODO(frank): clean up Rot3 mess
static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); 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 Point3 initial_position(10, 20, 0);
static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0)); static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0));
}
TEST(ImuFactor, StraightLineSecondOrder) {
using namespace straight;
imuBias::ConstantBias biasHat, bias; imuBias::ConstantBias biasHat, bias;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::Params::MakeSharedFRD(); PreintegratedImuMeasurements::Params::MakeSharedFRD();
p->use2ndOrderIntegration = true;
p->b_gravity = Vector3(0, 0, g); // FRD convention p->b_gravity = Vector3(0, 0, g); // FRD convention
PreintegratedImuMeasurements pim(p, biasHat); PreintegratedImuMeasurements pim(p, biasHat);
@ -186,39 +175,6 @@ TEST(ImuFactor, StraightLineSecondOrder) {
EXPECT(assert_equal(expected, pim.predict(state1, bias))); EXPECT(assert_equal(expected, pim.predict(state1, bias)));
} }
TEST(ImuFactor, StraightLineFirstOrder) {
using namespace straight;
imuBias::ConstantBias biasHat, bias;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::Params::MakeSharedFRD();
p->use2ndOrderIntegration = false;
p->b_gravity = Vector3(0, 0, g); // FRD convention
// We do a rough approximation:
PreintegratedImuMeasurements pim(p, biasHat);
for (size_t i = 0; i < 10; i++)
pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
// Check integrated quantities in body frame: gravity measured by IMU is integrated!
const double approx = exact * 0.9; // approximation for dt split into 10 intervals
EXPECT(assert_equal(Rot3(), pim.deltaRij()));
EXPECT(assert_equal(Vector3(a * approx, 0, -g * approx), pim.deltaPij()));
EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij())); // still correct
// Check bias-corrected delta: also in body frame
Vector9 expectedBC;
expectedBC << 0, 0, 0, a * approx, 0, -g * approx, a * dt, 0, -g * dt;
EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias)));
// Check prediction, note we move along x in body, along y in nav
// In this instance the position is just an approximation,
// but gravity should be subtracted out exactly
NavState expected(nRb, Point3(10, 20 + a * approx, 0), Velocity3(0, a * dt, 0));
GTSAM_PRINT(pim);
EXPECT(assert_equal(expected, pim.predict(state1, bias)));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) { TEST(ImuFactor, PreintegratedMeasurements) {
// Linearization point // Linearization point
@ -236,11 +192,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5); double expectedDeltaT1(0.5);
bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance, PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT( EXPECT(
@ -294,12 +248,11 @@ double deltaT = 1.0;
TEST(ImuFactor, PreintegrationBaseMethods) { TEST(ImuFactor, PreintegrationBaseMethods) {
using namespace common; using namespace common;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::Params::MakeSharedFRD(); PreintegratedImuMeasurements::Params::MakeSharedFRD();
p->gyroscopeCovariance = kMeasuredOmegaCovariance; p->gyroscopeCovariance = kMeasuredOmegaCovariance;
p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
p->accelerometerCovariance = kMeasuredAccCovariance; p->accelerometerCovariance = kMeasuredAccCovariance;
p->integrationCovariance = kIntegrationErrorCovariance; p->integrationCovariance = kIntegrationErrorCovariance;
p->use2ndOrderIntegration = false;
p->use2ndOrderCoriolis = true; p->use2ndOrderCoriolis = true;
PreintegratedImuMeasurements pim(p, bias); PreintegratedImuMeasurements pim(p, bias);
@ -332,10 +285,8 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobians) { TEST(ImuFactor, ErrorAndJacobians) {
using namespace common; using namespace common;
bool use2ndOrderIntegration = true;
PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance, PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
use2ndOrderIntegration);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6)); EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6));
@ -631,11 +582,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
} }
bool use2ndOrderIntegration = false;
// Actual preintegrated values // Actual preintegrated values
PreintegratedImuMeasurements preintegrated = PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration); deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
@ -658,20 +608,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Compute expected f_pos_vel wrt positions // Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
deltaPij_old);
// Compute expected f_pos_vel wrt velocities // Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
deltaVij_old);
// Compute expected f_pos_vel wrt angles // Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>( Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
deltaRij_old);
Matrix FexpectedTop6(6, 9); Matrix FexpectedTop6(6, 9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
@ -698,14 +645,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Compute jacobian wrt acc noise // Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT, deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise // Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
newMeasuredOmega);
Matrix GexpectedTop6(6, 9); Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
@ -754,11 +699,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
} }
bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
PreintegratedImuMeasurements preintegrated = PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration); deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
@ -781,20 +725,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Compute expected f_pos_vel wrt positions // Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
deltaPij_old);
// Compute expected f_pos_vel wrt velocities // Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
deltaVij_old);
// Compute expected f_pos_vel wrt angles // Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>( Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
deltaRij_old);
Matrix FexpectedTop6(6, 9); Matrix FexpectedTop6(6, 9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
@ -821,14 +762,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Compute jacobian wrt acc noise // Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT, deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise // Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
newMeasuredOmega);
Matrix GexpectedTop6(6, 9); Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;

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

@ -178,12 +178,12 @@ TEST(NavState, PredictXi) {
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
double dt = 0.5; double dt = 0.5;
Matrix9 actualH1, actualH2; Matrix9 actualH1, actualH2;
boost::function<Vector9(const NavState&, const Vector9&)> predictXi = boost::function<Vector9(const NavState&, const Vector9&)> integrateTangent =
boost::bind(&NavState::predictXi, _1, _2, dt, kGravity, kOmegaCoriolis, boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none); false, boost::none, boost::none);
kState1.predictXi(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
EXPECT(assert_equal(numericalDerivative21(predictXi, kState1, xi), actualH1)); EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1));
EXPECT(assert_equal(numericalDerivative22(predictXi, kState1, xi), actualH2)); EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */