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