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

edit

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)