diff --git a/src/constraint_block_absolute.h b/src/constraint_block_absolute.h
index fe8eec7a9f01095838c0572d4517e4493d85fa3a..550ecc632f569c0da4766fa124b50a31abdd0bf9 100644
--- a/src/constraint_block_absolute.h
+++ b/src/constraint_block_absolute.h
@@ -39,20 +39,12 @@ class ConstraintBlockAbsolute: public ConstraintAutodiff<ConstraintBlockAbsolute
 template<typename T>
 inline bool ConstraintBlockAbsolute::operator ()(const T* const _sb, T* _residuals) const
 {
-
-    // states
-    Eigen::Matrix<T, 3, 1>  sb(_sb);
-
-    // measurements
-    Eigen::Vector3s     measured_state(getMeasurement().data() + 0);
-
-    // error
-    Eigen::Matrix<T, 3, 1> er;
-    er       = measured_state.cast<T>() - sb;
+    // Maps
+    Eigen::Map<T, 3, 1> sb(_sb); // state
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals); // residual
 
     // residual
-    Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
-    res               = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+    res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * (getMeasurement().cast<T>() - sb);
 
     return true;
 }