WIP Try removing some cast<T>() and see

It works.
Unclear whether this is faster in Ceres. Need more testing.

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

test/gtest_factor_diff_drive.cpp

@@ -620,7 +620,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
 
     WOLF_TRACE("\n  ========== SOLVE =========");
-    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE("\n", report);
 
     WOLF_TRACE("x1           : ", problem->getState(N*dt).transpose());