From 03862f99eaa7b5fcd7068e7401769d4e24e1e016 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 29 Aug 2019 13:37:29 +0200
Subject: [PATCH] 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 | 15 ++++-----------
test/gtest_factor_diff_drive.cpp | 2 +-
2 files changed, 5 insertions(+), 12 deletions(-)
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 274c226ed..d26cfcd7c 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 6caa5f868..d210d941b 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());
--
GitLab