Floor Detection using Kinect

• Download the source code

Kinect is primarily used in Body and Face tracking applications. Few people know, though, that Kinect is an amazing sensor for detecting the floor, too!

The native Kinect SDK provides us with all the information we need to accurately recognize whether a particular plane in the 3D space is, actually, a floor. We can also acquire information such as the height the Kinect sensor is placed at. We can even measure the distance between a point in the 3D space and the floor. All this can have a ton of impact in modern Augmented Reality applications.

In today’s tutorial, I am going to show you the following:

• How to detect the floor clip plane
• How to estimate the position (height) of the Kinect sensor
• How to measure the distance between a point and the floor

This is a snapshot of what we are going to develop:

Prerequisites

To run the code and samples provided in this article, you’ll need the following:

• Kinect for XBOX v2 sensor with an adapter (or Kinect for Windows v2 sensor)
• Kinect for Windows v2 SDK
• Windows 8.1 or higher
• Visual Studio 2013 or higher
• A dedicated USB 3 port

Source Code

The source code of the demo is hosted on GitHub. Moreover, all of this functionality is part of Vitruvius. Vitruvius is the most popular Kinect framework and will help you create Kinect apps very easily. So, if you are in a hurry, just download Vitruvius and integrate it into your apps.

• Download the source code
• Download Vitruvius

Detecting the Floor Clip Plane

Some floor-detection functionality is already built into the Kinect SDK. Please, meet the FloorClipPlane property. The FloorClipPlane is a member of the BodyFrame class. Very few people are aware of its existence, however, it’s one of the most useful components when developing motion apps.

This is how to access the FloorClipPlane property:

using (BodyFrame frame = e.FrameReference.AcquireFrame())
{
    if (frame != null)
    {
        Vector4 floorClipPlane = frame.FloorClipPlane;
    }
}

As you can see, the FloorClipPlane is a set of 4 floating-point values (Vector4): X, Y, Z, and W.

• The X, Y, and Z values indicate the orientation of the plane in the 3D space.
• The W value indicates the distance between the plane and the origin of the coordinate system.

Actually, the Vector4 structure represents the mathematical equation of a plane. You can read more about it on Wolfram MathWorld.

For our examples, we are going to encapsulate the FloorClipPlane vector into a handy Floor C# class:

public class Floor
{
    public float X { get; internal set; }
    public float Y { get; internal set; }
    public float Z { get; internal set; }
    public float W { get; internal set; }

    public Floor(Vector4 floorClipPlane)
    {
        X = floorClipPlane.X;
        Y = floorClipPlane.Y;
        Z = floorClipPlane.Z;
        W = floorClipPlane.W;
    }
}

Kinect Sensor Height – The Magic W

As we saw, the W value indicates the distance between the floor and the origin of the coordinate system. The origin of the Kinect coordinate system is the device itself! Consequently, the W parameter of the FloorClipPlane describes the height where the Kinect sensor is positioned!

Using the W value, you can provide feedback to your users:

Vector4 floorClipPlane = frame.FloorClipPlane;
float height = floorClipPlane.W;

if (height < 1f) // 1 meter
{
    Debug.WriteLine("The sensor is positioned too low!");
}

Field of View

When the W value is 0, it means that the field of view is limited. This is a very easy way to understand whether there are any objects, such as tables, chairs, etc. that are blocking the field of view.

if (height == 0f)
{
    Debug.WriteLine("The field of view is limited.");
}

Kinect Sensor Tilt Angle

Other than finding the height of the sensor, we can also detect the tilting angle of the device. All we need to do is use the orientation of the floor plane:

public double Tilt
{
    get
    {
        return Math.Atan(Z / Y) * (180.0 / Math.PI);
    }
}

Measuring Distances

In one of my previous articles, I explained how to measure the distance between 2 points in the 3D space using simple Mathematical equations.

Now, we need to measure the distance between a point and a plane in the 3D space. This is trickier, but we can easily figure it out with the help of Geometry.

The detected floor is a plane with a known distance (W) and orientation (X, Y, Z). A point in the 3D space is a set of X, Y, and Z coordinates.

// Plane (X, Y, Z, W)
Floor floor = new Floor(frame.FloorClipPlane);

// Point (X, Y, Z)
CameraSpacePoint point = body.Joints[JointType.WristLeft].Position;

To measure the distance between a plane and a point, we need to use the Point-Plane Distance Formula:

How shall we convert this formula into programming code? Let me do the Math for you:

public double DistanceFrom(CameraSpacePoint point)
{
   double numerator = X * point.X + Y * point.Y + Z * point.Z + W;
   double denominator = Math.Sqrt(X * X + Y * Y + Z * Z);

   return numerator / denominator;
}

Add this method in your Floor.cs class. This is it! You can now get the 3D position of any point, pass it as a parameter to the above method, and find its distance from the floor.

double distance = floor.DistanceFrom(point);

For your reference, this is the complete Floor.cs class:

public class Floor
{
    public float X { get; internal set; }
    public float Y { get; internal set; }
    public float Z { get; internal set; }
    public float W { get; internal set; }

    public Floor(Vector4 floorClipPlane)
    {
        X = floorClipPlane.X;
        Y = floorClipPlane.Y;
        Z = floorClipPlane.Z;
        W = floorClipPlane.W;
    }

    public float Height
    {
        get { return W; }
    }

    public double Tilt
    {
        get { return Math.Atan(Z / Y) * (180.0 / Math.PI); }
    }

    public double DistanceFrom(CameraSpacePoint point)
    {
        double numerator = X * point.X + Y * point.Y + Z * point.Z + W;
        double denominator = Math.Sqrt(X * X + Y * Y + Z * Z);

        return numerator / denominator;
    }
}

This is it, folks! You can now use the Floor.cs class to implement cool functionality! In my C# sample code, I have also added numerous Extension methods that will let you draw the points of the floor and even find the projection of any other point.

History

• 28^th January, 2019: Initial version