diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 5f1610c949c213796d2332e07e1023bb5f483d8b..fce46678b8045ab164e0f8c5a34bb0178956dc4d 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -56,7 +56,7 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data, // rotation in quaternion form _delta = _data; disp = _data.head<3>().norm(); - rot = 2.0 * acos(_data(3)); + rot = 2.0 * acos(_data(6)); // '6' is the real part of the quaternion } /* Jacobians of d = data2delta(data, dt) * with: d = [Dp Dq]