diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h index 7b37d079e931bf5a90daa91141770709f9e8fb7f..e036d4582e0714bf0dae9d776a5f79db08628b2a 100644 --- a/include/imu/factor/factor_imu.h +++ b/include/imu/factor/factor_imu.h @@ -269,8 +269,8 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1, * * We use the following functions from imu_tools.h: * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1 - * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T - * T = diff(D1,D2) : minus operator, T = D2 (-) D1 + * D2 = plus(D1,T) : blocl-plus operator, D2 = D1 <+> T + * T = diff(D1,D2) : block-minus operator, T = D2 <-> D1 * * Two methods are possible (select with #define below this note) : * @@ -313,16 +313,13 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1, // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint) // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint) - Eigen::Matrix<T, 9, 1> d_step; + Eigen::Matrix<T, 3, 1> dp_step, do_step, dv_step; - d_step.block(0,0,3,1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_); - d_step.block(3,0,3,1) = dDq_dwb_ * (_wb1 - gyro_bias_preint_); - d_step.block(6,0,3,1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_); + dp_step = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_); + do_step = /* it happens that dDq_dab = 0 ! */ dDq_dwb_ * (_wb1 - gyro_bias_preint_); + dv_step = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_); #ifdef METHOD_1 // method 1 - Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1); - Eigen::Matrix<T, 3, 1> do_step = d_step.block(3,0,3,1); - Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6,0,3,1); // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step Eigen::Matrix<T,3,1> dp_correct; @@ -336,7 +333,7 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1, // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr) // Note the Dt here is zero because it's the delta-time between the same time stamps! Eigen::Matrix<T, 9, 1> d_error; - Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() ); + Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() + 0); Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3); Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);