0
$\begingroup$

Rosanswers logo

Hello,

i'm currently detecting the ArUco Marker with the help of the ros_aruco package. The detection works perfectly, i assume. by doing: $ rostopic echo /aruco_single/pose we obtain

header: 
  seq: 14
  stamp: 
    secs: 1548768525
    nsecs:  49234896
  frame_id: "camera"
pose: 
  position: 
    x: -0.0381913892925
    y: -0.0619406141341
    z: 0.542845368385
  orientation: 
    x: -0.550447938313
    y: 0.445001542715
    z: -0.464275720555
    w: 0.532380267752

By looking to the Z-Axis we can confirm that it's reading correctly, and the x and y as well (the marker is almost on the center of the camera). But this is the result of the transformation marker -> camera. What i really want is the transformation camera -> marker. Meaning we would had to do the inverse of this.

The way i do it: Inverse of a Pose is given by: the inverse of a homogeneous transformation. Which eventually is just operating:

  1. Inverse of the rotation = R.T; where T is the transpose and R the Rotation Matrix
  2. Inverse of the position = -Rt * t; where Rt is the transpose of the Rotation matrix and t is the translation vector

What i get:

Tvec Inverted:
[[-0.43304875]
 [ 0.33646972]
 [-0.02163316]]

Which doesn't make sense because Z should maintain to be around 0.54. Does anybody know why the result is looking so strange? Here is the snippet of the code:

def invert_pose(self, ps):
        # Storing the received pose
        x = ps.pose.position.x
        y = ps.pose.position.y
        z = ps.pose.position.z
        xyz = x, y, z
        tvec = np.array(xyz)
        print ("Tvec: ")
        print tvec

        rx = ps.pose.orientation.x
        ry = ps.pose.orientation.y
        rz = ps.pose.orientation.z
        rw = ps.pose.orientation.w
        quaternion = [rx, ry, rz, rw]
        eule_ang = euler_from_quaternion(quaternion)
        rvec = np.array(eule_ang)
        R = np.matrix(cv2.Rodrigues(rvec)[0])
        Rt = R.T
        invR = self.rotation_matrix_to_euler_angles(Rt)
        invT = -Rt.dot(np.matrix(tvec).T)
    

        print "Tvec Inverted: "
        print invT
        self.publish_ros(invR, invT)

Originally posted by hpoleselo on ROS Answers with karma: 133 on 2019-01-29

Post score: 0


Original comments

Comment by PeteBlackerThe3rd on 2019-01-29:
Why should Z still be around 0.54? There is a fairly significant rotational element of the pose so I would expect the z translation to be shifted too. More generally, don't reinvent the wheel, the TF package has the inverse operation built in and tested.

Comment by gvdhoorn on 2019-01-29:\

More generally, don't reinvent the wheel, the TF package has the inverse operation built in and tested.

indeed. I was going to comment on this as well.

@hpoleselo: any reason you don't / cannot use tf?

Comment by hpoleselo on 2019-01-30:
Hi, thank you both for the quick response. I actually didn't know about this function from /tf package. Conclusion: I thought for some reason i had to publish the Pose on the camera frame_id and then invert it, but actually what i had to do was to create a broadcaster between the camera and marker->

Comment by hpoleselo on 2019-01-30:
-> then ROS identified automatically that in order to the relation marker->camera to be valid, it should be inverted to fit the tf_tree (Because the camera is attached to a robot on RVIZ). Bottom line, just create the broadcaster. I can post the code here if you guys think it would help others.

Comment by PeteBlackerThe3rd on 2019-01-30:
Yes, publishing to the TF system is the way forward then ROS takes care of everything.

Comment by gvdhoorn on 2019-01-30:
I'm not sure I understand: if aruco is already publishing marker -> camera and you're interested in the inverse, why are you broadcasting your own transform? Wouldn't a simple lookUp(..) invert it for you automatically?

Comment by PeteBlackerThe3rd on 2019-01-30:
Arcu ROS only publishes Pose messages, it doesn't broadcast the TFs itself.

Comment by hpoleselo on 2019-01-30:
ROS never fails to surprise me, very nice! Thank you both for the help! About the lookUp(): exactly like Pete said!

Comment by gvdhoorn on 2019-01-30:\

Arcu ROS only publishes Pose messages, it doesn't broadcast the TFs itself.

Right, I forgot about that. Depends on the pkg used though I believe.

Could probably be done using a(n almost) one-liner with topic_tools/transform.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

As @gvdhoorn and @PeteBlackerThe3rd answered on the comments, there was a built in function from /tf which is called inverse. But that wasn't needed since the /tf package itself recognizes the direction of the tf_tree (forward) and then inverse it (camera -> marker) for us.


Originally posted by hpoleselo with karma: 133 on 2019-01-31

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

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