private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
if (!(KinectStreamerConfig.StreamBodyData ||
KinectStreamerConfig.StreamColorData ||
KinectStreamerConfig.StreamDepthData ||
KinectStreamerConfig.StreamPointCloudData ||
KinectStreamerConfig.ProvideCalibrationData ||
KinectStreamerConfig.StreamColoredPointCloudData
))
{
return;
}
depthFrame = null;
colorFrame = null;
bodyFrame = null;
bodyStreamMessage = null;
colorStreamMessage = null;
pointCloudStreamMessage = null;
depthStreamMessage = null;
calibrationDataMessage = null;
coloredPointCloudStreamMessage = null;
multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// We use a try/finally to ensure that we clean up before we exit the function.
// This includes calling Dispose on any Frame objects that we may have and unlocking the bitmap back buffer.
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
//Debug.Write(colorFrame.RelativeTime.ToString());
bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
// The "finally" statement will Dispose any that are not null.
if ((depthFrame == null) || (colorFrame == null) || (bodyFrame == null))
{
return;
}
// Process color stream if needed
if (KinectStreamerConfig.StreamColorData || KinectStreamerConfig.StreamColoredPointCloudData)
{
ProcessColorData();
}
// Process depth frame if needed
if (KinectStreamerConfig.StreamDepthData || KinectStreamerConfig.StreamPointCloudData || KinectStreamerConfig.StreamColoredPointCloudData)
{
ProcessDepthData();
if (KinectStreamerConfig.StreamPointCloudData || KinectStreamerConfig.StreamColoredPointCloudData)
{
GenerateFullPointCloud();
}
}
if (KinectStreamerConfig.StreamColoredPointCloudData)
{
ProcessPointCloudColors();
}
// Process body data if needed
if (KinectStreamerConfig.StreamBodyData || KinectStreamerConfig.ProvideCalibrationData)
{
ProcessBodyData();
}
SendData();
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
}
}
}
// TODO: don't sent alpha
private void ProcessPointCloudColors()
{
byte[] pointCloudColors = new byte[depthArray.Length * 4];
ColorSpacePoint[] colorSpacePoints = new ColorSpacePoint[depthArray.Length];
var colorWidth = ColorFrameDescription.Width;
var colorHeight = ColorFrameDescription.Height;
CoordinateMapper.MapDepthFrameToColorSpace(depthArray, colorSpacePoints);
float maxX = 0;
float maxY = 0;
for (int i = 0; i < depthArray.Length; i++)
{
var point = colorSpacePoints[i];
if (GeometryHelper.IsValidPoint(point))
{
maxX = point.X > maxX ? point.X : maxX;
maxY = point.Y > maxY ? point.Y : maxY;
int colorX = (int) Math.Floor(point.X + 0.5);
int colorY = (int)Math.Floor(point.Y + 0.5);
if ((colorX > 0 && colorX < colorWidth) && (colorY > 0 && colorY < colorHeight))
{
int index = (int)(((colorWidth*colorY)+colorX) * 4);
pointCloudColors[i * 4] = colorPixels[index];
pointCloudColors[i * 4 + 1] = colorPixels[index + 1];
pointCloudColors[i * 4 + 2] = colorPixels[index + 2];
pointCloudColors[i * 4 + 3] = colorPixels[index + 3];
}
//int index = ((int)point.X + DepthFrameDescription.Width / 2) +
// ((int)point.Y + DepthFrameDescription.Height / 2) * DepthFrameDescription.Width;
//int index = (int)((point.X + DepthFrameDescription.Width / 2) * ColorFrameDescription.BytesPerPixel +
// (point.Y+ DepthFrameDescription.Height/2) * stride);
}
}
coloredPointCloudStreamMessage = new ColoredPointCloudStreamMessage(FullPointCloud, pointCloudColors);
}