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); }