From fa15264e83d0124fc58968f821ea0781c1e29d08 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 09:17:29 -0700 Subject: [PATCH 01/22] Add ifdefs that restore the old IMU factor functionality --- gtsam/navigation/CombinedImuFactor.cpp | 8 +- gtsam/navigation/ImuFactor.cpp | 11 +- gtsam/navigation/ImuFactor.h | 4 + gtsam/navigation/NavState.cpp | 2 +- gtsam/navigation/NavState.h | 2 +- gtsam/navigation/PreintegrationBase.cpp | 170 ++++++++++++++++-- gtsam/navigation/PreintegrationBase.h | 75 +++++++- .../tests/testCombinedImuFactor.cpp | 2 + gtsam/navigation/tests/testImuFactor.cpp | 11 +- .../tests/testPreintegrationBase.cpp | 2 + 10 files changed, 260 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a961a79bd..954ac9656 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -70,7 +70,13 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Update preintegrated measurements. Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); +#else + Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr + PreintegrationBase::update(measuredAcc, measuredOmega, dt, + &D_incrR_integratedOmega, &A, &B, &C); +#endif // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF @@ -79,7 +85,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - // TODO(frank): should we not also accout for bias on position? + // TODO(frank): should we not also account for bias on position? Matrix3 theta_H_biasOmega = - C.topRows<3>(); Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 6d36539e4..d1280688d 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -55,7 +55,13 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); +#else + Matrix3 D_incrR_integratedOmega; + PreintegrationBase::update(measuredAcc, measuredOmega, dt, + &D_incrR_integratedOmega, &A, &B, &C); +#endif // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a @@ -91,13 +97,14 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA } //------------------------------------------------------------------------------ +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // Matrix9* H1, Matrix9* H2) { PreintegrationBase::mergeWith(pim12, H1, H2); preintMeasCov_ = *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); } - +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PreintegratedImuMeasurements::PreintegratedImuMeasurements( @@ -174,6 +181,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { @@ -216,6 +224,7 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, f01->key5(), // B pim02); } +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 010550eb1..6f72ec9be 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -124,8 +124,10 @@ public: /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION /// Merge in a different set of measurements and update bias derivatives accordingly void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); +#endif #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated constructor @@ -230,6 +232,7 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION /// Merge two pre-integrated measurement classes static PreintegratedImuMeasurements Merge( const PreintegratedImuMeasurements& pim01, @@ -237,6 +240,7 @@ public: /// Merge two factors static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); +#endif #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index f01a55577..ee6135ede 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -239,7 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION) NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index eabd1f39b..27e083014 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,7 +203,7 @@ public: /// @name Dynamics /// @{ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION) /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c243ca860..f4951305e 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -37,15 +37,28 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, //------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION preintegrated_.setZero(); preintegrated_H_biasAcc_.setZero(); preintegrated_H_biasOmega_.setZero(); +#else + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +#endif } //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const PreintegrationBase& pim) { os << " deltaTij " << pim.deltaTij_ << endl; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION os << " deltaRij " << Point3(pim.theta()) << endl; +#else + os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl; +#endif os << " deltaPij " << Point3(pim.deltaPij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; @@ -64,9 +77,18 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); +#else + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +#endif } //------------------------------------------------------------------------------ @@ -105,7 +127,8 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( // Update derivative: centrifugal causes the correlation between acc and omega!!! if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() + const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal(); + *correctedAcc_H_unbiasedOmega = -( diag_wdp + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } @@ -114,6 +137,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION //------------------------------------------------------------------------------ // See extensive discussion in ImuFactor.lyx Vector9 PreintegrationBase::UpdatePreintegrated( @@ -224,29 +248,150 @@ Vector9 PreintegrationBase::biasCorrectedDelta( } return biasCorrected; } +#else + +//------------------------------------------------------------------------------ +pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& measuredAcc, const Vector3& measuredOmega, + OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, + OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, + OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { + + // Correct for bias in the sensor frame + Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega); + + return correctMeasurementsBySensorPose(unbiasedAcc, unbiasedOmega, + D_correctedAcc_measuredAcc, D_correctedAcc_measuredOmega, + D_correctedOmega_measuredOmega); +} + +//------------------------------------------------------------------------------ +NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, + OptionalJacobian<9, 9> D_updated_current, + OptionalJacobian<9, 3> D_updated_measuredAcc, + OptionalJacobian<9, 3> D_updated_measuredOmega) const { + + Vector3 correctedAcc, correctedOmega; + Matrix3 D_correctedAcc_measuredAcc, // + D_correctedAcc_measuredOmega, // + D_correctedOmega_measuredOmega; + bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + (needDerivs ? &D_correctedAcc_measuredAcc : 0), + (needDerivs ? &D_correctedAcc_measuredOmega : 0), + (needDerivs ? &D_correctedOmega_measuredOmega : 0)); + // Do update in one fell swoop + Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; + NavState updated = deltaXij_.update(correctedAcc, correctedOmega, dt, D_updated_current, + (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), + (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); + if (needDerivs) { + *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; + *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; + if (!p().body_P_sensor->translation().vector().isZero()) { + *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; + } + } + return updated; +} + +//------------------------------------------------------------------------------ +void PreintegrationBase::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, + Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, + D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + + // Update Jacobians + // TODO(frank): we are repeating some computation here: accessible in F ? + Vector3 correctedAcc, correctedOmega; + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); + + Matrix3 D_acc_R; + oldRij.rotate(correctedAcc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = correctedOmega * dt; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +#endif //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { // TODO(frank): make sure this stuff is still correct Matrix96 D_biasCorrected_bias; - Vector9 biasCorrected = - biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); + Vector9 biasCorrected = biasCorrectedDelta(bias_i, + H2 ? &D_biasCorrected_bias : 0); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, - H1 ? &D_delta_state : 0, - H2 ? &D_delta_biasCorrected : 0); + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); // 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); - if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state; - if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias; + if (H1) + *H1 = D_predict_state + D_predict_delta * D_delta_state; + if (H2) + *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; return state_j; } @@ -306,6 +451,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION //------------------------------------------------------------------------------ // sugar for derivative blocks #define D_R_R(H) (H)->block<3,3>(0,0) @@ -393,6 +539,7 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + (*H2) * pim12.preintegrated_H_biasOmega_; } +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -408,6 +555,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, p_ = q; return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); } + #endif //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9d751e92d..13a7461b4 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -81,10 +81,18 @@ class PreintegrationBase { * Note: relative position does not take into account velocity at time i, see deltap+, in [2] * Note: velocity is now also in frame i, as opposed to deltaVij in [2] */ +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION Vector9 preintegrated_; - Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias +#else + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate 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 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias +#endif /// Default constructor for serialization PreintegrationBase() { @@ -140,17 +148,22 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION const Vector9& preintegrated() const { return preintegrated_; } - - Vector3 theta() const { return preintegrated_.head<3>(); } - Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const { return preintegrated_.tail<3>(); } - - Rot3 deltaRij() const { return Rot3::Expmap(theta()); } + Vector3 theta() const { return preintegrated_.head<3>(); } + Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const { return Rot3::Expmap(theta()); } NavState deltaXij() const { return NavState::Retract(preintegrated_); } const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } +#else + const NavState& deltaXij() const { return deltaXij_; } + const Rot3& deltaRij() const { return deltaXij_.attitude(); } + Vector3 deltaPij() const { return deltaXij_.position().vector(); } + Vector3 deltaVij() const { return deltaXij_.velocity(); } +#endif // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -175,6 +188,8 @@ public: OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION + // Update integrated vector on tangent manifold preintegrated with acceleration // Static, functional version. static Vector9 UpdatePreintegrated(const Vector3& a_body, @@ -200,6 +215,38 @@ public: Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = boost::none) const; +#else + + /// Subtract estimate and correct for sensor pose + /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) + /// Ignore D_correctedOmega_measuredAcc as it is trivially zero + std::pair correctMeasurementsByBiasAndSensorPose( + const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, + OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, + OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; + + /// Calculate the updated preintegrated measurement, does not modify + /// It takes measured quantities in the j frame + NavState updatedDeltaXij(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + OptionalJacobian<9, 9> D_updated_current = boost::none, + OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, + OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, + Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const; + +#endif + /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1 = boost::none, @@ -219,6 +266,7 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, double deltaT12, @@ -229,13 +277,13 @@ public: /// The derivatives apply to the preintegrated Vector9 void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); /// @} +#endif /** Dummy clone for MATLAB */ virtual boost::shared_ptr clone() const { return boost::shared_ptr(); } - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ @@ -257,11 +305,22 @@ private: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_NVP(p_); +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); +#else + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); +#endif } }; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fa2e9d832..29624e70d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; @@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasOmega(), 1e-3)); } +#endif /* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity) { diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 54ca50797..ba6c6d30e 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -94,13 +94,13 @@ TEST(ImuFactor, PreintegratedMeasurements) { // Actual pre-integrated values PreintegratedImuMeasurements actual(testing::Params()); - EXPECT(assert_equal(kZero, actual.theta())); + EXPECT(assert_equal(Rot3(), actual.deltaRij())); EXPECT(assert_equal(kZero, actual.deltaPij())); EXPECT(assert_equal(kZero, actual.deltaVij())); DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR1, actual.theta())); + EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij())); DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9); @@ -129,7 +129,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { // Actual pre-integrated values actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR2, actual.theta())); + EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij())); DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9); @@ -439,6 +439,7 @@ TEST(ImuFactor, fistOrderExponential) { } /* ************************************************************************* */ +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { testing::SomeMeasurements measurements; @@ -458,7 +459,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), pim.preintegrated_H_biasOmega(), 1e-3)); } - +#endif /* ************************************************************************* */ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { @@ -789,6 +790,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { } /* ************************************************************************* */ +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; struct ImuFactorMergeTest { @@ -883,6 +885,7 @@ TEST(ImuFactor, MergeWithCoriolis) { mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1); mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); } +#endif /* ************************************************************************* */ // Same values as pre-integration test but now testing covariance diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index 527a6da7b..a1a361f8a 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -28,6 +28,7 @@ static const double kDt = 0.1; +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); } @@ -140,6 +141,7 @@ TEST(PreintegrationBase, MergedBiasDerivatives) { EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasOmega(), 1e-7)); } +#endif /* ************************************************************************* */ int main() { From 65745cac03cf19054df595990f55f77a8064ee3a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 09:56:49 -0700 Subject: [PATCH 02/22] Separate construction test --- gtsam/navigation/tests/testImuFactor.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index ba6c6d30e..c060b4c9c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -57,6 +57,16 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, } // namespace +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurementsConstruction) { + // Actual pre-integrated values + PreintegratedImuMeasurements actual(testing::Params()); + EXPECT(assert_equal(Rot3(), actual.deltaRij())); + EXPECT(assert_equal(kZero, actual.deltaPij())); + EXPECT(assert_equal(kZero, actual.deltaVij())); + DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); +} + /* ************************************************************************* */ TEST(ImuFactor, Accelerating) { const double a = 0.2, v = 50; @@ -83,22 +93,18 @@ TEST(ImuFactor, Accelerating) { /* ************************************************************************* */ TEST(ImuFactor, PreintegratedMeasurements) { // Measurements - Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); + const double a = 0.1, w = M_PI / 100.0; + Vector3 measuredAcc(a, 0.0, 0.0); + Vector3 measuredOmega(w, 0.0, 0.0); double deltaT = 0.5; // Expected pre-integrated values - Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0); - Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0); + Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0); + Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0); // Actual pre-integrated values PreintegratedImuMeasurements actual(testing::Params()); - EXPECT(assert_equal(Rot3(), actual.deltaRij())); - EXPECT(assert_equal(kZero, actual.deltaPij())); - EXPECT(assert_equal(kZero, actual.deltaVij())); - DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); - actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij())); From a560dd6576cb72c80ea25f36b0ae0f5cfb3e1c04 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 09:57:14 -0700 Subject: [PATCH 03/22] Synchronize both versions' treatment of bias and sensor pose --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/PreintegrationBase.cpp | 88 ++++++++----------------- gtsam/navigation/PreintegrationBase.h | 31 ++------- 4 files changed, 34 insertions(+), 89 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 954ac9656..86614f7dd 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -74,7 +74,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); #else Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr - PreintegrationBase::update(measuredAcc, measuredOmega, dt, + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &A, &B, &C); #endif diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index d1280688d..c549dd359 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); #else Matrix3 D_incrR_integratedOmega; - PreintegrationBase::update(measuredAcc, measuredOmega, dt, + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &A, &B, &C); #endif diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f4951305e..000403d4a 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -193,7 +193,7 @@ Vector9 PreintegrationBase::UpdatePreintegrated( //------------------------------------------------------------------------------ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, - double dt, Matrix9* A, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); @@ -202,10 +202,10 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, // Possibly correct for sensor pose Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega, + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - // Do update + // Do update deltaTij_ += dt; preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); @@ -227,7 +227,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - // NOTE(frank): integrateMeasuremtn always needs to compute the derivatives, + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, // even when not of interest to the caller. Provide scratch space here. Matrix9 A; Matrix93 B, C; @@ -251,78 +251,42 @@ Vector9 PreintegrationBase::biasCorrectedDelta( #else //------------------------------------------------------------------------------ -pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& measuredAcc, const Vector3& measuredOmega, - OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, - OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, - OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame - Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega); + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - return correctMeasurementsBySensorPose(unbiasedAcc, unbiasedOmega, - D_correctedAcc_measuredAcc, D_correctedAcc_measuredOmega, - D_correctedOmega_measuredOmega); -} - -//------------------------------------------------------------------------------ -NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current, - OptionalJacobian<9, 3> D_updated_measuredAcc, - OptionalJacobian<9, 3> D_updated_measuredOmega) const { - - Vector3 correctedAcc, correctedOmega; - Matrix3 D_correctedAcc_measuredAcc, // - D_correctedAcc_measuredOmega, // - D_correctedOmega_measuredOmega; - bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - (needDerivs ? &D_correctedAcc_measuredAcc : 0), - (needDerivs ? &D_correctedAcc_measuredOmega : 0), - (needDerivs ? &D_correctedOmega_measuredOmega : 0)); - // Do update in one fell swoop - Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; - NavState updated = deltaXij_.update(correctedAcc, correctedOmega, dt, D_updated_current, - (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), - (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); - if (needDerivs) { - *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; - *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; - if (!p().body_P_sensor->translation().vector().isZero()) { - *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; - } - } - return updated; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::update(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); // Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); // Do update deltaTij_ += dt; - deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, - D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } // Update Jacobians - // TODO(frank): we are repeating some computation here: accessible in F ? - Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); - + // TODO(frank): Try same simplification as in new approach Matrix3 D_acc_R; - oldRij.rotate(correctedAcc, D_acc_R); + oldRij.rotate(acc, D_acc_R); const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; - const Vector3 integratedOmega = correctedOmega * dt; + const Vector3 integratedOmega = omega * dt; const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! const Matrix3 incrRt = incrR.transpose(); delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 13a7461b4..0a15012f7 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -193,7 +193,7 @@ public: // Update integrated vector on tangent manifold preintegrated with acceleration // Static, functional version. static Vector9 UpdatePreintegrated(const Vector3& a_body, - const Vector3& w_body, double dt, + const Vector3& w_body, const double dt, const Vector9& preintegrated, OptionalJacobian<9, 9> A = boost::none, OptionalJacobian<9, 3> B = boost::none, @@ -202,13 +202,11 @@ public: /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT, + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, Matrix93* C); // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT); + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far @@ -217,28 +215,11 @@ public: #else - /// Subtract estimate and correct for sensor pose - /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) - /// Ignore D_correctedOmega_measuredAcc as it is trivially zero - std::pair correctMeasurementsByBiasAndSensorPose( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, - OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, - OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; - - /// Calculate the updated preintegrated measurement, does not modify - /// It takes measured quantities in the j frame - NavState updatedDeltaXij(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current = boost::none, - OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, - OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, + Matrix9* A, Matrix93* B, Matrix93* C); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far From 5b9c966022d984bf141dc2d646e41f96522814e4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 10:36:36 -0700 Subject: [PATCH 04/22] More sync between versions --- gtsam/navigation/CombinedImuFactor.cpp | 6 ------ gtsam/navigation/ImuFactor.cpp | 6 ------ gtsam/navigation/PreintegrationBase.cpp | 6 +++--- gtsam/navigation/PreintegrationBase.h | 21 +++++-------------- .../tests/testPreintegrationBase.cpp | 2 +- 5 files changed, 9 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 86614f7dd..d27e5d714 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -70,13 +70,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Update preintegrated measurements. Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -#else - Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, - &D_incrR_integratedOmega, &A, &B, &C); -#endif // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index c549dd359..6981dee9c 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -55,13 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -#else - Matrix3 D_incrR_integratedOmega; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, - &D_incrR_integratedOmega, &A, &B, &C); -#endif // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 000403d4a..b0931b930 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -253,7 +253,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta( //------------------------------------------------------------------------------ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C) { + Matrix9* A, Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); @@ -287,10 +287,10 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - *D_incrR_integratedOmega * dt; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; double dt22 = 0.5 * dt * dt; const Matrix3 dRij = oldRij.matrix(); // expensive diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 0a15012f7..f5560c37b 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -199,35 +199,24 @@ public: OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> C = boost::none); - /// Update preintegrated measurements and get derivatives - /// It takes measured quantities in the j frame - /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C); - // Version without derivatives void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); - /// Given the estimate of the bias, return a NavState tangent vector - /// summarizing the preintegrated IMU measurements so far - Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const; - -#else +#endif /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix3* D_incrR_integratedOmega, - Matrix9* A, Matrix93* B, Matrix93* C); + Matrix9* A, Matrix93* B, Matrix93* C); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = boost::none) const; -#endif - /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1 = boost::none, diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index a1a361f8a..4a32faef8 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -26,9 +26,9 @@ #include "imuFactorTesting.h" +#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION static const double kDt = 0.1; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); } From 308a75e49b78247b01278b12b42f227ec077c623 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 11:12:40 -0700 Subject: [PATCH 05/22] Created one base class and two derived classes --- gtsam/navigation/ImuFactor.cpp | 4 +- gtsam/navigation/ImuFactor.h | 4 +- gtsam/navigation/ManifoldPreintegration.cpp | 144 ++++++++ gtsam/navigation/ManifoldPreintegration.h | 126 +++++++ gtsam/navigation/NavState.cpp | 2 +- gtsam/navigation/NavState.h | 2 +- gtsam/navigation/PreintegrationBase.cpp | 328 ------------------ gtsam/navigation/PreintegrationBase.h | 114 +----- gtsam/navigation/TangentPreintegration.cpp | 253 ++++++++++++++ gtsam/navigation/TangentPreintegration.h | 143 ++++++++ .../tests/testCombinedImuFactor.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 +- ...Base.cpp => testTangentPreintegration.cpp} | 40 +-- 13 files changed, 705 insertions(+), 461 deletions(-) create mode 100644 gtsam/navigation/ManifoldPreintegration.cpp create mode 100644 gtsam/navigation/ManifoldPreintegration.h create mode 100644 gtsam/navigation/TangentPreintegration.cpp create mode 100644 gtsam/navigation/TangentPreintegration.h rename gtsam/navigation/tests/{testPreintegrationBase.cpp => testTangentPreintegration.cpp} (82%) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 6981dee9c..ea39368d6 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -91,7 +91,7 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA } //------------------------------------------------------------------------------ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // Matrix9* H1, Matrix9* H2) { PreintegrationBase::mergeWith(pim12, H1, H2); @@ -175,7 +175,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 6f72ec9be..f3bfd8c83 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -124,7 +124,7 @@ public: /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge in a different set of measurements and update bias derivatives accordingly void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); #endif @@ -232,7 +232,7 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge two pre-integrated measurement classes static PreintegratedImuMeasurements Merge( const PreintegratedImuMeasurements& pim01, diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp new file mode 100644 index 000000000..b9344a524 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ManifoldPreintegration.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "ManifoldPreintegration.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr& p, + const Bias& biasHat) + : PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) { + + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // Update Jacobians + // TODO(frank): Try same simplification as in new approach + Matrix3 D_acc_R; + oldRij.rotate(acc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +Vector9 ManifoldPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +//------------------------------------------------------------------------------ + +} // namespace gtsam diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h new file mode 100644 index 000000000..60ac7cf63 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ManifoldPreintegration.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * IMU pre-integration on NavSatet manifold. + * This corresponds to the original RSS paper (with one difference: V is rotated) + */ +class ManifoldPreintegration : public PreintegrationBase { + protected: + + /** + * Pre-integrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate 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 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + /// Default constructor for serialization + ManifoldPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + ManifoldPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + const NavState& deltaXij() const override { return deltaXij_; } + const Rot3& deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position().vector(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } + + /// @name Testable + /// @{ + bool equals(const ManifoldPreintegration& other, double tol) const override; + /// @} + + /// @name Main functionality + /// @{ + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index ee6135ede..467c5802a 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -239,7 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION) +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION) NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 27e083014..ed87a32d4 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,7 +203,7 @@ public: /// @name Dynamics /// @{ -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION) +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION) /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index b0931b930..146fdf8b7 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -34,31 +34,10 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, resetIntegration(); } -//------------------------------------------------------------------------------ -void PreintegrationBase::resetIntegration() { - deltaTij_ = 0.0; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - preintegrated_.setZero(); - preintegrated_H_biasAcc_.setZero(); - preintegrated_H_biasOmega_.setZero(); -#else - deltaXij_ = NavState(); - delRdelBiasOmega_.setZero(); - delPdelBiasAcc_.setZero(); - delPdelBiasOmega_.setZero(); - delVdelBiasAcc_.setZero(); - delVdelBiasOmega_.setZero(); -#endif -} - //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const PreintegrationBase& pim) { os << " deltaTij " << pim.deltaTij_ << endl; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - os << " deltaRij " << Point3(pim.theta()) << endl; -#else os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl; -#endif os << " deltaPij " << Point3(pim.deltaPij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; @@ -71,26 +50,6 @@ void PreintegrationBase::print(const string& s) const { cout << s << *this << endl; } -//------------------------------------------------------------------------------ -bool PreintegrationBase::equals(const PreintegrationBase& other, - double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol - && biasHat_.equals(other.biasHat_, tol) -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) - && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) - && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); -#else - && deltaXij_.equals(other.deltaXij_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); -#endif -} - //------------------------------------------------------------------------------ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, @@ -137,203 +96,6 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION -//------------------------------------------------------------------------------ -// See extensive discussion in ImuFactor.lyx -Vector9 PreintegrationBase::UpdatePreintegrated( - const Vector3& a_body, const Vector3& w_body, double dt, - const Vector9& preintegrated, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { - const auto theta = preintegrated.segment<3>(0); - const auto position = preintegrated.segment<3>(3); - const auto velocity = preintegrated.segment<3>(6); - - // This functor allows for saving computation when exponential map and its - // derivatives are needed at the same location in so<3> - so3::DexpFunctor local(theta); - - // Calculate exact mean propagation - Matrix3 w_tangent_H_theta, invH; - const Vector3 w_tangent = // angular velocity mapped back to tangent space - local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const SO3 R = local.expmap(); - const Vector3 a_nav = R * a_body; - const double dt22 = 0.5 * dt * dt; - - Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent* dt, // theta - position + velocity* dt + a_nav* dt22, // position - velocity + a_nav* dt; // velocity - - if (A) { - // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); - - A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta - } - if (B) { - B->block<3, 3>(0, 0) = Z_3x3; - B->block<3, 3>(3, 0) = R * dt22; - B->block<3, 3>(6, 0) = R * dt; - } - if (C) { - C->block<3, 3>(0, 0) = invH * dt; - C->block<3, 3>(3, 0) = Z_3x3; - C->block<3, 3>(6, 0) = Z_3x3; - } - - return preintegratedPlus; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - const double dt, Matrix9* A, - Matrix93* B, Matrix93* C) { - // Correct for bias in the sensor frame - Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - - // Possibly correct for sensor pose - Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - - // Do update - deltaTij_ += dt; - preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); - - if (p().body_P_sensor) { - // More complicated derivatives in case of non-trivial sensor pose - *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last - } - - // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias - preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - - // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias - preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); -} - -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt) { - // NOTE(frank): integrateMeasurement always needs to compute the derivatives, - // even when not of interest to the caller. Provide scratch space here. - Matrix9 A; - Matrix93 B, C; - integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -} -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::biasCorrectedDelta( - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // We correct for a change between bias_i and the biasHat_ used to integrate - // This is a simple linear correction with obvious derivatives - const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - const Vector9 biasCorrected = - preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + - preintegrated_H_biasOmega_ * biasIncr.gyroscope(); - - if (H) { - (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; - } - return biasCorrected; -} -#else - -//------------------------------------------------------------------------------ -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) { - - // Correct for bias in the sensor frame - Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - - // Possibly correct for sensor pose - Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - - // Save current rotation for updating Jacobians - const Rot3 oldRij = deltaXij_.attitude(); - - // Do update - deltaTij_ += dt; - deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional - - if (p().body_P_sensor) { - // More complicated derivatives in case of non-trivial sensor pose - *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last - } - - // Update Jacobians - // TODO(frank): Try same simplification as in new approach - Matrix3 D_acc_R; - oldRij.rotate(acc, D_acc_R); - const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; - - const Vector3 integratedOmega = omega * dt; - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! - const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; - - double dt22 = 0.5 * dt * dt; - const Matrix3 dRij = oldRij.matrix(); // expensive - delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; - delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; - delVdelBiasAcc_ += -dRij * dt; - delVdelBiasOmega_ += D_acc_biasOmega * dt; -} - -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::biasCorrectedDelta( - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // Correct deltaRij, derivative is delRdelBiasOmega_ - const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - Matrix3 D_correctedRij_bias; - const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); - const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, - H ? &D_correctedRij_bias : 0); - if (H) - D_correctedRij_bias *= delRdelBiasOmega_; - - Vector9 xi; - Matrix3 D_dR_correctedRij; - // TODO(frank): could line below be simplified? It is equivalent to - // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) - NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); - NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() - + delPdelBiasOmega_ * biasIncr.gyroscope(); - NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() - + delVdelBiasOmega_ * biasIncr.gyroscope(); - - if (H) { - Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; - D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; - D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; - D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; - (*H) << D_dR_bias, D_dP_bias, D_dV_bias; - } - return xi; -} - -#endif - //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, @@ -415,96 +177,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION -//------------------------------------------------------------------------------ -// sugar for derivative blocks -#define D_R_R(H) (H)->block<3,3>(0,0) -#define D_R_t(H) (H)->block<3,3>(0,3) -#define D_R_v(H) (H)->block<3,3>(0,6) -#define D_t_R(H) (H)->block<3,3>(3,0) -#define D_t_t(H) (H)->block<3,3>(3,3) -#define D_t_v(H) (H)->block<3,3>(3,6) -#define D_v_R(H) (H)->block<3,3>(6,0) -#define D_v_t(H) (H)->block<3,3>(6,3) -#define D_v_v(H) (H)->block<3,3>(6,6) - -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::Compose(const Vector9& zeta01, - const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) { - const auto t01 = zeta01.segment<3>(0); - const auto p01 = zeta01.segment<3>(3); - const auto v01 = zeta01.segment<3>(6); - - const auto t12 = zeta12.segment<3>(0); - const auto p12 = zeta12.segment<3>(3); - const auto v12 = zeta12.segment<3>(6); - - Matrix3 R01_H_t01, R12_H_t12; - const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); - const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); - - Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity - const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); - - Matrix3 t02_H_R02; - Vector9 zeta02; - const Matrix3 R = R01.matrix(); - zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta - p01 + v01 * deltaT12 + R * p12, // position - v01 + R * v12; // velocity - - if (H1) { - H1->setIdentity(); - D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; - D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; - D_t_v(H1) = I_3x3 * deltaT12; - D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; - } - - if (H2) { - H2->setZero(); - D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; - D_t_t(H2) = R; - D_v_v(H2) = R; - } - - return zeta02; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, - Matrix9* H2) { - if (!matchesParamsWith(pim12)) { - throw std::domain_error("Cannot merge pre-integrated measurements with different params"); - } - - if (params()->body_P_sensor) { - throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); - } - - const double t01 = deltaTij(); - const double t12 = pim12.deltaTij(); - deltaTij_ = t01 + t12; - - const Vector9 zeta01 = preintegrated(); - Vector9 zeta12 = pim12.preintegrated(); // will be modified. - - const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); - zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() - + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); - - preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); - - preintegrated_H_biasAcc_ = - (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; - - preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + - (*H2) * pim12.preintegrated_H_biasOmega_; -} -#endif - //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f5560c37b..04539e96b 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -75,37 +75,13 @@ class PreintegrationBase { /// Time interval from i to j double deltaTij_; - /** - * Preintegrated navigation state, from frame i to frame j - * Order is: theta, position, velocity - * Note: relative position does not take into account velocity at time i, see deltap+, in [2] - * Note: velocity is now also in frame i, as opposed to deltaVij in [2] - */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - Vector9 preintegrated_; - Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias - Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias -#else - NavState deltaXij_; - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate 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 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias -#endif - /// Default constructor for serialization - PreintegrationBase() { - resetIntegration(); - } + PreintegrationBase() {} public: /// @name Constructors /// @{ - /// @name Constructors - /// @{ - /** * Constructor, initializes the variables in the base class * @param p Parameters, typically fixed in a single application @@ -119,7 +95,7 @@ public: /// @name Basic utilities /// @{ /// Re-initialize PreintegratedMeasurements - void resetIntegration(); + virtual void resetIntegration()=0; /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { @@ -148,22 +124,10 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - const Vector9& preintegrated() const { return preintegrated_; } - Vector3 theta() const { return preintegrated_.head<3>(); } - Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const { return preintegrated_.tail<3>(); } - Rot3 deltaRij() const { return Rot3::Expmap(theta()); } - NavState deltaXij() const { return NavState::Retract(preintegrated_); } - - const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } - const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } -#else - const NavState& deltaXij() const { return deltaXij_; } - const Rot3& deltaRij() const { return deltaXij_.attitude(); } - Vector3 deltaPij() const { return deltaXij_.position().vector(); } - Vector3 deltaVij() const { return deltaXij_.velocity(); } -#endif + virtual Vector3 deltaPij() const=0; + virtual Vector3 deltaVij() const=0; + virtual Rot3 deltaRij() const=0; + virtual NavState deltaXij() const=0; // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -173,7 +137,7 @@ public: /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); void print(const std::string& s) const; - bool equals(const PreintegrationBase& other, double tol) const; + virtual bool equals(const PreintegrationBase& other, double tol) const = 0; /// @} /// @name Main functionality @@ -188,34 +152,16 @@ public: OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - - // Update integrated vector on tangent manifold preintegrated with acceleration - // Static, functional version. - static Vector9 UpdatePreintegrated(const Vector3& a_body, - const Vector3& w_body, const double dt, - const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); - - // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); - -#endif - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose - /// NOTE(frank): implementation is different in two versions - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C); + virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far - /// NOTE(frank): implementation is different in two versions - Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const; + virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const=0; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -236,19 +182,6 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives - static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, - double deltaT12, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none); - - /// Merge in a different set of measurements and update bias derivatives accordingly - /// The derivatives apply to the preintegrated Vector9 - void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); - /// @} -#endif - /** Dummy clone for MATLAB */ virtual boost::shared_ptr clone() const { return boost::shared_ptr(); @@ -267,31 +200,6 @@ public: #endif /// @} - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_NVP(p_); -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); - ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); - ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); -#else - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(deltaXij_); - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); - ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); - ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); - ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); - ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); -#endif - } }; } /// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp new file mode 100644 index 000000000..b7e62b528 --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -0,0 +1,253 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TangentPreintegration.cpp + * @author Frank Dellaert + * @author Adam Bry + **/ + +#include "TangentPreintegration.h" +#include +#include + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, + const Bias& biasHat) + : TangentPreintegration(p,biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::resetIntegration() { + deltaTij_ = 0.0; + preintegrated_.setZero(); + preintegrated_H_biasAcc_.setZero(); + preintegrated_H_biasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool TangentPreintegration::equals(const TangentPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) + && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); +} + +//------------------------------------------------------------------------------ +// See extensive discussion in ImuFactor.lyx +Vector9 TangentPreintegration::UpdatePreintegrated( + const Vector3& a_body, const Vector3& w_body, double dt, + const Vector9& preintegrated, OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { + const auto theta = preintegrated.segment<3>(0); + const auto position = preintegrated.segment<3>(3); + const auto velocity = preintegrated.segment<3>(6); + + // This functor allows for saving computation when exponential map and its + // derivatives are needed at the same location in so<3> + so3::DexpFunctor local(theta); + + // Calculate exact mean propagation + Matrix3 w_tangent_H_theta, invH; + const Vector3 w_tangent = // angular velocity mapped back to tangent space + local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); + const SO3 R = local.expmap(); + const Vector3 a_nav = R * a_body; + const double dt22 = 0.5 * dt * dt; + + Vector9 preintegratedPlus; + preintegratedPlus << // new preintegrated vector: + theta + w_tangent* dt, // theta + position + velocity* dt + a_nav* dt22, // position + velocity + a_nav* dt; // velocity + + if (A) { + // Exact derivative of R*a with respect to theta: + const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + + A->setIdentity(); + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + } + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } + + return preintegratedPlus; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + const double dt, Matrix9* A, + Matrix93* B, Matrix93* C) { + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Do update + deltaTij_ += dt; + preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias + preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); + + // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias + preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); +} + +void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) { + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, + // even when not of interest to the caller. Provide scratch space here. + Matrix9 A; + Matrix93 B, C; + integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); +} +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // We correct for a change between bias_i and the biasHat_ used to integrate + // This is a simple linear correction with obvious derivatives + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + const Vector9 biasCorrected = + preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); + + if (H) { + (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; + } + return biasCorrected; +} + +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::Compose(const Vector9& zeta01, + const Vector9& zeta12, double deltaT12, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { + const auto t01 = zeta01.segment<3>(0); + const auto p01 = zeta01.segment<3>(3); + const auto v01 = zeta01.segment<3>(6); + + const auto t12 = zeta12.segment<3>(0); + const auto p12 = zeta12.segment<3>(3); + const auto v12 = zeta12.segment<3>(6); + + Matrix3 R01_H_t01, R12_H_t12; + const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); + const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); + + Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 t02_H_R02; + Vector9 zeta02; + const Matrix3 R = R01.matrix(); + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity + + if (H1) { + H1->setIdentity(); + D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; + D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; + D_t_v(H1) = I_3x3 * deltaT12; + D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; + } + + if (H2) { + H2->setZero(); + D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; + D_t_t(H2) = R; + D_v_v(H2) = R; + } + + return zeta02; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix9* H1, + Matrix9* H2) { + if (!matchesParamsWith(pim12)) { + throw std::domain_error("Cannot merge pre-integrated measurements with different params"); + } + + if (params()->body_P_sensor) { + throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); + } + + const double t01 = deltaTij(); + const double t12 = pim12.deltaTij(); + deltaTij_ = t01 + t12; + + const Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); // will be modified. + + const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); + zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() + + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); + + preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2); + + preintegrated_H_biasAcc_ = + (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; + + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; +} + +//------------------------------------------------------------------------------ + +} // namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h new file mode 100644 index 000000000..2d44be3ab --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TangentPreintegration.h + * @author Frank Dellaert + * @author Adam Bry + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * Integrate on the 9D tangent space of the NavState manifold. + * See extensive discussion in ImuFactor.lyx + */ +class TangentPreintegration : public PreintegrationBase { + protected: + + /** + * Preintegrated navigation state, as a 9D vector on tangent space at frame i + * Order is: theta, position, velocity + */ + Vector9 preintegrated_; + Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias + Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias + + /// Default constructor for serialization + TangentPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + TangentPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } + NavState deltaXij() const override { return NavState::Retract(preintegrated_); } + + const Vector9& preintegrated() const { return preintegrated_; } + Vector3 theta() const { return preintegrated_.head<3>(); } + const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } + const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } + + /// @name Testable + /// @{ + bool equals(const TangentPreintegration& other, double tol) const override; + /// @} + + /// @name Main functionality + /// @{ + + // Update integrated vector on tangent manifold preintegrated with acceleration + // Static, functional version. + static Vector9 UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, const double dt, + const Vector9& preintegrated, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); + + // Version without derivatives + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives + static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, + double deltaT12, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none); + + /// Merge in a different set of measurements and update bias derivatives accordingly + /// The derivatives apply to the preintegrated Vector9 + void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2); + /// @} + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); + ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); + ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 29624e70d..c1c17a6d4 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -132,7 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c060b4c9c..8748a7fcf 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -445,7 +445,7 @@ TEST(ImuFactor, fistOrderExponential) { } /* ************************************************************************* */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { testing::SomeMeasurements measurements; @@ -796,7 +796,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { } /* ************************************************************************* */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; struct ImuFactorMergeTest { diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp similarity index 82% rename from gtsam/navigation/tests/testPreintegrationBase.cpp rename to gtsam/navigation/tests/testTangentPreintegration.cpp index 4a32faef8..af6e283bd 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -10,12 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file testPreintegrationBase.cpp + * @file testTangentPreintegration.cpp * @brief Unit test for the InertialNavFactor * @author Frank Dellaert */ -#include +#include #include #include #include @@ -26,11 +26,10 @@ #include "imuFactorTesting.h" -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION static const double kDt = 0.1; Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); + return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta); } namespace testing { @@ -45,8 +44,8 @@ static boost::shared_ptr Params() { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate1) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate1) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); @@ -59,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate2) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate2) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; @@ -74,8 +73,8 @@ TEST(PreintegrationBase, UpdateEstimate2) { } /* ************************************************************************* */ -TEST(PreintegrationBase, computeError) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, computeError) { + TangentPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; Matrix9 aH1, aH2; @@ -83,7 +82,7 @@ TEST(PreintegrationBase, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, boost::none, boost::none, boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); @@ -92,47 +91,47 @@ TEST(PreintegrationBase, computeError) { } /* ************************************************************************* */ -TEST(PreintegrationBase, Compose) { +TEST(TangentPreintegration, Compose) { testing::SomeMeasurements measurements; - PreintegrationBase pim(testing::Params()); + TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); boost::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { - return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); + return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); // Actual result Matrix9 H1, H2; - PreintegrationBase actual_pim02 = pim; + TangentPreintegration actual_pim02 = pim; actual_pim02.mergeWith(pim, &H1, &H2); const Vector9 zeta = pim.preintegrated(); const Vector9 actual_zeta = - PreintegrationBase::Compose(zeta, zeta, pim.deltaTij()); + TangentPreintegration::Compose(zeta, zeta, pim.deltaTij()); EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7)); EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); } /* ************************************************************************* */ -TEST(PreintegrationBase, MergedBiasDerivatives) { +TEST(TangentPreintegration, MergedBiasDerivatives) { testing::SomeMeasurements measurements; auto f = [=](const Vector3& a, const Vector3& w) { - PreintegrationBase pim02(testing::Params(), Bias(a, w)); + TangentPreintegration pim02(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02); return pim02.preintegrated(); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); @@ -141,7 +140,6 @@ TEST(PreintegrationBase, MergedBiasDerivatives) { EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasOmega(), 1e-7)); } -#endif /* ************************************************************************* */ int main() { From cbf062ff323118690069a25deea139c7d30a333c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 12:52:41 -0700 Subject: [PATCH 06/22] Everything compiles and runs with derived classes --- gtsam/navigation/CombinedImuFactor.cpp | 74 ++++++++-------- gtsam/navigation/CombinedImuFactor.h | 23 +++-- gtsam/navigation/ImuFactor.cpp | 89 ++++++++++--------- gtsam/navigation/ImuFactor.h | 31 ++++--- gtsam/navigation/ManifoldPreintegration.cpp | 21 +++-- gtsam/navigation/ManifoldPreintegration.h | 17 ++-- gtsam/navigation/PreintegrationBase.cpp | 11 ++- gtsam/navigation/PreintegrationBase.h | 20 ++--- gtsam/navigation/TangentPreintegration.cpp | 96 ++++++++++----------- gtsam/navigation/TangentPreintegration.h | 15 ++-- 10 files changed, 206 insertions(+), 191 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index d27e5d714..21c4200a9 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -31,22 +31,21 @@ using namespace std; //------------------------------------------------------------------------------ // Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -void PreintegratedCombinedMeasurements::print( - const string& s) const { - PreintegrationBase::print(s); +void PreintegratedCombinedMeasurements::print(const string& s) const { + PreintegrationType::print(s); cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedCombinedMeasurements::equals( const PreintegratedCombinedMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) && - equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); + return PreintegrationType::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() { void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements. - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF @@ -80,7 +79,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Single Jacobians to propagate covariance // TODO(frank): should we not also account for bias on position? - Matrix3 theta_H_biasOmega = - C.topRows<3>(); + Matrix3 theta_H_biasOmega = -C.topRows<3>(); Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); // overall Jacobian wrt preintegrated measurements (df/dx) @@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // BLOCK DIAGONAL TERMS D_t_t(&G_measCov_Gt) = dt * iCov; - D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * - (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * - (vel_H_biasAcc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * - (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * - (theta_H_biasOmega.transpose()); + D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc + * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) + * (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega + * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) + * (theta_H_biasOmega.transpose()); D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * - theta_H_biasOmega.transpose(); + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) + * theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; @@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) - throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; @@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const PreintegratedCombinedMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) {} +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), _PIM_(pim) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); // if we need the jacobians @@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor( const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) { +: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), +_PIM_(pim) { boost::shared_ptr p = - boost::make_shared(pim.p()); + boost::make_shared(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor( } 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& n_gravity, - const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { // use deprecated predict PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, omegaCoriolis, use2ndOrderCoriolis); @@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, } #endif -} /// namespace gtsam +} + /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3141f8245..bcad9d8f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -22,12 +22,19 @@ #pragma once /* GTSAM includes */ +#include +#include #include -#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -57,7 +64,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedCombinedMeasurements : public PreintegrationBase { +class PreintegratedCombinedMeasurements : public PreintegrationType { public: @@ -123,7 +130,7 @@ public: PreintegratedCombinedMeasurements( const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) - : PreintegrationBase(p, biasHat) { + : PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } @@ -133,10 +140,10 @@ public: /// @{ /// Re-initialize PreintegratedCombinedMeasurements - void resetIntegration(); + void resetIntegration() override; /// const reference to params, shadows definition in base class - Params& p() const { return *boost::static_pointer_cast(p_);} + Params& p() const { return *boost::static_pointer_cast(this->p_);} /// @} /// @name Access instance variables @@ -146,7 +153,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; /// @} @@ -163,7 +170,7 @@ public: * frame) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT); + const Vector3& measuredOmega, const double dt) override; /// @} @@ -183,7 +190,7 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index ea39368d6..7210f4dd2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -32,20 +32,20 @@ using namespace std; // Inner class PreintegratedImuMeasurements //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::print(const string& s) const { - PreintegrationBase::print(s); + PreintegrationType::print(s); cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedImuMeasurements::equals( const PreintegratedImuMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) + return PreintegrationType::equals(other, tol) && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() { void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements (also get Jacobian) - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a @@ -73,30 +73,31 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 - preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt; + preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; } //------------------------------------------------------------------------------ -void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, - const Matrix& measuredOmegas, - const Matrix& dts) { - assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); +void PreintegratedImuMeasurements::integrateMeasurements( + const Matrix& measuredAccs, const Matrix& measuredOmegas, + const Matrix& dts) { + assert( + measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); assert(dts.cols() >= 1); assert(measuredAccs.cols() == dts.cols()); assert(measuredOmegas.cols() == dts.cols()); size_t n = static_cast(dts.cols()); for (size_t j = 0; j < n; j++) { - integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j)); + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j)); } } //------------------------------------------------------------------------------ #ifdef GTSAM_TANGENT_PREINTEGRATION -void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // - Matrix9* H1, Matrix9* H2) { - PreintegrationBase::mergeWith(pim12, H1, H2); +void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // + Matrix9* H1, Matrix9* H2) { + PreintegrationType::mergeWith(pim12, H1, H2); preintMeasCov_ = - *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); + *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); } #endif //------------------------------------------------------------------------------ @@ -180,12 +181,12 @@ PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { if (!pim01.matchesParamsWith(pim12)) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with different params"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); if (pim01.params()->body_P_sensor) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); // the bias for the merged factor will be the bias from 01 PreintegratedImuMeasurements pim02 = pim01; @@ -198,25 +199,25 @@ PreintegratedImuMeasurements ImuFactor::Merge( //------------------------------------------------------------------------------ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, - const shared_ptr& f12) { + const shared_ptr& f12) { // IMU bias keys must be the same. if (f01->key5() != f12->key5()) - throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); + throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); // expect intermediate pose, velocity keys to matchup. if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) - throw std::domain_error( - "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); + throw std::domain_error( + "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); // return new factor auto pim02 = - Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key1(), // P0 - f01->key2(), // V0 - f12->key3(), // P2 - f12->key4(), // V2 - f01->key5(), // B - pim02); + Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); + return boost::make_shared(f01->key1(),// P0 + f01->key2(),// V0 + f12->key3(),// P2 + f12->key4(),// V2 + f01->key5(),// B + pim02); } #endif @@ -251,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, //------------------------------------------------------------------------------ // ImuFactor2 methods //------------------------------------------------------------------------------ -ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), - _PIM_(pim) {} +ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, + bias), _PIM_(pim) { +} //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactor2::clone() const { @@ -269,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { } //------------------------------------------------------------------------------ -void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) - << "," << keyFormatter(this->key3()) << ")\n"; +void ImuFactor2::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) + << ")\n"; cout << *this << endl; } @@ -284,15 +289,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, - const imuBias::ConstantBias& bias_i, // - boost::optional H1, - boost::optional H2, - boost::optional H3) const { +Vector ImuFactor2::evaluateError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1, boost::optional H2, + boost::optional H3) const { return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); } //------------------------------------------------------------------------------ } - // namespace gtsam +// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f3bfd8c83..532abdac0 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,11 +23,18 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating @@ -61,7 +68,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedImuMeasurements: public PreintegrationBase { +class PreintegratedImuMeasurements: public PreintegrationType { friend class ImuFactor; friend class ImuFactor2; @@ -85,29 +92,28 @@ public: */ PreintegratedImuMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : - PreintegrationBase(p, biasHat) { + PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } /** * Construct preintegrated directly from members: base class and preintMeasCov - * @param base PreintegrationBase instance + * @param base PreintegrationType instance * @param preintMeasCov Covariance matrix used in noise model. */ - PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) - : PreintegrationBase(base), + PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { } /// print - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; /// equals - bool equals(const PreintegratedImuMeasurements& expected, - double tol = 1e-9) const; + bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; /// Re-initialize PreintegratedIMUMeasurements - void resetIntegration(); + void resetIntegration() override; /** * Add a single IMU measurement to the preintegration. @@ -115,7 +121,8 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt); + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt) override; /// Add multiple measurements, in matrix columns void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, @@ -152,7 +159,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index b9344a524..cc88d9101 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -26,9 +26,9 @@ using namespace std; namespace gtsam { //------------------------------------------------------------------------------ -ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr& p, - const Bias& biasHat) - : PreintegrationBase(p, biasHat) { +ManifoldPreintegration::ManifoldPreintegration( + const boost::shared_ptr& p, const Bias& biasHat) : + PreintegrationBase(p, biasHat) { resetIntegration(); } @@ -46,8 +46,7 @@ void ManifoldPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && deltaXij_.equals(other.deltaXij_, tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) @@ -58,9 +57,9 @@ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, } //------------------------------------------------------------------------------ -void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) { +void ManifoldPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); @@ -83,8 +82,8 @@ void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc, // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } // Update Jacobians @@ -141,4 +140,4 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta( //------------------------------------------------------------------------------ -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 60ac7cf63..65ca8f4cc 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -21,7 +21,8 @@ #pragma once -#include +#include +#include namespace gtsam { @@ -72,14 +73,14 @@ public: /// @name Instance variables access /// @{ - const NavState& deltaXij() const override { return deltaXij_; } - const Rot3& deltaRij() const override { return deltaXij_.attitude(); } - Vector3 deltaPij() const override { return deltaXij_.position().vector(); } - Vector3 deltaVij() const override { return deltaXij_.velocity(); } + NavState deltaXij() const override { return deltaXij_; } + Rot3 deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position().vector(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } /// @name Testable /// @{ - bool equals(const ManifoldPreintegration& other, double tol) const override; + bool equals(const ManifoldPreintegration& other, double tol) const; /// @} /// @name Main functionality @@ -89,8 +90,8 @@ public: /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose /// NOTE(frank): implementation is different in two versions - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) override; + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 146fdf8b7..d99c86952 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -31,7 +31,6 @@ namespace gtsam { PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, const Bias& biasHat) : p_(p), biasHat_(biasHat), deltaTij_(0.0) { - resetIntegration(); } //------------------------------------------------------------------------------ @@ -96,6 +95,16 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } +//------------------------------------------------------------------------------ +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt) { + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, + // even when not of interest to the caller. Provide scratch space here. + Matrix9 A; + Matrix93 B, C; + update(measuredAcc, measuredOmega, dt, &A, &B, &C); +} + //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 04539e96b..08dcd1381 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -124,9 +124,9 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } - virtual Vector3 deltaPij() const=0; - virtual Vector3 deltaVij() const=0; - virtual Rot3 deltaRij() const=0; + virtual Vector3 deltaPij() const=0; + virtual Vector3 deltaVij() const=0; + virtual Rot3 deltaRij() const=0; virtual NavState deltaXij() const=0; // Exposed for MATLAB @@ -136,8 +136,7 @@ public: /// @name Testable /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); - void print(const std::string& s) const; - virtual bool equals(const PreintegrationBase& other, double tol) const = 0; + virtual void print(const std::string& s) const; /// @} /// @name Main functionality @@ -155,9 +154,13 @@ public: /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose - virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, + virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; + /// Version without derivatives + virtual void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt); + /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, @@ -182,11 +185,6 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); - } - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index b7e62b528..2df958d05 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -25,8 +25,8 @@ namespace gtsam { //------------------------------------------------------------------------------ TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, - const Bias& biasHat) - : TangentPreintegration(p,biasHat) { + const Bias& biasHat) : + PreintegrationBase(p, biasHat) { resetIntegration(); } @@ -41,20 +41,21 @@ void TangentPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool TangentPreintegration::equals(const TangentPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) - && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) - && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); + && equal_with_abs_tol(preintegrated_H_biasAcc_, + other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, + other.preintegrated_H_biasOmega_, tol); } //------------------------------------------------------------------------------ // See extensive discussion in ImuFactor.lyx -Vector9 TangentPreintegration::UpdatePreintegrated( - const Vector3& a_body, const Vector3& w_body, double dt, - const Vector9& preintegrated, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { +Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, double dt, const Vector9& preintegrated, + OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) { const auto theta = preintegrated.segment<3>(0); const auto position = preintegrated.segment<3>(3); const auto velocity = preintegrated.segment<3>(6); @@ -65,27 +66,27 @@ Vector9 TangentPreintegration::UpdatePreintegrated( // Calculate exact mean propagation Matrix3 w_tangent_H_theta, invH; - const Vector3 w_tangent = // angular velocity mapped back to tangent space + const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); const SO3 R = local.expmap(); const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent* dt, // theta - position + velocity* dt + a_nav* dt22, // position - velocity + a_nav* dt; // velocity + preintegratedPlus << // new preintegrated vector: + theta + w_tangent * dt, // theta + position + velocity * dt + a_nav * dt22, // position + velocity + a_nav * dt; // velocity if (A) { // Exact derivative of R*a with respect to theta: const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta } if (B) { B->block<3, 3>(0, 0) = Z_3x3; @@ -102,10 +103,9 @@ Vector9 TangentPreintegration::UpdatePreintegrated( } //------------------------------------------------------------------------------ -void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - const double dt, Matrix9* A, - Matrix93* B, Matrix93* C) { +void TangentPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 omega = biasHat_.correctGyroscope(measuredOmega); @@ -124,8 +124,8 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias @@ -135,24 +135,15 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); } -void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt) { - // NOTE(frank): integrateMeasurement always needs to compute the derivatives, - // even when not of interest to the caller. Provide scratch space here. - Matrix9 A; - Matrix93 B, C; - integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -} //------------------------------------------------------------------------------ Vector9 TangentPreintegration::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { // We correct for a change between bias_i and the biasHat_ used to integrate // This is a simple linear correction with obvious derivatives const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - const Vector9 biasCorrected = - preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + - preintegrated_H_biasOmega_ * biasIncr.gyroscope(); + const Vector9 biasCorrected = preintegrated() + + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); if (H) { (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; @@ -174,9 +165,8 @@ Vector9 TangentPreintegration::biasCorrectedDelta( //------------------------------------------------------------------------------ Vector9 TangentPreintegration::Compose(const Vector9& zeta01, - const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) { + const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { const auto t01 = zeta01.segment<3>(0); const auto p01 = zeta01.segment<3>(3); const auto v01 = zeta01.segment<3>(6); @@ -195,9 +185,9 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01, Matrix3 t02_H_R02; Vector9 zeta02; const Matrix3 R = R01.matrix(); - zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta - p01 + v01 * deltaT12 + R * p12, // position - v01 + R * v12; // velocity + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity if (H1) { H1->setIdentity(); @@ -218,14 +208,16 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01, } //------------------------------------------------------------------------------ -void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix9* H1, - Matrix9* H2) { +void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, + Matrix9* H1, Matrix9* H2) { if (!matchesParamsWith(pim12)) { - throw std::domain_error("Cannot merge pre-integrated measurements with different params"); + throw std::domain_error( + "Cannot merge pre-integrated measurements with different params"); } if (params()->body_P_sensor) { - throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); + throw std::domain_error( + "Cannot merge pre-integrated measurements with sensor pose yet"); } const double t01 = deltaTij(); @@ -241,13 +233,13 @@ void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2); - preintegrated_H_biasAcc_ = - (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; + preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + + (*H2) * pim12.preintegrated_H_biasAcc_; - preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + - (*H2) * pim12.preintegrated_H_biasOmega_; + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; } //------------------------------------------------------------------------------ -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 2d44be3ab..7f3aed303 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -64,9 +64,9 @@ public: /// @name Instance variables access /// @{ - Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } - Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } + Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } NavState deltaXij() const override { return NavState::Retract(preintegrated_); } const Vector9& preintegrated() const { return preintegrated_; } @@ -76,7 +76,7 @@ public: /// @name Testable /// @{ - bool equals(const TangentPreintegration& other, double tol) const override; + bool equals(const TangentPreintegration& other, double tol) const; /// @} /// @name Main functionality @@ -91,15 +91,12 @@ public: OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> C = boost::none); - // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose /// NOTE(frank): implementation is different in two versions - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) override; + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override; /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far From 34513f92b41cb9699e64c091f87561749b6510d8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 12:59:06 -0700 Subject: [PATCH 07/22] Re-enable method for manifold pre-integration --- gtsam/navigation/NavState.cpp | 2 -- gtsam/navigation/NavState.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 467c5802a..7e5d85cde 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -239,7 +239,6 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION) NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { @@ -282,7 +281,6 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, } return newState; } -#endif //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index ed87a32d4..02da46317 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,13 +203,11 @@ public: /// @name Dynamics /// @{ -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION) /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const; -#endif /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, From fb0a5489d7e4c7e25063021da0b2f976bb70bc42 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 15 May 2016 13:15:37 -0700 Subject: [PATCH 08/22] Added flag --- CMakeLists.txt | 16 +++++++++------- gtsam/config.h.in | 3 +++ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3ea81c85e..de9b4152f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,7 @@ option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -485,13 +486,14 @@ message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") -print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") -print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") -print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") -print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") +print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") +print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/gtsam/config.h.in b/gtsam/config.h.in index f9a576d14..42d5e7517 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -68,3 +68,6 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION + +// Support Metis-based nested dissection +#cmakedefine GTSAM_TANGENT_PREINTEGRATION From b173057e33d905fa986f8a47bbdf7573b5cd1778 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Thu, 2 Jun 2016 21:04:52 -0400 Subject: [PATCH 09/22] fix matlab compile issue, by removing abstract class PreintegrationBase from wrapper declearation --- gtsam.h | 40 ++++++++++++++++------------------------ 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/gtsam.h b/gtsam.h index c12055916..f9a7483bb 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2506,30 +2506,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { bool getUse2ndOrderCoriolis() const; }; -#include -virtual class PreintegrationBase { - // Constructors - PreintegrationBase(const gtsam::PreintegrationParams* params); - PreintegrationBase(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationBase& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - - // Standard Interface - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - #include -virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { +class PreintegratedImuMeasurements { // Constructors PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, @@ -2544,6 +2522,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { double deltaT); void resetIntegration(); Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; }; virtual class ImuFactor: gtsam::NonlinearFactor { @@ -2559,7 +2544,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include -virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { +class PreintegratedCombinedMeasurements { // Testable void print(string s) const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, @@ -2570,6 +2555,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { double deltaT); void resetIntegration(); Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; }; virtual class CombinedImuFactor: gtsam::NonlinearFactor { From c7e25e83638ac2338d90ae50dfab93bee7ce3697 Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 4 Jun 2016 16:44:26 -0400 Subject: [PATCH 10/22] adding tests for Manifold preintegration (still 2 tests failing) --- gtsam/navigation/ManifoldPreintegration.h | 6 ++ .../tests/testManifoldPreintegration.cpp | 101 ++++++++++++++++++ 2 files changed, 107 insertions(+) create mode 100644 gtsam/navigation/tests/testManifoldPreintegration.cpp diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 65ca8f4cc..8a9d44755 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -78,6 +78,12 @@ public: Vector3 deltaPij() const override { return deltaXij_.position().vector(); } Vector3 deltaVij() const override { return deltaXij_.velocity(); } + Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } + Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } + Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } + Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } + Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } + /// @name Testable /// @{ bool equals(const ManifoldPreintegration& other, double tol) const; diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp new file mode 100644 index 000000000..4c381d820 --- /dev/null +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -0,0 +1,101 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testManifoldPreintegration.cpp + * @brief Unit test for the ManifoldPreintegration + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "imuFactorTesting.h" + +static const double kDt = 0.1; + +Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { + NavState state = NavState::Expmap(zeta); + NavState newNavState = state.update(a, w, kDt,boost::none,boost::none,boost::none); + return state.localCoordinates(newNavState); +} + +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, UpdateEstimate1) { + ManifoldPreintegration pim(testing::Params()); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); + Vector9 zeta; + zeta.setZero(); + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, UpdateEstimate2) { + ManifoldPreintegration pim(testing::Params()); + ManifoldPreintegration pimActual(pim); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); + Vector9 zeta; + zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, computeError) { + ManifoldPreintegration pim(testing::Params()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From ab5ac480db7376842b107d476f41cc797b6d8767 Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 4 Jun 2016 16:54:23 -0400 Subject: [PATCH 11/22] reincluded old tests for imu factors: these are also failing --- gtsam/navigation/tests/testImuFactorOld.cpp | 948 ++++++++++++++++++ .../tests/testManifoldPreintegration.cpp | 18 +- 2 files changed, 959 insertions(+), 7 deletions(-) create mode 100644 gtsam/navigation/tests/testImuFactorOld.cpp diff --git a/gtsam/navigation/tests/testImuFactorOld.cpp b/gtsam/navigation/tests/testImuFactorOld.cpp new file mode 100644 index 000000000..afda8fe0a --- /dev/null +++ b/gtsam/navigation/tests/testImuFactorOld.cpp @@ -0,0 +1,948 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testImuFactor.cpp + * @brief Unit test for ImuFactor + * @author Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +#if 0 + +static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); +static const Vector3 kZeroOmegaCoriolis(0, 0, 0); +static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); +static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; + +/* ************************************************************************* */ +namespace { +// Auxiliary functions to test evaluate error in ImuFactor +/* ************************************************************************* */ +Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { + return Rot3::Expmap( + factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); +} + +// Define covariance matrices +/* ************************************************************************* */ +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +PreintegratedImuMeasurements evaluatePreintegratedMeasurements( + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + + list::const_iterator itAcc = measuredAccs.begin(); + list::const_iterator itOmega = measuredOmegas.begin(); + list::const_iterator itDeltaT = deltaTs.begin(); + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); + } + return result; +} + +Vector3 evaluatePreintegratedMeasurementsPosition( + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); +} + +Vector3 evaluatePreintegratedMeasurementsVelocity( + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); +} + +Rot3 evaluatePreintegratedMeasurementsRotation( + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); +} + +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, + const double deltaT) { + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); +} + +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { + return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); +} + +} // namespace + +/* ************************************************************************* */ +bool MonteCarlo(const PreintegratedImuMeasurements& pim, + const NavState& state, const imuBias::ConstantBias& bias, double dt, + const Pose3& body_P_sensor, const Vector3& measuredAcc, + const Vector3& measuredOmega, size_t N = 10, + size_t M = 1) { + // Get mean prediction from "ground truth" measurements + PreintegratedImuMeasurements pim1 = pim; + for (size_t k = 0; k < M; k++) + pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); + NavState prediction = pim1.predict(state, bias); + Matrix9 actual = pim1.preintMeasCov(); + + // Do a Monte Carlo analysis to determine empirical density on the predicted state + Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar / dt)), 0); + Sampler sampleOmegaNoise(Vector3::Constant(sqrt(omegaNoiseVar / dt)), 10); + Matrix samples(9, N); + Vector9 sum = Vector9::Zero(); + for (size_t i = 0; i < N; i++) { + PreintegratedImuMeasurements pim2 = pim; + for (size_t k = 0; k < M; k++) { + Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); + Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); + pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, + body_P_sensor); + } + NavState sampled = pim2.predict(state, bias); + Vector9 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + // Vector9 sampleMean = sum / N; + Matrix9 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector9 xi = samples.col(i); + // xi -= sampleMean; + Q += xi * xi.transpose() / (N - 1); + } + + // Compare Monte-Carlo value with actual (computed) value + return assert_equal(Matrix(1000000*Q), 1000000*actual, 1); +} + +/* ************************************************************************* */ +TEST(ImuFactor, StraightLine) { + // Set up IMU measurements + static const double g = 10; // make gravity 10 to make this easy to check + static const double v = 50.0, a = 0.2, dt = 3.0; + const double dt22 = dt * dt / 2; + + // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X + // The body itself has Z axis pointing down + static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + static const Point3 initial_position(10, 20, 0); + static const Vector3 initial_velocity(v, 0, 0); + static const NavState state1(nRb, initial_position, initial_velocity); + + // set up acceleration in X direction, no angular velocity. + // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z + Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + + // Create parameters assuming nav frame has Z up + boost::shared_ptr p = + PreintegratedImuMeasurements::Params::MakeSharedU(g); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + + // Check G1 and G2 derivatives of pim.update + + // Now, preintegrate for 3 seconds, in 10 steps + PreintegratedImuMeasurements pim(p, kZeroBiasHat); + for (size_t i = 0; i < 10; i++) + pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); + +// Matrix9 aG0; Matrix93 aG1,aG2; +// boost::function f = +// boost::bind(&ManifoldPreintegration::update, pim, _1, _2, dt/10, +// boost::none, boost::none, boost::none); +// pim.update(measuredAcc, measuredOmega, dt / 10, aG0, aG1, aG2); +// EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); +// EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); + + // Do Monte-Carlo analysis + PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, + p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + EXPECT(MonteCarlo(pimMC, state1, kZeroBias, dt/10, Pose3(), measuredAcc, measuredOmega)); + + // Check integrated quantities in body frame: gravity measured by IMU is integrated! + Vector3 b_deltaP(a * dt22, 0, -g * dt22); + Vector3 b_deltaV(a * dt, 0, -g * dt); + EXPECT(assert_equal(Rot3(), pim.deltaRij())); + EXPECT(assert_equal(b_deltaP, pim.deltaPij())); + EXPECT(assert_equal(b_deltaV, pim.deltaVij())); + + // Check bias-corrected delta: also in body frame + Vector9 expectedBC; + expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; + EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); + + // Check prediction in nav, note we move along x in body, along y in nav + Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); + Velocity3 expected_velocity(v, a * dt, 0); + NavState expected(nRb, expected_position, expected_velocity); + EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurements) { + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); + double deltaT = 0.5; + + // Expected preintegrated values + Vector3 expectedDeltaP1; + expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + Vector3 expectedDeltaV1(0.05, 0.0, 0.0); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); + double expectedDeltaT1(0.5); + + // Actual preintegrated values + PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + 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; + expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); + double expectedDeltaT2(1); + + // Actual preintegrated values + PreintegratedImuMeasurements actual2 = actual1; + actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + 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 { +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 +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 p = + PreintegratedImuMeasurements::Params::MakeSharedD(); + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->integrationCovariance = kIntegrationErrorCovariance; + p->use2ndOrderCoriolis = true; + + PreintegratedImuMeasurements pim(p, kZeroBiasHat); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(kZeroBias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), kZeroBias); + EXPECT(assert_equal(expectedH, actualH)); + + Matrix9 aH1; + Matrix96 aH2; + NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); + Matrix eH1 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, + boost::none), state1); + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, + boost::none), kZeroBias); + EXPECT(assert_equal(eH2, aH2)); + return; + +} + +/* ************************************************************************* */ +TEST(ImuFactor, ErrorAndJacobians) { + using namespace common; + PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Expected error + Vector expectedError(9); + expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; + EXPECT( + assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), kZeroBias); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); + + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a); + + // Make sure rotation part is correct when error is interpreted as axis-angle + // Jacobians are around zero, so the rotation part is the same as: + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1); + EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); + + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); + EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); + + // Evaluate error with wrong values + Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); + values.update(V(2), v2_wrong); + expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901; + EXPECT( + assert_equal(expectedError, + factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2)); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2)); + + // Make sure the whitening is done correctly + Matrix cov = pim.preintMeasCov(); + Matrix R = RtR(cov.inverse()); + 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-3); +} + +/* ************************************************************************* */ +TEST(ImuFactor, ErrorAndJacobianWithBiases) { + using common::x1; + using common::v1; + using common::v2; + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); + + // Measurements + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + + Vector3(0.2, 0.0, 0.0); + double deltaT = 1.0; + + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); + PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Make sure of biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(bias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), bias); + EXPECT(assert_equal(expectedH, actualH)); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); + + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); +} + +/* ************************************************************************* */ +TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + using common::x1; + using common::v1; + using common::v2; + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); + + // Measurements + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + + Vector3(0.2, 0.0, 0.0); + double deltaT = 1.0; + + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + Pose3 bodyPsensor = Pose3(); + bool use2ndOrderCoriolis = true; + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); + + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PartialDerivative_wrt_Bias) { + // Linearization point + Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega(0.1, 0, 0); + double deltaT = 0.5; + + // Compute numerical derivatives + Matrix expectedDelRdelBiasOmega = numericalDerivative11( + boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), + Vector3(biasOmega)); + + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); + + Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + + // Compare Jacobians + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9)); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PartialDerivativeLogmap) { + // Linearization point + Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias + + // Measurements + Vector3 deltatheta(0, 0, 0); + + // Compute numerical derivatives + Matrix expectedDelFdeltheta = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); + + Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); +} + +/* ************************************************************************* */ +TEST(ImuFactor, fistOrderExponential) { + // Linearization point + Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega(0.1, 0, 0); + double deltaT = 1.0; + + // change w.r.t. linearization point + double alpha = 0.0; + Vector3 deltabiasOmega; + deltabiasOmega << alpha, alpha, alpha; + + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); + + Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + + const Matrix expectedRot = Rot3::Expmap( + (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + + const Matrix3 hatRot = + Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = hatRot + * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + + // This is a first order expansion so the equality is only an approximation + EXPECT(assert_equal(expectedRot, actualRot)); +} + +/* ************************************************************************* */ +TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for (int i = 1; i < 100; i++) { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); + deltaTs.push_back(0.01); + } + + // Actual preintegrated values + PreintegratedImuMeasurements preintegrated = + evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, + deltaTs); + + // Compute numerical derivatives + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), kZeroBias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); + + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), kZeroBias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); + + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), kZeroBias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); +} + +/* ************************************************************************* */ +TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); + + // Measurements + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + + Vector3(0.2, 0.0, 0.0); + double dt = 0.1; + + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 0)); + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + + // Get mean prediction from "ground truth" measurements + PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor, + measuredAcc, measuredOmega)); + + Matrix expected(9,9); + expected << + 0.0290780477, 4.63277848e-07, 9.23468723e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 4.63277848e-07, 0.0290688208, 4.62505461e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 9.23468723e-05, 4.62505461e-06, 0.0299907267, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, // + 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01; + pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); + EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-6)); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis); + + // Predict + Pose3 actual_x2; + Vector3 actual_v2; + ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim, + kGravityAlongNavZDown, kZeroOmegaCoriolis); + + // Regression test with + Rot3 expectedR( // + 0.456795409, -0.888128414, 0.0506544554, // + 0.889548908, 0.45563417, -0.0331699173, // + 0.00637924528, 0.0602114814, 0.998165258); + Point3 expectedT(5.30373101, 0.768972495, -49.9942188); + Vector3 expected_v2(0.107462014, -0.46205501, 0.0115624037); + Pose3 expected_x2(expectedR, expectedT); + EXPECT(assert_equal(expected_x2, actual_x2, 1e-7)); + EXPECT(assert_equal(Vector(expected_v2), actual_v2, 1e-7)); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); + + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PredictPositionAndVelocity) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + + // Measurements + Vector3 measuredOmega; + measuredOmega << 0, 0, 0; // M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1, -9.81; + double deltaT = 0.001; + + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, true); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + 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, + kGravityAlongNavZDown, kZeroOmegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PredictRotation) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + + // Measurements + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; + double deltaT = 0.001; + + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, true); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + 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, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 0, 0; + EXPECT(assert_equal(expectedPose, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PredictArbitrary) { + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + + // Measurements + Vector3 measuredOmega(M_PI / 10, M_PI / 10, M_PI / 10); + Vector3 measuredAcc(0.1, 0.2, -9.81); + double dt = 0.001; + + ImuFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + Pose3 x1; + Vector3 v1 = Vector3(0, 0, 0); + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, Pose3(), + measuredAcc, measuredOmega)); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + + Matrix expected(9,9); + expected << // + 0.0299999995, 2.46739898e-10, 2.46739896e-10, -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, // + 2.46739898e-10, 0.0299999995, 2.46739902e-10, -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, // + 2.46739896e-10, 2.46739902e-10, 0.0299999995, 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, // + -0.0144839494, -0.0454268484, 0.00489577218, 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, // + 0.044978128, -0.0149428917, 0.00839301168, 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, // + 0.0100471195, 0.00609093775, 0.000448720395, -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, // + -0.0409843415, -0.13554868, 0.00879031682, 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, // + 0.134423822, -0.0471183681, 0.0162199769, 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, // + 0.0383280513, 0.0247643646, 0.00112485862, -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764; + EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Predict + Pose3 x2; + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Regression test for Imu Refactor + Rot3 expectedR( // + +0.903715275, -0.250741668, 0.347026393, // + +0.347026393, 0.903715275, -0.250741668, // + -0.250741668, 0.347026393, 0.903715275); + Point3 expectedT(-0.505517319, 0.569413747, 0.0861035711); + Vector3 expectedV(-1.59121524, 1.55353139, 0.3376838540); + Pose3 expectedPose(expectedR, expectedT); + EXPECT(assert_equal(expectedPose, x2, 1e-7)); + EXPECT(assert_equal(Vector(expectedV), v2, 1e-7)); +} + +/* ************************************************************************* */ +TEST(ImuFactor, bodyPSensorNoBias) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Measurements + Vector3 n_gravity(0, 0, -9.81); // z-up nav frame + Vector3 omegaCoriolis(0, 0, 0); + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 s_accMeas(0, 0, -9.81); + double dt = 0.001; + + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + + ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); + + // Predict + Pose3 x1; + Vector3 v1(0, 0, 0); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, + omegaCoriolis); + + Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + + Vector3 expectedVelocity(0, 0, 0); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); +} + +/* ************************************************************************* */ +#include +#include +#include +#include +#include + +TEST(ImuFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + typedef imuBias::ConstantBias Bias; + + int numFactors = 80; + Vector6 noiseBetweenBiasSigma; + noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, + 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements + Vector3 n_gravity(0, 0, -9.81); + Vector3 omegaCoriolis(0, 0, 0); + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); + + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + Vector6 priorNoisePoseSigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas( + (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0, 0); + + // Create a factor graph with priors on initial pose, vlocity and bias + NonlinearFactorGraph graph; + Values values; + + PriorFactor priorPose(X(0), Pose3(), priorNoisePose); + graph.add(priorPose); + values.insert(X(0), Pose3()); + + PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); + graph.add(priorVel); + values.insert(V(0), zeroVel); + + // The key to this test is that we specify the bias, in the sensor frame, as known a priori + // We also create factors below that encode our assumption that this bias is constant over time + // In theory, after optimization, we should recover that same bias estimate + Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); + graph.add(priorBiasFactor); + values.insert(B(0), priorBias); + + // Now add IMU factors and bias noise models + Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + for (int i = 1; i < numFactors; i++) { + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(zeroBias, accCov, gyroCov, + integrationCov, true); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + // Create factors + graph.add( + ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, + omegaCoriolis)); + graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), priorBias); + } + + // Finally, optimize, and get bias at last time step + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + Bias biasActual = results.at(B(numFactors - 1)); + + // And compare it with expected value (our prior) + Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); +} +#endif + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + + diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 4c381d820..96c391112 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -28,9 +28,12 @@ static const double kDt = 0.1; -Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - NavState state = NavState::Expmap(zeta); - NavState newNavState = state.update(a, w, kDt,boost::none,boost::none,boost::none); +Vector9 f(ManifoldPreintegration pim, const Vector3& a, const Vector3& w) { + NavState state = pim.deltaXij(); + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.update(a, w, kDt, &aH1, &aH2, &aH3); + NavState newNavState = pim.deltaXij(); return state.localCoordinates(newNavState); } @@ -48,21 +51,22 @@ static boost::shared_ptr Params() { /* ************************************************************************* */ TEST(ManifoldPreintegration, UpdateEstimate1) { ManifoldPreintegration pim(testing::Params()); + ManifoldPreintegration pimActual(pim); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); Matrix9 aH1; Matrix93 aH2, aH3; pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); - EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); - EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); - EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); + EXPECT(assert_equal(numericalDerivative31(f, pimActual, acc, omega), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, pimActual, acc, omega), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, pimActual, acc, omega), aH3, 1e-9)); } /* ************************************************************************* */ TEST(ManifoldPreintegration, UpdateEstimate2) { ManifoldPreintegration pim(testing::Params()); - ManifoldPreintegration pimActual(pim); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; From 87446914dfd6d32b59ccc09aebc17ff56cb3726f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 4 Jun 2016 16:22:04 -0700 Subject: [PATCH 12/22] Clarifying comments --- gtsam/navigation/TangentPreintegration.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 2df958d05..0ac57d05c 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -128,10 +128,14 @@ void TangentPreintegration::update(const Vector3& measuredAcc, *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } - // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias + // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc + // where acc_H_biasAcc = -I_3x3, hence + // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias + // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega + // where omega_H_biasOmega = -I_3x3, hence + // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); } From 3d30f4ac5a0bfee0cc1eded186e2b7bd8b32d3ac Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 4 Jun 2016 16:22:31 -0700 Subject: [PATCH 13/22] Small change, prototype inline retract --- gtsam/navigation/NavState.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7e5d85cde..3a8876ec5 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -104,9 +104,16 @@ NavState NavState::inverse() const { NavState NavState::operator*(const NavState& bTc) const { TIE(nRb, n_t, n_v, *this); TIE(bRc, b_t, b_v, bTc); - return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); + return NavState(nRb * bRc, n_t + nRb * b_t, n_v + nRb * b_v); } +//------------------------------------------------------------------------------ +//NavState NavState::retract(const Vector9& xi) const { +// TIE(nRb, n_t, n_v, *this); +// const Rot3 bRc = Rot3::Expmap(dR(xi)); +// return NavState(nRb * bRc, n_t + nRb * dP(xi), n_v + nRb * dV(xi)); +//} + //------------------------------------------------------------------------------ NavState::PositionAndVelocity // NavState::operator*(const PositionAndVelocity& b_tv) const { From 105a8183fb206c9be2adc463f4603a55a88c6c55 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 4 Jun 2016 16:22:43 -0700 Subject: [PATCH 14/22] Fixed comment --- gtsam/navigation/TangentPreintegration.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 7f3aed303..73842ffca 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -33,8 +33,8 @@ class TangentPreintegration : public PreintegrationBase { * Order is: theta, position, velocity */ Vector9 preintegrated_; - Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias - Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias + Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated_ w.r.t. acceleration bias + Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated_ w.r.t. angular rate bias /// Default constructor for serialization TangentPreintegration() { From facab116ce21ba7158cfeab0b390854042a8322a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 4 Jun 2016 16:53:20 -0700 Subject: [PATCH 15/22] Now testing bias-correction Jacobians in right place --- gtsam/navigation/tests/testImuFactor.cpp | 22 ----- .../tests/testManifoldPreintegration.cpp | 83 ++++++++++--------- .../tests/testTangentPreintegration.cpp | 23 +++++ 3 files changed, 69 insertions(+), 59 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 8748a7fcf..9db7aac72 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -444,28 +444,6 @@ TEST(ImuFactor, fistOrderExponential) { EXPECT(assert_equal(expectedRot, actualRot)); } -/* ************************************************************************* */ -#ifdef GTSAM_TANGENT_PREINTEGRATION -TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - testing::SomeMeasurements measurements; - - boost::function preintegrated = - [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; - - // Actual pre-integrated values - PreintegratedImuMeasurements pim(testing::Params()); - testing::integrateMeasurements(measurements, &pim); - - EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero), - pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), - pim.preintegrated_H_biasOmega(), 1e-3)); -} -#endif /* ************************************************************************* */ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 96c391112..a3f688dda 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -26,17 +26,6 @@ #include "imuFactorTesting.h" -static const double kDt = 0.1; - -Vector9 f(ManifoldPreintegration pim, const Vector3& a, const Vector3& w) { - NavState state = pim.deltaXij(); - Matrix9 aH1; - Matrix93 aH2, aH3; - pim.update(a, w, kDt, &aH1, &aH2, &aH3); - NavState newNavState = pim.deltaXij(); - return state.localCoordinates(newNavState); -} - namespace testing { // Create default parameters with Z-down and above noise parameters static boost::shared_ptr Params() { @@ -49,34 +38,54 @@ static boost::shared_ptr Params() { } /* ************************************************************************* */ -TEST(ManifoldPreintegration, UpdateEstimate1) { - ManifoldPreintegration pim(testing::Params()); - ManifoldPreintegration pimActual(pim); - const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); - Vector9 zeta; - zeta.setZero(); - Matrix9 aH1; - Matrix93 aH2, aH3; - pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); - EXPECT(assert_equal(numericalDerivative31(f, pimActual, acc, omega), aH1, 1e-9)); - EXPECT(assert_equal(numericalDerivative32(f, pimActual, acc, omega), aH2, 1e-9)); - EXPECT(assert_equal(numericalDerivative33(f, pimActual, acc, omega), aH3, 1e-9)); -} +TEST(ManifoldPreintegration, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; -/* ************************************************************************* */ -TEST(ManifoldPreintegration, UpdateEstimate2) { - ManifoldPreintegration pim(testing::Params()); + boost::function deltaRij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaRij(); + }; - const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); - Vector9 zeta; - zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; - Matrix9 aH1; - Matrix93 aH2, aH3; - pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); - // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 - EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); - EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8)); - EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); + boost::function deltaPij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaPij(); + }; + + boost::function deltaVij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaVij(); + }; + + // Actual pre-integrated values + ManifoldPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT( + assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index af6e283bd..65fd55fa3 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -72,6 +72,29 @@ TEST(TangentPreintegration, UpdateEstimate2) { EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); } +/* ************************************************************************* */ +TEST(ImuFactor, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function preintegrated = + [=](const Vector3& a, const Vector3& w) { + TangentPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; + + // Actual pre-integrated values + TangentPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(preintegrated, kZero, kZero), + pim.preintegrated_H_biasAcc())); + EXPECT( + assert_equal(numericalDerivative22(preintegrated, kZero, kZero), + pim.preintegrated_H_biasOmega(), 1e-3)); +} + /* ************************************************************************* */ TEST(TangentPreintegration, computeError) { TangentPreintegration pim(testing::Params()); From cdf9c53b965ad159ea9dbfffd6da94b8a775db51 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 4 Jun 2016 17:52:10 -0700 Subject: [PATCH 16/22] Removed base class so python wrapper compiles --- python/handwritten/navigation/ImuFactor.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index 2d7e36f47..4e55c8fc4 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -34,7 +34,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PreintegratedImuMeasurements::equals, 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationBase::predict, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegratedImuMeasurements::predict, 2, 4) void exportImuFactor() { class_("OptionalJacobian39", init<>()); @@ -80,18 +80,13 @@ void exportImuFactor() { // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html register_ptr_to_python< boost::shared_ptr >(); - class_( - "PreintegrationBase", - init&, const imuBias::ConstantBias&>()) - .def("predict", &PreintegrationBase::predict, predict_overloads()) - .def("computeError", &PreintegrationBase::computeError) - .def("resetIntegration", &PreintegrationBase::resetIntegration) - .def("deltaTij", &PreintegrationBase::deltaTij); - - class_>( - "PreintegratedImuMeasurements", + class_("PreintegratedImuMeasurements", init&, const imuBias::ConstantBias&>()) .def(repr(self)) + .def("predict", &PreintegratedImuMeasurements::predict, predict_overloads()) + .def("computeError", &PreintegratedImuMeasurements::computeError) + .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) + .def("deltaTij", &PreintegratedImuMeasurements::deltaTij) .def("equals", &PreintegratedImuMeasurements::equals, equals_overloads(args("other", "tol"))) .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement) .def("integrateMeasurements", &PreintegratedImuMeasurements::integrateMeasurements) From 4709925c983155be0ec7715ddc49d8bb9447004f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 4 Jun 2016 21:36:55 -0400 Subject: [PATCH 17/22] deleted old test --- gtsam/navigation/tests/testImuFactorOld.cpp | 948 -------------------- 1 file changed, 948 deletions(-) delete mode 100644 gtsam/navigation/tests/testImuFactorOld.cpp diff --git a/gtsam/navigation/tests/testImuFactorOld.cpp b/gtsam/navigation/tests/testImuFactorOld.cpp deleted file mode 100644 index afda8fe0a..000000000 --- a/gtsam/navigation/tests/testImuFactorOld.cpp +++ /dev/null @@ -1,948 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; -using symbol_shorthand::B; - -#if 0 - -static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); -static const Vector3 kZeroOmegaCoriolis(0, 0, 0); -static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); -static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; - -/* ************************************************************************* */ -namespace { -// Auxiliary functions to test evaluate error in ImuFactor -/* ************************************************************************* */ -Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, - const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) { - return Rot3::Expmap( - factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); -} - -// Define covariance matrices -/* ************************************************************************* */ -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; - -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ -/* ************************************************************************* */ -PreintegratedImuMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - - list::const_iterator itAcc = measuredAccs.begin(); - list::const_iterator itOmega = measuredOmegas.begin(); - list::const_iterator itDeltaT = deltaTs.begin(); - for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { - result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); - } - return result; -} - -Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaPij(); -} - -Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaVij(); -} - -Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaRij()); -} - -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, - const double deltaT) { - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); -} - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { - return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); -} - -} // namespace - -/* ************************************************************************* */ -bool MonteCarlo(const PreintegratedImuMeasurements& pim, - const NavState& state, const imuBias::ConstantBias& bias, double dt, - const Pose3& body_P_sensor, const Vector3& measuredAcc, - const Vector3& measuredOmega, size_t N = 10, - size_t M = 1) { - // Get mean prediction from "ground truth" measurements - PreintegratedImuMeasurements pim1 = pim; - for (size_t k = 0; k < M; k++) - pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); - NavState prediction = pim1.predict(state, bias); - Matrix9 actual = pim1.preintMeasCov(); - - // Do a Monte Carlo analysis to determine empirical density on the predicted state - Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar / dt)), 0); - Sampler sampleOmegaNoise(Vector3::Constant(sqrt(omegaNoiseVar / dt)), 10); - Matrix samples(9, N); - Vector9 sum = Vector9::Zero(); - for (size_t i = 0; i < N; i++) { - PreintegratedImuMeasurements pim2 = pim; - for (size_t k = 0; k < M; k++) { - Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); - Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); - pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, - body_P_sensor); - } - NavState sampled = pim2.predict(state, bias); - Vector9 xi = sampled.localCoordinates(prediction); - samples.col(i) = xi; - sum += xi; - } - // Vector9 sampleMean = sum / N; - Matrix9 Q; - Q.setZero(); - for (size_t i = 0; i < N; i++) { - Vector9 xi = samples.col(i); - // xi -= sampleMean; - Q += xi * xi.transpose() / (N - 1); - } - - // Compare Monte-Carlo value with actual (computed) value - return assert_equal(Matrix(1000000*Q), 1000000*actual, 1); -} - -/* ************************************************************************* */ -TEST(ImuFactor, StraightLine) { - // Set up IMU measurements - static const double g = 10; // make gravity 10 to make this easy to check - static const double v = 50.0, a = 0.2, dt = 3.0; - const double dt22 = dt * dt / 2; - - // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X - // The body itself has Z axis pointing down - static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - static const Point3 initial_position(10, 20, 0); - static const Vector3 initial_velocity(v, 0, 0); - static const NavState state1(nRb, initial_position, initial_velocity); - - // set up acceleration in X direction, no angular velocity. - // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z - Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); - - // Create parameters assuming nav frame has Z up - boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedU(g); - p->accelerometerCovariance = kMeasuredAccCovariance; - p->gyroscopeCovariance = kMeasuredOmegaCovariance; - - // Check G1 and G2 derivatives of pim.update - - // Now, preintegrate for 3 seconds, in 10 steps - PreintegratedImuMeasurements pim(p, kZeroBiasHat); - for (size_t i = 0; i < 10; i++) - pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); - -// Matrix9 aG0; Matrix93 aG1,aG2; -// boost::function f = -// boost::bind(&ManifoldPreintegration::update, pim, _1, _2, dt/10, -// boost::none, boost::none, boost::none); -// pim.update(measuredAcc, measuredOmega, dt / 10, aG0, aG1, aG2); -// EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); -// EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); - - // Do Monte-Carlo analysis - PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, - p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - EXPECT(MonteCarlo(pimMC, state1, kZeroBias, dt/10, Pose3(), measuredAcc, measuredOmega)); - - // Check integrated quantities in body frame: gravity measured by IMU is integrated! - Vector3 b_deltaP(a * dt22, 0, -g * dt22); - Vector3 b_deltaV(a * dt, 0, -g * dt); - EXPECT(assert_equal(Rot3(), pim.deltaRij())); - EXPECT(assert_equal(b_deltaP, pim.deltaPij())); - EXPECT(assert_equal(b_deltaV, pim.deltaVij())); - - // Check bias-corrected delta: also in body frame - Vector9 expectedBC; - expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; - EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); - - // Check prediction in nav, note we move along x in body, along y in nav - Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); - Velocity3 expected_velocity(v, a * dt, 0); - NavState expected(nRb, expected_position, expected_velocity); - EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); -} - -/* ************************************************************************* */ -TEST(ImuFactor, PreintegratedMeasurements) { - // Measurements - Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); - double deltaT = 0.5; - - // Expected preintegrated values - Vector3 expectedDeltaP1; - expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; - Vector3 expectedDeltaV1(0.05, 0.0, 0.0); - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT1(0.5); - - // Actual preintegrated values - PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - 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; - expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; - Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) - + expectedDeltaR1.matrix() * measuredAcc * 0.5; - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT2(1); - - // Actual preintegrated values - PreintegratedImuMeasurements actual2 = actual1; - actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - 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 { -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 -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 p = - PreintegratedImuMeasurements::Params::MakeSharedD(); - p->gyroscopeCovariance = kMeasuredOmegaCovariance; - p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); - p->accelerometerCovariance = kMeasuredAccCovariance; - p->integrationCovariance = kIntegrationErrorCovariance; - p->use2ndOrderCoriolis = true; - - PreintegratedImuMeasurements pim(p, kZeroBiasHat); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // biasCorrectedDelta - Matrix96 actualH; - pim.biasCorrectedDelta(kZeroBias, actualH); - Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), kZeroBias); - EXPECT(assert_equal(expectedH, actualH)); - - Matrix9 aH1; - Matrix96 aH2; - NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); - Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, - boost::none), state1); - EXPECT(assert_equal(eH1, aH1)); - Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, - boost::none), kZeroBias); - EXPECT(assert_equal(eH2, aH2)); - return; - -} - -/* ************************************************************************* */ -TEST(ImuFactor, ErrorAndJacobians) { - using namespace common; - PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); - - // Expected error - Vector expectedError(9); - expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; - EXPECT( - assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); - - Values values; - values.insert(X(1), x1); - values.insert(V(1), v1); - values.insert(X(2), x2); - values.insert(V(2), v2); - values.insert(B(1), kZeroBias); - EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); - - // Make sure linearization is correct - double diffDelta = 1e-7; - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); - - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a); - - // Make sure rotation part is correct when error is interpreted as axis-angle - // Jacobians are around zero, so the rotation part is the same as: - Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1); - EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); - - Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); - EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); - - // Evaluate error with wrong values - Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); - values.update(V(2), v2_wrong); - expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901; - EXPECT( - assert_equal(expectedError, - factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2)); - EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2)); - - // Make sure the whitening is done correctly - Matrix cov = pim.preintMeasCov(); - Matrix R = RtR(cov.inverse()); - 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-3); -} - -/* ************************************************************************* */ -TEST(ImuFactor, ErrorAndJacobianWithBiases) { - using common::x1; - using common::v1; - using common::v2; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); - - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) - + Vector3(0.2, 0.0, 0.0); - double deltaT = 1.0; - - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); - PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Make sure of biasCorrectedDelta - Matrix96 actualH; - pim.biasCorrectedDelta(bias, actualH); - Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), bias); - EXPECT(assert_equal(expectedH, actualH)); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis); - - Values values; - values.insert(X(1), x1); - values.insert(V(1), v1); - values.insert(X(2), x2); - values.insert(V(2), v2); - values.insert(B(1), bias); - - // Make sure linearization is correct - double diffDelta = 1e-7; - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); -} - -/* ************************************************************************* */ -TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { - using common::x1; - using common::v1; - using common::v2; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); - - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) - + Vector3(0.2, 0.0, 0.0); - double deltaT = 1.0; - - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create factor - Pose3 bodyPsensor = Pose3(); - bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis); - - Values values; - values.insert(X(1), x1); - values.insert(V(1), v1); - values.insert(X(2), x2); - values.insert(V(2), v2); - values.insert(B(1), bias); - - // Make sure linearization is correct - double diffDelta = 1e-7; - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); -} - -/* ************************************************************************* */ -TEST(ImuFactor, PartialDerivative_wrt_Bias) { - // Linearization point - Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias - - // Measurements - Vector3 measuredOmega(0.1, 0, 0); - double deltaT = 0.5; - - // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11( - boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), - Vector3(biasOmega)); - - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); - - Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign - - // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9)); -} - -/* ************************************************************************* */ -TEST(ImuFactor, PartialDerivativeLogmap) { - // Linearization point - Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias - - // Measurements - Vector3 deltatheta(0, 0, 0); - - // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - - Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); - - // Compare Jacobians - EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); -} - -/* ************************************************************************* */ -TEST(ImuFactor, fistOrderExponential) { - // Linearization point - Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias - - // Measurements - Vector3 measuredOmega(0.1, 0, 0); - double deltaT = 1.0; - - // change w.r.t. linearization point - double alpha = 0.0; - Vector3 deltabiasOmega; - deltabiasOmega << alpha, alpha, alpha; - - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); - - Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign - - const Matrix expectedRot = Rot3::Expmap( - (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - - const Matrix3 hatRot = - Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = hatRot - * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - - // This is a first order expansion so the equality is only an approximation - EXPECT(assert_equal(expectedRot, actualRot)); -} - -/* ************************************************************************* */ -TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); - - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - - // Actual preintegrated values - PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, - deltaTs); - - // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - Matrix expectedDelRdelBias = - numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); - Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); - - // Compare Jacobians - EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT( - assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); - EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT( - assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); - EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); -} - -/* ************************************************************************* */ -TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); - - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) - + Vector3(0.2, 0.0, 0.0); - double dt = 0.1; - - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 0)); - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); - - // Get mean prediction from "ground truth" measurements - PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, dt, body_P_sensor, - measuredAcc, measuredOmega)); - - Matrix expected(9,9); - expected << - 0.0290780477, 4.63277848e-07, 9.23468723e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // - 4.63277848e-07, 0.0290688208, 4.62505461e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // - 9.23468723e-05, 4.62505461e-06, 0.0299907267, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // - 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, 0.0, // - 0.0, 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, // - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, // - 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, 0.0, // - 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, // - 0.0, 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01; - pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); - EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-6)); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis); - - // Predict - Pose3 actual_x2; - Vector3 actual_v2; - ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim, - kGravityAlongNavZDown, kZeroOmegaCoriolis); - - // Regression test with - Rot3 expectedR( // - 0.456795409, -0.888128414, 0.0506544554, // - 0.889548908, 0.45563417, -0.0331699173, // - 0.00637924528, 0.0602114814, 0.998165258); - Point3 expectedT(5.30373101, 0.768972495, -49.9942188); - Vector3 expected_v2(0.107462014, -0.46205501, 0.0115624037); - Pose3 expected_x2(expectedR, expectedT); - EXPECT(assert_equal(expected_x2, actual_x2, 1e-7)); - EXPECT(assert_equal(Vector(expected_v2), actual_v2, 1e-7)); - - Values values; - values.insert(X(1), x1); - values.insert(V(1), v1); - values.insert(X(2), x2); - values.insert(V(2), v2); - values.insert(B(1), bias); - - // Make sure linearization is correct - double diffDelta = 1e-7; - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); -} - -/* ************************************************************************* */ -TEST(ImuFactor, PredictPositionAndVelocity) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, 0; // M_PI/10.0+0.3; - Vector3 measuredAcc; - measuredAcc << 0, 1, -9.81; - double deltaT = 0.001; - - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, true); - - for (int i = 0; i < 1000; ++i) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create factor - 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, - kGravityAlongNavZDown, kZeroOmegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; - expectedVelocity << 0, 1, 0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); -} - -/* ************************************************************************* */ -TEST(ImuFactor, PredictRotation) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3; - Vector3 measuredAcc; - measuredAcc << 0, 0, -9.81; - double deltaT = 0.001; - - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, true); - - for (int i = 0; i < 1000; ++i) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create factor - 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, kGravityAlongNavZDown, - kZeroOmegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); - Vector3 expectedVelocity; - expectedVelocity << 0, 0, 0; - EXPECT(assert_equal(expectedPose, x2)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); -} - -/* ************************************************************************* */ -TEST(ImuFactor, PredictArbitrary) { - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); - - // Measurements - Vector3 measuredOmega(M_PI / 10, M_PI / 10, M_PI / 10); - Vector3 measuredAcc(0.1, 0.2, -9.81); - double dt = 0.001; - - ImuFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - Pose3 x1; - Vector3 v1 = Vector3(0, 0, 0); - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, Pose3(), - measuredAcc, measuredOmega)); - - for (int i = 0; i < 1000; ++i) - pim.integrateMeasurement(measuredAcc, measuredOmega, dt); - - Matrix expected(9,9); - expected << // - 0.0299999995, 2.46739898e-10, 2.46739896e-10, -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, // - 2.46739898e-10, 0.0299999995, 2.46739902e-10, -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, // - 2.46739896e-10, 2.46739902e-10, 0.0299999995, 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, // - -0.0144839494, -0.0454268484, 0.00489577218, 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, // - 0.044978128, -0.0149428917, 0.00839301168, 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, // - 0.0100471195, 0.00609093775, 0.000448720395, -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, // - -0.0409843415, -0.13554868, 0.00879031682, 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, // - 0.134423822, -0.0471183681, 0.0162199769, 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, // - 0.0383280513, 0.0247643646, 0.00112485862, -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764; - EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); - - // Predict - Pose3 x2; - Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); - - // Regression test for Imu Refactor - Rot3 expectedR( // - +0.903715275, -0.250741668, 0.347026393, // - +0.347026393, 0.903715275, -0.250741668, // - -0.250741668, 0.347026393, 0.903715275); - Point3 expectedT(-0.505517319, 0.569413747, 0.0861035711); - Vector3 expectedV(-1.59121524, 1.55353139, 0.3376838540); - Pose3 expectedPose(expectedR, expectedT); - EXPECT(assert_equal(expectedPose, x2, 1e-7)); - EXPECT(assert_equal(Vector(expectedV), v2, 1e-7)); -} - -/* ************************************************************************* */ -TEST(ImuFactor, bodyPSensorNoBias) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) - - // Measurements - Vector3 n_gravity(0, 0, -9.81); // z-up nav frame - Vector3 omegaCoriolis(0, 0, 0); - // Sensor frame is z-down - // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame - Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); - // Acc measurement is acceleration of sensor in the sensor frame, when stationary, - // table exerts an equal and opposite force w.r.t gravity - Vector3 s_accMeas(0, 0, -9.81); - double dt = 0.001; - - // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); - - ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); - - for (int i = 0; i < 1000; ++i) - pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0, 0); - PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, - omegaCoriolis); - - Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - - Vector3 expectedVelocity(0, 0, 0); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); -} - -/* ************************************************************************* */ -#include -#include -#include -#include -#include - -TEST(ImuFactor, bodyPSensorWithBias) { - using noiseModel::Diagonal; - typedef imuBias::ConstantBias Bias; - - int numFactors = 80; - Vector6 noiseBetweenBiasSigma; - noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, - 3.0e-6, 3.0e-6); - SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); - - // Measurements - Vector3 n_gravity(0, 0, -9.81); - Vector3 omegaCoriolis(0, 0, 0); - - // Sensor frame is z-down - // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame - Vector3 measuredOmega(0, 0.01, 0); - // Acc measurement is acceleration of sensor in the sensor frame, when stationary, - // table exerts an equal and opposite force w.r.t gravity - Vector3 measuredAcc(0, 0, -9.81); - - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); - - Matrix3 accCov = 1e-7 * I_3x3; - Matrix3 gyroCov = 1e-8 * I_3x3; - Matrix3 integrationCov = 1e-9 * I_3x3; - double deltaT = 0.005; - - // Specify noise values on priors - Vector6 priorNoisePoseSigmas( - (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); - Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); - Vector6 priorNoiseBiasSigmas( - (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); - SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); - SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas); - SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); - Vector3 zeroVel(0, 0, 0); - - // Create a factor graph with priors on initial pose, vlocity and bias - NonlinearFactorGraph graph; - Values values; - - PriorFactor priorPose(X(0), Pose3(), priorNoisePose); - graph.add(priorPose); - values.insert(X(0), Pose3()); - - PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); - graph.add(priorVel); - values.insert(V(0), zeroVel); - - // The key to this test is that we specify the bias, in the sensor frame, as known a priori - // We also create factors below that encode our assumption that this bias is constant over time - // In theory, after optimization, we should recover that same bias estimate - Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) - PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); - graph.add(priorBiasFactor); - values.insert(B(0), priorBias); - - // Now add IMU factors and bias noise models - Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - for (int i = 1; i < numFactors; i++) { - ImuFactor::PreintegratedMeasurements pim = - ImuFactor::PreintegratedMeasurements(zeroBias, accCov, gyroCov, - integrationCov, true); - for (int j = 0; j < 200; ++j) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, - body_P_sensor); - - // Create factors - graph.add( - ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, - omegaCoriolis)); - graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); - - values.insert(X(i), Pose3()); - values.insert(V(i), zeroVel); - values.insert(B(i), priorBias); - } - - // Finally, optimize, and get bias at last time step - Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); - Bias biasActual = results.at(B(numFactors - 1)); - - // And compare it with expected value (our prior) - Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); - EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); -} -#endif - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - - From 6302a795330a00b44c5a61aab142e17238958252 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 4 Jun 2016 23:00:48 -0400 Subject: [PATCH 18/22] added functionality to change bias and reset integration --- gtsam/navigation/PreintegrationBase.cpp | 6 ++++++ gtsam/navigation/PreintegrationBase.h | 5 +++++ gtsam/navigation/tests/testImuFactor.cpp | 25 ++++++++++++++++++++++++ 3 files changed, 36 insertions(+) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index d99c86952..0e8cf67c9 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -49,6 +49,12 @@ void PreintegrationBase::print(const string& s) const { cout << s << *this << endl; } +//------------------------------------------------------------------------------ +void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) { + biasHat_ = biasHat; + resetIntegration(); +} + //------------------------------------------------------------------------------ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 08dcd1381..06be4642d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -97,6 +97,11 @@ public: /// Re-initialize PreintegratedMeasurements virtual void resetIntegration()=0; + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements and set new bias + void resetIntegrationAndSetBias(const Bias& biasHat); + /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { return p_.get() == other.p_.get(); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9db7aac72..d1dc6316d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -67,6 +67,31 @@ TEST(ImuFactor, PreintegratedMeasurementsConstruction) { DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); } +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurementsReset) { + + auto p = testing::Params(); + // Create a preintegrated measurement struct and integrate + PreintegratedImuMeasurements pimActual(p); + Vector3 measuredAcc(0.5, 1.0, 0.5); + Vector3 measuredOmega(0.1, 0.3, 0.1); + double deltaT = 1.0; + pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // reset and make sure that it is the same as a fresh one + pimActual.resetIntegration(); + CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p))); + + // Now create one with a different bias .. + Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); + PreintegratedImuMeasurements pimExpected(p, nonZeroBias); + + // integrate again, then reset to a new bias + pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pimActual.resetIntegrationAndSetBias(nonZeroBias); + CHECK(assert_equal(pimActual, pimExpected)); +} + /* ************************************************************************* */ TEST(ImuFactor, Accelerating) { const double a = 0.2, v = 50; From 6bed20b28af4e91f4ad63674a2f391e8f33097d7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 5 Jun 2016 00:58:38 -0700 Subject: [PATCH 19/22] Abandoned silly coercion of NavState into Lie group. It's just a manifold: it does not make sense to compose two NavStates, or take the inverse of a NavState. --- gtsam/navigation/NavState.cpp | 178 +++++++---------------- gtsam/navigation/NavState.h | 88 +++-------- gtsam/navigation/TangentPreintegration.h | 2 +- gtsam/navigation/tests/testNavState.cpp | 91 ++++-------- 4 files changed, 105 insertions(+), 254 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3a8876ec5..50949c761 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -24,6 +24,18 @@ namespace gtsam { #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; +//------------------------------------------------------------------------------ +NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v, + OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 3> H3) { + if (H1) + *H1 << I_3x3, Z_3x3, Z_3x3; + if (H2) + *H2 << Z_3x3, R.transpose(), Z_3x3; + if (H3) + *H3 << Z_3x3, Z_3x3, R.transpose(); + return NavState(R, t, v); +} //------------------------------------------------------------------------------ NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { @@ -94,145 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const { } //------------------------------------------------------------------------------ -NavState NavState::inverse() const { +NavState NavState::retract(const Vector9& xi, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { TIE(nRb, n_t, n_v, *this); - const Rot3 bRn = nRb.inverse(); - return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); -} - -//------------------------------------------------------------------------------ -NavState NavState::operator*(const NavState& bTc) const { - TIE(nRb, n_t, n_v, *this); - TIE(bRc, b_t, b_v, bTc); - return NavState(nRb * bRc, n_t + nRb * b_t, n_v + nRb * b_v); -} - -//------------------------------------------------------------------------------ -//NavState NavState::retract(const Vector9& xi) const { -// TIE(nRb, n_t, n_v, *this); -// const Rot3 bRc = Rot3::Expmap(dR(xi)); -// return NavState(nRb * bRc, n_t + nRb * dP(xi), n_v + nRb * dV(xi)); -//} - -//------------------------------------------------------------------------------ -NavState::PositionAndVelocity // -NavState::operator*(const PositionAndVelocity& b_tv) const { - TIE(nRb, n_t, n_v, *this); - const Point3& b_t = b_tv.first; - const Velocity3& b_v = b_tv.second; - return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); -} - -//------------------------------------------------------------------------------ -Point3 NavState::operator*(const Point3& b_t) const { - return Point3(R_ * b_t + t_); -} - -//------------------------------------------------------------------------------ -NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, - OptionalJacobian<9, 9> H) { - Matrix3 D_R_xi; - const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0); - const Point3 p = Point3(dP(xi)); - const Vector v = dV(xi); - const NavState result(R, p, v); - if (H) { - *H << D_R_xi, Z_3x3, Z_3x3, // - Z_3x3, R.transpose(), Z_3x3, // - Z_3x3, Z_3x3, R.transpose(); + Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; + const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); + const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); + const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0); + const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0); + if (H1) { + *H1 << D_R_nRb, Z_3x3, Z_3x3, // + // Note(frank): the derivative of n_t with respect to xi is nRb + // We pre-multiply with nRc' to account for NavState::Create + // Then we make use of the identity nRc' * nRb = bRc' + nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3, + // Similar reasoning for v: + nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose(); } - return result; + if (H2) { + *H2 << D_bRc_xi, Z_3x3, Z_3x3, // + Z_3x3, bRc.transpose(), Z_3x3, // + Z_3x3, Z_3x3, bRc.transpose(); + } + return NavState(nRc, t, v); } //------------------------------------------------------------------------------ -Vector9 NavState::ChartAtOrigin::Local(const NavState& x, - OptionalJacobian<9, 9> H) { +Vector9 NavState::localCoordinates(const NavState& g, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + Matrix3 D_dR_R, D_dt_R, D_dv_R; + const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + Vector9 xi; Matrix3 D_xi_R; - xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); - if (H) { - *H << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, x.R(), Z_3x3, // - Z_3x3, Z_3x3, x.R(); + xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + if (H1) { + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; + } + if (H2) { + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } return xi; } -//------------------------------------------------------------------------------ -NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { - if (H) - throw runtime_error("NavState::Expmap derivative not implemented yet"); - - Eigen::Block n_omega_nb = dR(xi); - Eigen::Block v = dP(xi); - Eigen::Block a = dV(xi); - - // NOTE(frank): See Pose3::Expmap - Rot3 nRb = Rot3::Expmap(n_omega_nb); - double theta2 = n_omega_nb.dot(n_omega_nb); - if (theta2 > numeric_limits::epsilon()) { - // Expmap implements a "screw" motion in the direction of n_omega_nb - Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis - Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis - Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel) - / theta2; - Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis - Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis - Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2; - return NavState(nRb, n_t, n_v); - } else { - return NavState(nRb, Point3(v), a); - } -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { - if (H) - throw runtime_error("NavState::Logmap derivative not implemented yet"); - - TIE(nRb, n_p, n_v, nTb); - Vector3 n_t = n_p; - - // NOTE(frank): See Pose3::Logmap - Vector9 xi; - Vector3 n_omega_nb = Rot3::Logmap(nRb); - double theta = n_omega_nb.norm(); - if (theta * theta <= numeric_limits::epsilon()) { - xi << n_omega_nb, n_t, n_v; - } else { - Matrix3 W = skewSymmetric(n_omega_nb / theta); - // Formula from Agrawal06iros, equation (14) - // simplified with Mathematica, and multiplying in n_t to avoid matrix math - double factor = (1 - theta / (2. * tan(0.5 * theta))); - Vector3 n_x = W * n_t; - Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x); - Vector3 n_y = W * n_v; - Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y); - xi << n_omega_nb, v, a; - } - return xi; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::AdjointMap() const { - // NOTE(frank): See Pose3::AdjointMap - const Matrix3 nRb = R(); - Matrix3 pAr = skewSymmetric(t()) * nRb; - Matrix3 vAr = skewSymmetric(v()) * nRb; - Matrix9 adj; - // nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV - adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb; - return adj; -} - -//------------------------------------------------------------------------------ -Matrix7 NavState::wedge(const Vector9& xi) { - const Matrix3 Omega = skewSymmetric(dR(xi)); - Matrix7 T; - T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0; - return T; -} - //------------------------------------------------------------------------------ // sugar for derivative blocks #define D_R_R(H) (H)->block<3,3>(0,0) diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 02da46317..a1544735d 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -20,7 +20,7 @@ #include #include -#include +#include namespace gtsam { @@ -29,9 +29,9 @@ typedef Vector3 Velocity3; /** * Navigation state: Pose (rotation, translation) + velocity - * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity + * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold */ -class NavState: public LieGroup { +class NavState { private: // TODO(frank): @@ -42,6 +42,10 @@ private: public: + enum { + dimension = 9 + }; + typedef std::pair PositionAndVelocity; /// @name Constructors @@ -49,7 +53,7 @@ public: /// Default constructor NavState() : - t_(0,0,0), v_(Vector3::Zero()) { + t_(0, 0, 0), v_(Vector3::Zero()) { } /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : @@ -59,15 +63,15 @@ public: NavState(const Pose3& pose, const Velocity3& v) : R_(pose.rotation()), t_(pose.translation()), v_(v) { } - /// Construct from Matrix group representation (no checking) - NavState(const Matrix7& T) : - R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) { - } /// Construct from SO(3) and R^6 NavState(const Matrix3& R, const Vector9 tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } /// Named constructor with derivatives + static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, + OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 3> H3); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); @@ -116,7 +120,8 @@ public: /// @{ /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const NavState& state); /// print void print(const std::string& s = "") const; @@ -124,29 +129,6 @@ public: /// equals bool equals(const NavState& other, double tol = 1e-8) const; - /// @} - /// @name Group - /// @{ - - /// identity for group operation - static NavState identity() { - return NavState(); - } - - /// inverse transformation with derivatives - NavState inverse() const; - - /// Group compose is the semi-direct product as specified below: - /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v) - NavState operator*(const NavState& bTc) const; - - /// Native group action is on position/velocity pairs *in the body frame* as follows: - /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v) - PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const; - - /// Act on position alone, n_t = nRb * b_t + n_t - Point3 operator*(const Point3& b_t) const; - /// @} /// @name Manifold /// @{ @@ -172,32 +154,15 @@ public: return v.segment<3>(6); } - // Chart at origin, constructs components separately (as opposed to Expmap) - struct ChartAtOrigin { - static NavState Retract(const Vector9& xi, // - OptionalJacobian<9, 9> H = boost::none); - static Vector9 Local(const NavState& x, // - OptionalJacobian<9, 9> H = boost::none); - }; + /// retract with optional derivatives + NavState retract(const Vector9& v, // + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = + boost::none) const; - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map at identity - create a NavState from canonical coordinates - static NavState Expmap(const Vector9& xi, // - OptionalJacobian<9, 9> H = boost::none); - - /// Log map at identity - return the canonical coordinates for this NavState - static Vector9 Logmap(const NavState& p, // - OptionalJacobian<9, 9> H = boost::none); - - /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms - /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); - Matrix9 AdjointMap() const; - - /// wedge creates Lie algebra element from tangent vector - static Matrix7 wedge(const Vector9& xi); + /// localCoordinates with optional derivatives + Vector9 localCoordinates(const NavState& g, // + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = + boost::none) const; /// @} /// @name Dynamics @@ -237,14 +202,7 @@ private: // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors template<> -struct traits : Testable, internal::LieGroupTraits { +struct traits : internal::Manifold { }; -// Partial specialization of wedge -// TODO: deprecate, make part of traits -template<> -inline Matrix wedge(const Vector& xi) { - return NavState::wedge(xi); -} - } // namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 73842ffca..dddafad7a 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -67,7 +67,7 @@ public: Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } - NavState deltaXij() const override { return NavState::Retract(preintegrated_); } + NavState deltaXij() const override { return NavState().retract(preintegrated_); } const Vector9& preintegrated() const { return preintegrated_; } Vector3 theta() const { return preintegrated_.head<3>(); } diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index fb6045d33..8ea8ce9b5 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { + boost::function create = + boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none, + boost::none); + Matrix aH1, aH2, aH3; + EXPECT( + assert_equal(kState1, + NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); + EXPECT( + assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); + EXPECT( + assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT( + assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +} + +/* ************************************************************************* */ +TEST(NavState, Constructor2) { boost::function construct = boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::none); @@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) { EXPECT(assert_equal((Matrix )eH, aH)); } -/* ************************************************************************* */ -TEST( NavState, MatrixGroup ) { - // check roundtrip conversion to 7*7 matrix representation - Matrix7 T = kState1.matrix(); - EXPECT(assert_equal(kState1, NavState(T))); - - // check group product agrees with matrix product - NavState state2 = kState1 * kState1; - Matrix T2 = T * T; - EXPECT(assert_equal(state2, NavState(T2))); - EXPECT(assert_equal(T2, state2.matrix())); -} - /* ************************************************************************* */ TEST( NavState, Manifold ) { // Check zero xi @@ -114,7 +121,9 @@ TEST( NavState, Manifold ) { Rot3 drot = Rot3::Expmap(xi.head<3>()); Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); - NavState state2 = kState1 * NavState(drot, dt, dvel); + NavState state2 = NavState(kState1.attitude() * drot, + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -122,27 +131,6 @@ TEST( NavState, Manifold ) { NavState state3 = state2.retract(xi); EXPECT(assert_equal(xi, state2.localCoordinates(state3))); - // Check derivatives for ChartAtOrigin::Retract - Matrix9 aH; - // For zero xi - boost::function Retract = boost::bind( - NavState::ChartAtOrigin::Retract, _1, boost::none); - NavState::ChartAtOrigin::Retract(kZeroXi, aH); - EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH)); - // For non-zero xi - NavState::ChartAtOrigin::Retract(xi, aH); - EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH)); - - // Check derivatives for ChartAtOrigin::Local - // For zero xi - boost::function Local = boost::bind( - NavState::ChartAtOrigin::Local, _1, boost::none); - NavState::ChartAtOrigin::Local(kIdentity, aH); - EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH)); - // For non-zero xi - NavState::ChartAtOrigin::Local(kState1, aH); - EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH)); - // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); @@ -151,6 +139,12 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); + // Check retract derivatives on state 2 + const Vector9 xi2 = -3.0*xi; + state2.retract(xi2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1)); + EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2)); + // Check localCoordinates derivatives boost::function local = boost::bind(&NavState::localCoordinates, _1, _2, boost::none, @@ -169,29 +163,6 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } -/* ************************************************************************* */ -TEST( NavState, Lie ) { - // Check zero xi - EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi))); - EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity))); - EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi))); - EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1))); - - // Expmap/Logmap roundtrip - Vector xi(9); - xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; - NavState state2 = NavState::Expmap(xi); - EXPECT(assert_equal(xi, NavState::Logmap(state2))); - - // roundtrip from state2 to state3 and back - NavState state3 = state2.expmap(xi); - EXPECT(assert_equal(xi, state2.logmap(state3))); - - // For the expmap/logmap (not necessarily expmap/local) -xi goes other way - EXPECT(assert_equal(state2, state3.expmap(-xi))); - EXPECT(assert_equal(xi, -state3.logmap(state2))); -} - /* ************************************************************************* */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 TEST(NavState, Update) { @@ -201,8 +172,8 @@ TEST(NavState, Update) { Matrix9 aF; Matrix93 aG1, aG2; boost::function update = - boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, - boost::none, boost::none); + boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, + boost::none, boost::none); Vector3 b_acc = kAttitude * acc; NavState expected(kAttitude.expmap(dt * omega), kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), From 8aecbfd9515274c63562fb39612b7336b529e51f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 5 Jun 2016 01:04:01 -0700 Subject: [PATCH 20/22] Trying different fix for Python wrapper --- python/handwritten/navigation/ImuFactor.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index 4e55c8fc4..2e477e506 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -34,7 +34,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PreintegratedImuMeasurements::equals, 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegratedImuMeasurements::predict, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationType::predict, 2, 4) void exportImuFactor() { class_("OptionalJacobian39", init<>()); @@ -80,13 +80,18 @@ void exportImuFactor() { // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html register_ptr_to_python< boost::shared_ptr >(); - class_("PreintegratedImuMeasurements", + class_( + "PreintegrationType", + init&, const imuBias::ConstantBias&>()) + .def("predict", &PreintegrationType::predict, predict_overloads()) + .def("computeError", &PreintegrationType::computeError) + .def("resetIntegration", &PreintegrationType::resetIntegration) + .def("deltaTij", &PreintegrationType::deltaTij); + + class_>( + "PreintegratedImuMeasurements", init&, const imuBias::ConstantBias&>()) .def(repr(self)) - .def("predict", &PreintegratedImuMeasurements::predict, predict_overloads()) - .def("computeError", &PreintegratedImuMeasurements::computeError) - .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) - .def("deltaTij", &PreintegratedImuMeasurements::deltaTij) .def("equals", &PreintegratedImuMeasurements::equals, equals_overloads(args("other", "tol"))) .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement) .def("integrateMeasurements", &PreintegratedImuMeasurements::integrateMeasurements) From 9f84b46e3f843149bfe2d2ed201912bc36132c87 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 5 Jun 2016 01:16:35 -0700 Subject: [PATCH 21/22] TRying to get past compile issue on Jenkins --- python/handwritten/navigation/ImuFactor.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index 2e477e506..afc6f331d 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -34,7 +34,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PreintegratedImuMeasurements::equals, 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationType::predict, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationBase::predict, 2, 4) void exportImuFactor() { class_("OptionalJacobian39", init<>()); @@ -81,7 +81,11 @@ void exportImuFactor() { register_ptr_to_python< boost::shared_ptr >(); class_( - "PreintegrationType", +#ifdef GTSAM_TANGENT_PREINTEGRATION + "TangentPreintegration", +#else + "ManifoldPreintegration", +#endif init&, const imuBias::ConstantBias&>()) .def("predict", &PreintegrationType::predict, predict_overloads()) .def("computeError", &PreintegrationType::computeError) From 05cc8d85670f79a9da54211aeb1a463bb80b56ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 5 Jun 2016 11:02:54 -0700 Subject: [PATCH 22/22] Added ImuFactor things in readme --- README.md | 134 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 76 insertions(+), 58 deletions(-) diff --git a/README.md b/README.md index 040f1134f..88f69dec4 100644 --- a/README.md +++ b/README.md @@ -1,59 +1,77 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== - -What is GTSAM? --------------- - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -Quickstart ----------- - -In the root library folder execute: - -``` -#!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install -``` - -Prerequisites: - -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) -- A modern compiler, i.e., at least gcc 4.7.3 on Linux. - -Optional prerequisites - used automatically if findable by CMake: - -- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) - -GTSAM 4 Compatibility ---------------------- - -GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. - -Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. - -Additional Information ----------------------- - -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. - -See the [`INSTALL`](INSTALL) file for more detailed installation instructions. - -GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. - -Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. - +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- A modern compiler, i.e., at least gcc 4.7.3 on Linux. + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +GTSAM 4 Compatibility +--------------------- + +GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. + +Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. + +The Preintegrated IMU Factor +---------------------------- + +GTSAM includes a state of the art IMU handling scheme based on + +- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + +Our implementation improves on this using integration on the manifold, as detailed in + +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. +- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. + +If you are using the factor in academic work, please cite the publications above. + +In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. + + + +Additional Information +---------------------- + +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. + +See the [`INSTALL`](INSTALL) file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. + +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. + GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file