WIP: Resolve "Eigen::Matrix::cast<ceres::Jet>() not needed if upgrading to Eigen 3.3"

Closes #257 (closed)

include/core/factor/factor_diff_drive.h

@@ -104,26 +104,19 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
 
     Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
 
-    // Compute corrected delta
 
-    /// Is this my delta_preint ? Yes!
-    const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>();
+    // Correct delta due to changes in the calibration parameters
+    auto delta_corrected = getMeasurement() + J_delta_calib_ * (c - calib_preint_);
 
-    Matrix<T, 3, 1> c_preint = calib_preint_.cast<T>();
-
-    Eigen::Matrix<T, 3, 1> delta_corrected = delta_preint + J_delta_calib_.cast<T>() * (c - c_preint);
-
-    // 2d pose residual
 
+    // Predict delta from states
     Eigen::Matrix<T, 3, 1> delta_predicted;
-
     // position
     delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
-
     // orientation
     delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
 
-    // residual
+    // 2d pose residual
     residuals = delta_corrected - delta_predicted;
 
     // angle remapping