diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ea68f724e..0b23d299c 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -50,18 +50,33 @@ void PreintegratedCombinedMeasurements::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) +#define D_a_a(H) (H)->block<3,3>(9,9) +#define D_g_g(H) (H)->block<3,3>(12,12) + //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional F_test, boost::optional G_test) { + OptionalJacobian<15, 15> F_test, OptionalJacobian<15, 21> G_test) { - const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion + const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion // Update preintegrated measurements. Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 G1,G2; updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT, - &D_incrR_integratedOmega, &F_9x9); + &D_incrR_integratedOmega, &F_9x9, &G1, &G2); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we @@ -74,17 +89,11 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // overall Jacobian wrt preintegrated measurements (df/dx) Eigen::Matrix F; - // for documentation: - // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, - // Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, - // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; F.setZero(); F.block<9, 9>(0, 0) = F_9x9; + F.block<3, 3>(0, 12) = H_angles_biasomega; + F.block<3, 3>(6, 9) = H_vel_biasacc; F.block<6, 6>(9, 9) = I_6x6; - F.block<3, 3>(3, 9) = H_vel_biasacc; - F.block<3, 3>(6, 12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() @@ -92,38 +101,36 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( G_measCov_Gt.setZero(15, 15); // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3, 3>(0, 0) = deltaT * p().integrationCovariance; - G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) + D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; + D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc) * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) * (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) + D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega) * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * p().biasAccCovariance; - G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * p().biasOmegaCovariance; + D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance; + D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) + Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) * H_angles_biasomega.transpose(); - G_measCov_Gt.block<3, 3>(3, 6) = block23; - G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose(); + D_v_R(&G_measCov_Gt) = temp; + D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; // F_test and G_test are used for testing purposes and are not needed by the factor if (F_test) { - F_test->resize(15, 15); (*F_test) << F; } if (G_test) { - G_test->resize(15, 21); // This is for testing & documentation ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) (*G_test) << // - I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // - Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // - Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // + Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } } @@ -198,7 +205,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + 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); @@ -242,7 +249,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, // overall error Vector r(15); - r << r_pvR, fbias; // vector of size 15 + r << r_Rpv, fbias; // vector of size 15 return r; } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 1289f22c6..5ac6d8a7e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -142,10 +142,10 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { * @param body_P_sensor Optional sensor frame (pose of the IMU in the body * frame) */ - void integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional F_test = boost::none, - boost::optional G_test = boost::none); + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, + OptionalJacobian<15, 15> F_test = boost::none, + OptionalJacobian<15, 21> G_test = boost::none); /// methods to access class variables Matrix preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 53c0d7e23..8da1cc3e7 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -47,36 +47,30 @@ namespace { Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, - const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration) { + const Vector3& correctedOmega, const double deltaT) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 deltaPij_new; - if (!use2ndOrderIntegration) { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - } else { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; - } + deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; Vector3 deltaVij_new = deltaVij_old + temp; Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT); Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more imuBias::ConstantBias bias_new(bias_old); Vector result(15); - result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + result << logDeltaRij_new, deltaPij_new, deltaVij_new, bias_new.vector(); return result; } Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, - const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration) { + const Vector3& correctedOmega, const double deltaT) { Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, - deltaT, use2ndOrderIntegration); + deltaT); return Rot3::Expmap(result.segment < 3 > (6)); } @@ -377,10 +371,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { Matrix oldPreintCovariance = pim.preintMeasCov(); Matrix Factual, Gactual; - pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, Factual, Gactual); - - bool use2ndOrderIntegration = false; + pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, Factual, + Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F @@ -388,51 +380,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Compute expected F wrt positions (15,3) Matrix df_dpos = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaPij_old); + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT + ), deltaPij_old); // rotation part has to be done properly, on manifold - df_dpos.block<3, 3>(6, 0) = numericalDerivative11( + df_dpos.block<3, 3>(0, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaPij_old); + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT + ), deltaPij_old); + EXPECT(assert_equal(df_dpos, Factual.block<15, 3>(0, 3), 1e-5)); // Compute expected F wrt velocities (15,3) Matrix df_dvel = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaVij_old); + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT + ), deltaVij_old); // rotation part has to be done properly, on manifold - df_dvel.block<3, 3>(6, 0) = numericalDerivative11( + df_dvel.block<3, 3>(0, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaVij_old); + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT + ), deltaVij_old); + EXPECT(assert_equal(df_dvel, Factual.block<15, 3>(0, 6), 1e-5)); // Compute expected F wrt angles (15,3) Matrix df_dangle = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), deltaRij_old); + newDeltaT), deltaRij_old); // rotation part has to be done properly, on manifold - df_dangle.block<3, 3>(6, 0) = numericalDerivative11( + df_dangle.block<3, 3>(0, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), deltaRij_old); + newDeltaT), deltaRij_old); + EXPECT(assert_equal(df_dangle, Factual.block<15, 3>(0, 0), 1e-5)); // Compute expected F wrt biases (15,6) Matrix df_dbias = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); + newDeltaT), bias_old); // rotation part has to be done properly, on manifold - df_dbias.block<3, 6>(6, 0) = + df_dbias.block<3, 6>(0, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); - - Matrix Fexpected(15, 15); - Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; - EXPECT(assert_equal(Fexpected, Factual,1e-5)); + newDeltaT), bias_old); + EXPECT(assert_equal(df_dbias, Factual.block<15, 6>(0, 9), 1e-5)); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR G @@ -444,24 +436,24 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Compute expected G wrt acc noise (15,3) Matrix df_daccNoise = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT + ), newMeasuredAcc); // rotation part has to be done properly, on manifold df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT + ), newMeasuredAcc); // Compute expected G wrt gyro noise (15,3) Matrix df_domegaNoise = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, - use2ndOrderIntegration), newMeasuredOmega); + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT + ), newMeasuredOmega); // rotation part has to be done properly, on manifold df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, - use2ndOrderIntegration), newMeasuredOmega); + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT + ), newMeasuredOmega); // Compute expected G wrt bias random walk noise (15,6) Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries @@ -472,13 +464,13 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { Matrix df_dinitBias = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); + newDeltaT), bias_old); // rotation part has to be done properly, on manifold df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); + newDeltaT), bias_old); df_dinitBias.block<6, 6>(9, 0) = Z_6x6; // only has to influence first 9 rows Matrix Gexpected(15, 21);