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