(Another) Math error in convert_to_quaternion

[ekuusi]

Not sure if this is somehow related to the proposed fix to having abs() inside math.sqrt(), but r_w just got a value of 0 for me which caused a divide by zero error in

r_x = (pose_mat[2][1]-pose_mat[1][2])/(4*r_w)

Are the calculation formulas correct if there can be a case where you get a divide by zero error?

[ekuusi]

My earlier implementation for the quaternion calculations were picked from Steam discussions (can't find the link), they went like this (in javascript):

w: Math.sqrt(Math.max(0, 1 + pose[0][0] + pose[1][1] + pose[2][2] )) / 2,
x: Math.sign(pose[2][1] - pose[1][2]) * Math.sqrt(Math.max(0, 1 + pose[0][0] - pose[1][1] - pose[2][2] )) / 2,
y: Math.sign(pose[0][2] - pose[2][0]) * Math.sqrt(Math.max(0, 1 - pose[0][0] + pose[1][1] - pose[2][2] )) / 2,
z: Math.sign(pose[1][0] - pose[0][1]) * Math.sqrt(Math.max(0, 1 - pose[0][0] - pose[1][1] + pose[2][2] )) / 2

As I don't really know what the correct formulas are to form the quaternion, I can't say if the above is a better or even a correct way of doing the calculations. The approach seems to differ a bit from how your quaternion is calculated. It does have a similar fix for negative numbers in calculating w though.

[ekuusi]

So the values you get in this aren't quite the same as with your formula (I guess the axes are different?), but here's that javascript calculation implemented in python, and seems to work ok in three.js:

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.copysign(math.sqrt(max(0, 1 + pose_mat[0][0] - pose_mat[1][1] - pose_mat[2][2] )) / 2,pose_mat[2][1] - pose_mat[1][2])
    r_y = math.copysign(math.sqrt(max(0, 1 - pose_mat[0][0] + pose_mat[1][1] - pose_mat[2][2] )) / 2,pose_mat[0][2] - pose_mat[2][0])
    r_z = math.copysign(math.sqrt(max(0, 1 - pose_mat[0][0] - pose_mat[1][1] + pose_mat[2][2] )) / 2,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]

[lgbeno]

I also looked into a different reference from Unity, it looks similar to the code that you have pasted above. Peeling back the SteamVR C# code in the Unity plugin, they use the following built in function:

public RigidTransform(HmdMatrix34_t pose)
{
    var m = Matrix4x4.identity;

    m[0, 0] =  pose.m0;
    m[0, 1] =  pose.m1;
    m[0, 2] = -pose.m2;
    m[0, 3] =  pose.m3;

    m[1, 0] =  pose.m4;
    m[1, 1] =  pose.m5;
    m[1, 2] = -pose.m6;
    m[1, 3] =  pose.m7;

    m[2, 0] = -pose.m8;
    m[2, 1] = -pose.m9;
    m[2, 2] =  pose.m10;
    m[2, 3] = -pose.m11;

    this.pos = m.GetPosition();
    this.rot = m.GetRotation();
}

r_w = math.sqrt( math.max( 0, 1 + pose_mat[0][0] + pose_mat[1][1]+ pose_mat[2][2] ) ) / 2
r_x = math.sqrt( math.max( 0, 1 + pose_mat[0][0] - pose_mat[1][1] - pose_mat[2][2] ) ) / 2
r_y = math.sqrt( math.max( 0, 1 - pose_mat[0][0] + pose_mat[1][1] - pose_mat[2][2] ) ) / 2
r_z = math.sqrt( math.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] ) )

I need to do more research and testing before I commit.  WRT to axes conventions, I try to make everything match Unity.

[hgdebarba]

Hi Luke, thanks a lot for your wrapper!

I changed the matrix to quaternion conversion to

    r_w = math.sqrt(max(0, 1 + pose_mat[0][0] + pose_mat[1][1] + pose_mat[2][2])) * 0.5;
    r_x = math.sqrt(max(0, 1 + pose_mat[0][0] - pose_mat[1][1] - pose_mat[2][2])) * 0.5;
    r_y = math.sqrt(max(0, 1 - pose_mat[0][0] + pose_mat[1][1] - pose_mat[2][2])) * 0.5;
    r_z = math.sqrt(max(0, 1 - pose_mat[0][0] - pose_mat[1][1] + pose_mat[2][2])) * 0.5;
    r_x *= 1 if ((r_x * (pose_mat[2][1] - pose_mat[1][2])) >= 0) else -1;
    r_y *= 1 if ((r_y * (pose_mat[0][2] - pose_mat[2][0])) >= 0) else -1;
    r_z *= 1 if ((r_z * (pose_mat[1][0] - pose_mat[0][1])) >= 0) else -1;

(http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/)

And in Unity I did this mess

public static void ConvertVIVEToUnity(ref Vector3 pos, ref Quaternion rot)
        {
            pos = new Vector3(-pos[0], pos[1], pos[2]);

            rot = new Quaternion(-rot[2], -rot[1], rot[0], rot[3]);
            Quaternion rot180 = Quaternion.AngleAxis(180.0f, Vector3.up);
            Quaternion rot90 = Quaternion.AngleAxis(90.0f, Vector3.forward);

            rot = rot * rot90 * rot180;

        }

to have meaningful direction of movement and rotation relative to the lighthouse position. I still need to check it carefully as it might be redundant somewhere with all those modifications to the rotation quaternion ....

Hope this can be useful.

By the way, could you set a license to this repository?

Thanks!