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

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

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.

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.