Revision history [back]

I'd suggest converting both of the orientations into transformation objects, then create a point object 1m along the Z axis. Then use the two transformations to transform the point from the frame of one transform into the other. This will give you two unit vectors one if which is (0 0 1)^ the other which is something else, you can then use the dot product to find the angle between the two.

In a little bit more detail you'd create the two transform objects like this.

tf2::Quaternion q1;
q1.setRPY(roll, pitch, yaw); // node the order
tf2::Transform t1(q1);

Do the same with q2 and t2 for the other orientation then create the Z axis unit point.

tf2::Vector3 zPoint(0,0,1);

Transform it from one frame to the other.

tf2::Vector3 otherZPoint = t1.inverseTimes(t2) * zPoint;

Now you can calculate the angle between the two unit vectors zPoint and otherZPoint as so.

float angle = acos(zPoint.dot(otherZPoint) );

It's a bit long winded but I hope this solves the problem you're asking about!