Skip to content
Snippets Groups Projects
Commit 03862f99 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

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

It works.
Unclear whether this is faster in Ceres. Need more testing.
parent 540e87c2
No related branches found
No related tags found
1 merge request!324WIP: Resolve "Eigen::Matrix::cast<ceres::Jet>() not needed if upgrading to Eigen 3.3"
Pipeline #4342 failed
This commit is part of merge request !324. Comments created here will be created in the context of that merge request.
...@@ -104,26 +104,19 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, ...@@ -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); Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
// Compute corrected delta
/// Is this my delta_preint ? Yes! // Correct delta due to changes in the calibration parameters
const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>(); 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; Eigen::Matrix<T, 3, 1> delta_predicted;
// position // position
delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1); delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
// orientation // orientation
delta_predicted(2) = pi2pi(_o2[0] - _o1[0]); delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
// residual // 2d pose residual
residuals = delta_corrected - delta_predicted; residuals = delta_corrected - delta_predicted;
// angle remapping // angle remapping
......
...@@ -620,7 +620,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) ...@@ -620,7 +620,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
WOLF_TRACE("\n ========== SOLVE ========="); 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("\n", report);
WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose()); WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment