diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp index 62c6ea0aaff95d1eab6049ba96a0304a06068f9d..8657878e92780feb67a10f9999dec12a1106565f 100644 --- a/src/local_parametrization_homogeneous.cpp +++ b/src/local_parametrization_homogeneous.cpp @@ -60,11 +60,11 @@ bool LocalParametrizationHomogeneous::computeJacobian(const Eigen::Map<const Eig assert(_h.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); - _jacobian << _h(3), _h(2), -_h(1), - -_h(2), _h(3), _h(0), - _h(1), -_h(0), _h(3), - -_h(0), -_h(1), -_h(2) ; - _jacobian /= 2; + Eigen::Vector4s hh = _h/2; + _jacobian << hh(3), hh(2), -hh(1), + -hh(2), hh(3), hh(0), + hh(1), -hh(0), hh(3), + -hh(0), -hh(1), -hh(2) ; return true; }