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; }