public static extern int infer_palmpose(ref PointXYZ v1, ref PointXYZ v2, ref PointXYZ v3, [In, Out] PointXYZ[] pointArray, ref int size, ref PointXYZ hand, ref PointXYZ wrist);
private void KinectSensorOnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
{
if (null == this.sensor)
{
return;
}
bool depthReceived = false;
bool colorReceived = false;
bool bodyReceived = false;
Skeleton[] skeletons = null;
List <Point> points = new List <Point>();
Point handPointColor = new Point()
{
};
Point wristPointColor = new Point()
{
};
PointXYZ handXYZ = new PointXYZ();
PointXYZ wristXYZ = new PointXYZ();
using (var colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame())
{
if (colorImageFrame == null)
{
return;
}
colorImageFrame.CopyPixelDataTo(this.colorPixels);
colorReceived = true;
}
using (SkeletonFrame skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
bodyReceived = true;
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
int leftBound = 0;
int topBound = 0;
int rightBound = 0;
int bottomBound = 0;
if (bodyReceived)
{
foreach (var skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
Joint handLeft = skel.Joints[JointType.HandLeft];
Joint wristLeft = skel.Joints[JointType.WristLeft];
Joint shoulder0 = skel.Joints[JointType.ShoulderLeft];
Joint shoulder1 = skel.Joints[JointType.ShoulderRight];
var shoulder0Color = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(shoulder0.Position, ColorImageFormat.RgbResolution640x480Fps30);
var shoulder1Color = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(shoulder1.Position, ColorImageFormat.RgbResolution640x480Fps30);
// !!!important parameter to adapt the recognition region
// we have to tune it
float bodyScaleColor = 9.0f + 32.0f / handLeft.Position.Z;
ColorImagePoint handColor =
this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(handLeft.Position,
ColorImageFormat.RgbResolution640x480Fps30);
ColorImagePoint wristColor =
this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(wristLeft.Position,
ColorImageFormat.RgbResolution640x480Fps30);
leftBound = (handColor.X - bodyScaleColor) < 0 ? 0 : (int)(handColor.X - bodyScaleColor);
topBound = (handColor.Y - bodyScaleColor) < 0 ? 0 : (int)(handColor.Y - bodyScaleColor);
rightBound = (handColor.X + bodyScaleColor) > colorWidth ? colorWidth : (int)(handColor.X + bodyScaleColor);
bottomBound = (handColor.Y + bodyScaleColor) > colorHeight ? colorHeight : (int)(handColor.Y + bodyScaleColor);
handPointColor = new Point(handColor.X, handColor.Y);
wristPointColor = new Point(wristColor.X, wristColor.Y);
handXYZ = new PointXYZ(handLeft.Position.X, handLeft.Position.Y, handLeft.Position.Z);
wristXYZ = new PointXYZ(wristLeft.Position.X, wristLeft.Position.Y, wristLeft.Position.Z);
// quit after handling one player
// assuming only one player
break;
}
}
}
using (DepthImageFrame depthFrame = allFramesReadyEventArgs.OpenDepthImageFrame())
{
if (null != depthFrame)
{
// Copy the pixel data from the image to a temporary array
depthFrame.CopyDepthImagePixelDataTo(this.depthPixels);
depthReceived = true;
}
}
List <SkeletonPoint> candidatePoints = new List <SkeletonPoint>();
if (true == depthReceived && true == bodyReceived && true == colorReceived)
{
this.sensor.CoordinateMapper.MapDepthFrameToColorFrame(
DepthFormat,
this.depthPixels,
ColorFormat,
this.colorCoordinates);
System.Drawing.Bitmap bmp = new System.Drawing.Bitmap(colorWidth, colorHeight);
Image <Bgra, Byte> img = new Image <Bgra, byte>(colorWidth, colorHeight);
img.Bytes = this.colorPixels;
Image <Bgra, Byte> crop = null;
Image <Gray, Byte> res = null;
if (bottomBound > topBound && rightBound > leftBound)
{
crop = img.Copy(new System.Drawing.Rectangle(
leftBound, topBound, rightBound - leftBound, bottomBound - topBound));
Image <Gray, Byte> canny = crop.Convert <Gray, Byte>().Canny(50, 150);
VectorOfVectorOfPoint vvp = new VectorOfVectorOfPoint();
CvInvoke.FindContours(canny, vvp, null, RetrType.Ccomp, ChainApproxMethod.ChainApproxNone);
res = new Image <Gray, byte>(crop.Width, crop.Height);
for (int i = 0; i < vvp.Size; i++)
{
for (int j = 0; j < vvp[i].Size; j++)
{
res.Data[vvp[i][j].Y, vvp[i][j].X, 0] = 255;
}
}
}
// loop over each row and column of the depth
for (int y = 0; y < this.depthHeight; ++y)
{
for (int x = 0; x < this.depthWidth; ++x)
{
// calculate index into depth array
int depthIndex = x + (y * this.depthWidth);
DepthImagePixel depthPixel = this.depthPixels[depthIndex];
int player = depthPixel.PlayerIndex;
// assuming one player
if (player > 0 && depthPixel.IsKnownDepth)
{
// retrieve the depth to color mapping for the current depth pixel
ColorImagePoint colorImagePoint = this.colorCoordinates[depthIndex];
if (colorImagePoint.X >= leftBound && colorImagePoint.X < rightBound &&
colorImagePoint.Y >= topBound && colorImagePoint.Y < bottomBound)
{
SkeletonPoint p = this.sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(
DepthFormat,
new DepthImagePoint()
{
X = x, Y = y, Depth = depthPixel.Depth
});
candidatePoints.Add(p);
}
}
}
}
PointXYZ v1 = new PointXYZ();
PointXYZ v2 = new PointXYZ();
PointXYZ v3 = new PointXYZ();
int nPoint = candidatePoints.Count;
const int min_point_size = 100;
if (nPoint > min_point_size)
{
PointXYZ[] pointCloud = new PointXYZ[nPoint];
for (int i = 0; i < nPoint; i++)
{
pointCloud[i] = new PointXYZ(candidatePoints[i].X, candidatePoints[i].Y, candidatePoints[i].Z);
}
int pointCloudLength = pointCloud.Length;
ExternalAPI.infer_palmpose(ref v1, ref v2, ref v3, pointCloud, ref pointCloudLength,
ref handXYZ, ref wristXYZ);
SkeletonPoint[] skelPoints = new SkeletonPoint[pointCloudLength];
ColorImagePoint[] newColorPoints = new ColorImagePoint[pointCloudLength];
for (int i = 0; i < pointCloudLength; i++)
{
skelPoints[i].X = pointCloud[i].x;
skelPoints[i].Y = pointCloud[i].y;
skelPoints[i].Z = pointCloud[i].z;
}
for (int i = 0; i < pointCloudLength; i++)
{
ColorImagePoint colorP = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(skelPoints[i], ColorFormat);
newColorPoints[i] = colorP;
}
foreach (var cp in newColorPoints)
{
{
int index = cp.X + cp.Y * this.colorWidth;
this.colorPixels[index * 4 + 0] = 0;
this.colorPixels[index * 4 + 1] = 255;
this.colorPixels[index * 4 + 2] = 255;
this.colorPixels[index * 4 + 3] = 255;
}
}
}
if (res != null)
{
//Image<Bgra, Byte> resColor = res.Convert<Bgra, Byte>();
//int x = leftBound;
//int y = topBound;
//int w = rightBound - leftBound;
//int h = bottomBound - topBound;
//int offset = x + y * colorWidth;
//int ii = offset * 4;
//for (int i = 0; i < h; i++)
//{
// for (int j = 0; j < w; j++)
// {
// this.colorPixels[ii++] = resColor.Data[i, j, 0];
// this.colorPixels[ii++] = resColor.Data[i, j, 1];
// this.colorPixels[ii++] = resColor.Data[i, j, 2];
// this.colorPixels[ii++] = 255;
// }
// ii += (colorWidth - w) * 4;
//}
}
this.colorBitmap.WritePixels(new Int32Rect(0, 0, this.colorWidth, this.colorHeight),
this.colorPixels,
this.colorBitmap.PixelWidth * sizeof(int), 0);
using (DrawingContext dc = this.drawingGroup.Open())
{
dc.DrawImage(this.colorBitmap, new Rect(0, 0, this.colorWidth, this.colorHeight));
// if draw hands
if (bodyReceived)
{
SkeletonPoint origin = new SkeletonPoint();
origin.X = handXYZ.x;
origin.Y = handXYZ.y;
origin.Z = handXYZ.z;
ColorImagePoint originDepth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(origin, ColorFormat);
const float len = 0.2f;
SkeletonPoint end1 = new SkeletonPoint();
end1.X = origin.X + len * v1.x;
end1.Y = origin.Y + len * v1.y;
end1.Z = origin.Z + len * v1.z;
ColorImagePoint end1Depth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(end1, ColorFormat);
SkeletonPoint end2 = new SkeletonPoint();
end2.X = origin.X + len * v2.x;
end2.Y = origin.Y + len * v2.y;
end2.Z = origin.Z + len * v2.z;
ColorImagePoint end2Depth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(end2, ColorFormat);
SkeletonPoint end3 = new SkeletonPoint();
end3.X = origin.X + len * v3.x;
end3.Y = origin.Y + len * v3.y;
end3.Z = origin.Z + len * v3.z;
ColorImagePoint end3Depth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(end3, ColorFormat);
dc.DrawLine(penX, new Point(originDepth.X, originDepth.Y), new Point(end1Depth.X, end1Depth.Y));
dc.DrawLine(penY, new Point(originDepth.X, originDepth.Y), new Point(end2Depth.X, end2Depth.Y));
dc.DrawLine(penZ, new Point(originDepth.X, originDepth.Y), new Point(end3Depth.X, end3Depth.Y));
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.colorWidth, this.colorHeight));
}
}
}
