Ask Your Question
0

Rotation Angle in Pose Orientation

asked 2019-02-04 10:05:52 -0500

Jägermeister gravatar image

updated 2019-02-06 02:35:32 -0500

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):
        return math.cos(math.radians(angle + 180))
    else:
        return math.cos(math.radians(angle + 90))



...
 # 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 flag offensive close merge delete

Comments

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

kosmastsk gravatar imagekosmastsk ( 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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 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?

Jägermeister gravatar imageJägermeister ( 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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 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.

Jägermeister gravatar imageJägermeister ( 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

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 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.

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

2 Answers

Sort by » oldest newest most voted
2

answered 2019-02-04 18:58:30 -0500

robustify gravatar image

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.

edit flag offensive delete link more

Comments

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.

Jägermeister gravatar imageJägermeister ( 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.

VictorLamoine gravatar imageVictorLamoine ( 2019-02-05 05:46:49 -0500 )edit

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

Jägermeister gravatar imageJägermeister ( 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

robustify gravatar imagerobustify ( 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?

Jägermeister gravatar imageJägermeister ( 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.

VictorLamoine gravatar imageVictorLamoine ( 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.

robustify gravatar imagerobustify ( 2019-02-05 09:48:19 -0500 )edit
1

answered 2019-02-04 10:37:38 -0500

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.

edit flag offensive delete link more

Comments

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?

Jägermeister gravatar imageJägermeister ( 2019-02-04 12:01:37 -0500 )edit

They are all the rotation. Quaternions are not an easy thing to get your head around, I recommend reading all of the Wikipedia page I linked, to give you an introduction.

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-02-04 10:05:52 -0500

Seen: 730 times

Last updated: Feb 06