Rotation Angle in Pose Orientation

I have a 2D image on which I conduct an algorithm to find its rotation, and I get it in radian. No problem until here.

Now that I want to fill in the pose object with what I collected from the 2D vision, I get stuck at where to insert the rotation. I do know I cannot fill in everything, I don't intend to, either. I only need the position (I have it already) and the rotation.

            obj.pose.position.x = v['pose'][0] #x
obj.pose.position.y = v['pose'][1] #y
obj.pose.position.z = v['pose'][2] #z

# which one below is the one I should use to pass the rotation angle?
obj.pose.orientation.x = v['pose'][3]
obj.pose.orientation.y = v['pose'][4]
obj.pose.orientation.z = v['pose'][5]
obj.pose.orientation.w = v['pose'][6]


Any thoughts?

EDIT: So I came up with the following after the useful insights given by the commentators.

       self.some_srv = rospy.Service("/bla/request_poses", PartArray, self.some_service_srv)

def some_service_srv(self, req):
# some irrelevant stuff happening here

# this is where the actual thing shall happen
obj.pose.position.x = v['pose'][0]
obj.pose.position.y = v['pose'][1]
obj.pose.position.z = v['pose'][2]
rotation_angle = v['pose'][3] # extract the rotation angle
zaxis = (0, 0, 1) #rotation is around the Z axis in the image
quaternion = quaternion_about_axis(rotation_angle, zaxis) # use rotation angle + axis to get the quaternion
obj.pose.orientation.x = quaternion[0]
obj.pose.orientation.y = quaternion[1]
obj.pose.orientation.z = quaternion[2] # this is the rotation we are talking about
obj.pose.orientation.w = quaternion[3]

list_detected.part_array.append(obj)
return list_detected


This runs, however I always get 0 values for all of the quarternions, which is somehow nonsense because the rotation angle is definitely non-zero (I print its value).

            position:
x: 274.0
y: 250.0
z: 602.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0


I print the quarternion list separately, and there is a non-zero value inside, which seems fine, but I don't know why the published value gets zero.

         [0.000000e+00 0.000000e+00 3.061617e-17 1.000000e+00]


I also printed the value of rotation (in radian) and here are some values of it:

0.980580687945
6.12323399574e-17
-0.707106781187
0.294085862655
-1.0
0.56419054038
-0.0114934175734
6.12323399574e-17
6.12323399574e-17
-1.0
0.707106781187
6.12323399574e-17


And this is how I get the angle using OpenCV:

# helper function to find out which side of the rectangle is longer, then add the angle appropriately
def getAngle(rect):
angle = rect[2]
width, height = rect[1]
if (width < height):
else:

