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