0
$\begingroup$

Rosanswers logo

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
...

Originally posted by Jägermeister on ROS Answers with karma: 81 on 2019-02-04

Post score: 0


Original comments

Comment by kosmastsk on 2019-02-04:
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

Comment by PeteBlackerThe3rd on 2019-02-04:
It is indeed more complicated than that, in all cases I'm afraid. See my answer below.

Comment by PeteBlackerThe3rd on 2019-02-05:
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.

Comment by Jägermeister on 2019-02-05:
Is the function problematic or the way I use it? I don't get it. Is there a bug in ROS, seriously?

Comment by PeteBlackerThe3rd on 2019-02-05:
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.

Comment by Jägermeister on 2019-02-05:
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.

Comment by PeteBlackerThe3rd on 2019-02-05:
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

Comment by PeteBlackerThe3rd on 2019-02-05:
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.

Comment by Jägermeister on 2019-02-06:
I put everything on the ROS side. If you want to see the OpenCV side (how I get the rotation angle out of 2D vision) I can do that too, I doubt it is relevant though.

Comment by PeteBlackerThe3rd on 2019-02-06:
I've just checked and the values coming out of the quaternion_about_axis function are correct, but there must be a bug between there and where your publishing the pose. Can you post all your code between this function and the publish call so we can try and find where it's going wrong.

Comment by Jägermeister on 2019-02-06:
Normally I couldn't do this due to confidential issues but now I found that I can give a time-based link, so perhaps you can look at it, and if you find the problem, we could reshape the question without exposing the code. Here for a short amount of time.

Comment by PeteBlackerThe3rd on 2019-02-06:
There are still no ros publishers in that code. But I think you need to carefully debug through the code because the orientation values are not being copied at some point. The quaternion_about_axis function calculates them correctly but the values are not making it into the pose message you show.

Comment by Jägermeister on 2019-02-06:
I traced the values until the end of the function and they are fine, but then it's all about the ROS service, which returns them as zero. It's really weird, I am about to call it a bug almost. Never seen such a thing before tbh, but I am not the ROS master either. Anyway, thank you for the effort.

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by robustify with karma: 956 on 2019-02-04

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by Jägermeister on 2019-02-05:
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.

Comment by VictorLamoine on 2019-02-05:
Also make sure your quaternions are always normalized otherwise most operations you will do on them will yeld undefined behavior.

Comment by Jägermeister on 2019-02-05:
Thanks for the warning. Do you have any normalization example I can take a look at?

Comment by robustify on 2019-02-05:
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

Comment by Jägermeister on 2019-02-05:
It's not all zeros though: [0.000000e+00 0.000000e+00 3.061617e-17 1.000000e+00] does this also look weird?

Comment by VictorLamoine on 2019-02-05:
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.

Comment by robustify on 2019-02-05:
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.

$\endgroup$
0
$\begingroup$

Rosanswers logo

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.


Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-02-04

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by Jägermeister on 2019-02-04:
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?

Comment by PeteBlackerThe3rd on 2019-02-04:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.