How do I convert the pose information to just XYZ position

[mungewell]

Working on a DIY project using various projects from GitHub.

I am using a Kinect to track head position, and I want to blend the position with my PSVR's rotation (already have kalman working with VRidge and SteamVR).
https://github.com/SDraw/driver_kinectV1

pyOpenVR lets me access the tracking pose, and report something like this
[[-0.9185850024223328, 0.0, 0.3952234089374542, 0.4742681086063385], [0.0, 1.0, 0.0, -20.290109634399414], [-0.3952234089374542, 0.0, -0.9185850024223328, -1.102302074432373]]

What is the format of this matrix?

[risa2000]

@mungewell I am not sure from which function you get the pose exactly, but in general, you should be getting equivalent of this:

/** describes a single pose for a tracked object */
struct TrackedDevicePose_t
{
	HmdMatrix34_t mDeviceToAbsoluteTracking;
	HmdVector3_t vVelocity;				// velocity in tracker space in m/s
	HmdVector3_t vAngularVelocity;		// angular velocity in radians/s (?)
	ETrackingResult eTrackingResult;
	bool bPoseIsValid;

	// This indicates that there is a device connected for this spot in the pose array.
	// It could go from true to false if the user unplugs the device.
	bool bDeviceIsConnected;
};

What you post here could be mDeviceToAbsoluteTracking which is 3x4 matrix, representing affine transformation (rotation and translation) in "dense" format (without the last row). It is

A = T * R

where R is the rotation and T is the translation, and it defines the relative position and orientation of the tracked device in the world space. Incidentally A is a transformation from the device space to the world space, so:

dev_pos_world = A * [0, 0, 0, 1]
dev_direction_world = A * [dev_direction_dev | 0]

where [dev_direction_dev | 0] is the direction vector of the device in device coordinates (e.g [0, 0, -1] for a HMD), extended by zero.

[mungewell]

@risa2000 Thank you for a very helpful reply, I will try this when I have the kit hooked up again.

I had taken the value from;
https://github.com/cmbruns/pyopenvr/blob/master/src/samples/track_hmd.py

But with 'k_unTrackedDeviceIndex_Hmd' replaced with the index of the tracker I was interested in.

[mungewell]

tried some simple code with sample 'track_hmd.py', but had some issue getting it to handle the HmdMatrix34_t. Ended up with this...

    hmd_pose = poses[openvr.k_unTrackedDeviceIndex_Hmd]
    mat = hmd_pose.mDeviceToAbsoluteTracking
    A = numpy.matrix(
            ((mat.m[0][0], mat.m[0][1], mat.m[0][2], mat.m[0][3]),
             (mat.m[1][0], mat.m[1][1], mat.m[1][2], mat.m[1][3]),
             (mat.m[2][0], mat.m[2][1], mat.m[2][2], mat.m[2][3]),)
        , numpy.float32)

    print(A.dot(numpy.matrix('0; 0; 0; 1')))

After starting VRidge it gives a position (believed to be 0,0,0)

[[  9.80908925e-45]
 [ -2.41901112e+01]
 [ -7.00649232e-45]]

If I reposition HMD to x=1, y=1, z=1 I get

[[  0.52336156]
 [-24.89011192]
 [  1.31380844]]

If I reposition HMD to x=1, y=1, z=2 I get

[[  0.12813818]
 [-24.89011192]
 [  2.2323935 ]]

If I reposition HMD to x=1, y=2, z=2 I get

[[  0.12813818]
 [-23.89011192]
 [  2.2323935 ]]

Seems like the co-ordinate frames are not quite aligned, so perhaps I'd a little off in my code.

I'll try building this up into a full script (Kinect camera watching me wear a headset and sending me the HMD position).