Transform world space using Kinect FloorClipPlane to move origin to floor while keeping orientation

Question

The Kinect SDK provides a property SkeletonFrame.FloorClipPlane that describes the orientation of the floor plane relative to the camera. MSDN contains slightly more information in the Coordinate Spaces page for Kinect.

I want to determine a matrix that transforms the camera coordinate space to a 'world space'. In this world space the origin would correspond be a projection of the camera position on the floor, ie straight below the camera.

• The X axis is to the sides of the camera, but rotated to be parallel to the floor, compensating for pitch
• The Y axis is straight up from the ground to the Kinect, compensating for yaw
• The Z axis is straight forward from the camera/origin, but rotated to be parallel to the floor, compensating for tilt

The 2 steps in the transformation would be

1. Translate from the camera position to the ground
2. Rotate around the origin while keeping orientation

Unfortunately my linear algebra is rusty and always has been.

Solution update:

I adapted the solution below to create the following method that creates a transformation matrix based on the floor normal and camera height:

Matrix CameraTransform(Vector3 normal, float scalar)
{
    // Note the first three coefficients of the plane equation are the normal
    Vector3 yNew = new Vector3(normal.X, normal.Y, normal.Z);
    Vector3 zNew = new Vector3(0, 1, -normal.Y / normal.Z);
    zNew.Normalize();
    Vector3 xNew = Vector3.Cross(yNew, zNew);

    // assumes column vectors, multiplied on the right
    Matrix rotMatrix = new Matrix(
        xNew.X, yNew.X, zNew.X, 0,
        xNew.Y, yNew.Y, zNew.Y, 0,
        xNew.Z, yNew.Z, zNew.Z, 0,
        0, 0, 0, 1);

    Matrix translationMatrix = Matrix.CreateTranslation(0, scalar, 0);

    return rotMatrix * translationMatrix;
}

Answer 1

You're looking for a change of basis. The way to do it is to find the coordinates of the new basis in the old one.

For simplicity, we'll say that the FloorClipPlane represents the equation Ax+By+Cz+D=0

Let's do the rotation first, and then we can translate it to the floor.

1. We'll find the coordinate of the Y axis, which happens to be the normal of the floor plane.
2. In order to find the coordinate of the Z axis, we're looking for a unit vector in the line that represents the intersection of the floor plane (translated to the origin) and the x=0 plane; if the planes are Ax+By+Cz=0 and x=0, the line will be z=-B/C*y. Fixing y to 1, we can then normalize.
3. Finally, the X axis is just the cross product of Y and Z (in that order, assuming a right-hand coordinate system).

Now, we'll translate the basis to the floor. The vertical distance is the fourth parameter of the floor plane equation. We'll have to use a 4x4 matrix to support translation, and we'll combine them so the rotation matrix is applied first, the translation second.

Let's see some pseudocode:

// Note the first three coefficients of the plane equation are the normal
yNew = vector(floorPlane.A, floorPlane.B, floorPlane.C);
zNew = vector(0, 1, -floorPlane.B/floorPlane.C);
zNew.Normalize();
xNew = CrossProduct(yNew, zNew);

// assumes column vectors, multiplied on the right

rotMatrix = matrix(xNew.x, xNew.y, xNew.z, 0,
                   yNew.x, yNew.y, yNew.z, 0,
                   zNew.x, zNew.y, zNew.z, 0,
                   0,      0,      0,      1);

translationMatrix = matrix(1, 0, 0, 0,
                           0, 1, 0, -floorPlane.D,
                           0, 0, 1, 0,
                           0, 0, 0, 1);

transform = translationMatrix * rotMatrix;

Hopefully it all makes sense. Unfortunately, I haven't been able to test this as I don't have a Kinect setup. Let me know if you have issues.

Answer 2

I have tried to implement what's described in Sergio's answer, in Matlab.

I wanted my yNew joint height to be the distance from the floor. But when I put -floorPlaneD in the translation matrix, I didn't obtain what I expected.

The solution for me was to put +floorPlaneD instead.