private void WindowLoaded(object sender, RoutedEventArgs e)
{
KinectSensor kinect = KinectSensor.KinectSensors[0];
ColorImageStream clrStream = kinect.ColorStream;
clrStream.Enable(rgbFormat);
DepthImageStream depStream = kinect.DepthStream;
depStream.Enable(depthFormat);
kinect.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
pixelBuffer = new byte[kinect.ColorStream.FramePixelDataLength];
depthBuffer = new DepthImagePixel[depStream.FramePixelDataLength];
clrPntBuffer = new ColorImagePoint[depStream.FramePixelDataLength];
depthMaskBuffer = new byte[clrStream.FramePixelDataLength];
bmpBuffer = new RenderTargetBitmap(clrStream.FrameWidth, clrStream.FrameHeight,
96, 96, PixelFormats.Default);
rgbImage.Source = bmpBuffer;
kinect.AllFramesReady += AllFramesReady;
textBox1.Clear();
kinect.Start();
kinect.ElevationAngle = 0;
}
private void InitializeKinectSensor(KinectSensor sensor)
{
if (sensor != null)
{
sensor.DepthStream.Range = DepthRange.Default;
sensor.SkeletonStream.Enable();
sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
sensor.ColorStream.Enable(ColorImageFormat.RgbResolution1280x960Fps12);
DepthImageStream depthStream = sensor.DepthStream;
this._GreenScreenImage = new WriteableBitmap(depthStream.FrameWidth, depthStream.FrameHeight, 96, 96, PixelFormats.Bgra32, null);
this._GreenScreenImageRect = new Int32Rect(0, 0, (int)Math.Ceiling(this._GreenScreenImage.Width), (int)Math.Ceiling(this._GreenScreenImage.Height));
this._GreenScreenImageStride = depthStream.FrameWidth * 4;
this.GreenScreenImage.Source = this._GreenScreenImage;
this._DepthPixelData = new short[this._KinectDevice.DepthStream.FramePixelDataLength];
this._ColorPixelData = new byte[this._KinectDevice.ColorStream.FramePixelDataLength];
this.frameSkeletons = new Skeleton[this._KinectDevice.SkeletonStream.FrameSkeletonArrayLength];
if (!this._DoUsePolling)
{
sensor.AllFramesReady += KinectDevice_AllFramesReady;
}
sensor.Start();
}
}
// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
// that displays different players in different colors
private void ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream, ref Color[] depthFrame32)
{
for (int i16 = 0; i16 < depthFrame.Length; i16++)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == depthStream.TooNearDepth)
{
// white
depthFrame32[i16] = new Color((int)255, (int)255, (int)255);
}
else if (player == 0 && realDepth == depthStream.TooFarDepth)
{
// dark purple
depthFrame32[i16] = new Color((int)66, 0, (int)66);
}
else if (player == 0 && realDepth == depthStream.UnknownDepth)
{
// dark brown
depthFrame32[i16] = new Color((int)66, (int)66, (int)33);
}
else
{
// tint the intensity by dividing by per-player values
depthFrame32[i16] = new Color((int)(intensity >> IntensityShiftByPlayerR[player]), (int)(intensity >> IntensityShiftByPlayerG[player]), (int)(intensity >> IntensityShiftByPlayerB[player]));
}
}
}
/// <summary>
/// BitmapSourceに変換する
/// </summary>
/// <param name="depthFrame"></param>
/// <param name="depthStream"></param>
/// <returns></returns>
public static BitmapSource ToBitmapSource(this DepthImageFrame depthFrame, DepthImageStream depthStream)
{
return(BitmapSource.Create(depthFrame.Width,
depthFrame.Height, 96, 96, DepthPixelFormat, null,
ConvertDepthToColor(depthFrame, depthStream),
depthFrame.Width * BytesPerPixel));
}
/// <summary>
/// 距離データの画像変換
/// </summary>
/// <param name="kinect">Kinect センサー</param>
/// <param name="depthFrame">深度フレームデータ</param>
/// <returns>距離データの画像データ</returns>
private byte[] _ConvertDepthColor(KinectSensor kinect, DepthImageFrame depthFrame)
{
ColorImageStream colorStream = kinect.ColorStream;
DepthImageStream depthStream = kinect.DepthStream;
// 距離カメラのピクセル毎のデータを取得する
short[] depthPixel = new short[depthFrame.PixelDataLength];
depthFrame.CopyPixelDataTo(depthPixel);
// 距離カメラの座標に対する RGB カメラ座標を取得する（座標合わせ）
ColorImagePoint[] colorPoint = new ColorImagePoint[depthFrame.PixelDataLength];
kinect.MapDepthFrameToColorFrame(depthStream.Format, depthPixel, colorStream.Format, colorPoint);
byte[] depthColor = new byte[depthFrame.PixelDataLength * Bgr32BytesPerPixel];
int pxLen = depthPixel.Length;
for (int i = 0; i < pxLen; i++)
{
int distance = depthPixel[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// 変換した結果がフレームサイズを超えないよう、小さい方を採用
int x = Math.Min(colorPoint[i].X, colorStream.FrameWidth - 1);
int y = Math.Min(colorPoint[i].Y, colorStream.FrameHeight - 1);
int colorIndex = ((y * depthFrame.Width) + x) * Bgr32BytesPerPixel;
// サポート外 0-40cm
if (distance == depthStream.UnknownDepth)
{
depthColor[colorIndex] = 0;
depthColor[colorIndex + 1] = 0;
depthColor[colorIndex + 2] = 255;
}
// 近すぎ 40cm-80cm（Default）
else if (distance == depthStream.TooNearDepth)
{
depthColor[colorIndex] = 0;
depthColor[colorIndex + 1] = 255;
depthColor[colorIndex + 2] = 0;
}
// 遠すぎ 3m（Near）, 4m（Default）-8m
else if (distance == depthStream.TooFarDepth)
{
depthColor[colorIndex] = 255;
depthColor[colorIndex + 1] = 0;
depthColor[colorIndex + 2] = 0;
}
// 有効な距離データ
else
{
depthColor[colorIndex] = 0;
depthColor[colorIndex + 1] = 255;
depthColor[colorIndex + 2] = 255;
}
}
return(depthColor);
}
// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
// that displays different players in different colors
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream, int depthFrame32Length)
{
const int BlueIndex = 0;
const int GreenIndex = 1;
const int RedIndex = 2;
int[] IntensityShiftByPlayerR = { 1, 2, 0, 2, 0, 0, 2, 0 };
int[] IntensityShiftByPlayerG = { 1, 2, 2, 0, 2, 0, 0, 1 };
int[] IntensityShiftByPlayerB = { 1, 0, 2, 2, 0, 2, 0, 2 };
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
byte[] depthFrame32 = new byte[depthFrame32Length];
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == 0)
{
// white
depthFrame32[i32 + RedIndex] = 225;
depthFrame32[i32 + GreenIndex] = 255;
depthFrame32[i32 + BlueIndex] = 255;
}
else if (player == 0 && realDepth == tooFarDepth)
{
// dark purple
depthFrame32[i32 + RedIndex] = 0;
depthFrame32[i32 + GreenIndex] = 255;
depthFrame32[i32 + BlueIndex] = 0;
}
else if (player == 0 && realDepth == unknownDepth)
{
// dark brown
depthFrame32[i32 + RedIndex] = 225;
depthFrame32[i32 + GreenIndex] = 0;
depthFrame32[i32 + BlueIndex] = 0;
}
else
{
//tint the intensity by dividing by per-player values
depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
}
}
return(depthFrame32);
}
/// <summary>
/// return the depthStream
/// </summary>
/// <returns>the depth stream</returns>
public DepthImageStream GetDepthStream()
{
DepthImageStream depthStream = null;
if (_sensor != null)
{
depthStream = _sensor.DepthStream;
}
return(depthStream);
}
byte[] GetRGB(DepthImageFrame PImage, short[] depthFrame, DepthImageStream depthStream)
{
obsCell = new int[3, 3];
byte[] rgbs = new byte[PImage.Width * PImage.Height * 4];
int nSavei32 = 0;
for (int i = 0; i < m_col; i++)
{
for (int j = 0; j < m_row; j++)
{
_rectPoint[i, j].min_X = int.MaxValue;
_rectPoint[i, j].min_Y = int.MaxValue;
_rectPoint[i, j].max_X = int.MinValue;
_rectPoint[i, j].max_Y = int.MinValue;
}
}
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < rgbs.Length; i16++, i32 += 4)
{
int nDistance = depthFrame[i16] >>
DepthImageFrame.PlayerIndexBitmaskWidth;
int nX = i16 % PImage.Width;
int nY = i16 / PImage.Width;
int i = (nY - 1) / (PImage.Height / m_col);
int j = (nX - 1) / (PImage.Width / m_row);
int nColor = 0xFF * nDistance / 4000;
if (nDistance >= 0 && nDistance <= 1000)
{
if (!((i == m_col) || (j == m_row)))
{
_rectPoint[i, j].min_X = Math.Min(_rectPoint[i, j].min_X, nX);
_rectPoint[i, j].min_Y = Math.Min(_rectPoint[i, j].min_Y, nY);
_rectPoint[i, j].max_X = Math.Max(_rectPoint[i, j].max_X, nX);
_rectPoint[i, j].max_Y = Math.Max(_rectPoint[i, j].max_Y, nY);
obsCell[j / (m_row / 3), i / (m_col / 3)] += 1;
}
nSavei32 = i32;
SetRGB(rgbs, i32, 0xff, 0, 0);
}
else
{
SetRGB(rgbs, i32, (byte)nColor, (byte)nColor, (byte)nColor);
}
}
SetRGB(rgbs, nSavei32, 0xFF, 0, 0);
return(rgbs);
}
/// <summary>
/// DoReadDepthFrame Frame Read Behavior
/// </summary>
/// <param name="rawFrames">Raw frames</param>
/// <param name="kinectStream">Kinect stream</param>
private void DoReadDepthFrame(RawKinectFrames rawFrames, DepthImageStream kinectStream)
{
DepthImageFrame kinectFrame = kinectStream.OpenNextFrame(AVeryLargeNumberOfMilliseconds);
if (kinectFrame != null)
{
rawFrames.RawDepthFrameInfo = new KinectFrameInfo(kinectFrame);
rawFrames.RawDepthFrameData = new short[kinectFrame.PixelDataLength];
kinectFrame.CopyPixelDataTo(rawFrames.RawDepthFrameData);
}
}
//:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
//:::::::Return the depth image from the sensor, the image is a Emgu type. The image is noise free with a median filter:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
public Image<Gray, Byte> PollDepth(int numKinect)
{
Image<Bgra, Byte> depthFrameKinectBGR = new Image<Bgra, Byte>(640, 480);
Kinect = Sensor[numKinect];
if (this.Kinect != null)
{
this.DepthStream = this.Kinect.DepthStream;
this.DepthPixels = new DepthImagePixel[DepthStream.FramePixelDataLength];
this.DepthImagenPixeles = new byte[DepthStream.FramePixelDataLength * 4];
this.depthFrameKinect = new Image<Gray, Byte>(DepthStream.FrameWidth, DepthStream.FrameHeight);
Array.Clear(DepthImagenPixeles, 0, DepthImagenPixeles.Length);
try
{
using (DepthImageFrame frame = this.Kinect.DepthStream.OpenNextFrame(100))
{
if (frame != null)
{
frame.CopyDepthImagePixelDataTo(this.DepthPixels);
int index = 0;
for (int i = 0; i < DepthPixels.Length; ++i)
{
short depth = DepthPixels[i].Depth;
byte intensity = (byte)((depth >= minDepth) && (depth <= maxDepth) ? depth : 0);
DepthImagenPixeles[index++] = intensity;
DepthImagenPixeles[index++] = intensity;
DepthImagenPixeles[index++] = intensity;
++index;
}
depthFrameKinectBGR.Bytes = DepthImagenPixeles; //The bytes are converted to a Imagen(Emgu). This to work with the functions of opencv.
}
}
}
catch
{
MessageBox.Show("No se pueden leer los datos del sensor", "Error");
}
}
depthFrameKinect = depthFrameKinectBGR.Convert<Gray, Byte>();
depthFrameKinect = removeNoise(depthFrameKinect, 13);
return depthFrameKinect;
}//fin PollDepth()
private int CalculateIndex(Microsoft.Xna.Framework.Vector4 position, DepthImageStream depth)
{
float x = -position.X / position.W, y = -position.Y / position.W;
x = (x + 1) * depth.FrameWidth / 2;
y = (y + 1) * depth.FrameHeight / 2;
int xCoord = (int)MathHelper.Clamp(x, 0, depth.FrameWidth - 1);
int yCoord = (int)MathHelper.Clamp(y, 0, depth.FrameHeight - 1);
return(yCoord * depth.FrameHeight + xCoord);
}
// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
// that displays different players in different colors
private void ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
// Test that the buffer lengths are appropriately correlated, which allows us to use only one
// value as the loop condition.
if ((depthFrame.Length * 4) != this.depthFrame32.Length)
{
throw new InvalidOperationException();
}
for (int i16 = 0, i32 = 0; i32 < this.depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if (player == 0 && realDepth == tooNearDepth)
{
// white
this.depthFrame32[i32 + RedIndex] = 255;
this.depthFrame32[i32 + GreenIndex] = 255;
this.depthFrame32[i32 + BlueIndex] = 255;
}
else if (player == 0 && realDepth == tooFarDepth)
{
// dark purple
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 0;
this.depthFrame32[i32 + BlueIndex] = 66;
}
else if (player == 0 && realDepth == unknownDepth)
{
// dark brown
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 66;
this.depthFrame32[i32 + BlueIndex] = 33;
}
else
{
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
byte intensity = (byte)(~(realDepth >> 4));
// tint the intensity by dividing by per-player values
this.depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
this.depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
this.depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
}
}
}
/// <summary>
/// Kinectで取得したデータを点群に変換する
/// </summary>
/// <returns></returns>
bool GetPoint()
{
KinectSensor kinect = KinectSensor.KinectSensors[0];
ColorImageStream colorStream = kinect.ColorStream;
DepthImageStream depthStream = kinect.DepthStream;
// RGBカメラと距離カメラのフレームデータを取得する
using (ColorImageFrame colorFrame = kinect.ColorStream.OpenNextFrame(100)) {
using (DepthImageFrame depthFrame = kinect.DepthStream.OpenNextFrame(100)) {
if (colorFrame == null || depthFrame == null)
{
return(false);
}
// RGBカメラのデータを作成する
byte[] colorPixel = new byte[colorFrame.PixelDataLength];
colorFrame.CopyPixelDataTo(colorPixel);
rgb = new RGB[colorFrame.Width * colorFrame.Height];
for (int i = 0; i < rgb.Length; i++)
{
int colorIndex = i * 4;
rgb[i] = new RGB(colorPixel[colorIndex + 2], colorPixel[colorIndex + 1],
colorPixel[colorIndex]);
}
// 距離カメラのピクセルデータを取得する
short[] depthPixel = new short[depthFrame.PixelDataLength];
depthFrame.CopyPixelDataTo(depthPixel);
// 距離カメラの座標に対応するRGBカメラの座標を取得する(座標合わせ)
ColorImagePoint[] colorPoint = new ColorImagePoint[depthFrame.PixelDataLength];
kinect.MapDepthFrameToColorFrame(depthStream.Format, depthPixel,
colorStream.Format, colorPoint);
// 距離データを作成する
depth = new Depth[depthFrame.Width * depthFrame.Height];
for (int i = 0; i < depth.Length; i++)
{
int x = Math.Min(colorPoint[i].X, colorStream.FrameWidth - 1);
int y = Math.Min(colorPoint[i].Y, colorStream.FrameHeight - 1);
int distance = depthPixel[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;
depth[i] = new Depth(x, y, distance);
}
}
}
return(true);
}
} //fin CompositionTarget_Rendering()
private Image <Gray, Byte> PollDepth()
{
if (this.Kinect != null)
{
this.DepthStream = this.Kinect.DepthStream;
this.DepthValoresStream = new short[DepthStream.FramePixelDataLength];
this.DepthImagenPixeles = new byte[DepthStream.FramePixelDataLength];
this.depthFrameKinect = new Image <Gray, Byte>(DepthStream.FrameWidth, DepthStream.FrameHeight);
try
{
using (DepthImageFrame frame = this.Kinect.DepthStream.OpenNextFrame(100))
{
if (frame != null)
{
frame.CopyPixelDataTo(this.DepthValoresStream);
int index = 0;
for (int i = 0; i < frame.PixelDataLength; i++)
{
int valorDistancia = DepthValoresStream[i] >> 3;
if (valorDistancia == this.Kinect.DepthStream.UnknownDepth)
{
DepthImagenPixeles[index] = 0;
}
else if (valorDistancia == this.Kinect.DepthStream.TooFarDepth)
{
DepthImagenPixeles[index] = 0;
}
else
{
byte byteDistancia = (byte)(255 - (valorDistancia >> 5));
DepthImagenPixeles[index] = byteDistancia;
}
index++; //= index + 4;
}
depthFrameKinect.Bytes = DepthImagenPixeles; //The bytes are converted to a Imagen(Emgu). This to work with the functions of opencv.
}
}
}
catch
{
MessageBox.Show("No se pueden leer los datos del sensor", "Error");
}
}
return(depthFrameKinect);
}//fin PollDepth()
/// <summary>
/// Allows the game to perform any initialization it needs to before starting to run.
/// This is where it can query for any required services and load any non-graphic
/// related content. Calling base.Initialize will enumerate through any components
/// and initialize them as well.
/// </summary>
protected override void Initialize()
{
headPosition = new Vector3(0, 0, STANDARD_Z_DEPTH);
shoulderLeftPosition = new Vector3(0, 0, STANDARD_Z_DEPTH);
shoulderRightPosition = new Vector3(0, 0, STANDARD_Z_DEPTH);
shoulderCenterPosition = new Vector3(0, 0, STANDARD_Z_DEPTH);
handLeftPosition = new Vector3(0, 0, STANDARD_Z_DEPTH);
hipCenterPosition = new Vector3(0, 0, STANDARD_Z_DEPTH);
position = new Vector2(0, 0);
destinationRectangle = new Rectangle(0, 0, 1280, 960);
headSourceRec = new Rectangle();
leftEyeSourceRectangle = new Rectangle();
color = Color.White;
origin = new Vector2(0, 0);
rotation = 0;
scale = new Vector2(1f, 1f);
spriteEffects = SpriteEffects.None;
layerDepth = 1;
resolution = new Vector2(640, 480);
camera = new Camera(this, new Vector3(0, 0, 5), Vector3.Zero, Vector3.Up);
Components.Add(camera);
_KinectDevice = KinectSensor.KinectSensors[0];
_KinectDevice.ColorStream.Enable(ColorImageFormat.RgbResolution1280x960Fps12);
_KinectDevice.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
spriteBatch = new SpriteBatch(graphics.GraphicsDevice);
var parameters = new TransformSmoothParameters
{
};
_KinectDevice.SkeletonStream.Enable(parameters);
_KinectDevice.AllFramesReady += kinect_AllFramesReady;
DepthImageStream depthStream = this._KinectDevice.DepthStream;
this._DepthPixelData = new short[depthStream.FramePixelDataLength];
this._SkeletonData = new Skeleton[this._KinectDevice.SkeletonStream.FrameSkeletonArrayLength];
_KinectDevice.Start();
backGroundSnapShot = new byte[this._KinectDevice.ColorStream.FramePixelDataLength];
coordinateMapper = new CoordinateMapper(_KinectDevice);
colorVideo = new Texture2D(graphics.GraphicsDevice, _KinectDevice.ColorStream.FrameWidth, _KinectDevice.ColorStream.FrameHeight);
texture = new Texture2D(graphics.GraphicsDevice, _KinectDevice.ColorStream.FrameWidth, _KinectDevice.ColorStream.FrameHeight);
base.Initialize();
}
// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
// that displays different players in different colors
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < this.depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == 0)
{
// white
this.depthFrame32[i32 + RedIndex] = 255;
this.depthFrame32[i32 + GreenIndex] = 255;
this.depthFrame32[i32 + BlueIndex] = 255;
}
else if (player == 0 && realDepth == tooFarDepth)
{
// dark purple
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 0;
this.depthFrame32[i32 + BlueIndex] = 66;
}
else if (player == 0 && realDepth == unknownDepth)
{
// dark brown
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 66;
this.depthFrame32[i32 + BlueIndex] = 33;
}
else
{
// tint the intensity by dividing by per-player values
this.depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
this.depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
this.depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
}
}
return(this.depthFrame32);
}
// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
// that displays different players in different colors
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < this.depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == 0)
{
// white
this.depthFrame32[i32 + RedIndex] = 255;
this.depthFrame32[i32 + GreenIndex] = 255;
this.depthFrame32[i32 + BlueIndex] = 255;
}
else if (player == 0 && realDepth == tooFarDepth)
{
// dark purple
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 0;
this.depthFrame32[i32 + BlueIndex] = 66;
}
else if (player == 0 && realDepth == unknownDepth)
{
// dark brown
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 66;
this.depthFrame32[i32 + BlueIndex] = 33;
}
else
{
// tint the intensity by dividing by per-player values
this.depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
this.depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
this.depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
}
}
return this.depthFrame32;
}
private void DiscoverKinectSensor()
{
if (this._Kinect != null && this._Kinect.Status != KinectStatus.Connected)
{
this._Kinect = null;
}
if (this._Kinect == null)
{
this.Kinect = KinectSensor.KinectSensors.FirstOrDefault(x => x.Status == KinectStatus.Connected);
if (this._Kinect != null)
{
this._Kinect.ColorStream.Enable();
this._Kinect.Start();
ColorImageStream colorStream = this._Kinect.ColorStream;
DepthImageStream depthStream = this._Kinect.DepthStream;
this._Kinect.DepthStream.Enable();
if (!lowResource)
{
this.ColorImageElement.Dispatcher.BeginInvoke(new Action(() =>
{
this._ColorImageBitmap = new WriteableBitmap(colorStream.FrameWidth, colorStream.FrameHeight, 96, 96, PixelFormats.Bgr32, null);
this._ColorImageBitmapRect = new Int32Rect(0, 0, colorStream.FrameWidth, colorStream.FrameHeight);
this._ColorImageStride = colorStream.FrameWidth * colorStream.FrameBytesPerPixel;
ColorImageElement.Source = this._ColorImageBitmap;
this._ColorImagePixelData = new byte[colorStream.FramePixelDataLength];
}));
this.DepthImageModified.Dispatcher.BeginInvoke(new Action(() =>
{
this._DepthImageBitmap = new WriteableBitmap(depthStream.FrameWidth, depthStream.FrameHeight, 96, 96, PixelFormats.Gray16, null);
this._DepthImageBitmapRect = new Int32Rect(0, 0, depthStream.FrameWidth, depthStream.FrameHeight);
this._DepthImageStride = depthStream.FrameWidth * depthStream.FrameBytesPerPixel;
this._DepthImagePixelData = new short[depthStream.FramePixelDataLength];
}));
}
else
{
this._ColorImagePixelData = new byte[colorStream.FramePixelDataLength];
this._DepthImagePixelData = new short[depthStream.FramePixelDataLength];
}
}
}
}
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream, int depthFrame32Length)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
byte[] depthFrame32 = new byte[depthFrame32Length];
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == tooNearDepth)
{
depthFrame32[i32 + _RedIndex] = 0;
depthFrame32[i32 + _GreenIndex] = 0;
depthFrame32[i32 + _BlueIndex] = 0;
}
else if (player == 0 && realDepth == 0)
{
depthFrame32[i32 + _RedIndex] = 0;
depthFrame32[i32 + _GreenIndex] = 0;
depthFrame32[i32 + _BlueIndex] = 0;
}
else if (player == 0 && realDepth == tooFarDepth)
{
depthFrame32[i32 + _RedIndex] = 0;
depthFrame32[i32 + _GreenIndex] = 0;
depthFrame32[i32 + _BlueIndex] = 0;
}
else if (player == 0 && realDepth == unknownDepth)
{
depthFrame32[i32 + _RedIndex] = 0;
depthFrame32[i32 + _GreenIndex] = 0;
depthFrame32[i32 + _BlueIndex] = 0;
}
else
{
depthFrame32[i32 + _RedIndex] = 1;
depthFrame32[i32 + _GreenIndex] = 0;
depthFrame32[i32 + _BlueIndex] = 50;
}
}
return(depthFrame32);
}
/// <summary>
/// ConvertDepthFrame:
/// Converts the depth frame into its RGB version taking out all the player information and leaving only the depth.
/// </summary>
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
//System.Diagnostics.Debug.WriteLine("depthframe len :{0}", depthFrame.Length);
//Run through the depth frame making the correlation between the two arrays
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < this.depthFrame32.Length; i16++, i32 += 4) //4
{
//We don’t care about player’s information here, so we are just going to rule it out by shifting the value.
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// System.Diagnostics.Debug.WriteLine("bitmaskwidth len :{0}", DepthImageFrame.PlayerIndexBitmaskWidth);
//We are left with 13 bits of depth information that we need to convert into an 8 bit number for each pixel.
//There are hundreds of ways to do this. This is just the simplest one.
//Lets create a byte variable called Distance.
//We will assign this variable a number that will come from the conversion of those 13 bits.
byte Distance = 0;
//XBox Kinects (default) are limited between 800mm and 4096mm.
int MinimumDistance = 800;
int MaximumDistance = 4096;
//XBox Kinects (default) are not reliable closer to 800mm, so let’s take those useless measurements out.
//If the distance on this pixel is bigger than 800mm, we will paint it in its equivalent gray
if (realDepth > MinimumDistance)
{
//White = Close
//Black = Far
Distance = (byte)(255 - ((realDepth - MinimumDistance) * 255 / (MaximumDistance - MinimumDistance)));
//Use the distance to paint each layer (R G & B) of the current pixel.
//Painting R, G and B with the same color will make it go from black to gray
this.depthFrame32[i32 + RedIndex] = (byte)(Distance);
this.depthFrame32[i32 + GreenIndex] = (byte)(Distance);
this.depthFrame32[i32 + BlueIndex] = (byte)(Distance);
}
//If we are closer than 800mm, the just paint it red so we know this pixel is not giving a good value
else
{
this.depthFrame32[i32 + RedIndex] = 0;
this.depthFrame32[i32 + GreenIndex] = 150;
this.depthFrame32[i32 + BlueIndex] = 0;
}
}
//Now that we are done painting the pixels, we can return the byte array to be painted
return(this.depthFrame32);
}
private void InitializeKinectSensor(KinectSensor kinect)
{
if(kinect != null)
{
DepthImageStream depthSream = kinect.DepthStream;
kinect.DepthStream.Enable();
this._DepthImageBitmap = new WriteableBitmap(depthSream.FrameWidth, depthSream.FrameHeight, 96, 96, PixelFormats.Gray16, null);
this._DepthImageBitmapRect = new Int32Rect(0, 0, depthSream.FrameWidth, depthSream.FrameHeight);
this._DepthImageStride = depthSream.FrameWidth * depthSream.FrameBytesPerPixel;
ImagemProfundidade.Source = this._DepthImageBitmap;
// Chama o event handler
this._Kinect.DepthFrameReady += Kinect_DepthFrameReady;
//kinect.DepthFrameReady += Kinect_DepthFrameReady;
kinect.Start();
}
}
private void EnableDepthStreamBasedOnDepthOrSkeletonEnabled(
DepthImageStream depthImageStream, ComboBox depthFormatsValue)
{
if (depthFormatsValue.SelectedItem != null)
{
// SkeletonViewer needs depth. So if DepthViewer or SkeletonViewer is enabled, enabled depthStream.
if ((DepthStreamEnable.IsChecked.HasValue && DepthStreamEnable.IsChecked.Value)
|| (SkeletonStreamEnable.IsChecked.HasValue && SkeletonStreamEnable.IsChecked.Value))
{
depthImageStream.Enable((DepthImageFormat)depthFormatsValue.SelectedItem);
}
else
{
depthImageStream.Disable();
}
}
}
byte[] GetRGB(DepthImageFrame PImage, short[] depthFrame, DepthImageStream depthStream)
{
byte[] rgbs = new byte[PImage.Width * PImage.Height * 4];
int nNear = 999999;
int nSavei32 = 0;
int nTimer = (int)(m_saveFrame % 150 / 30);
// 각 영역의 좌표값 초기화
for (int i = 0; i < partitionWidth; i++)
{
for (int j = 0; j < partitionHeight; j++)
{
m_whichCell[i, j, 0] = int.MaxValue;
m_whichCell[i, j, 1] = int.MaxValue;
m_whichCell[i, j, 2] = int.MinValue;
m_whichCell[i, j, 3] = int.MinValue;
m_whichCell[i, j, 4] = int.MinValue;
}
}
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < rgbs.Length; i16++, i32 += 4)
{
int nDistance = depthFrame[i16] >>
DepthImageFrame.PlayerIndexBitmaskWidth;
int nX = i16 % PImage.Width;
int nY = i16 / PImage.Width;
int nCellX = (nX - 1) / (PImage.Width / partitionWidth);
int nCellY = (nY - 1) / (PImage.Height / partitionHeight);
// 화면이 나눠떨어지지 않는경우 예외처리
if (nCellX >= partitionWidth)
nCellX--;
if (nCellY >= partitionHeight)
nCellY--;
if (nNear > nDistance && nDistance > 400)
{
nNear = nDistance;
nSavei32 = i32;
}
int nColor = 0xFF * nDistance / 4000;
// 거리가 0~200 이라면 장애물로 판단,
if (nDistance >= 0 && nDistance <= 200)
{
// 해당 물체의 색을 red 로 처리하고 좌표값을 저장하는 함수와 장애물의 방향을 판단하는 함수를 호출한다.
SetRGB(rgbs, i32, 0xff, 0, 0);
SetCellObjCoordinate(nCellX, nCellY, nX, nY, nDistance);
CheckDirection(PImage, nCellX, nCellY, 0);
}
else
SetRGB(rgbs, i32, (byte)nColor, (byte)nColor, (byte)nColor);
}
SetRGB(rgbs, nSavei32, 0xFF, 0, 0);
return rgbs;
}
/// <summary>
/// BitmapSourceに変換する
/// </summary>
/// <param name="depthFrame"></param>
/// <param name="depthStream"></param>
/// <returns></returns>
public static BitmapSource ToBitmapSource( this DepthImageFrame depthFrame, DepthImageStream depthStream )
{
return BitmapSource.Create( depthFrame.Width,
depthFrame.Height, 96, 96, DepthPixelFormat, null,
ConvertDepthToColor( depthFrame, depthStream ),
depthFrame.Width * BytesPerPixel );
}
static void SendDepth(UdpClient client, DepthImageStream stream, Commands command, IPEndPoint destination)
{
using (var frame = stream.OpenNextFrame(50))
{
if (frame == null)
return;
var dc = GetDepthCommand(command);
if (!dc.HasValue)
return;
client.Send(ToBytes(dc.Value), sizeof(int), destination);
byte[] pixelData = new byte[frame.PixelDataLength * frame.BytesPerPixel];
unsafe
{
fixed (byte* p = pixelData)
{
IntPtr ptr = (IntPtr)p;
frame.CopyPixelDataTo((IntPtr)ptr, frame.PixelDataLength);
}
}
Send(client, pixelData, destination);
}
}
/// <summary>
/// ConvertDepthFrame:
/// Converts the depth frame into its RGB version taking out all the player information and leaving only the depth.
/// </summary>
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
//Run through the depth frame making the correlation between the two arrays
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < this.depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
//We don't care about player's information here, so we are just going to rule it out by shifting the value.
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
//We are left with 13 bits of depth information that we need to convert into an 8 bit number for each pixel.
//There are hundreds of ways to do this. This is just the simplest one.
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == 0)
{
// white
this.depthFrame32[i32 + RedIndex] = 255;
this.depthFrame32[i32 + GreenIndex] = 255;
this.depthFrame32[i32 + BlueIndex] = 255;
}
else if (player == 0 && realDepth == tooFarDepth)
{
// dark purple
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 0;
this.depthFrame32[i32 + BlueIndex] = 66;
}
else if (player == 0 && realDepth == unknownDepth)
{
// dark brown
this.depthFrame32[i32 + RedIndex] = 66;
this.depthFrame32[i32 + GreenIndex] = 66;
this.depthFrame32[i32 + BlueIndex] = 33;
}
else
{
if (player != 0)
Console.WriteLine("Player : "+player);
// tint the intensity by dividing by per-player values
this.depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
this.depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
this.depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
}
}
//Now that we are done painting the pixels, we can return the byte array to be painted
return this.depthFrame32;
}
/// <summary>
/// ConvertDepthFrame:
/// Converts the depth frame into its RGB version taking out all the player information and leaving only the depth.
/// </summary>
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
//System.Diagnostics.Debug.WriteLine("depthframe len :{0}", depthFrame.Length);
//Run through the depth frame making the correlation between the two arrays
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < this.depthFrame32.Length; i16++, i32 += 4) //4
{
//We don’t care about player’s information here, so we are just going to rule it out by shifting the value.
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// System.Diagnostics.Debug.WriteLine("bitmaskwidth len :{0}", DepthImageFrame.PlayerIndexBitmaskWidth);
//We are left with 13 bits of depth information that we need to convert into an 8 bit number for each pixel.
//There are hundreds of ways to do this. This is just the simplest one.
//Lets create a byte variable called Distance.
//We will assign this variable a number that will come from the conversion of those 13 bits.
byte Distance = 0;
//XBox Kinects (default) are limited between 800mm and 4096mm.
int MinimumDistance = 800;
int MaximumDistance = 4096;
//XBox Kinects (default) are not reliable closer to 800mm, so let’s take those useless measurements out.
//If the distance on this pixel is bigger than 800mm, we will paint it in its equivalent gray
if (realDepth > MinimumDistance)
{
//White = Close
//Black = Far
Distance = (byte)(255 - ((realDepth - MinimumDistance) * 255 / (MaximumDistance - MinimumDistance)));
//Use the distance to paint each layer (R G & B) of the current pixel.
//Painting R, G and B with the same color will make it go from black to gray
this.depthFrame32[i32 + RedIndex] = (byte)(Distance);
this.depthFrame32[i32 + GreenIndex] = (byte)(Distance);
this.depthFrame32[i32 + BlueIndex] = (byte)(Distance);
}
//If we are closer than 800mm, the just paint it red so we know this pixel is not giving a good value
else
{
this.depthFrame32[i32 + RedIndex] = 0;
this.depthFrame32[i32 + GreenIndex] = 150;
this.depthFrame32[i32 + BlueIndex] = 0;
}
}
//Now that we are done painting the pixels, we can return the byte array to be painted
return this.depthFrame32;
}
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream, int depthFrame32Length)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
byte[] depthFrame32 = new byte[depthFrame32Length];
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; ++i16, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == 0)
{
depthFrame32[i32 + RedIndex] = 255;
depthFrame32[i32 + GreenIndex] = 255;
depthFrame32[i32 + BlueIndex] = 255;
}
else if (player == 0 && realDepth == tooFarDepth)
{
depthFrame32[i32 + RedIndex] = 66;
depthFrame32[i32 + GreenIndex] = 00;
depthFrame32[i32 + BlueIndex] = 66;
}
else if (player == 0 && realDepth == unknownDepth)
{
depthFrame32[i32 + RedIndex] = 66;
depthFrame32[i32 + GreenIndex] = 66;
depthFrame32[i32 + BlueIndex] = 33;
}
else
{
depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
}
}
return depthFrame32;
}
private void UpdateColorArray(DepthImageStream depthImageStream)
{
// Kinects depth recognition is limited (e.g. 800mm to 4000mm).
// Take that into account for scaling the colors.
int minDist = depthImageStream.MinDepth;
int maxDist = depthImageStream.MaxDepth;
for (int i = 0, i8 = 0; i < depthArray.Length; i++, i8 += 4)
{
int depth = depthArray[i];
byte colorScaled8Bit;
// depth can even be -1!
if (depth < minDist)
{
colorScaled8Bit = 0;
}
else if (depth > maxDist)
{
colorScaled8Bit = 255;
}
else
{
int range = maxDist - minDist;
int whereInThatRange = depth - minDist;
colorScaled8Bit = (byte)(255 * whereInThatRange / range);
}
// Set R, G, B to the color (gives us gray)
this.colorArray[i8 + RedIndex] = colorScaled8Bit;
this.colorArray[i8 + GreenIndex] = colorScaled8Bit;
this.colorArray[i8 + BlueIndex] = colorScaled8Bit;
}
}
/// <summary>
/// Depth Frame Conversion Method
/// </summary>
/// <param name="depthFrame">current depth frame</param>
/// <param name="depthStream">originating depth stream</param>
/// <returns>depth pixel data</returns>
///
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int colorPixelIndex = 0;
this.rawDepth = new int[depthFrame.Length];
int realDepth = 0;
for (int i = 0; i < depthFrame.Length; i++)
{
this.rawDepth[i] = depthFrame[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;
realDepth = depthFrame[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if (skelDepth < 0)
{
if (realDepth < 1066)
{
this.depthFrame32[colorPixelIndex++] = (byte)(255 * realDepth / 1066);
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = 0;
++colorPixelIndex;
}
else if ((1066 <= realDepth) && (realDepth < 2133))
{
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (2133 - realDepth) / 1066);
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (realDepth - 1066) / 1066);
this.depthFrame32[colorPixelIndex++] = 0;
++colorPixelIndex;
}
else if ((2133 <= realDepth) && (realDepth < 3199))
{
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (3198 - realDepth) / 1066);
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (realDepth - 2133) / 1066);
++colorPixelIndex;
}
else if ((3199 <= realDepth) && (realDepth < 4000))
{
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (4000 - realDepth) / 801);
++colorPixelIndex;
}
else
{
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = 0;
++colorPixelIndex;
}
}
else
{
if ((((skelDepth - skelDepthDelta) <= realDepth) && (realDepth < (skelDepth + skelDepthDelta))) && (((skelL - skelLDelta) <= (colorPixelIndex % 2560)) && ((colorPixelIndex % 2560) < (skelR + skelRDelta))))// && ((skelB + skelBDelta) >= (colorPixelIndex / 2560)))
{
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (realDepth - skelDepth + skelDepthDelta) / (2 * skelDepthDelta));
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (realDepth - skelDepth + skelDepthDelta) / (2 * skelDepthDelta));
this.depthFrame32[colorPixelIndex++] = (byte)(255 * (realDepth - skelDepth + skelDepthDelta) / (2 * skelDepthDelta));
++colorPixelIndex;
}
else
{
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = 0;
this.depthFrame32[colorPixelIndex++] = 0;
rawDepth[i] = -1;
++colorPixelIndex;
}
}
}
rawDepthClone = rawDepth;
return this.depthFrame32;
}
/// <summary>
/// Converts a 16-bit grayscale depth frame which includes player indexes into a 3D point array.
/// </summary>
/// <param name="depthFrame">The depth frame.</param>
/// <param name="depthStream">The depth stream.</param>
private void ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int tooNearDepth = depthStream.TooNearDepth;
int tooFarDepth = depthStream.TooFarDepth;
int unknownDepth = depthStream.UnknownDepth;
int width = depthStream.FrameWidth;
int height = depthStream.FrameHeight;
int cx = width / 2;
int cy = height / 2;
double fxinv = 1.0 / 476;
double fyinv = 1.0 / 476;
double scale = 0.001;
Parallel.For(
0,
height,
iy =>
{
for (int ix = 0; ix < width; ix++)
{
int i = (iy * width) + ix;
this.rawDepth[i] = depthFrame[(iy * width) + ix] >> DepthImageFrame.PlayerIndexBitmaskWidth;
if (rawDepth[i] == unknownDepth || rawDepth[i] < tooNearDepth || rawDepth[i] > tooFarDepth)
{
this.rawDepth[i] = -1;
this.depthFramePoints[i] = new Point3D();
}
else
{
double zz = this.rawDepth[i] * scale;
double x = (cx - ix) * zz * fxinv;
double y = zz;
double z = (cy - iy) * zz * fyinv;
this.depthFramePoints[i] = new Point3D(x, y, z);
}
}
});
}
// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
// that displays different players in different colors
private void ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream, ref Color[] depthFrame32)
{
for (int i16 = 0; i16 < depthFrame.Length; i16++)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
byte intensity = (byte)(~(realDepth >> 4));
if (player == 0 && realDepth == depthStream.TooNearDepth)
{
// white
depthFrame32[i16] = new Color((int)255, (int)255, (int)255);
}
else if (player == 0 && realDepth == depthStream.TooFarDepth)
{
// dark purple
depthFrame32[i16] = new Color((int)66, 0, (int)66);
}
else if (player == 0 && realDepth == depthStream.UnknownDepth)
{
// dark brown
depthFrame32[i16] = new Color((int)66, (int)66, (int)33);
}
else
{
// tint the intensity by dividing by per-player values
depthFrame32[i16] = new Color((int)(intensity >> IntensityShiftByPlayerR[player]), (int)(intensity >> IntensityShiftByPlayerG[player]), (int)(intensity >> IntensityShiftByPlayerB[player]));
}
}
}
/// <summary>
/// 距離データをカラー画像に変換する
/// </summary>
/// <param name="kinect"></param>
/// <param name="depthFrame"></param>
/// <returns></returns>
private static byte[] ConvertDepthToColor( DepthImageFrame depthFrame, DepthImageStream depthStream )
{
// 距離カメラのピクセルごとのデータを取得する
short[] depthPixel = depthFrame.ToPixelData();
// 画像化データを作成する
byte[] depthColor = new byte[depthFrame.Width * depthFrame.Height * BytesPerPixel];
// 画像化する
for ( int i = 0; i < depthPixel.Length; i++ ) {
// 距離カメラのデータから、距離とプレイヤーIDを取得する
int distance = depthPixel[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;
int player = depthPixel[i] & DepthImageFrame.PlayerIndexBitmask;
// バイトインデックスを計算する
int index = i * BytesPerPixel;
// プレイヤーがいる座標
if ( player != 0 ) {
depthColor[index + 0] = PlayerColors[player].B;
depthColor[index + 1] = PlayerColors[player].G;
depthColor[index + 2] = PlayerColors[player].R;
}
// プレイヤーがいない座標
else {
// サポート外 0-40cm
if ( distance == depthStream.UnknownDepth ) {
// Colors.DarkGoldenrod
depthColor[index + 0] = 0x0B;
depthColor[index + 1] = 0x86;
depthColor[index + 2] = 0xB8;
}
// 近すぎ 40cm-80cm(default mode)
else if ( distance == depthStream.TooNearDepth ) {
// Colors.White
depthColor[index + 0] = 0xFF;
depthColor[index + 1] = 0xFF;
depthColor[index + 2] = 0xFF;
}
// 遠すぎ 3m(Near),4m(Default)-8m
else if ( distance == depthStream.TooFarDepth ) {
// Colors.Purple
depthColor[index + 0] = 0x80;
depthColor[index + 1] = 0x00;
depthColor[index + 2] = 0x80;
}
// 有効な距離データ
else {
// Colors.DarkCyan
depthColor[index + 0] = 0x8B;
depthColor[index + 1] = 0x8B;
depthColor[index + 2] = 0x00;
}
}
}
return depthColor;
}
byte[] GetRGB(DepthImageFrame PImage, short[] depthFrame, DepthImageStream depthStream)
{
byte[] rgbs = new byte[PImage.Width * PImage.Height * 4];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
rect_cnt[i, j] = 0;
//작은 사각형 값의 초기화
for (int i = 0; i < col; i++)
{
for (int j = 0; j < row; j++)
{
point[i, j].min_X = int.MaxValue;
point[i, j].min_Y = int.MaxValue;
point[i, j].max_X = int.MinValue;
point[i, j].max_Y = int.MinValue;
point[i, j].sGape = int.MaxValue;
point[i, j].bGape = int.MinValue;
}
}
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < rgbs.Length; i16++, i32 += 4)
{
int nDistance = depthFrame[i16] >>
DepthImageFrame.PlayerIndexBitmaskWidth;
int nX = i16 % PImage.Width;
int nY = i16 / PImage.Width;
int i = (nY - 1) / (PImage.Height / col);
int j = (nX - 1) / (PImage.Width / row);
int nColor = 0xFF * nDistance / 4000;
if (nDistance < 1000 && nDistance >= 0)
{
SetRGB(rgbs, i32, 0xff, 0, 0);
point[i, j].min_X = Math.Min(point[i, j].min_X, nX); //작은 사각형 정보 입력
point[i, j].min_Y = Math.Min(point[i, j].min_Y, nY);
point[i, j].max_X = Math.Max(point[i, j].max_X, nX);
point[i, j].max_Y = Math.Max(point[i, j].max_Y, nY);
point[i, j].sGape = Math.Min(point[i, j].sGape, nDistance);
point[i, j].bGape = Math.Max(point[i, j].bGape, nDistance);
rect_cnt[i / 4, j / 4]++;
}
else if (nDistance < 2000 && nDistance > 1000)
{
SetRGB(rgbs, i32, 0xFF, 0xFF, 0);
}
else
SetRGB(rgbs, i32, (byte)nColor, (byte)nColor, (byte)nColor);
}
return rgbs;
}
byte[] GetRGB(DepthImageFrame PImage, short[] depthFrame, DepthImageStream depthStream)
{
obsCell = new int[3, 3];
byte[] rgbs = new byte[PImage.Width * PImage.Height * 4];
int nSavei32 = 0;
for (int i = 0; i < m_col; i++)
{
for (int j = 0; j < m_row; j++)
{
_rectPoint[i, j].min_X = int.MaxValue;
_rectPoint[i, j].min_Y = int.MaxValue;
_rectPoint[i, j].max_X = int.MinValue;
_rectPoint[i, j].max_Y = int.MinValue;
}
}
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < rgbs.Length; i16++, i32 += 4)
{
int nDistance = depthFrame[i16] >>
DepthImageFrame.PlayerIndexBitmaskWidth;
int nX = i16 % PImage.Width;
int nY = i16 / PImage.Width;
int i = (nY - 1) / (PImage.Height / m_col);
int j = (nX - 1) / (PImage.Width / m_row);
int nColor = 0xFF * nDistance / 4000;
if (nDistance >= 0 && nDistance <= 1000)
{
if (!((i == m_col) || (j == m_row)))
{
_rectPoint[i, j].min_X = Math.Min(_rectPoint[i, j].min_X, nX);
_rectPoint[i, j].min_Y = Math.Min(_rectPoint[i, j].min_Y, nY);
_rectPoint[i, j].max_X = Math.Max(_rectPoint[i, j].max_X, nX);
_rectPoint[i, j].max_Y = Math.Max(_rectPoint[i, j].max_Y, nY);
obsCell[j / (m_row / 3), i / (m_col / 3)] += 1;
}
nSavei32 = i32;
SetRGB(rgbs, i32, 0xff, 0, 0);
}
else SetRGB(rgbs, i32, (byte)nColor, (byte)nColor, (byte)nColor);
}
SetRGB(rgbs, nSavei32, 0xFF, 0, 0);
return rgbs;
}
public byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int RedIndex = 0, GreenIndex = 1, BlueIndex = 2, AlphaIndex = 3;
var depthFrame32 = new byte[depthStream.FrameWidth*depthStream.FrameHeight*4];
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
// transform 13-bit depth information into an 8-bit intensity appropriate
// for display (we disregard information in most significant bit)
var intensity = (byte) (~(realDepth >> 4));
depthFrame32[i32 + RedIndex] = intensity;
depthFrame32[i32 + GreenIndex] = intensity;
depthFrame32[i32 + BlueIndex] = intensity;
depthFrame32[i32 + AlphaIndex] = 255;
}
return depthFrame32;
}
private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
{
int RedIndex = 0, GreenIndex = 1, BlueIndex = 2, AlphaIndex = 3;
byte[] depthFrame32 = new byte[depthStream.FrameWidth * depthStream.FrameHeight * 4];
for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
{
int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
byte intensity = (byte)(~(realDepth >> 4));
depthFrame32[i32 + RedIndex] = (byte)(intensity);
depthFrame32[i32 + GreenIndex] = (byte)(intensity);
depthFrame32[i32 + BlueIndex] = (byte)(intensity);
depthFrame32[i32 + AlphaIndex] = 255;
}
return depthFrame32;
}
