protected void ShowpictureBoxEntropy()
{
this.pictureBoxEntropy.Image = DepthMetaData.ToEntropyImage_Bitmap(this.bitmapEntropy, DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect, pixelsEntropyArray);
this.pointsEntropyChart = Helper_CreateAndShowEntropyImageStatistics(pixelsEntropyArray);
this.pictureBoxPolygon.Image = ImageUtils.CreatePolygon(PointsEntropyChart);
}
private void UnusedGenerate_Depth(Bitmap bm)
{
this.DepthMetaData.Vectors = new List <Vector3>();
LockBitmap lockBitmap = new LockBitmap(bm);
lockBitmap.LockBits();
byte[] pixelData = lockBitmap.Pixels;
DepthMetaData = new DepthMetaData();
DepthMetaData.FrameData = new ushort[pixelData.GetLength(0)];
//depthArr = new uint[bm.Width, bm.Height];
for (int iy = 0; iy < bm.Height; iy++)
{
int step24 = bm.Width * 3 * iy;
for (int ix = 0; ix < bm.Width; ix++)
{
int pixel24 = step24 + 3 * ix;
ushort depth = BitConverter.ToUInt16(pixelData, pixel24);
int depthIndex = (iy * DepthMetaData.XDepthMaxRealSense) + ix;
if (depth != 0)
{
Vector3 v = ConvertDepthTomm(ix, iy, depth);
this.DepthMetaData.Vectors.Add(v);
DepthMetaData.FrameData[depthIndex] = Convert.ToUInt16(v.Z);
}
}
}
lockBitmap.UnlockBits();
//---------------
}
public RealsenseBO(System.Windows.Forms.Control myParentControl, System.Windows.Forms.PictureBox myPictureBoxColor, System.Windows.Forms.PictureBox myPictureBoxDepth, System.Windows.Forms.PictureBox mypictureBoxIR, OGLControl myOpenGLControl,
System.Windows.Forms.Label mycameraFpsLabel) : this()
{
parentControl = myParentControl;
openGLPart = new OpenGLPart(this, myParentControl, myOpenGLControl);
DepthMetaData = new DepthMetaData();
PathModels = System.IO.Path.GetDirectoryName(System.Reflection.Assembly.GetExecutingAssembly().Location) + GLSettings.PathPointClouds;
try
{
pictureBoxDepth = myPictureBoxDepth;
pictureBoxColor = myPictureBoxColor;
pictureBoxIR = mypictureBoxIR;
cameraFpsLabel = mycameraFpsLabel;
openGLControl = myOpenGLControl;
GetRealSenseCameras(out DevicesDepth, out DevicesColor, out videoDevices);
NumberOfDevices = DevicesDepth.Count;
SetConversionMatrices();
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("SW Error initializing Scanner : " + err.Message);
}
}
private static PointCloud PointCloudBW(CameraSpacePoint[] myRealWorldPoints, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
{
int x = 0;
int y = 0;
// int indexVertex = 0;
List <Vector3> list = new List <Vector3>();
try
{
for (x = 0; x < DepthMetaData.XDepthMaxKinect; x++)
{
for (y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
//int depthIndex = ((DepthMetaData.YResDefault - y - 1) * DepthMetaData.XResDefault) + x;
if (myDepthMetaData.FrameData[depthIndex] != 0)
{
int depthIndexColor = depthIndex * MetaDataBase.BYTES_PER_PIXEL;
Vector3 vect = new Vector3(myRealWorldPoints[depthIndex].X, myRealWorldPoints[depthIndex].Y, -myRealWorldPoints[depthIndex].Z);
list.Add(vect);
}
}
}
}
catch
{
System.Diagnostics.Debug.WriteLine("..Debug : " + x.ToString() + " : " + y.ToString() + " : ");
}
return(PointCloud.FromListVector3(list));
}
private void HelperInterpolation_End(int xMax, int yMax)
{
iFrameInterpolation = 0;
listPointsInterpolated = DepthMetaData.CreateListPoints_Depth(this.DepthMetaData.FrameData, xMax, yMax);
}
/// <summary>
/// creates color info for all DEPTH pixels (to later e.g. write ply file from it)
/// </summary>
/// <param name="myColorMetaData"></param>
/// <param name="myDepthMetaData"></param>
/// <param name="myCoordinateMapper"></param>
/// <returns></returns>
public static byte[] ColorInfoForDepth(ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, CoordinateMapper myCoordinateMapper)
{
//Test_GetCameraPoints(myDepthMetaData, myCoordinateMapper);
byte[] mydisplayPixels = new byte[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect * MetaDataBase.BYTES_PER_PIXEL];
//mapped data
int numberOfPoints = 0;
int notMappedPixels = -1;
if (myColorMetaData != null)
{
ColorSpacePoint[] mycolorPointsInDepthSpace = new ColorSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
myCoordinateMapper.MapDepthFrameToColorSpace(myDepthMetaData.FrameData, mycolorPointsInDepthSpace);
for (int x = 0; x < DepthMetaData.XDepthMaxKinect; ++x)
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; ++y)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
if (myDepthMetaData.FrameData[depthIndex] != 0)
{
ColorSpacePoint colorPoint = mycolorPointsInDepthSpace[depthIndex];
int xColor = Convert.ToInt32(colorPoint.X);
int yColor = Convert.ToInt32(colorPoint.Y);
int depthIndexColor = depthIndex * MetaDataBase.BYTES_PER_PIXEL;
if ((xColor >= 0) && (xColor < ColorMetaData.XColorMaxKinect) && (yColor >= 0) && (yColor < ColorMetaData.YColorMaxKinect))
{
int colorIndex = ((yColor * ColorMetaData.XColorMaxKinect) + xColor) * MetaDataBase.BYTES_PER_PIXEL;
mydisplayPixels[depthIndexColor + 0] = myColorMetaData.Pixels[colorIndex];
mydisplayPixels[depthIndexColor + 1] = myColorMetaData.Pixels[colorIndex + 1];
mydisplayPixels[depthIndexColor + 2] = myColorMetaData.Pixels[colorIndex + 2];
mydisplayPixels[depthIndexColor + 3] = 0xff;
numberOfPoints++;
}
else
{
notMappedPixels++;
mydisplayPixels[depthIndexColor + 0] = 255;
mydisplayPixels[depthIndexColor + 1] = 255;
mydisplayPixels[depthIndexColor + 2] = 255;
mydisplayPixels[depthIndexColor + 3] = 0xff;
}
}
}
}
}
//System.Diagnostics.Debug.WriteLine("---------> Number of Depth points: " + numberOfPoints.ToString());
return(mydisplayPixels);
}
protected void SaveDepth_BW_Interpolated()
{
if (listPointsInterpolated == null)
{
return;
}
//-----------------------------------------------
//now interpolate last 10 frames to one frame and save
System.Drawing.Image depthInterpolated = DepthMetaData.ToImage(listPointsInterpolated);
depthInterpolated.SaveImage(PathModels, "DepthInterpolated", true);
}
public void SavePointCloud_BW()
{
if (this.DepthMetaData == null)
{
MessageBox.Show("No Depth Data to save - please capture, or open last saved depth data");
return;
}
//ushort[] rotatedPoints = DepthMetaData.RotateDepthFrame(this.DepthMetaData.FrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault);
List <Vector3> listPoints = DepthMetaData.CreateListPoints_Depth(this.DepthMetaData.FrameData, DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect);
GLSettings.FileNamePointCloudLast1 = DateTime.Now.Year.ToString() + "." + DateTime.Now.Month.ToString() + "." + DateTime.Now.Day.ToString() + "." + DateTime.Now.Hour.ToString() + "." + DateTime.Now.Minute.ToString() + "." + DateTime.Now.Second.ToString() + "_PointCloud.xyz";
UtilsPointCloudIO.ToXYZFile(listPoints, GLSettings.FileNamePointCloudLast1, PathModels);
}
public System.Drawing.Image Color_Image(ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
{
WriteableBitmap myBitmapColorPlayer = Color_Bitmap(myColorMetaData, myDepthMetaData, myBodyMetaData);
if (myBitmapColorPlayer != null)
{
return(myBitmapColorPlayer.ToImage());
}
else
{
return(null);
}
}
private bool ProcessDepthFrame(MultiSourceFrame reference)
{
// Depth
using (var frame = reference.DepthFrameReference.AcquireFrame())
{
if (frame != null)
{
DepthFrame frameDepth = frame;
if (PointCloudScannerSettings.ScannerMode == ScannerMode.Depth || PointCloudScannerSettings.ScannerMode == ScannerMode.Color_Depth || PointCloudScannerSettings.ScannerMode == ScannerMode.Color_Depth_3DDisplay)
{
this.DepthMetaData = new DepthMetaData(frameDepth, false);
if (PointCloudScannerSettings.CutFrames)
{
this.DepthMetaData.FrameData = DepthMetaData.CutDepth(this.DepthMetaData.FrameData, PointCloudScannerSettings.CutFrameMaxDistance, PointCloudScannerSettings.CutFrameMinDistance, ref numberOfCutPoints);
}
//this.pictureBoxDepth.Image = this.bitmapDepth.Update_Gray(DepthMetaData.FrameData);
if (this.openGLPart.ShowingIn3DControl)
{
openGLCounter++;
// if (openGLCounter == this.openGLRefreshAt)
{
openGLCounter = 0;
this.UpdateOpenGLControl();
}
}
else
{
this.bitmapDepth.Update_Gray(DepthMetaData.FrameData);
if (PointCloudScannerSettings.ScannerMode != ScannerMode.Color_Depth_3DDisplay)
{
this.pictureBoxDepth.Refresh();
}
}
}
return(true);
}
}
return(false);
}
public static void Test_GetCameraPoints(DepthMetaData myDepthMetaData, CoordinateMapper myCoordinateMapper)
{
//test
CameraSpacePoint[] cameraMappedPoints = new CameraSpacePoint[ColorMetaData.XColorMaxKinect * ColorMetaData.YColorMaxKinect];
myCoordinateMapper.MapColorFrameToCameraSpace(myDepthMetaData.FrameData, cameraMappedPoints);
List <CameraSpacePoint> myMappedPoints = new List <CameraSpacePoint>();
for (int i = 0; i < cameraMappedPoints.Length; ++i)
{
double colorMappedToDepthX = cameraMappedPoints[i].X;
double colorMappedToDepthY = cameraMappedPoints[i].Y;
if (!double.IsNegativeInfinity(colorMappedToDepthX) &&
!double.IsNegativeInfinity(colorMappedToDepthY))
{
myMappedPoints.Add(cameraMappedPoints[i]);
//System.Diagnostics.Debug.WriteLine("--> Camera Points : " + cameraMappedPoints[colorIndex].X.ToString() + " : " + cameraMappedPoints[colorIndex].Y.ToString() + " : " + cameraMappedPoints[colorIndex].Z.ToString());
}
}
System.Diagnostics.Debug.WriteLine("--> Number of ColorSpacePoints: " + myMappedPoints.Count.ToString());
//System.Diagnostics.Debug.WriteLine("--> Old.Camera Points : " + cameraMappedPoints[0].X.ToString() + " : " + cameraMappedPoints[0].X.ToString() + " : " + cameraMappedPoints[0].Z.ToString() + " : ");
DepthSpacePoint[] colorMappedToDepthPoints = new DepthSpacePoint[ColorMetaData.XColorMaxKinect * ColorMetaData.YColorMaxKinect];
myCoordinateMapper.MapColorFrameToDepthSpace(myDepthMetaData.FrameData, colorMappedToDepthPoints);
List <DepthSpacePoint> myMappedPointsDepth = new List <DepthSpacePoint>();
for (int i = 0; i < colorMappedToDepthPoints.Length; ++i)
{
double colorMappedToDepthX = colorMappedToDepthPoints[i].X;
double colorMappedToDepthY = colorMappedToDepthPoints[i].Y;
if (!double.IsNegativeInfinity(colorMappedToDepthX) &&
!double.IsNegativeInfinity(colorMappedToDepthY))
{
myMappedPointsDepth.Add(colorMappedToDepthPoints[i]);
//System.Diagnostics.Debug.WriteLine("--> Camera Points : " + cameraMappedPoints[colorIndex].X.ToString() + " : " + cameraMappedPoints[colorIndex].Y.ToString() + " : " + cameraMappedPoints[colorIndex].Z.ToString());
}
}
System.Diagnostics.Debug.WriteLine("--> Number of ColorDepthPoints: " + myMappedPointsDepth.Count.ToString());
//System.Diagnostics.Debug.WriteLine("--> Depth Points : " + colorMappedToDepthPoints[0].X.ToString() + " : " + colorMappedToDepthPoints[0].X.ToString() + " : " );
}
///// <summary>
///// creates color info for all DEPTH pixels (to later e.g. write ply file from it)
///// </summary>
///// <param name="myColorMetaData"></param>
///// <param name="myDepthMetaData"></param>
///// <param name="myCoordinateMapper"></param>
///// <returns></returns>
//public static PointCloud ToPointCloud(ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData, CoordinateMapper myCoordinateMapper)
//{
// ColorSpacePoint[] mycolorPointsInDepthSpace = new ColorSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
// CameraSpacePoint[] myRealWorldPoints = new CameraSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
// myCoordinateMapper.MapDepthFrameToCameraSpace(myDepthMetaData.FrameData, myRealWorldPoints);
// myCoordinateMapper.MapDepthFrameToColorSpace(myDepthMetaData.FrameData, mycolorPointsInDepthSpace);
// CameraIntrinsics ci = myCoordinateMapper.GetDepthCameraIntrinsics();
// PointF[] myConversionTable = myCoordinateMapper.GetDepthFrameToCameraSpaceTable();
// ConversionTable = new Vector2[myConversionTable.Length];
// for (int i = 0; i < myConversionTable.Length; i++)
// {
// ConversionTable[i] = new Vector2(myConversionTable[i].X, myConversionTable[i].Y);
// }
// if (myColorMetaData != null)
// {
// return PointCloudWithColorParallel(mycolorPointsInDepthSpace, myRealWorldPoints, myColorMetaData, myDepthMetaData, myBodyMetaData);
// }
// else
// {
// return PointCloudBW(myRealWorldPoints, myDepthMetaData);
// }
//}
private static PointCloud PointCloudBW(CameraSpacePoint[] myRealWorldPoints, DepthMetaData myDepthMetaData)
{
int x = 0;
int y = 0;
List <Vector3> vecs = new List <Vector3>();
List <Vector3> cols = new List <Vector3>();
PointCloud pc = new PointCloud();
try
{
int i = -1;
for (x = 0; x < DepthMetaData.XDepthMaxKinect; x++)
{
for (y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
//int depthIndex = ((DepthMetaData.YResDefault - y - 1) * DepthMetaData.XResDefault) + x;
if (myDepthMetaData.FrameData[depthIndex] != 0)
{
i++;
int depthIndexColor = depthIndex * MetaDataBase.BYTES_PER_PIXEL;
Vector3 vect = new Vector3(myRealWorldPoints[depthIndex].X, myRealWorldPoints[depthIndex].Y, -myRealWorldPoints[depthIndex].Z);
vecs.Add(vect);
}
}
}
}
catch
{
System.Diagnostics.Debug.WriteLine("..Debug : " + x.ToString() + " : " + y.ToString() + " : ");
}
pc.Vectors = vecs.ToArray();
return(pc);
}
/// <summary>
/// creates color info for all DEPTH pixels (to later e.g. write ply file from it)
/// </summary>
/// <param name="myColorMetaData"></param>
/// <param name="myDepthMetaData"></param>
/// <param name="myCoordinateMapper"></param>
/// <returns></returns>
public static PointCloud ToPointCloud(ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData, CoordinateMapper myCoordinateMapper)
{
if (myColorMetaData == null && myDepthMetaData == null)
{
return(null);
}
ColorSpacePoint[] mycolorPointsInDepthSpace = new ColorSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
CameraSpacePoint[] myRealWorldPoints = new CameraSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
myCoordinateMapper.MapDepthFrameToCameraSpace(myDepthMetaData.FrameData, myRealWorldPoints);
myCoordinateMapper.MapDepthFrameToColorSpace(myDepthMetaData.FrameData, mycolorPointsInDepthSpace);
if (myColorMetaData != null)
{
return(PointCloudWithColorParallel(mycolorPointsInDepthSpace, myRealWorldPoints, myColorMetaData, myDepthMetaData, myBodyMetaData));
}
else
{
return(PointCloudBW(myRealWorldPoints, myDepthMetaData, myBodyMetaData));
}
}
/// <summary>
/// updates myDepthMetaData.FrameData
/// </summary>
/// <param name="myDepthMetaData"></param>
/// <param name="myBodyMetaData"></param>
public int DepthFrameData_RemoveBackground(DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
{
if (myBodyMetaData == null || myDepthMetaData == null)
{
return(-1);
}
int numberOfPixelsRemoved = 0;
int nCount = myDepthMetaData.FrameData.GetLength(0);
ushort[] myPixels = new ushort[nCount];
for (int i = 0; i < nCount; i++)
{
myPixels[i] = myDepthMetaData.FrameData[i];
}
for (int x = 0; x < DepthMetaData.XDepthMaxKinect; ++x)
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; ++y)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
byte player = myBodyMetaData.Pixels[depthIndex];
if (player != 0xff)
{
}
else
{
myDepthMetaData.FrameData[depthIndex] = 0;
numberOfPixelsRemoved++;
}
}
}
return(numberOfPixelsRemoved);
}
//private static PointCloud PointCloudWithColorParallel(ColorSpacePoint[] mycolorPointsInDepthSpace, CameraSpacePoint[] myRealWorldPoints, ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
//{
// VectorColor[,] arrVectors = Helper_CreateArray(mycolorPointsInDepthSpace, myRealWorldPoints, myColorMetaData, myDepthMetaData);
// PointCloud pc = new PointCloud();
// List<Vector3> vecList = new List<Vector3>();
// List<Vector3> colList = new List<Vector3>();
// int indV = -1;
// for (int x = 0; x < DepthMetaData.XDepthMaxKinect; x++)
// {
// for (int y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
// {
// indV++;
// if (arrVectors[x, y].Vector != Vector3.Zero)
// {
// vecList.Add(arrVectors[x, y].Vector);
// colList.Add(arrVectors[x, y].Color);
// }
// }
// }
// pc.Vectors = vecList.ToArray();
// pc.Colors = colList.ToArray();
// return pc;
//}
private static VectorColor[,] Helper_CreateArray(ColorSpacePoint[] mycolorPointsInDepthSpace, CameraSpacePoint[] myRealWorldPoints, ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData)
{
VectorColor[,] arrVectors = new VectorColor[DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect];
try
{
//for (int x = 0; x < DepthMetaData.XResDefault; x++)
System.Threading.Tasks.Parallel.For(0, DepthMetaData.XDepthMaxKinect, x =>
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
if (myDepthMetaData.FrameData[depthIndex] != 0)
{
ColorSpacePoint colorPoint = mycolorPointsInDepthSpace[depthIndex];
int xColor = Convert.ToInt32(colorPoint.X);
int yColor = Convert.ToInt32(colorPoint.Y);
int depthIndexColor = depthIndex * MetaDataBase.BYTES_PER_PIXEL;
if ((xColor >= 0) && (xColor < ColorMetaData.XColorMaxKinect) && (yColor >= 0) && (yColor < ColorMetaData.YColorMaxKinect))
{
int colorIndex = ((yColor * ColorMetaData.XColorMaxKinect) + xColor) * MetaDataBase.BYTES_PER_PIXEL;
Vector3 vect = new Vector3(myRealWorldPoints[depthIndex].X, myRealWorldPoints[depthIndex].Y, myRealWorldPoints[depthIndex].Z);
Vector3 col = new Vector3(myColorMetaData.Pixels[colorIndex] / 255f, myColorMetaData.Pixels[colorIndex + 1] / 255f, myColorMetaData.Pixels[colorIndex + 2] / 255f);
arrVectors[x, y].Vector = vect;
arrVectors[x, y].Color = col;
}
}
}
});
//};
}
catch (Exception err)
{
System.Diagnostics.Debug.WriteLine(" error in PointCloudWithColor " + err.Message);
}
return(arrVectors);
}
public WriteableBitmap ColorBitmap2(ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
{
if (myDepthMetaData == null || myBodyMetaData == null || myColorMetaData == null)
{
return(null);
}
this.colorArrayWithDepthInfo = new DepthSpacePoint[ColorMetaData.XColorMaxKinect * ColorMetaData.YColorMaxKinect];
coordinateMapper.MapColorFrameToDepthSpace(myDepthMetaData.FrameData, this.colorArrayWithDepthInfo);
if (colorArrayWithDepthInfo == null)
{
return(null);
}
this.colorArrayWithDepthInfo = new DepthSpacePoint[ColorMetaData.XColorMaxKinect * ColorMetaData.YColorMaxKinect];
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
myColorMetaData.WriteableBitmapColor.Lock();
unsafe
{
int colorMappedToDepthPointCount = this.colorArrayWithDepthInfo.Length;
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = this.colorArrayWithDepthInfo)
{
for (int colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++colorIndex)
{
double colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
double colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
if (!double.IsNegativeInfinity(colorMappedToDepthX) &&
!double.IsNegativeInfinity(colorMappedToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
int depthX = (int)(colorMappedToDepthX + 0.5f);
int depthY = (int)(colorMappedToDepthY + 0.5f);
// If the point is not valid, there is no body index there.
if ((depthX >= 0) && (depthX < DepthMetaData.XDepthMaxKinect) && (depthY >= 0) && (depthY < DepthMetaData.YDepthMaxKinect))
{
int depthIndex = (depthY * DepthMetaData.XDepthMaxKinect) + depthX;
byte player = myBodyMetaData.Pixels[depthIndex];
if (player != 0xff)
{
continue;
}
}
}
int displayIndex = colorIndex * BYTES_PER_PIXEL;
myColorMetaData.Pixels[colorIndex + 1] = 0;
myColorMetaData.Pixels[colorIndex + 2] = 0;
myColorMetaData.Pixels[colorIndex + 3] = 0xff;
}
myColorMetaData.WriteableBitmapColor.AddDirtyRect(new Int32Rect(0, 0, myColorMetaData.WriteableBitmapColor.PixelWidth, myColorMetaData.WriteableBitmapColor.PixelHeight));
}
}
myColorMetaData.WriteableBitmapColor.Unlock();
return(myColorMetaData.WriteableBitmapColor);
//}
}
public WriteableBitmap Color_Bitmap(ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
{
if (myDepthMetaData == null || myBodyMetaData == null || myColorMetaData == null)
{
return(null);
}
/// <summary>
/// The RGB pixel values used for the background removal (green-screen) effect.
/// </summary>
byte[] myPixelsColorPlayer = null;
ColorSpacePoint[] _colorPointsPlayer = null;
_colorPointsPlayer = new ColorSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
myPixelsColorPlayer = new byte[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect * BYTES_PER_PIXEL];
coordinateMapper.MapDepthFrameToColorSpace(myDepthMetaData.FrameData, _colorPointsPlayer);
for (int x = 0; x < DepthMetaData.XDepthMaxKinect; ++x)
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; ++y)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
byte player = myBodyMetaData.Pixels[depthIndex];
int displayIndex = depthIndex * BYTES_PER_PIXEL;
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPointsPlayer[depthIndex];
//int colorX = (int)Math.Floor(colorPoint.X + 0.5);
//int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
//EDGAR TO DO _ player
if ((colorPoint.X >= 0) && (colorPoint.X < ColorMetaData.XColorMaxKinect) && (colorPoint.Y >= 0) && (colorPoint.Y < ColorMetaData.YColorMaxKinect))
{
int colorIndex = ((Convert.ToInt32(colorPoint.Y) * ColorMetaData.XColorMaxKinect) + Convert.ToInt32(colorPoint.X)) * BYTES_PER_PIXEL;
myPixelsColorPlayer[displayIndex + 0] = myColorMetaData.Pixels[colorIndex];
myPixelsColorPlayer[displayIndex + 1] = myColorMetaData.Pixels[colorIndex + 1];
myPixelsColorPlayer[displayIndex + 2] = myColorMetaData.Pixels[colorIndex + 2];
myPixelsColorPlayer[displayIndex + 3] = 0xff;
}
else
{
}
}
else
{
myPixelsColorPlayer[displayIndex + 3] = 0;//identify later....
}
}
}
WriteableBitmap myBitmapColorPlayer = null;
myBitmapColorPlayer = new WriteableBitmap(DepthMetaData.XDepthMaxKinect, DepthMetaData.XDepthMaxKinect, DPI, DPI, FORMAT, null);
myBitmapColorPlayer.Lock();
Marshal.Copy(myPixelsColorPlayer, 0, myBitmapColorPlayer.BackBuffer, myPixelsColorPlayer.Length);
myBitmapColorPlayer.AddDirtyRect(new Int32Rect(0, 0, DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect));
myBitmapColorPlayer.Unlock();
return(myBitmapColorPlayer);
}
private void GenerateDepthMetaData(Bitmap bm)
{
DepthMetaData = new DepthMetaData();
this.DepthMetaData.Vectors = new List <Vector3>();
LockBitmap lockBitmap = new LockBitmap(bm);
lockBitmap.LockBits();
byte[] pixelData = lockBitmap.Pixels;
infrared = new byte[bm.Width * bm.Height];
depthForImage = new byte[bm.Width * bm.Height];
int indexInfrared = 0;
List <byte[]> listPixels = new List <byte[]>();
DepthMetaData.FrameData = new ushort[DepthMetaData.XDepthMaxRealSense * DepthMetaData.YDepthMaxRealSense];
for (int iy = 0; iy < bm.Height; iy++)
{
int step24 = bm.Width * 3 * iy;
for (int ix = 0; ix < bm.Width; ix++)
{
int pixel24 = step24 + 3 * ix;
ushort depth = BitConverter.ToUInt16(pixelData, pixel24);
try
{
byte[] pixel = new byte[3];
pixel[0] = pixelData[pixel24];
pixel[1] = pixelData[pixel24 + 1];
pixel[2] = pixelData[pixel24 + 2];
depth = BitConverter.ToUInt16(pixel, 0);
listPixels.Add(pixel);
infrared[indexInfrared] = pixelData[pixel24 + 2];
if (depth != 0)
{
//Vector3 vTest = new Vector3(ix, iy, depth);
Vector3 v = ConvertDepthTomm(ix, -iy, depth);
this.DepthMetaData.Vectors.Add(v);
depthForImage[indexInfrared] = (byte)v.Z;
int depthIndex = (iy * DepthMetaData.XDepthMaxRealSense) + ix;
DepthMetaData.FrameData[depthIndex] = Convert.ToUInt16(v.Z);
}
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error converting to depth: " + err.Message);
}
indexInfrared++;
}
}
//UtilsPointCloudIO.Write_OBJ(listPixels, pathModels, "RawData_" + DateTime.Now.Millisecond.ToString() + PointCloudScannerSettings.FileNameDepthOBJ);
//UtilsPointCloudIO.Write_OBJ(depthVectors, pathModels, "Depth_" + DateTime.Now.Millisecond.ToString() + PointCloudScannerSettings.FileNameDepthOBJ);
if (PointCloudScannerSettings.InterpolateFrames)
{
CalculateInterpolatedPixels();
}
lockBitmap.UnlockBits();
}
private static PointCloud PointCloudWithColorParallel_AllPoints(ColorSpacePoint[] mycolorPointsInDepthSpace, CameraSpacePoint[] myRealWorldPoints, ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData)
{
VectorColor[,] arrVectors = Helper_CreateArray_AllPoints(mycolorPointsInDepthSpace, myRealWorldPoints, myColorMetaData, myDepthMetaData);
PointCloud pc = new PointCloud();
pc.Vectors = new Vector3[arrVectors.Length];
pc.Colors = new Vector3[arrVectors.Length];
int indV = -1;
for (int x = 0; x < DepthMetaData.XDepthMaxKinect; x++)
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
{
indV++;
pc.Vectors[indV] = arrVectors[x, y].Vector;
pc.Colors[indV] = arrVectors[x, y].Color;
}
}
return(pc);
}
private static PointCloud PointCloudWithColorParallel(ColorSpacePoint[] mycolorPointsInDepthSpace, CameraSpacePoint[] myRealWorldPoints, ColorMetaData myColorMetaData, DepthMetaData myDepthMetaData, BodyMetaData myBodyMetaData)
{
Vector3[,] arrV = new Vector3[DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect];
Vector3[,] arrC = new Vector3[DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect];
try
{
System.Threading.Tasks.Parallel.For(0, DepthMetaData.XDepthMaxKinect, x =>
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
{
int depthIndex = (y * DepthMetaData.XDepthMaxKinect) + x;
if (myDepthMetaData.FrameData[depthIndex] != 0)
{
if (PointCloudScannerSettings.BackgroundRemoved && myBodyMetaData != null)
{
byte player = myBodyMetaData.Pixels[depthIndex];
if (player != 0xff)
{
SetPoint(depthIndex, x, y, arrV, arrC, mycolorPointsInDepthSpace, myRealWorldPoints, myColorMetaData);
}
}
else
{
SetPoint(depthIndex, x, y, arrV, arrC, mycolorPointsInDepthSpace, myRealWorldPoints, myColorMetaData);
}
}
}
});
//};
}
catch (Exception err)
{
System.Diagnostics.Debug.WriteLine(" error in PointCloudWithColor " + err.Message);
}
List <Vector3> listV = new List <Vector3>();
List <Vector3> listC = new List <Vector3>();
for (int x = 0; x < DepthMetaData.XDepthMaxKinect; x++)
{
for (int y = 0; y < DepthMetaData.YDepthMaxKinect; y++)
{
if (arrV[x, y] != Vector3.Zero)
{
listV.Add(arrV[x, y]);
listC.Add(arrC[x, y]);
}
}
}
PointCloud pc = new PointCloud(listV, listC, null, null, null, null);
if (GLSettings.ShowPointCloudAsTexture)
{
pc.TextureCreateFromColors(DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect);
}
return(pc);
}
