Removed more .cast<T>()

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;

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;
         }

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);

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;

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