From 0df60e55f26908f8e9bd1a6d021df687fdf15331 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 5 Feb 2019 14:30:21 +0100
Subject: [PATCH] simplified & fixed constriant_block_absolute

---
 src/constraint_block_absolute.h | 16 ++++------------
 1 file changed, 4 insertions(+), 12 deletions(-)

diff --git a/src/constraint_block_absolute.h b/src/constraint_block_absolute.h
index fe8eec7a9..550ecc632 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;
 }
-- 
GitLab