pcl_xyzrgb[] GetPCL()
{
if (this._KinectDevice != null)
{
DepthImageStream depthStream = this._KinectDevice.DepthStream;
ColorImageStream colorStream = this._KinectDevice.ColorStream;
DepthImageFrame depthFrame = null; //può non essere reallocata
ColorImageFrame colorFrame = null;
do
{ //polling
depthFrame = depthStream.OpenNextFrame(100);
colorFrame = colorStream.OpenNextFrame(100);
if ((depthFrame == null) || (colorFrame == null)) Console.WriteLine("Depth or Color Frame = null");
} while ((depthFrame == null) || (colorFrame == null));
Console.WriteLine(depthFrame.Height + " " + depthFrame.Width);
depthFrame.CopyPixelDataTo(this._DepthPixelData);
//Scopre quanti punti deve allocare (index)
int index = 0;
int step = 1;
int Start_origin = 4000; //fix per far partire il terreno dall'origine
for (int i = 0; i < depthFrame.Height; i += step)
for (int j = 0; j < depthFrame.Width; j += step)
{
int depth = this._DepthPixelData[i * depthFrame.Width + j] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if ((depth > near_range) && (depth < far_range))
{
index++;
if (depth < Start_origin)
Start_origin = depth;
}
}
//color pixel matrix
colorFrame.CopyPixelDataTo(_ColorPixelData);
//_ColorPixelData = HightSaturation(_ColorPixelData);
int grandezzamatrice = index;
var pcldata = new pcl_xyzrgb[index];
int depthPixelIndex;
int colorPixelIndex;
ColorImagePoint colorPoint;
int colorStride = colorFrame.BytesPerPixel * colorFrame.Width;
//int bytesPerPixel = 4;
index = 0;
/* double theta1 = Math.Tan(Math.PI * 43 / 180.0); //???
double theta2 = Math.Tan(Math.PI * 57 / 180.0); //??? */
// int min_x = 999, max_x = 0; //allineamento con gli assi
int x, y;
double minDistance = -10;
double scaleFactor = 0.0021;
for (int i = 0; i < depthFrame.Height; i += step)
for (int j = 0; j < depthFrame.Width; j += step)
{
depthPixelIndex = i * depthFrame.Width + j;
int depth = this._DepthPixelData[depthPixelIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if ((depth > near_range) && (depth < far_range))
{
//immagine sotto sopra
x = j;// depthFrame.Width - j;
y = depthFrame.Height - i;
/* Correction
x = (int)Math.Round( (j - depthFrame.Width / 2) * (depth + minDistance) * scaleFactor * (depthFrame.Width / depthFrame.Height) );
y = depthFrame.Height - (int)Math.Round( (i - depthFrame.Height / 2) * (depth + minDistance) * scaleFactor);
*/
colorPoint = depthFrame.MapToColorImagePoint(j, i, colorFrame.Format);
colorPixelIndex = (colorPoint.X * colorFrame.BytesPerPixel) + (colorPoint.Y * colorStride);
if (colorPixelIndex >= this._ColorPixelData.Length) break;
//Console.WriteLine("Color index(" + colorPixelIndex.ToString() + "): " + this._ColorPixelData[colorPixelIndex].ToString());
//Console.WriteLine(Color.FromArgb(this._ColorPixelData[colorPixelIndex + 2], this._ColorPixelData[colorPixelIndex + 1], this._ColorPixelData[colorPixelIndex]));
uint intColor = (uint)ColorTranslator.ToOle(Color.FromArgb(this._ColorPixelData[colorPixelIndex + 2], this._ColorPixelData[colorPixelIndex + 1], this._ColorPixelData[colorPixelIndex]));
pcldata[index].rgba = intColor;
depth -= Start_origin; //normalizzato 1220
//è già la depth o disparity? disparity to meters 0> z = 1 / ( value * scale + offset);
pcldata[index].xyz = new Vector3(x, y, depth);//(float)(j+depth*theta1),(float)(i+depth*theta2)
index++;
if (index == grandezzamatrice)
{
Console.WriteLine("Errore nella matrice");
break;
}
}
}
this._KinectDevice.Stop();
return pcldata;
}
return null;
}
pcl_xyzrgb[] GetPCL()
{
if (this._KinectDevice != null)
{
DepthImageStream depthStream = this._KinectDevice.DepthStream;
ColorImageStream colorStream = this._KinectDevice.ColorStream;
DepthImageFrame depthFrame = null; //può non essere reallocata
ColorImageFrame colorFrame = null;
do
{ //polling
depthFrame = depthStream.OpenNextFrame(100);
colorFrame = colorStream.OpenNextFrame(100);
if ((depthFrame == null) || (colorFrame == null)) Console.WriteLine("Depth or Color Frame = null");
} while ((depthFrame == null) || (colorFrame == null));
depthFrame.CopyPixelDataTo(this._DepthPixelData);
//Scopre quanti punti deve allocare (index)
int index = 0;
int step = 1;
for (int i = 0; i < 480; i += step)
for (int j = 0; j < 640; j += step)
{
int depth = this._DepthPixelData[i * depthFrame.Width + j] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if ((depth > 1220) && (depth < 3048))
{
index++;
}
}
//color pixel matrix
colorFrame.CopyPixelDataTo(_ColorPixelData);
var pcldata = new pcl_xyzrgb[index];
int depthPixelIndex;
int colorPixelIndex;
ColorImagePoint colorPoint;
int colorStride = colorFrame.BytesPerPixel * colorFrame.Width;
//int bytesPerPixel = 4;
index = 0;
double theta1 = Math.Tan(Math.PI * 43 / 180.0); //???
double theta2 = Math.Tan(Math.PI * 57 / 180.0); //???
int x, y;
for (int i = 0; i < depthFrame.Height; i += step)
for (int j = 0; j < depthFrame.Width; j += step)
{
depthPixelIndex = i * depthFrame.Width + j;
int depth = this._DepthPixelData[depthPixelIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if ((depth > 1220) && (depth < 3048))
{
x = j;// depthFrame.Width - j;
y = depthFrame.Height - i;
colorPoint = depthFrame.MapToColorImagePoint(j, i, colorFrame.Format);
colorPixelIndex = (colorPoint.X * colorFrame.BytesPerPixel) + (colorPoint.Y * colorStride);
//Console.WriteLine("Color index(" + colorPixelIndex.ToString() + "): " + this._ColorPixelData[colorPixelIndex].ToString());
uint intColor = (uint)ColorTranslator.ToOle(Color.FromArgb(this._ColorPixelData[colorPixelIndex + 2], this._ColorPixelData[colorPixelIndex + 1], this._ColorPixelData[colorPixelIndex]));
pcldata[index].rgba = intColor;
depth -= 1828;
pcldata[index++].xyz = new Vector3(x, y, depth);//(float)(j+depth*theta1),(float)(i+depth*theta2)
}
}
this._KinectDevice.Stop();
return pcldata;
}
return null;
}
