public static Blkd CreateDepthToCameraBlkd(PXCMPoint3DF32[] _depthToCameraMapping, int width, int height)
{
var BytesPerPixel = 3 * sizeof(float);
byte[] buffer = new byte[width * height * BytesPerPixel];
for (int i = 0; i < _depthToCameraMapping.Length; i++)
{
var point = _depthToCameraMapping[i];
var smallfloatBuffer = new float[] { point.x, point.y, point.z };
Buffer.BlockCopy(smallfloatBuffer, 0, buffer, i * BytesPerPixel, BytesPerPixel);
}
Blkd result = new Blkd
{
Width = (UInt16)width,
Height = (UInt16)height,
BytesPerPixel = (byte)BytesPerPixel,
Version = 2,
Data = buffer
};
return result;
}
public static PXCMPoint3DF32 ToPXCMPoint3DF32(this Vector3D v)
{
var ret = new PXCMPoint3DF32();
ret.x = (float)v.x;
ret.y = (float)v.y;
ret.z = (float)v.z;
return ret;
}
// Update is called once per frame
void Update ()
{
if (!init)
{
OnEnable();
}
if (SenseToolkitManager.Instance.Initialized &&
SenseToolkitManager.Instance.ImageRgbOutput != null &&
SenseToolkitManager.Instance.ImageDepthOutput != null)
{
PXCMImage.ImageData depthData;
int width = SenseToolkitManager.Instance.ImageDepthOutput.info.width;
int height = SenseToolkitManager.Instance.ImageDepthOutput.info.height;
// 1. Get the depth image
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out depthData);
if (_depthArray == null)
{
_depthArray = new float[width * height];
}
depthData.ToFloatArray(0,_depthArray);
PXCMPoint3DF32[] pos_uvz = new PXCMPoint3DF32[1]{new PXCMPoint3DF32(){x = 0, y= 0 , z = 0}};
// 2. Find a "good" pixel (not too far)
int x = width / 4;
int y = height / 4;
int xx = 5;
int yy = 5;
while (pos_uvz[0].z == 0 || (pos_uvz[0].z / 10) > MaxDepthThreshold)
{
x += xx;
if ( x >= 3 * width / 4 )
{
x = width / 4 ;
y += yy;
if (y >= 3 * height / 4)
{
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(depthData);
return;
}
}
pos_uvz[0].x = x;
pos_uvz[0].y = y;
pos_uvz[0].z = _depthArray[x + y * width];
}
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(depthData);
// 3. Projet it to the color image
PXCMPointF32[] pos_ij = new PXCMPointF32[1]{new PXCMPointF32()};
var sts = SenseToolkitManager.Instance.Projection.MapDepthToColor( pos_uvz, pos_ij);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// 4. Project it to the Real World coordinates
PXCMPoint3DF32[] pos3d = new PXCMPoint3DF32[1]{new PXCMPoint3DF32()};
sts = SenseToolkitManager.Instance.Projection.ProjectDepthToCamera( pos_uvz, pos3d );
pos3d[0].x /= -10;
pos3d[0].y /= 10;
pos3d[0].z /= 10;
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR || pos3d[0].x == 0 || pos3d[0].y == 0)
{
return;
}
Vector3 vec = new Vector3();
vec.x = pos3d[0].x;
vec.y = pos3d[0].y;
vec.z = pos3d[0].z;
// 5. Normalize the color pixel location
Vector3 vecScreen = new Vector3();
vecScreen.x = (SenseToolkitManager.Instance.ImageRgbOutput.info.width - pos_ij[0].x) / SenseToolkitManager.Instance.ImageRgbOutput.info.width;
vecScreen.y = (SenseToolkitManager.Instance.ImageRgbOutput.info.height - pos_ij[0].y) / SenseToolkitManager.Instance.ImageRgbOutput.info.height;
// 6. Calibrate the world point to the screen point
Calibrate(vec, vecScreen);
}
}
public Vector3 ProcessSmoothing(SmoothingTypes type, float factor, Vector3 vec)
{
Init();
if (_dataSmoothing == null)
{
SenseToolkitManager.Instance.SenseManager.session.CreateImpl<PXCMDataSmoothing>(out _dataSmoothing);
}
if (_smoother3D == null || _type3D != type || factor != _factor3D)
{
if (_smoother3D != null)
{
_smoother3D.Dispose();
}
CreateSmootherType(type, factor, out _smoother3D);
_type3D = type;
_factor3D = factor;
}
PXCMPoint3DF32 point = new PXCMPoint3DF32(){x = vec.x, y = vec.y, z = vec.z};
_smoother3D.AddSample(point);
point = _smoother3D.GetSample();
return new Vector3(point.x, point.y, point.z);
}
public void DisplayBlobNumber(PXCMPoint3DF32 centerPoint, int index)
{
if (bitmap == null) return;
lock (this)
{
Graphics g = Graphics.FromImage(bitmap);
var rect = new RectangleF(centerPoint.x, centerPoint.y, 20, 20);
//Filling a rectangle before drawing the string.
g.FillRectangle(Brushes.White, rect);
g.DrawString(index.ToString(), drawFont, drawBrush, centerPoint.x, centerPoint.y);
g.Dispose();
}
}
protected float Distance(PXCMPoint3DF32 a, PXCMPoint3DF32 b)
{
float dx = a.x - b.x;
float dy = a.y - b.y;
float dz = a.z - b.z;
return (float) Math.Sqrt((double)(dx * dx + dy * dy + dz * dz));
}
/* Displaying Mask Images */
private unsafe void DisplayPicture(PXCMImage depth, PXCMBlobData blobData)
{
if (depth == null)
return;
PXCMImage image = depth;
PXCMImage.ImageInfo info = image.QueryInfo();
int numOfBlobs = blobData.QueryNumberOfBlobs();
if (_maxBlobToShow > numOfBlobs)
{
_maxBlobToShow = numOfBlobs;
}
PXCMBlobData.IBlob[] blobList = new PXCMBlobData.IBlob[_maxBlobToShow];
PXCMPointI32[][] pointOuter = new PXCMPointI32[_maxBlobToShow][];
PXCMPointI32[][] pointInner = new PXCMPointI32[_maxBlobToShow][];
Bitmap picture = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
PXCMImage.ImageData bdata;
pxcmStatus results = pxcmStatus.PXCM_STATUS_NO_ERROR;
PXCMBlobData.AccessOrderType accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_LARGE_TO_SMALL;
int accessOrderBy = form.GetAccessOrder();
switch (accessOrderBy)
{
case 1:
accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR;
break;
case 2:
accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_RIGHT_TO_LEFT;
break;
case 0:
default:
accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_LARGE_TO_SMALL;
break;
}
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = picture.LockBits(rect, ImageLockMode.ReadWrite, picture.PixelFormat);
for (int j = 0; j < _maxBlobToShow; j++)
{
byte tmp1 = (Byte)(255 - (255 / (_maxBlobToShow) * j));
results = blobData.QueryBlobByAccessOrder(j, accessOrder, out blobList[j]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
bool isSegmentationImage = true;
results = blobList[j].QuerySegmentationImage(out image);
if (results != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMImage.ImageInfo imgInfo = new PXCMImage.ImageInfo();
imgInfo.width = 640;
imgInfo.height = 480;
imgInfo.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
PXCMSession session = PXCMSession.CreateInstance();
if (session != null)
{
image = session.CreateImage(info);
if (image == null) return;
}
image.AcquireAccess(PXCMImage.Access.ACCESS_WRITE, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out bdata);
byte* numPtr = (byte*)bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*)bdata.planes[0]; //row
int imagesize = image.info.width * image.info.height;
byte tmp;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte)(0);
numPtr[0] = tmp;
numPtr[1] = tmp;
numPtr[2] = tmp;
numPtr[3] = tmp;
}
image.ReleaseAccess(bdata);
isSegmentationImage = false;
}
results = image.AcquireAccess(PXCMImage.Access.ACCESS_READ_WRITE, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out bdata);
if (form.GetBlobState() && isSegmentationImage == true)
{
byte* numPtr = (byte*)bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*)bdata.planes[0]; //row
int imagesize = image.info.width * image.info.height;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
byte tmp = (Byte)(numPtr2[0] == 0 ? 0 : tmp1);
tmp |= numPtr[0];
numPtr[0] = tmp;
numPtr[1] = tmp;
numPtr[2] = tmp;
numPtr[3] = 0xff;
}
}
if ((form.GetContourState()))
{
int contourNumber = blobList[j].QueryNumberOfContours();
if (contourNumber > 0)
{
for (int k = 0; k < contourNumber; ++k)
{
int contourSize = blobList[j].QueryContourSize(k);
if (blobList[j].IsContourOuter(k) == true)
blobList[j].QueryContourPoints(k, out pointOuter[j]);
else
{
blobList[j].QueryContourPoints(k, out pointInner[j]);
}
}
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR && form.GetBlobState() == false)
{
byte* numPtr = (byte*)bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*)bdata.planes[0]; //row
int imagesize = image.info.width * image.info.height;
byte tmp;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte)(0);
numPtr[0] = tmp;
numPtr[1] = tmp;
numPtr[2] = tmp;
numPtr[3] = tmp;
}
}
}
}
image.ReleaseAccess(bdata);
image.Dispose();
}
}
picture.UnlockBits(bitmapdata);
form.DisplayBitmap(picture);
///////// that is my polygon zone
Bitmap imageInstance2 = picture;
i++;
Bitmap croppedImage = null;
if (contourMostRight.x > 0) {
int rectWidth = (int)(contourMostRight.x - contourMostLeft.x);
int rectHeight = (int)(contourMostTop.y - contourMostBottom.y);
Rectangle sourceRectangle = new Rectangle(new Point((int)contourMostLeft.x,
(int)contourMostBottom.y),
new Size(rectWidth, rectHeight));
croppedImage = CropImage(imageInstance2, sourceRectangle);
}
String[] origArray = {
"d:\\origG.jpeg",
"d:\\origX.jpeg",
"d:\\origY.jpeg"
};
for (int i = 0; i < origArray.Length; i++)
{
Bitmap orig = null;
if (File.Exists(origArray[i]))
orig = new Bitmap(@origArray[i]);
if (orig != null && croppedImage != null)
{
float diff = 0;
orig = ScaleImage(orig, 150, 100);
croppedImage = ScaleImage(croppedImage, 150, 100);
bool isImageBlank = true;
for (int y = 0; y < orig.Height; y++)
{
for (int x = 0; x < orig.Width; x++)
{
if(croppedImage.GetPixel(x, y).R > 1 && croppedImage.GetPixel(x, y).B > 1
&& croppedImage.GetPixel(x, y).G > 1)
{
isImageBlank = false;
break;
}
}
}
if (!isImageBlank && orig.Size.Width == croppedImage.Size.Width)
{
for (int y = 0; y < orig.Height; y++)
{
for (int x = 0; x < orig.Width; x++)
{
diff += (float)Math.Abs(orig.GetPixel(x, y).R -
croppedImage.GetPixel(x, y).R) / 255;
diff += (float)Math.Abs(orig.GetPixel(x, y).G -
croppedImage.GetPixel(x, y).G) / 255;
diff += (float)Math.Abs(orig.GetPixel(x, y).B -
croppedImage.GetPixel(x, y).B) / 255;
}
}
}
float percentMatch = 100 * diff / (orig.Width * orig.Height * 2);
Console.WriteLine("diff: {0} %", percentMatch);
if (percentMatch >= 90){
Console.WriteLine(origArray[i].ToString());
Environment.Exit(0);
}
}
}
/*
if (croppedImage != null)
{
croppedImage.Save("d:\\cropedImage.jpeg", System.Drawing.Imaging.ImageFormat.Jpeg);
// croppedImage.Save("d:\\origG.jpeg", System.Drawing.Imaging.ImageFormat.Jpeg);
}*/
if (i == 100)
Environment.Exit(0);
picture.Dispose();
for (int i = 0; i < _maxBlobToShow; i++)
{
if (form.GetContourState())
{
if (pointOuter[i] != null && pointOuter[i].Length > 0)
form.DisplayContour(pointOuter[i], i);
if (pointInner[i] != null && pointInner[i].Length > 0)
form.DisplayContour(pointInner[i], i);
}
if (form.GetBlobDataPointsState())
{
form.DisplayBlobDataPoints(blobList[i], i + 1);
}
PXCMPoint3DF32 point = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_CENTER);
contourMostRight = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_LEFT_MOST);
contourMostLeft = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_RIGHT_MOST);
contourMostBottom = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_TOP_MOST);
contourMostTop = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_BOTTOM_MOST);
form.DisplayBlobNumber(point, i + 1);
}
}
/// <summary>
/// HandTrackingで取得したHandDataを受け取り
/// 画面にHandDataの情報を表示する
/// </summary>
private void DisplayHandTrackingData(PXCMHandData handData)
{
// SegmentationImageの情報格納用の変数
int segWbStride = 0;
Int32Rect segWbRect = new Int32Rect(0, 0, 0, 0);
Byte[] segImageBuffer = null;
// HandJointの情報格納用の変数
List<List<PXCMHandData.JointData>> handJointsList = new List<List<PXCMHandData.JointData>>();
for (int handIndex = 0; handIndex < handData.QueryNumberOfHands(); handIndex++)
{
// IHandDataを取得
PXCMHandData.IHand iHandData;
if (handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, handIndex, out iHandData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// SegmentationImageを取得
PXCMImage image;
iHandData.QuerySegmentationImage(out image); // 取得出来る画像は8bitGrayスケール画像 手の部分が0xff(白) 背景が0x00(黒)
// Imageから画像データを取得
PXCMImage.ImageData data = null ;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// ImageDataが取得できたらWritableBitmapに書き込むためにバイト配列に変換する
int length = data.pitches[0] * image.info.height;
Byte[] tmpBuffer = data.ToByteArray(0, length);
// ストライドと描画領域を取得
segWbStride = data.pitches[0];
segWbRect = new Int32Rect(0, 0, image.info.width, image.info.height);
// Imageデータのアクセスを終了する
image.ReleaseAccess(data);
// HandSegmentationImageは複数ある可能性があるためすでにバイト配列を取得ずみの場合は重ね合わせる
if (segImageBuffer == null)
{
// まだない場合は、そのまま使用する
Array.Resize(ref segImageBuffer, tmpBuffer.Length);
tmpBuffer.CopyTo(segImageBuffer, 0);
}
else
{
// 既にひとつの手の情報がある場合は手の白部分(0xff)のみ重ね合わせる
for (int i=0; i<segImageBuffer.Length; i++)
{
segImageBuffer[i] = (byte)(segImageBuffer[i] | tmpBuffer[i]);
}
}
}
// TODO:後で取得してみる
//iHandData.QueryBoundingBoxImage // 手の領域
//iHandData.QueryMassCenterImage // 2D Image coordinatesでの手の中心座標
//iHandData.QueryMassCenterWorld // 3D World Coordinatesでの手の中心座標
//iHandData.QueryExtremityPoint // TODO:Extremitiesモードで取得してみる
// 1つの手のJointを入れるListを生成
List<PXCMHandData.JointData> jointList = new List<PXCMHandData.JointData>();
// 手のJoint座標を取得してListに格納
for (int jointIndex = 0; jointIndex < Enum.GetNames(typeof(PXCMHandData.JointType)).Length; jointIndex++)
{
// 手の1つのJoint座標を取得
PXCMHandData.JointData jointData;
iHandData.QueryTrackedJoint((PXCMHandData.JointType)jointIndex, out jointData);
jointList.Add(jointData);
}
// 作成した1つの手のJoint座標リストをListに格納
handJointsList.Add(jointList);
}
}
// SegmentationImageデータをバイト配列にしたものをWriteableBitmapに書き込む
if (segImageBuffer != null)
{
m_HandSegmentWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
m_HandSegmentWBitmap.WritePixels(segWbRect, segImageBuffer, segWbStride, 0);
}
));
}
// HandJointの座標を画面に表示
if (handJointsList.Count > 0)
{
m_ColorWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
foreach (List<PXCMHandData.JointData> jointList in handJointsList)
{
foreach (PXCMHandData.JointData joint in jointList)
{
PXCMPoint3DF32[] depthPoint = new PXCMPoint3DF32[1];
depthPoint[0].x = joint.positionImage.x;
depthPoint[0].y = joint.positionImage.y;
depthPoint[0].z = joint.positionWorld.z * 1000; // mmとpixcelを合わす
PXCMPointF32[] colorPoint = new PXCMPointF32[1];
pxcmStatus status = m_Projection.MapDepthToColor(depthPoint, colorPoint);
// 指の位置を描画
m_ColorWBitmap.FillEllipseCentered((int)colorPoint[0].x,
(int)colorPoint[0].y,
10, 10, Colors.YellowGreen);
}
}
}
));
}
m_HandJointWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
m_HandJointWBitmap.Clear();
foreach (List<PXCMHandData.JointData> jointList in handJointsList)
{
foreach (PXCMHandData.JointData joint in jointList)
{
m_HandJointWBitmap.FillEllipse(
(int)joint.positionImage.x, (int)joint.positionImage.y,
(int)joint.positionImage.x + 6, (int)joint.positionImage.y + 6, Colors.YellowGreen);
}
}
}
));
}
private Point3D ToPoint3D(PXCMPoint3DF32 p)
{
return new Point3D(p.x, p.y, p.z);
}
private static extern bool ProjectRealWorldToImageC(IntPtr pp, Int32 npoints, PXCMPoint3DF32[] pos3d, PXCMPointF32[] pos2d);
private static extern bool ProjectImageToRealWorldC(IntPtr pp, Int32 npoints, PXCMPoint3DF32[] pos2d, PXCMPoint3DF32[] pos3d);
private static extern bool MapDepthToColorCoordinatesC(IntPtr pp, Int32 npoints, PXCMPoint3DF32[] pos2d, PXCMPointF32[] posc);
public bool ProjectRealWorldToImage(PXCMPoint3DF32[] pos3d, out PXCMPointF32[] pos2d)
{
pos2d=null;
if (!initialized || pos3d==null) return false;
pos2d=new PXCMPointF32[pos3d.Length];
return ProjectRealWorldToImageC(instance,pos3d.Length,pos3d,pos2d);
}
public bool MapDepthToColorCoordinates(PXCMPoint3DF32[] pos2d, out PXCMPointF32[] posc)
{
posc=null;
if (!initialized || pos2d==null) return false;
posc=new PXCMPointF32[pos2d.Length];
return MapDepthToColorCoordinatesC(instance,pos2d.Length,pos2d,posc);
}
private Coord3D GetTransformPosition(PXCMPoint3DF32 handPosition)
{
// x +- 0.25, y +- 0.15 z =0.2- 0.6
// The limits of the system will be determined by the length of the two arm bones and the angle of the base, the range of vertical motion of the base
// The Z height limit will be maximum of the arm length and minimum of the length of the forearm and hand.
// The co-ordinates are positive
// e.g x+0.25 *maxlengthX and so forth.
// 300,300,270
// x is -+165 to
//handPosition.x = (handPosition.x + 0.25f)*(300/0.25f);
//handPosition.y *= (handPosition.y + 0.15f) * (300 / 0.15f);
//handPosition.z *= (handPosition.z) * (150 / 0.55f);
var outputX = -(handPosition.x/0.25f)*300f;
var outputZ = 1.1f*((handPosition.y + 0.15f)/0.3f)*280f;
var outputY = 280f - (handPosition.z/1.25f)*280f;
return new Coord3D((int)Math.Ceiling(outputX),(int)Math.Ceiling(outputY),(int)Math.Ceiling(outputZ));
}
public void DepthToCameraCoordinates(PXCMImage depth, PXCMPoint3DF32[] cameraSpacePts)
{
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
pxcmStatus status = projection.ProjectDepthToCamera(dcords, cameraSpacePts);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new InvalidOperationException("Projection depth to camera failed");
}
}
}
public byte[] DepthToColorCoordinatesByFunction(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (projection == null || cpixels == null) return cpixels;
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
PXCMPointF32[] ccords = new PXCMPointF32[dwidth * dheight];
projection.MapDepthToColor(dcords, ccords);
/* Draw dots onto the color pixels */
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
UInt16 d = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
if (d == invalid_value) continue; // no mapping based on unreliable depth values
Int32 xx = (Int32)ccords[k].x, yy = (Int32)ccords[k].y;
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 2);
}
}
}
return cpixels;
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for ( int i = 0; i < numOfHands; i++ ) {
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand );
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
// 指の関節を列挙する
for ( int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++ ) {
// 指のデータを取得する
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint( (PXCMHandData.JointType)j, out jointData );
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor( depthPoint, colorPoint );
AddEllipse( CanvasFaceParts,
new Point( colorPoint[0].x, colorPoint[0].y ),
5, Brushes.Green );
}
}
}
