From 3b71f7126a4fb936e479d98986efed74aea54abf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 5 Nov 2016 11:41:25 +0100
Subject: [PATCH] Divide vector instead of matrix saves time
Now for homogeneous
---
src/local_parametrization_homogeneous.cpp | 10 +++++-----
1 file changed, 5 insertions(+), 5 deletions(-)
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 62c6ea0aa..8657878e9 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;
}
--
GitLab