Kinect v2 + Unity-> MapDepthFrameToColorSpaceUsingIntPtr

Question

I'm trying to map the Depth Data from KinectV2 to the Color Space in a Unity-Script. It worked as expected with the usual coordinate mapper function

_sensor.CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorSpacePoints)

But it reduced my Framerate by 11 - wich is not acceptable =)

Therefore i took a look in the unity examples provided by Microsoft and found some piece for Mapping working with Pointer.

var pDepthData = GCHandle.Alloc(pDepthBuffer, GCHandleType.Pinned);
var pDepthCoordinatesData = GCHandle.Alloc(m_pDepthCoordinates, GCHandleType.Pinned);

m_pCoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
        pDepthData.AddrOfPinnedObject(), 
        (uint)pDepthBuffer.Length * sizeof(ushort),
        pDepthCoordinatesData.AddrOfPinnedObject(), 
        (uint)m_pDepthCoordinates.Length);

pDepthCoordinatesData.Free();
pDepthData.Free();

The equivalent method for my needs exist as well. I tried the pointer version of MapDepthFrameToColorSpace

var pDepthData = GCHandle.Alloc(pDepthBuffer, GCHandleType.Pinned);
var pColorData = GCHandle.Alloc(m_pColorSpacePoints, GCHandleType.Pinned);

m_pCoordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
        pDepthData.AddrOfPinnedObject(), 
        pDepthBuffer.Length * sizeof(ushort),
        pColorData.AddrOfPinnedObject(),
        (uint)m_pColorSpacePoints.Length * sizeof(float) * 2);

pColorData.Free();
pDepthData.Free();

The pDepthBuffer has valid data at method call and m_pColorSpacePoints is initialized and has the same length as pDepthBuffer (as recommended in the MSDN Documentation) Im still working with 1408 SDK version. The Result after the Function is an Array with Empty/NegativeInfinity Float values and no valid ColorSpacePoints. No Error-Message as well. Any suggestions?

Answer 1

I got it working. After updating to the latest SDK Version 1409 the messageall returned the error message

ArgumentException: Value does not fall within the expected range. Rethrow as ArgumentException: This API has returned an exception from an HRESULT: 0x80070057

So i played a little bit with the parameters and now i'm getting valid Data.

Here are my modifications to the CoordinateMapperManager of the GreenScreen Example. I added a buffer for the mapped color values to get a pixel position in the HD Color Stream by a depth value position,

//added declaration and initialization
private ColorSpacePoint[] m_pColorSpacePoints;

//in awake method
m_pColorSpacePoints = new ColorSpacePoint[pDepthBuffer.Length];

//accesor to the colorspacepoints
public ColorSpacePoint[] GetColorSpacePointBuffer()
{
  return m_pColorSpacePoints;
}

//the new process frame method
void ProcessFrame()
{
  var pDepthData = GCHandle.Alloc(pDepthBuffer, GCHandleType.Pinned);
  var pDepthCoordinatesData = GCHandle.Alloc(m_pDepthCoordinates, GCHandleType.Pinned);
  var pColorData = GCHandle.Alloc(m_pColorSpacePoints, GCHandleType.Pinned);

  m_pCoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
    pDepthData.AddrOfPinnedObject(), 
    (uint)pDepthBuffer.Length * sizeof(ushort),
    pDepthCoordinatesData.AddrOfPinnedObject(), 
    (uint)m_pDepthCoordinates.Length);


  m_pCoordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
    pDepthData.AddrOfPinnedObject(),
    pDepthBuffer.Length * sizeof(ushort),
    pColorData.AddrOfPinnedObject(),
    (uint)m_pColorSpacePoints.Length);

  pColorData.Free();
  pDepthCoordinatesData.Free();
  pDepthData.Free();

  m_pColorRGBX.LoadRawTextureData(pColorBuffer);
  m_pColorRGBX.Apply ();
}

And here an example how to use it. I used Untiy to do the rendering in GPU Shader. The following Code is a shortened untested piece of code for demonstration how to get the data:

//Size of the Kinect V2 Depth Stream
var _width = 512;
var _height = 424;
var _particleCount = _width * _height;

//Initialize an Array to store position data for particles
var _particleArray = new Vector3[_particleCount];
var _colorArray = new Color[_particleCount];

//Get Depth Data
var _depthData = _coordinateMapperManager.GetDepthPointBuffer();

//Get Mapped Color Space Points
var _colorSpacePoints = _coordinateMapperManager.GetColorSpacePointBuffer();

//Get the Colorstream as Texture
var _texture = _coordinateMapperManager.GetColorTexture();

var c = 0;
for (var i = 0; i < _height; ++i)
{
  for (var j = 0; j < _width; ++j)
  {
    _particleArray[c++] = new Vector3(j, i);
  }
}

for (var i = 0; i < _depthData.Length; ++i)
{
    _colorArray[i] = _texture.GetPixel((int)_colorSpacePoints[i].X, (int)_colorSpacePoints[i].Y);
    _particleArray[i].z = _depthData[i];
}

//Now we have all Data to build a colored 3D PointCloud