diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h index 1b5ff40497096ac74f35024ce0b37b1c88076266..6975a798db580e1c127b0215609f49904b29ae40 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().cast<T>(); - Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance().cast<T>(); + Matrix<T, 2, 1> range_bearing = getMeasurement(); + Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance(); // 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 0ca3ec0a4f31a403424f928b38cc98015ea728d7..436b79e4a0eee67aa8ddd97de34ec14127251d01 100644 --- a/hello_wolf/factor_range_bearing.h +++ b/hello_wolf/factor_range_bearing.h @@ -115,7 +115,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 - Matrix<T, 2, 1> meas_rb = getMeasurement().cast<T>(); // cast Eigen type vector to have scalar type 'T' + auto& meas_rb = getMeasurement(); // 5. Get the error by comparing the expected against the measurement Matrix<T, 2, 1> err_rb = meas_rb - exp_rb; @@ -131,7 +131,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().cast<T>() * err_rb; + res = getMeasurementSquareRootInformationUpper() * 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 7c5c2d3767cef907138b8d60fdbc9090a3a71158..624112000fe6f019e6d86fc9e310e4da9a4a2b3f 100644 --- a/include/core/factor/factor_autodiff_distance_3D.h +++ b/include/core/factor/factor_autodiff_distance_3D.h @@ -60,10 +60,12 @@ 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().cast<T>(); + // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>()); + // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper(); - res = sqrt_info_upper * (dist_meas - dist_exp); + // res = sqrt_info_upper * (dist_meas - dist_exp); + + res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp); return true; } diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h index 5f6e149883ff1499757af2db05a9f8fc2a956b16..2338f699de73d38c74ce13509cdc0b70a1a82130 100644 --- a/include/core/factor/factor_diff_drive.h +++ b/include/core/factor/factor_diff_drive.h @@ -110,26 +110,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 @@ -139,7 +132,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().cast<T>() * residuals; + residuals = getMeasurementSquareRootInformationUpper() * residuals; return true; } diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h index 2f1a011c19ae11ae78aaea9a17fd1c5b0d23fe5c..64d09368e97d0f5e394672de4218d7e608c49445 100644 --- a/include/core/factor/factor_odom_2D.h +++ b/include/core/factor/factor_odom_2D.h @@ -71,14 +71,14 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co expected_measurement(2) = o2 - o1; // Error - er = expected_measurement - getMeasurement().cast<T>(); + er = expected_measurement - getMeasurement(); 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().cast<T>() * er; + res = getMeasurementSquareRootInformationUpper() * 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 d359adbbd275a3faca43d1a154590830cd9425a6..1b53915402fe5adb6a1f1481fefc06add006c291 100644 --- a/include/core/factor/factor_odom_3D.h +++ b/include/core/factor/factor_odom_3D.h @@ -214,7 +214,8 @@ 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>(); + // Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>(); + auto& dp_m = getMeasurement().head<3>(); Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>()); Eigen::Matrix<T,3,1> dp = expected.head(3); @@ -227,7 +228,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().cast<T>() * residuals; + residuals = getMeasurementSquareRootInformationUpper() * residuals; return true; } diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h index 91229e4c83614df3a92d03ff9e24a407a719eadb..fece6f0bc06d0352319ceaa30e614200a6d254e9 100644 --- a/include/core/factor/factor_pose_2D.h +++ b/include/core/factor/factor_pose_2D.h @@ -47,7 +47,8 @@ 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>(); + // Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>(); + auto& meas = getMeasurement(); // error Eigen::Matrix<T,3,1> er; @@ -61,7 +62,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().cast<T>() * er; + res = getFeature()->getMeasurementSquareRootInformationUpper() * 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 81b1837969a3ef62aa069aece3864713d6b31e76..c82a1788f6919fd66844e3790c616c6afd54b7b8 100644 --- a/include/core/factor/factor_pose_3D.h +++ b/include/core/factor/factor_pose_3D.h @@ -56,12 +56,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.cast<T>() - p; + er.head(3) = p_measured - p; er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>()); // residual Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals); - res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er; + res = getFeature()->getMeasurementSquareRootInformationUpper() * er; return true; } diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h index 4c8c7bf1a735b76b9ac3cbcb12d246a728fd806a..6533bd414d54a9eb45da8c92336ba3d1f0e07739 100644 --- a/include/core/factor/factor_quaternion_absolute.h +++ b/include/core/factor/factor_quaternion_absolute.h @@ -78,7 +78,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua // residual Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals); - res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er; + res = getFeature()->getMeasurementSquareRootInformationUpper() * er; return true; }