diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 6975a798db580e1c127b0215609f49904b29ae40..1b5ff40497096ac74f35024ce0b37b1c88076266 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 cc9e70fdac0f11027825901e35c5981f4cadd52e..ffe896f75932fb269068ad1210ad37350e62ce30 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 dc62037fd45e28b7bf51c761a53beecd004983d1..e26c412812fd7c7e869a79c8b95085eac43ed633 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 eaf64a11ed59127337ae69a37189687ac8e433cd..274c226ed779ced623f5ae8c0eb76c0e5bc4d291 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 3eb1c566796728c454cc2d918656607b3ba1516c..e5cfd57a69329ca9c82c9019f29d4ee591b27f77 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 c191eaf4fd40aff5dd5c2343145d25ddf9dfacbd..587472541124f2d0d158b3d999c9b65bb1bc5dcb 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 ecdb08eeda0e7db0a00a92037eca3e8ea9da0d2d..e4caa3b2dbd2ed26ef8b23a83443b0662b1ec823 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 d5920de235ba7ae2bd2076621b6487e1f1c8ad39..d36bcdb32a832695b29bbbe5a0610ded37746218 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 2f17c14a54a5b8d8e46fb24165ceb140a6b1d55d..477db6368bb4217b41e154095cbca3bd2c00cf6f 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 d210d941b516b0651fb6f871bbdcc6a48b1ec69f..6caa5f868adceb291690590455bea94520771b0f 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());