Add ifdefs that restore the old IMU factor functionality
							parent
							
								
									aea1f1e572
								
							
						
					
					
						commit
						fa15264e83
					
				|  | @ -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>(); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = | ||||
|           boost::none, boost::optional<Matrix&> 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
 | ||||
|  |  | |||
|  | @ -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 { | ||||
|  |  | |||
|  | @ -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, | ||||
|  |  | |||
|  | @ -37,15 +37,28 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& 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<Vector3, Vector3> 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<Vector3, Vector3> 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<Vector3, Vector3> 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 | ||||
| //------------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Vector3, Vector3> 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<PreintegrationBase> clone() const { | ||||
|     return boost::shared_ptr<PreintegrationBase>(); | ||||
|   } | ||||
| 
 | ||||
| 
 | ||||
| #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 | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1), | ||||
|                       pim.preintegrated_H_biasOmega(), 1e-3)); | ||||
| } | ||||
| #endif | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(CombinedImuFactor, PredictPositionAndVelocity) { | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1), | ||||
|                       expected_pim02.preintegrated_H_biasOmega(), 1e-7)); | ||||
| } | ||||
| #endif | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue