Ask Your Question
0

Aruco Detection: operating the inverse of the Pose results in strange value for Translation

asked 2019-01-29 07:57:59 -0600

hpoleselo gravatar image

updated 2019-01-31 02:37:16 -0600

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)
edit retag flag offensive close merge delete

Comments

1

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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-01-29 08:04:34 -0600 )edit
1

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?

gvdhoorn gravatar imagegvdhoorn ( 2019-01-29 08:08:41 -0600 )edit

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

hpoleselo gravatar imagehpoleselo ( 2019-01-30 08:26:03 -0600 )edit

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

hpoleselo gravatar imagehpoleselo ( 2019-01-30 08:27:56 -0600 )edit
1

Yes, publishing to the TF system is the way forward then ROS takes care of everything.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-01-30 08:37:09 -0600 )edit

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?

gvdhoorn gravatar imagegvdhoorn ( 2019-01-30 08:53:47 -0600 )edit
1

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

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-01-30 10:38:40 -0600 )edit

ROS never fails to surprise me, very nice! Thank you both for the help! About the lookUp(): exactly like Pete said!

hpoleselo gravatar imagehpoleselo ( 2019-01-30 10:56:50 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-01-31 02:35:58 -0600

hpoleselo gravatar image

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.

edit flag offensive delete link more

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-01-29 07:57:59 -0600

Seen: 335 times

Last updated: Jan 31