diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h index 274c226ed779ced623f5ae8c0eb76c0e5bc4d291..d26cfcd7c996f10ae7b81bdb4a7eb406a876fbf2 100644 --- a/include/core/factor/factor_diff_drive.h +++ b/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 diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index 6caa5f868adceb291690590455bea94520771b0f..d210d941b516b0651fb6f871bbdcc6a48b1ec69f 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/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());