0

I would like to extract information about pose of my Vive Tracker sensor.

I found this tutorial which is pretty accurate and it works, but...

When I put the vive tracker on my table, for instance, and rotate it with my hands, instead of changing the YAW, it changes the PITCH of my tracker.

How should I read the pose matrix in order to have the correct axis?

Paolo Gambardella
  • 31
  • 2

1 Answers1

0

I had the same problem, because the rotation matrix is read with the same functions of the HMD and Controllers.

In case of Vive Tracker, instead, you should use other functions of OpenVR, in order to read the pose properly.

so do:

#get the pose with orientation in radians
    def get_pose_radians(self):
        pose = self.vr.getControllerStateWithPose(openvr.TrackingUniverseStanding, self.index,openvr.k_unMaxTrackedDeviceCount)
        return convert_to_radians(pose[2].mDeviceToAbsoluteTracking)

and the function which gives your pose in radians could be:

#Convert the 3x4 position/rotation matrix to a x,y,z location and the appropriate Euler angles (in radians)
def convert_to_radians(pose_mat):

roundfact = 16

x = pose_mat[0][3]
y = pose_mat[1][3]
z = pose_mat[2][3]

# Algorhitm 1

#read here: https://steamcommunity.com/app/358720/discussions/0/343787920117426152/
pitch = math.atan2(pose_mat[2][1], pose_mat[2][2])
yaw = math.asin(pose_mat[2][0])
roll = math.atan2(-pose_mat[1][0], pose_mat[0][0])
return [x,y,z,yaw, pitch, roll]

In order to avoid gimbal locks, etc, I suggest you to use quaternions, with this function:

def convert_to_quaternion(pose_mat):
    r_w = math.sqrt( max( 0, 1 + pose_mat[0][0] + pose_mat[1][1]+ pose_mat[2][2] ) ) / 2
    r_x = math.sqrt( max( 0, 1 + pose_mat[0][0] - pose_mat[1][1] - pose_mat[2][2] ) ) / 2
    r_y = math.sqrt( max( 0, 1 - pose_mat[0][0] + pose_mat[1][1] - pose_mat[2][2] ) ) / 2
    r_z = math.sqrt( max( 0, 1 - pose_mat[0][0] - pose_mat[1][1] + pose_mat[2][2] ) ) / 2
    r_x *= math.copysign(1, r_x * ( -pose_mat[2][1] + pose_mat[1][2] ) )
    r_y *= math.copysign(1,r_y * ( -pose_mat[0][2] + pose_mat[2][0] ) )
    r_z *= math.copysign(1,r_z * ( pose_mat[1][0] - pose_mat[0][1] ) )

    x = pose_mat[0][3]
    y = pose_mat[1][3]
    z = pose_mat[2][3]
    return [x,y,z,r_w,r_x,r_y,r_z]
Paolo Gambardella
  • 31
  • 2