From fb548edec8467f86e50914fa70f7c1d587374a09 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joaquim=20Casals=20Bu=C3=B1uel?= <jcasals@iri.upc.edu>
Date: Wed, 6 Nov 2019 13:05:54 +0100
Subject: [PATCH] Revert "Merge branch
'257-eigen-matrix-cast-ceres-jet-not-needed-if-upgrading-to-eigen-3-3' into
devel"
This reverts merge request !324
---
hello_wolf/factor_bearing.h | 4 ++--
hello_wolf/factor_range_bearing.h | 4 ++--
.../core/factor/factor_autodiff_distance_3D.h | 8 +++-----
include/core/factor/factor_diff_drive.h | 17 ++++++++++++-----
include/core/factor/factor_odom_2D.h | 4 ++--
include/core/factor/factor_odom_3D.h | 5 ++---
include/core/factor/factor_pose_2D.h | 5 ++---
include/core/factor/factor_pose_3D.h | 4 ++--
.../core/factor/factor_quaternion_absolute.h | 2 +-
test/gtest_factor_diff_drive.cpp | 2 +-
10 files changed, 29 insertions(+), 26 deletions(-)
diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 6975a798d..1b5ff4049 100644
--- a/hello_wolf/factor_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -69,8 +69,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
T bearing = atan2(point_r(1), point_r(0));
// 4. Get the measured range-and-bearing to the point, and its covariance
- Matrix<T, 2, 1> range_bearing = getMeasurement();
- Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance();
+ Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
+ Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance().cast<T>();
// 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing;
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index cc9e70fda..ffe896f75 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -110,7 +110,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
exp_rb(1) = atan2(lmk_s(1), lmk_s(0)); // bearing
// 4. Get the measured range-and-bearing to the point
- auto& meas_rb = getMeasurement();
+ Matrix<T, 2, 1> meas_rb = getMeasurement().cast<T>(); // cast Eigen type vector to have scalar type 'T'
// 5. Get the error by comparing the expected against the measurement
Matrix<T, 2, 1> err_rb = meas_rb - exp_rb;
@@ -126,7 +126,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
// R^T * R = Omega
// in other words, R is the Cholesky decomposition of Omega.
// NOTE: you get R directly from the Feature with getMeasurementSquareRootInformationUpper()
- res = getMeasurementSquareRootInformationUpper() * err_rb;
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * err_rb;
return true;
}
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index dc62037fd..e26c41281 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3D.h
@@ -55,12 +55,10 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
norm_squared += (T)1e-8;
}
Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
- // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
- // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper();
+ Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
+ Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
- // res = sqrt_info_upper * (dist_meas - dist_exp);
-
- res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
+ res = sqrt_info_upper * (dist_meas - dist_exp);
return true;
}
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index eaf64a11e..274c226ed 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -104,19 +104,26 @@ 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
- // Correct delta due to changes in the calibration parameters
- auto delta_corrected = getMeasurement() + J_delta_calib_ * (c - calib_preint_);
+ /// Is this my delta_preint ? Yes!
+ const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>();
+ 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]);
- // 2d pose residual
+ // residual
residuals = delta_corrected - delta_predicted;
// angle remapping
@@ -126,7 +133,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
residuals(2) = residuals(2) + T(2. * M_PI);
// weighted residual
- residuals = getMeasurementSquareRootInformationUpper() * residuals;
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
return true;
}
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index 3eb1c5667..e5cfd57a6 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -64,14 +64,14 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
expected_measurement(2) = o2 - o1;
// Error
- er = expected_measurement - getMeasurement();
+ er = expected_measurement - getMeasurement().cast<T>();
while (er(2) > T( M_PI ))
er(2) -= T( 2 * M_PI );
while (er(2) < T( -M_PI ))
er(2) += T( 2 * M_PI );
// Residuals
- res = getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index c191eaf4f..587472541 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -208,8 +208,7 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
// measured motion increment, dp_m, dq_m
- // Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
- auto& dp_m = getMeasurement().head<3>();
+ Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>());
Eigen::Matrix<T,3,1> dp = expected.head(3);
@@ -222,7 +221,7 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
residuals.tail(3) = q2v(dq.conjugate() * dq_m);
- residuals = getMeasurementSquareRootInformationUpper() * residuals;
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
return true;
}
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index ecdb08eed..e4caa3b2d 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -34,8 +34,7 @@ template<typename T>
inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// measurement
- // Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
- auto& meas = getMeasurement();
+ Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
// error
Eigen::Matrix<T,3,1> er;
@@ -49,7 +48,7 @@ inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index d5920de23..d36bcdb32 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -43,12 +43,12 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
// error
Eigen::Matrix<T, 6, 1> er;
- er.head(3) = p_measured - p;
+ er.head(3) = p_measured.cast<T>() - p;
er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>());
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
return true;
}
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 2f17c14a5..477db6368 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -63,7 +63,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
return true;
}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index d210d941b..6caa5f868 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::FULL);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
--
GitLab