Kinect V2 SDK 2.0 – colored point clouds

The code posted here allows you to create and store a colored 3D point cloud taken using Kinect for Windows v2. The point cloud can easily be imported into a software like MeshLab for visualizing as the data is stored in a common .PLY format.

kinect

Note: A more flexible solution is described in a newer blog post.

Note that the program will capture images as long as you don’t press any keyboard buttons.


using System;
using System.ComponentModel;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using Microsoft.Kinect;
using System.Reflection;
using System.Text;
using System.Threading;

public class Program
{
int count = 0;

///

/// Indicates opaque in an opacity mask
///

private const int OpaquePixel = -1;

///

/// Size of the RGB pixel in the bitmap
///

private readonly int bytesPerPixel = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;

///

/// Active Kinect sensor
///

private KinectSensor kinectSensor = null;

///

/// Coordinate mapper to map one type of point to another
///

private CoordinateMapper coordinateMapper = null;

///

/// Reader for depth/color/body index frames
///

private MultiSourceFrameReader multiFrameSourceReader = null;

///

/// Intermediate storage for receiving depth frame data from the sensor
///

private ushort[] depthFrameData = null;

///

/// Intermediate storage for receiving color frame data from the sensor
///

private byte[] colorFrameData = null;

///

/// Intermediate storage for the depth to color mapping
///

private ColorSpacePoint[] colorPoints = null;

///

/// Intermediate storage for the depth to color mapping
///

private CameraSpacePoint[] cameraPoints = null;

///

/// Initializes a new instance of the MainWindow class.
///

public Program()
{
// get the kinectSensor object
this.kinectSensor = KinectSensor.GetDefault();

// open multiframereader for depth, color, and bodyindex frames
this.multiFrameSourceReader = this.kinectSensor.OpenMultiSourceFrameReader(FrameSourceTypes.Depth | FrameSourceTypes.Color | FrameSourceTypes.BodyIndex);

// wire handler for frames arrival
this.multiFrameSourceReader.MultiSourceFrameArrived += this.Reader_MultiSourceFrameArrived;

// get the coordinate mapper
this.coordinateMapper = this.kinectSensor.CoordinateMapper;

// get FrameDescription from DepthFrameSource
FrameDescription depthFrameDescription = this.kinectSensor.DepthFrameSource.FrameDescription;

int depthWidth = depthFrameDescription.Width;
int depthHeight = depthFrameDescription.Height;

// allocate space to put the pixels being received and converted
this.depthFrameData = new ushort[depthWidth * depthHeight];
this.colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
this.cameraPoints = new CameraSpacePoint[depthWidth * depthHeight];

// get FrameDescription from ColorFrameSource
FrameDescription colorFrameDescription = this.kinectSensor.ColorFrameSource.FrameDescription;

int colorWidth = colorFrameDescription.Width;
int colorHeight = colorFrameDescription.Height;

// allocate space to put the pixels being received
this.colorFrameData = new byte[colorWidth * colorHeight * this.bytesPerPixel];

// open the sensor
this.kinectSensor.Open();

Console.ReadLine();

this.multiFrameSourceReader.Dispose();
this.multiFrameSourceReader = null;

if (this.kinectSensor != null)
{
this.kinectSensor.Close();
this.kinectSensor = null;
}

}

///

/// INotifyPropertyChangedPropertyChanged event to allow window controls to bind to changeable data
///

public event PropertyChangedEventHandler PropertyChanged;

///

/// Handles the depth/color/body index frame data arriving from the sensor
///

/// object sending the event /// event arguments private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
int depthWidth = 0;
int depthHeight = 0;

int colorWidth = 0;
int colorHeight = 0;

bool multiSourceFrameProcessed = false;
bool colorFrameProcessed = false;
bool depthFrameProcessed = false;

MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();

if (multiSourceFrame != null)
{
// Frame Acquisition should always occur first when using multiSourceFrameReader
using (DepthFrame depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
{
if (depthFrame != null)
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
depthWidth = depthFrameDescription.Width;
depthHeight = depthFrameDescription.Height;

if ((depthWidth * depthHeight) == this.depthFrameData.Length)
{
depthFrame.CopyFrameDataToArray(this.depthFrameData);

depthFrameProcessed = true;
}
}

if (colorFrame != null)
{
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
colorWidth = colorFrameDescription.Width;
colorHeight = colorFrameDescription.Height;

if ((colorWidth * colorHeight * this.bytesPerPixel) == this.colorFrameData.Length)
{
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(this.colorFrameData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(this.colorFrameData, ColorImageFormat.Bgra);
}

colorFrameProcessed = true;
}
}
multiSourceFrameProcessed = true;
}
}
}

