slight code optimization

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