...
# Find contours:
(im, contours, hierarchy) = cv2.findContours(im, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
cnt = contours[0]

# get the rotated rectangle to get the rotation
rect = cv2.minAreaRect(cnt)
rotation = getAngle(rect) # in radians
...

edit retag close merge delete

Considering that the image is on the xy plane, I would say that the rotation is on the orientation.z. Think of it, as turning the image around the z axis, but keeping the other axes stable. If the image is not on one axis, it may be more complicated though

( 2019-02-04 10:24:19 -0500 )edit

It is indeed more complicated than that, in all cases I'm afraid. See my answer below.

( 2019-02-04 10:38:25 -0500 )edit

Even if your rotation was zero the quaternion would contain positive values. The quaternion values you printed out is very small (x10-17) so is being rounded to zero. There appears to be a problem with the quaternion_about_axis function since it's returning an invalid rotation quaternion.

( 2019-02-05 04:51:34 -0500 )edit

Is the function problematic or the way I use it? I don't get it. Is there a bug in ROS, seriously?

( 2019-02-05 05:05:37 -0500 )edit

The function definitely works, can you show us more of your code. It's probably something about how your getting the values into the message.

( 2019-02-05 05:46:23 -0500 )edit

I edited my question, butt there isn't much to write really, I mean, I wrote all that is relevant. It's just a service which is called, and it should return the pose (position + quarternion). Position values are filled in, no problem, but the quaternion values are always zero for some reason.

( 2019-02-05 07:15:56 -0500 )edit
1

Can you print the value and type of v['pose'][3] is this not numeric then it may explain this behaviour. I've just checked the syntax and I think you're doing anything obvious wrong. The function really should be returning a 4D unit vector

( 2019-02-05 09:26:18 -0500 )edit

One of the values your printing out 6.12323399574e-17 is Exactly twice the value in the quaternion of 3.061617e-17 I suspect there is some other code you haven't put in your question which is causing the problem. Can you post all your code which is producing the problem.

( 2019-02-05 09:52:45 -0500 )edit

Sort by » oldest newest most voted

The quaternion for a given Z axis (yaw) rotation is given by:

w = cos(theta / 2) x = 0 y = 0 z = sin(theta / 2)

where theta is the yaw angle in radians. If X (roll) or Y (pitch) axis rotations are involved too, the formula is a little more tedious, and can be found on Wikipedia.

The tf library has very helpful functions for constructing quaternions from Euler angles. For yaw quaternions, there is tf::createQuaternionFromYaw(double yaw) which takes a single argument which is the yaw angle in radians and returns a tf::Quaternion object that is equivalent. There is also tf::createQuaternionMsgFromYaw(double yaw) (notice the 'Msg') which is similar, but returns a geometry_msgs::Quaternion ROS message.

For all three axes, there is tf::createQuaternionFromRPY(double roll, double pitch, double yaw) and tf::createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw).

Of course, this is all available in C++ by including <tf/tf.h>. In Python, it looks like you can convert Euler angles into a quaternion using transformations.py (doc_link). I haven't used it myself before though.

more

I am not using C++, but the Python equivalent of what you are saying should be quaternion_about_axis(angle, axis), right? All I have is angle around the axis Z, so this should be it. Edited the question.

( 2019-02-05 01:39:34 -0500 )edit

Also make sure your quaternions are always normalized otherwise most operations you will do on them will yeld undefined behavior.

( 2019-02-05 05:46:49 -0500 )edit

Thanks for the warning. Do you have any normalization example I can take a look at?

( 2019-02-05 07:17:19 -0500 )edit

The quaternion should never be all zeros, as it is always should be a unit-length vector. I would first try manually computing w and z according to the formula in my answer and make sure that you at least get the result that way to rule out any weirdness

( 2019-02-05 08:34:19 -0500 )edit

It's not all zeros though: [0.000000e+00 0.000000e+00 3.061617e-17 1.000000e+00] does this also look weird?

( 2019-02-05 09:12:42 -0500 )edit

This looks fine, 3.0e-17 is a very small number, close enough to zero. The norm is this quaternion is 1 so you are good.

( 2019-02-05 09:35:43 -0500 )edit

Oh, didn't notice that w is 1! Yes, that is a valid quaternion, but it seems like the angle you passed into it is very close to zero. A quaternion with w = 1 and the rest are zero represents no rotation.

( 2019-02-05 09:48:19 -0500 )edit

Rotations in ROS are described using rotation quaternions, which are represented by the four orientation values you see in the pose message. A rotation quaternion must be a 4D unit vector for it to be valid, and it is non-trivial to calculate the elements yourself.

However there are useful helper classes to do this for you. The tf2::Quaternion object has a constructor which accepts angle and axis representation which is exactly what you want in your case. You have a known rotation and you know it's about the Z axis. So if you use this constructor to create a quaternion object you can then use its getX(), getY() . . getW() methods to calculate the values you need.

Hope this makes sense.

more

My rotation is about the Z axis yes, correct. I'll then create that object accordingly and then I'll call those methods to assign obj.pose.orientation.w and so on? Which of these is the rotation though? I still don't know that. Is it Z?

( 2019-02-04 12:01:37 -0500 )edit

( 2019-02-04 12:32:21 -0500 )edit