diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 187261685..6a720972c 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -48,6 +48,16 @@ void PreintegratedImuMeasurements::resetIntegration() { preintMeasCov_.setZero(); } +// 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) //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, @@ -81,20 +91,19 @@ void PreintegratedImuMeasurements::integrateMeasurement( // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise, // as G and measurementCovariance are block-diagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - preintMeasCov_.block<3, 3>(0, 0) += p().integrationCovariance * deltaT; - preintMeasCov_.block<3, 3>(3, 3) += dRij * p().accelerometerCovariance - * dRij.transpose() * deltaT; - preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega - * p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT; + D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT; + D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT; + D_R_R(&preintMeasCov_) += D_Rincr_integratedOmega * p().gyroscopeCovariance + * D_Rincr_integratedOmega.transpose() * deltaT; Matrix3 F_pos_noiseacc; F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; - preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc + D_t_t(&preintMeasCov_) += (1 / deltaT) * F_pos_noiseacc * p().accelerometerCovariance * F_pos_noiseacc.transpose(); Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose(); // has 1/deltaT - preintMeasCov_.block<3, 3>(0, 3) += temp; - preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); + D_t_v(&preintMeasCov_) += temp; + D_v_t(&preintMeasCov_) += temp.transpose(); // F_test and G_test are given as output for testing purposes and are not needed by the factor if (F_test) { @@ -102,10 +111,11 @@ void PreintegratedImuMeasurements::integrateMeasurement( } if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise - // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, dRij * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + // omegaNoise intNoise accNoise + (*G_test) << + D_Rincr_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle + Z_3x3, I_3x3 * deltaT, F_pos_noiseacc, // pos + Z_3x3, Z_3x3, dRij * deltaT; // vel } } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 980f7d3f3..32ee4185e 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -63,7 +63,7 @@ class PreintegratedImuMeasurements: public PreintegrationBase { protected: - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). /// Default constructor for serialization diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 367a62360..92f50fa3a 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -63,25 +63,22 @@ void PreintegrationBase::updatePreintegratedMeasurements( const Vector3& correctedAcc, const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - const Matrix3 dRij = deltaRij_.matrix(); // expensive - const Vector3 i_acc = dRij * correctedAcc; // acceleration in i frame + // Calculate acceleration in *current* i frame, i.e., before rotation update below + Matrix3 D_acc_R; + const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F? &D_acc_R : 0); - double dt22 = 0.5 * deltaT * deltaT; - deltaPij_ += deltaVij_ * deltaT + dt22 * i_acc; - deltaVij_ += deltaT * i_acc; - - Matrix3 R_i, F_angles_angles; + Matrix3 F_angles_angles; updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); - if (F) { - const Matrix3 F_vel_angles = -dRij * skewSymmetric(correctedAcc) * deltaT; - Matrix3 F_pos_angles; + double dt22 = 0.5 * deltaT * deltaT; + deltaPij_ += dt22 * i_acc + deltaT * deltaVij_; + deltaVij_ += deltaT * i_acc; - // pos vel angle - *F << // - I_3x3, I_3x3 * deltaT, 0.5 * F_vel_angles * deltaT, // pos - Z_3x3, I_3x3, F_vel_angles, // vel - Z_3x3, Z_3x3, F_angles_angles; // angle + if (F) { + *F << // angle pos vel + F_angles_angles, Z_3x3, Z_3x3, // angle + dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos + deltaT * D_acc_R, Z_3x3, I_3x3; // vel } } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c95c1d667..9ab2d8a51 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -628,18 +628,15 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old); - Matrix FexpectedTop6(6, 9); - FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - // Compute expected f_rot wrt angles Matrix dfr_dangle = numericalDerivative11( boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), deltaRij_old); - Matrix FexpectedBottom3(3, 9); - FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; Matrix Fexpected(9, 9); - Fexpected << FexpectedTop6, FexpectedBottom3; + Fexpected << // + dfr_dangle, Z_3x3, Z_3x3, // + dfpv_dangle, dfpv_dpos, dfpv_dvel; EXPECT(assert_equal(Fexpected, Factual)); @@ -659,25 +656,25 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { Matrix dgpv_domegaNoise = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega); - Matrix GexpectedTop6(6, 9); - GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; // Compute expected f_rot wrt gyro noise Matrix dgr_dangle = numericalDerivative11( boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), newMeasuredOmega); - Matrix GexpectedBottom3(3, 9); - GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; Matrix Gexpected(9, 9); - Gexpected << GexpectedTop6, GexpectedBottom3; + Gexpected << // + dgr_dangle, Z_3x3, Z_3x3, // + dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar - * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; + measurementCovariance << // + omegaNoiseVar * I_3x3, Z_3x3, Z_3x3, // + Z_3x3, intNoiseVar * I_3x3, Z_3x3, // + Z_3x3, Z_3x3, accNoiseVar * I_3x3; Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() @@ -745,18 +742,15 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old); - Matrix FexpectedTop6(6, 9); - FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - // Compute expected f_rot wrt angles Matrix dfr_dangle = numericalDerivative11( boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), deltaRij_old); - Matrix FexpectedBottom3(3, 9); - FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; Matrix Fexpected(9, 9); - Fexpected << FexpectedTop6, FexpectedBottom3; + Fexpected << // + dfr_dangle, Z_3x3, Z_3x3, // + dfpv_dangle, dfpv_dpos, dfpv_dvel; EXPECT(assert_equal(Fexpected, Factual)); @@ -776,25 +770,25 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { Matrix dgpv_domegaNoise = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega); - Matrix GexpectedTop6(6, 9); - GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; // Compute expected f_rot wrt gyro noise Matrix dgr_dangle = numericalDerivative11( boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), newMeasuredOmega); - Matrix GexpectedBottom3(3, 9); - GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; Matrix Gexpected(9, 9); - Gexpected << GexpectedTop6, GexpectedBottom3; + Gexpected << // + dgr_dangle, Z_3x3, Z_3x3, // + dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar - * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; + measurementCovariance << // + omegaNoiseVar * I_3x3, Z_3x3, Z_3x3, // + Z_3x3, intNoiseVar * I_3x3, Z_3x3, // + Z_3x3, Z_3x3, accNoiseVar * I_3x3; Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose()