diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h index cbe5de62ff34a98c83c8a5192292f7fe6051f018..cc9e70fdac0f11027825901e35c5981f4cadd52e 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 - 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; diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h index c213624e554a15ec319da15c6374769cd3b9f167..dc62037fd45e28b7bf51c761a53beecd004983d1 100644 --- a/include/core/factor/factor_autodiff_distance_3D.h +++ b/include/core/factor/factor_autodiff_distance_3D.h @@ -55,12 +55,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> dist_meas (getMeasurement().cast<T>()); // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper(); // res = sqrt_info_upper * (dist_meas - dist_exp); - res = getMeasurementSquareRootInformationUpper() * (dist_meas - dist_exp); + res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp); return true; } diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h index b3b265f5af12f100fc74c6ba5e163d655451e0dc..c191eaf4fd40aff5dd5c2343145d25ddf9dfacbd 100644 --- a/include/core/factor/factor_odom_3D.h +++ b/include/core/factor/factor_odom_3D.h @@ -208,7 +208,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); diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h index 2918c61af692c0750fed7c8975f8bc8e640a5d61..ecdb08eeda0e7db0a00a92037eca3e8ea9da0d2d 100644 --- a/include/core/factor/factor_pose_2D.h +++ b/include/core/factor/factor_pose_2D.h @@ -34,7 +34,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; diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h index 722d7b0c13cd9d1e1d2a6abe1cd3864149e76c4b..d5920de235ba7ae2bd2076621b6487e1f1c8ad39 100644 --- a/include/core/factor/factor_pose_3D.h +++ b/include/core/factor/factor_pose_3D.h @@ -43,7 +43,7 @@ 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