From e83c49af7713a3275d239b22bc1c9d59dfe0532a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 25 Aug 2019 00:19:22 +0200
Subject: [PATCH] Fix tiny bug that was inadverted for years!
---
src/processor/processor_odom_3D.cpp | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 6c20d3e16..27659e3ee 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -56,9 +56,9 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
else
{
// rotation in quaternion form
- _delta = _data;
- disp = _data.head<3>().norm();
- rot = 2 * acos(_data(3));
+ _delta = _data;
+ disp = _data.head<3>().norm();
+ rot = 2.0 * acos(_data(6)); // '6' is the real part of the quaternion
}
/* Jacobians of d = data2delta(data, dt)
* with: d = [Dp Dq]
--
GitLab