Remove cast<T> and some size errors

include/imu/factor/factor_imu.h

@@ -216,7 +216,7 @@ inline bool FactorImu::operator ()(const T* const _p1,
     Map<const Quaternion<T> > q2(_q2);
     Map<const Matrix<T,3,1> > v2(_v2);
 
-    Map<Matrix<T,15,1> > res(_res);
+    Map<Matrix<T,9,1> > res(_res);
 
     residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
 
@@ -301,23 +301,23 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
     //needed typedefs
     typedef typename D1::Scalar T;
 
-    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 15);
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 9);
 
     // 1. Expected delta from states
     Eigen::Matrix<T, 3, 1 >   dp_exp;
     Eigen::Quaternion<T>      dq_exp;
     Eigen::Matrix<T, 3, 1>    dv_exp;
 
-    imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
+    imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, dt_, dp_exp, dq_exp, dv_exp);
 
     // 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;
 
-    d_step.block(0,0,3,1) = dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
-    d_step.block(3,0,3,1) =                                                             dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
-    d_step.block(6,0,3,1) = dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
+    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_);
 
 #ifdef METHOD_1 // method 1
     Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
@@ -351,10 +351,10 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
     J_err_corr.block(3,3,3,3) = J_do_dq2;
 
     // 4. Residuals are the weighted errors
-    _res.head(9)       = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+    _res.head(9)       = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper() * d_error;
 #else
     imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
-    _res.head(9)       = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+    _res = getMeasurementSquareRootInformationUpper() * d_error;
 #endif
 
 #else // method 2
@@ -372,7 +372,7 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
     d_error << d_diff - d_step;
 
     // 4. Residuals are the weighted errors
-    _res.head(9)       = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+    _res       = getMeasurementSquareRootInformationUpper() * d_error;
 
 #endif