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:
- Inverse of the rotation = R.T; where T is the transpose and R the Rotation Matrix
- 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)
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.
indeed. I was going to comment on this as well.
@hpoleselo: any reason you don't / cannot use tf?
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->
-> 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.
Yes, publishing to the TF system is the way forward then ROS takes care of everything.
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 simplelookUp(..)
invert it for you automatically?Arcu ROS only publishes Pose messages, it doesn't broadcast the TFs itself.
ROS never fails to surprise me, very nice! Thank you both for the help! About the lookUp(): exactly like Pete said!