private BitmapSource ToCombinedData(ColorFrame colorFrame, DepthFrame depthFrame, String formated_date)
{
int colorwidth = colorFrame.FrameDescription.Width;
int colorheight = colorFrame.FrameDescription.Height;
var depthWidth = depthFrame.FrameDescription.Width;
var depthHeight = depthFrame.FrameDescription.Height;
PixelFormat format = PixelFormats.Bgr32;
int bytesPerPixel = ((format.BitsPerPixel + 7) / 8);
byte[] _colorFrameData = new byte[colorwidth * colorheight * bytesPerPixel];
byte[] pixels = new byte[depthWidth * depthHeight * bytesPerPixel];
ushort[] _depthData = new ushort[depthWidth * depthHeight];
ColorSpacePoint[] _colorSpacePoints = new ColorSpacePoint[depthWidth * depthHeight];
CameraSpacePoint[] _cameraSpacePoints = new CameraSpacePoint[depthHeight * depthWidth];
colorFrame.CopyConvertedFrameDataToArray(_colorFrameData, ColorImageFormat.Bgra);
depthFrame.CopyFrameDataToArray(_depthData);
byte[] full_pixelData = new byte[depthHeight * depthWidth];
Buffer.BlockCopy(_depthData, 0, full_pixelData, 0, depthWidth * depthHeight);
SaveDepthMatrixTimestamp.AddLast(formated_date);
SaveDepthMatrix.AddLast(_depthData);
_sensor.CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorSpacePoints);
_sensor.CoordinateMapper.MapDepthFrameToCameraSpace(_depthData, _cameraSpacePoints);
SaveXYZTimestamps.AddLast(formated_date);
SaveCameraSpacePoints.AddLast(_cameraSpacePoints);
for (int depthY = 0; depthY < depthHeight; depthY++)
{
for (int depthX = 0; depthX < depthWidth; depthX++)
{
int depthIndex = depthY * depthWidth + depthX;
int colorX = (int)(_colorSpacePoints[depthIndex].X + 0.5);
int colorY = (int)(_colorSpacePoints[depthIndex].Y + 0.5);
int colorIndex = colorY * colorwidth + colorX;
if ((0 <= colorX) && (colorX < colorwidth) && (0 <= colorY) && (colorY < colorheight))
{
pixels[depthIndex * bytesPerPixel + 0] = _colorFrameData[colorIndex * bytesPerPixel + 0];
pixels[depthIndex * bytesPerPixel + 1] = _colorFrameData[colorIndex * bytesPerPixel + 1];
pixels[depthIndex * bytesPerPixel + 2] = _colorFrameData[colorIndex * bytesPerPixel + 2];
pixels[depthIndex * bytesPerPixel + 3] = _colorFrameData[colorIndex * bytesPerPixel + 3];
}
}
}
int stride = depthWidth * bytesPerPixel;
return(BitmapSource.Create(depthWidth, depthHeight, 96, 96, format, null, pixels, stride));
}
public void kill()
{
StartSavingFrames = false;
_mode = Mode.Stop;
/*
* IEnumerator<String> a = SaveColorTimestamps.GetEnumerator();
* foreach (BitmapSource node in SaveColorFrames)
* {
* a.MoveNext();
* PngBitmapEncoder enc = new PngBitmapEncoder();
* enc.Frames.Add(BitmapFrame.Create(node));
* string temppath = System.IO.Path.Combine(@"../output/kinect/color/", a.Current + ".png");
* using (System.IO.FileStream fs = new System.IO.FileStream(temppath, System.IO.FileMode.Create))
* {
* enc.Save(fs);
* fs.Close();
* }
* }
* SaveColorTimestamps.Clear();
* SaveColorFrames.Clear();
*
* a.Dispose();
* a = SaveDepthTimestamps.GetEnumerator();
* foreach (BitmapSource node in SaveDepthFrames)
* {
* a.MoveNext();
* PngBitmapEncoder enc = new PngBitmapEncoder();
* enc.Frames.Add(BitmapFrame.Create(node));
* string temppath = System.IO.Path.Combine(@"../output/kinect/depth/", a.Current + ".png");
* using (System.IO.FileStream fs = new System.IO.FileStream(temppath, System.IO.FileMode.Create))
* {
* enc.Save(fs);
* fs.Close();
*
* }
*
* }
* SaveDepthTimestamps.Clear();
* SaveDepthFrames.Clear();
* this.Close();
*/
IEnumerator <String> a = SaveDepthTimestamps.GetEnumerator();
foreach (BitmapSource node in SaveDepthFrames)
{
a.MoveNext();
PngBitmapEncoder enc = new PngBitmapEncoder();
enc.Frames.Add(BitmapFrame.Create(node));
string temppath = System.IO.Path.Combine(@"../output/kinect/depth/", a.Current + ".png");
using (System.IO.FileStream fs = new System.IO.FileStream(temppath, System.IO.FileMode.Create))
{
enc.Save(fs);
fs.Close();
}
}
a.Dispose();
SaveDepthTimestamps.Clear();
SaveDepthFrames.Clear();
a = SaveCombinedTimestamps.GetEnumerator();
foreach (BitmapSource node in SaveCombinedFrames)
{
a.MoveNext();
PngBitmapEncoder enc = new PngBitmapEncoder();
enc.Frames.Add(BitmapFrame.Create(node));
string temppath = System.IO.Path.Combine(@"../output/kinect/color/", a.Current + ".png");
using (System.IO.FileStream fs = new System.IO.FileStream(temppath, System.IO.FileMode.Create))
{
enc.Save(fs);
fs.Close();
}
}
SaveCombinedTimestamps.Clear();
SaveCombinedFrames.Clear();
a.Dispose();
a = SaveXYZTimestamps.GetEnumerator();
foreach (CameraSpacePoint[] node in SaveCameraSpacePoints)
{
a.MoveNext();
string temppath = System.IO.Path.Combine(@"../output/kinect/XYZmatrix/", a.Current + ".txt");
using (System.IO.StreamWriter file =
new System.IO.StreamWriter(temppath, true))
{
for (int depthY = 0; depthY < height; depthY++)
{
for (int depthX = 0; depthX < width; depthX++)
{
int depthIndex = depthY * width + depthX;
float x = node[depthIndex].X;
float y = node[depthIndex].Y;
float z = node[depthIndex].Z;
file.WriteLine("{0:.4f} {1:.4f} {2:.4f},", x.ToString(), y.ToString(), z.ToString());
}
}
}
}
a.Dispose();
a = SaveDepthMatrixTimestamp.GetEnumerator();
foreach (ushort[] node in SaveDepthMatrix)
{
a.MoveNext();
string temppath = System.IO.Path.Combine(@"../output/kinect/depthmatrix/", a.Current + ".txt");
File.WriteAllLines(temppath, node.Select(d => d.ToString()));
}
SaveDepthMatrixTimestamp.Clear();
SaveDepthMatrix.Clear();
a.Dispose();
this.Close();
}