how to measure an angle?

Question

Dear,

I want to measure yaw angle (radian). What command should I use ? In my c++ file, I used like this;

  double q0 = odom->pose.pose.orientation.w;
  double q1 = odom->pose.pose.orientation.x;
  double q2 = odom->pose.pose.orientation.y;
  double q3 = odom->pose.pose.orientation.z;
  double omega = odom->twist.twist.angular.z;
  double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

This yaw angle produce only between -pi and pi. But I want to measure directly the yaw angle in radian without the limitation of -pi and +pi, that is, measure the counter-clockwise one and half circling as +3 pi and the clockwise two circling as -4*pi.

One alternative can be coded this way:

  double omega = odom->twist.twist.angular.z;
  double delta_psi = omega*dt;
  psi += delta_psi;

But the heading angle psi might accumulate some integration error. Do I have to implement more accurate integration method ?

---

Originally posted by maruchi on ROS Answers with karma: 157 on 2012-01-17

Post score: 0

Answer 1

To derive Euler Angles from Quaternions, you need to know (or define) the sequence of rotations around the coordinate axes. You can only get the Yaw in this context. Take a look at this very nice document. Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)

Otherwise I'd recommend to make use of a linear algebra library (e.g Eigen). Conversion can be done in one line:

Eigen::Quaterniond orientation(w,x,y,z);
orientation.toRotationMatrix().eulerAngles(0,1,2)

EDIT: I looked at the conversion formulas for all the angle sequences and could not find anything similar to your calculation. Should rather look similar to this (seq: 0,1,2):

atan2(2xy + 2wz, w*w + x*x - y*y - z*z)

compare to Formula (290) in linked document.

EDIT: Actually your formula is correct, because the norm of the quaternion is always 1:

w*w + x*x - y*y - z*z = 1-2*(y*y+z*z)

Output should be between -PI and PI.

---

Originally posted by rado0x54 with karma: 191 on 2012-01-17

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by maruchi on 2012-01-23:
I edited the original question. Could you check it ?

Answer 2

Take a look at the getYaw() function in tf: http://www.ros.org/doc/api/tf/html/c++/namespacetf.html#a33e93f413622e296d0a2a93b252bbb4b

In general, tf has a couple of handy functions for working with quaternions that represent angles in the plane.

---

Originally posted by ahendrix with karma: 47576 on 2012-01-17

This answer was NOT ACCEPTED on the original site

Post score: 5

---

Original comments

Comment by maruchi on 2012-01-23:
I edited the original question. Could you check it?