// we got all frames
if (multiSourceFrameProcessed && depthFrameProcessed && colorFrameProcessed)
{
int len = 0;
StringBuilder sb = new StringBuilder();

this.coordinateMapper.MapDepthFrameToColorSpace(this.depthFrameData, this.colorPoints);
this.coordinateMapper.MapDepthFrameToCameraSpace(this.depthFrameData, this.cameraPoints);

// loop over each row and column of the depth
for (int y = 0; y < depthHeight; y+=2) { for (int x = 0; x < depthWidth; x+=2) { // calculate index into depth array int depthIndex = (y * depthWidth) + x; CameraSpacePoint p = this.cameraPoints[depthIndex]; // retrieve the depth to color mapping for the current depth pixel ColorSpacePoint colorPoint = this.colorPoints[depthIndex]; byte r = 0; byte g = 0; byte b = 0; // make sure the depth pixel maps to a valid point in color space int colorX = (int)Math.Floor(colorPoint.X + 0.5); int colorY = (int)Math.Floor(colorPoint.Y + 0.5); if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight)) { // calculate index into color array int colorIndex = ((colorY * colorWidth) + colorX) * this.bytesPerPixel; // set source for copy to the color pixel int displayIndex = depthIndex * this.bytesPerPixel; b = this.colorFrameData[colorIndex++]; g = this.colorFrameData[colorIndex++]; r = this.colorFrameData[colorIndex++]; } if (!(Double.IsInfinity(p.X)) && !(Double.IsInfinity(p.Y)) && !(Double.IsInfinity(p.Z))) { if (p.X < 3.0 && p.Y < 3.0 && p.Z < 3.0) { sb.Append(String.Format(CultureInfo.InvariantCulture, "{0} {1} {2} {3} {4} {5}\n", p.X, p.Y, p.Z, r, g, b)); len++; } } } } String header = "ply \n" + "format ascii 1.0 \n" + "element vertex " + len + "\n" + "property float x \n" + "property float y \n" + "property float z \n" + "property uchar red \n" + "property uchar green \n" + "property uchar blue \n" + "end_header \n"; System.IO.StreamWriter file = new System.IO.StreamWriter("cloud_" + count++ + ".ply"); file.WriteLine(header + sb.ToString()); Console.WriteLine("Wrote data"); file.Close(); } } public static void Main() { //Thread.Sleep(2000); new Program(); } } } [/csharp]

5 thoughts on “Kinect V2 SDK 2.0 – colored point clouds”

    1. tem.IO.StreamWriter file = new System.IO.StreamWriter(“cloud_” + count++ + “.ply”);

      So that would mean that it gets saved in your project/executable working folder

      1. Thank you very much ! From your answer I can infer that I have wrongly compiled the program( did it using visual studio-2013 WPF (c#)). I am new to Kinect as well as Visual studio, as a part of my project I need to obtain colored point cloud , it’s my humble request to make file available in DOWNLOADS section of your blog

        1. What kind of file? If it’s a project you want to make available, you can create a github repository for it, and I can link to it from this blog.

  1. I downloaded KinectRecorder.zip from downloads link and I executed by using visual studio 2013 but it just showed press any key to exit.. after that I waited for 1 minute. I found one zip file inside project folder. However, inside zip file there is nothing included . Could you help me solve this problem ?

Leave a Reply

Your email address will not be published. Required fields are marked *