private void ProcessKinectFrame(KinectFrameWorkItem workItem)
{
ProcessSkeletonFrame(workItem);
ProcessColorFrame(workItem);
ProcessDepthFrame(workItem);
FusionManager.ProcessFusionFrame(workItem);
//var skeletonList = new List<Skeleton>();
//if (_activeSkeleton != null)
//{
// skeletonList.Add(_activeSkeleton);
//}
//FaceTrackingVM.TrackFrame(DefaultColorImageFormat,
// _colorImageData,
// DefaultDepthImageFormat,
// _depthImagePixels,
// skeletonList,
// fusionWorkItem.FrameNumber);
rawFrameCount++;
_kinectFrameWorkItemPool.Push(workItem);
_syncContext.Post((SendOrPostCallback)UpdateKinectFrameUI, null);
}
/*
* private unsafe void ProcessDepthFrameMod()
* {
* int width = _depthWidth;
* int height = _depthHeight;
*
* //Array.Clear(_modDepthImagePixels, 0, _modDepthImagePixels.Length);
*
* //short defaultDepth = (short)KinectSensor.DepthStream.UnknownDepth;
* //var defaultDIP = new DepthImagePixel() { Depth = defaultDepth };
*
* double maxDepth = 4000;
* double minDepth = 400;
*
* //bool processModImage = _activeSkeleton != null;
*
* //double hx = 0, hy = 0, hzMax = 0, hzMin = 0;
* //double headNeckDist2 = 0;
* //double headDepthThreshold = 300;
*
* //if (processModImage)
* //{
* // var mapper = _kinectSensor.CoordinateMapper;
*
* // var headJoint = _activeSkeleton.Joints[JointType.Head];
* // var neckJoint = _activeSkeleton.Joints[JointType.ShoulderCenter];
* // if (headJoint.TrackingState == JointTrackingState.Tracked &&
* // neckJoint.TrackingState == JointTrackingState.Tracked)
* // {
* // var headPoint = mapper.MapSkeletonPointToDepthPoint(headJoint.Position, DefaultDepthImageFormat);
* // var pos = new SkeletonPoint()
* // {
* // X = headJoint.Position.X,
* // Y = headJoint.Position.Y - 0.200f,
* // Z = headJoint.Position.Z
* // };
* // var neckPoint = mapper.MapSkeletonPointToDepthPoint(pos, DefaultDepthImageFormat);
*
* // hx = depthWidth - headPoint.X;
* // hy = headPoint.Y;
* // hzMax = headPoint.Depth + headDepthThreshold;
* // hzMin = headPoint.Depth - headDepthThreshold;
* // double factor = 2;
* // headNeckDist2 = (Math.Pow(neckPoint.X - headPoint.X, 2) +
* // Math.Pow(neckPoint.Y - headPoint.Y, 2)) * factor * factor;
* // }
* // else
* // {
* // processModImage = false;
* // }
* //}
*
* fixed (byte* depthPtrFixed = _depthImageData)//, modDepthPtrFixed = _modDepthImageData)
* {
* int* depthIntPtr = (int*)depthPtrFixed;
* //int* modDepthIntPtr = (int*)modDepthPtrFixed;
*
* int len = width * height;
*
* for (int y = 0; y < height; y++)
* {
* for (int x = 0; x < width; x++)
* {
* int srcIndex = (width - 1 - x) + y * width;
* int targetIndex = x + y * width;
*
* var dip = _depthImagePixels[srcIndex];
* short depth = dip.Depth;
* int playerIndex = dip.PlayerIndex;
*
* byte value = (byte)(255 - 255 * (depth - minDepth) / maxDepth);
*
* //byte modValue = 0;
* //byte modRMult = 0;
*
* ////if (processModImage &&
* //// playerIndex == _activeSkeletonId)
* //{
* // var dx = x - hx;
* // var dy = y - hy;
* // var dist2 = dx * dx + dy * dy;
*
* // modValue = value;
*
* // //if (dist2 < headNeckDist2 &&
* // // depth >= hzMin &&
* // // depth <= hzMax)
* // {
* // modRMult = 1;
* // _modDepthImagePixels[srcIndex] = new DepthImagePixel() { Depth = depth };
* // }
* //}
* //else
* //{
* // _modDepthImagePixels[targetIndex] = defaultDIP;
* //}
*
* if (depth <= 0)
* {
* value = 0;
* }
*
* //_depthImageData[targetIndex * 4 + 0] = value;
* //_depthImageData[targetIndex * 4 + 1] = value;
* //_depthImageData[targetIndex * 4 + 2] = value;
*
* int shiftValue = ((255 << 24) | (value << 16) | (value << 8) | (value));
*
* depthIntPtr = shiftValue;
* depthIntPtr++;
*
* //_modDepthImageData[targetIndex * 4 + 0] = modValue;
* //_modDepthImageData[targetIndex * 4 + 1] = modValue;
* //_modDepthImageData[targetIndex * 4 + 2] = modValue;
*
* //int shiftModValue = ((255 << 24) | (modValue << 16) | ((modValue * modRMult) << 8) | ((modValue * modRMult)));
*
* //*modDepthIntPtr = shiftModValue;
* //modDepthIntPtr++;
*
* }
* }
* }
*
* _depthImageWritableBitmap.WritePixels(
* new Int32Rect(0, 0, width, height),
* _depthImageData,
* width * 4,
* 0);
*
* //_modDepthImageWritableBitmap.WritePixels(
* // new Int32Rect(0, 0, width, height),
* // _modDepthImageData,
* // width * 4,
* // 0);
*
* //if (UserFusionOnly)
* //{
* // ProcessFusionFrame((DepthImagePixel[])_modDepthImagePixels.Clone());
* //}
* //else
* //{
* //}
* ProcessFusionFrame((DepthImagePixel[])_depthImagePixels.Clone());
* }
*/
#endregion
private unsafe void ProcessColorFrame(KinectFrameWorkItem workItem)
{
var depthSize = FormatHelper.GetDepthSize(workItem.Format.DepthImageFormat);
var colorSize = FormatHelper.GetColorSize(workItem.Format.ColorImageFormat);
int colorWidth = (int)colorSize.Width;
int colorHeight = (int)colorSize.Height;
int depthWidth = (int)depthSize.Width;
int depthHeight = (int)depthSize.Height;
Array.Clear(_mappedColorImageData, 0, _mappedColorImageData.Length);
var map = workItem.ColorMappedToDepthPoints;
int depthWidthMinusOne = depthWidth - 1;
fixed(byte *colorPtrFixed = workItem.ColorPixels, mappedColorPtrFixed = _mappedColorImageData)
{
fixed(DepthImagePoint *mapPtrFixed = workItem.ColorMappedToDepthPoints)
{
int * colorIntPtr = (int *)colorPtrFixed;
int * mappedColorIntPtr = (int *)mappedColorPtrFixed;
DepthImagePoint *mapPtr = mapPtrFixed;
//for (int y = 0; y < colorHeight; y += 1)
//{
// for (int x = 0; x < colorWidth; x += 1)
// {
// int srcIndex = x + y * colorWidth;
int len = colorWidth * colorHeight;
for (int i = 0; i < len; i++)
{
var coord = *mapPtr;
int cx = coord.X;
int cy = coord.Y;
if (cx >= 0 && cx < depthWidth &&
cy >= 0 && cy < depthHeight)
{
int targetIndex = (depthWidthMinusOne - cx) + cy * depthWidth;
*(mappedColorIntPtr + targetIndex) = *(colorIntPtr);
}
mapPtr++;
colorIntPtr++;
}
// }
//}
}
}
}
private unsafe void ProcessDepthFrame(KinectFrameWorkItem workItem)
{
var depthSize = FormatHelper.GetDepthSize(workItem.Format.DepthImageFormat);
int width = (int)depthSize.Width;
int height = (int)depthSize.Height;
double maxDepth = 4000;
double minDepth = 400;
fixed(byte *depthPtrFixed = _depthImageData)
{
fixed(DepthImagePixel *pixelPtrFixed = workItem.DepthImagePixels)
{
int * depthIntPtr = (int *)depthPtrFixed;
DepthImagePixel *pixelPtr = pixelPtrFixed;
int len = width * height;
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
int srcIndex = (width - 1 - x) + y * width;
int targetIndex = x + y * width;
var dip = *(pixelPtr + srcIndex);
short depth = dip.Depth;
int playerIndex = dip.PlayerIndex;
byte value = (byte)(255 - 255 * (depth - minDepth) / maxDepth);
if (depth <= 0)
{
value = 0;
}
int shiftValue = ((255 << 24) | (value << 16) | (value << 8) | (value));
*depthIntPtr = shiftValue;
depthIntPtr++;
}
}
}
}
}
private void ProcessSkeletonFrame(KinectFrameWorkItem workItem)
{
var skeletonList = workItem.Skeletons.ToList();
var closestSkeleton = skeletonList.Where(s => s.TrackingState == SkeletonTrackingState.Tracked)
.OrderBy(s => s.Position.Z * Math.Abs(s.Position.X))
.FirstOrDefault();
bool newSkeleton = false;
if (closestSkeleton != null &&
(_activeSkeleton == null ||
_activeSkeleton.TrackingId != closestSkeleton.TrackingId))
{
newSkeleton = true;
}
if (closestSkeleton == null)
{
_activeSkeletonLostCount++;
if (_activeSkeletonLostCount > _activeSkeletonLostLimit)
{
_activeSkeleton = null;
}
_activeSkeletonId = InactiveSkeletonId;
}
else
{
_activeSkeletonLostCount = 0;
_activeSkeleton = closestSkeleton;
_activeSkeletonId = skeletonList.IndexOf(closestSkeleton) + 1;
var headJoint = closestSkeleton.Joints[JointType.Head];
var neckJoint = closestSkeleton.Joints[JointType.ShoulderCenter];
float headFraction = (float)HeadNeckOffset;
_volumeCenter.X = (headJoint.Position.X * headFraction) + (neckJoint.Position.X) * (1.0f - headFraction);
_volumeCenter.Y = (headJoint.Position.Y * headFraction) + (neckJoint.Position.Y) * (1.0f - headFraction);
_volumeCenter.Z = (headJoint.Position.Z * headFraction) + (neckJoint.Position.Z) * (1.0f - headFraction);
}
if (newSkeleton)
{
ResetFusion();
}
}
private void ProcessSkeletonFrame(KinectFrameWorkItem workItem)
{
var skeletonList = workItem.Skeletons.ToList();
var closestSkeleton = skeletonList.Where(s => s.TrackingState == SkeletonTrackingState.Tracked)
.OrderBy(s => s.Position.Z * Math.Abs(s.Position.X))
.FirstOrDefault();
bool newSkeleton = false;
if (closestSkeleton != null &&
(_activeSkeleton == null ||
_activeSkeleton.TrackingId != closestSkeleton.TrackingId))
{
newSkeleton = true;
}
if (closestSkeleton == null)
{
_activeSkeletonLostCount++;
if (_activeSkeletonLostCount > _activeSkeletonLostLimit)
{
_activeSkeleton = null;
}
_activeSkeletonId = InactiveSkeletonId;
}
else
{
_activeSkeletonLostCount = 0;
_activeSkeleton = closestSkeleton;
_activeSkeletonId = skeletonList.IndexOf(closestSkeleton) + 1;
var headJoint = closestSkeleton.Joints[JointType.Head];
var neckJoint = closestSkeleton.Joints[JointType.ShoulderCenter];
float headFraction = (float)HeadNeckOffset;
_volumeCenter.X = (headJoint.Position.X * headFraction) + (neckJoint.Position.X) * (1.0f - headFraction);
_volumeCenter.Y = (headJoint.Position.Y * headFraction) + (neckJoint.Position.Y) * (1.0f - headFraction);
_volumeCenter.Z = (headJoint.Position.Z * headFraction) + (neckJoint.Position.Z) * (1.0f - headFraction);
}
if (newSkeleton)
{
ResetFusion();
}
}
private void ReturnKinectFrameWorkItem(KinectFrameWorkItem workItem)
{
_kinectFrameWorkItemPool.Push(workItem);
}
private void ProcessKinectFrame(KinectFrameWorkItem workItem)
{
ProcessSkeletonFrame(workItem);
ProcessColorFrame(workItem);
ProcessDepthFrame(workItem);
FusionManager.ProcessFusionFrame(workItem);
//var skeletonList = new List<Skeleton>();
//if (_activeSkeleton != null)
//{
// skeletonList.Add(_activeSkeleton);
//}
//FaceTrackingVM.TrackFrame(DefaultColorImageFormat,
// _colorImageData,
// DefaultDepthImageFormat,
// _depthImagePixels,
// skeletonList,
// fusionWorkItem.FrameNumber);
rawFrameCount++;
_kinectFrameWorkItemPool.Push(workItem);
_syncContext.Post((SendOrPostCallback)UpdateKinectFrameUI, workItem);
}
private unsafe void ProcessDepthFrame(KinectFrameWorkItem workItem)
{
var depthSize = FormatHelper.GetDepthSize(workItem.Format.DepthImageFormat);
int width = (int)depthSize.Width;
int height = (int)depthSize.Height;
double maxDepth = 4000;
double minDepth = 400;
fixed (byte* depthPtrFixed = _depthImageData)
{
fixed (DepthImagePixel* pixelPtrFixed = workItem.DepthImagePixels)
{
int* depthIntPtr = (int*)depthPtrFixed;
DepthImagePixel* pixelPtr = pixelPtrFixed;
int len = width * height;
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
int srcIndex = (width - 1 - x) + y * width;
int targetIndex = x + y * width;
var dip = *(pixelPtr + srcIndex);
short depth = dip.Depth;
int playerIndex = dip.PlayerIndex;
byte value = (byte)(255 - 255 * (depth - minDepth) / maxDepth);
if (depth <= 0)
{
value = 0;
}
int shiftValue = ((255 << 24) | (value << 16) | (value << 8) | (value));
*depthIntPtr = shiftValue;
depthIntPtr++;
}
}
}
}
}
private unsafe void ProcessColorFrame(KinectFrameWorkItem workItem)
{
var depthSize = FormatHelper.GetDepthSize(workItem.Format.DepthImageFormat);
var colorSize = FormatHelper.GetColorSize(workItem.Format.ColorImageFormat);
int colorWidth = (int)colorSize.Width;
int colorHeight = (int)colorSize.Height;
int depthWidth = (int)depthSize.Width;
int depthHeight = (int)depthSize.Height;
Array.Clear(workItem.MappedColorImageData, 0, workItem.MappedColorImageData.Length);
var map = workItem.ColorMappedToDepthPoints;
int depthWidthMinusOne = depthWidth - 1;
fixed (byte* colorPtrFixed = workItem.ColorPixels, mappedColorPtrFixed = workItem.MappedColorImageData)
{
fixed (DepthImagePoint* mapPtrFixed = workItem.ColorMappedToDepthPoints)
{
int* colorIntPtr = (int*)colorPtrFixed;
int* mappedColorIntPtr = (int*)mappedColorPtrFixed;
DepthImagePoint* mapPtr = mapPtrFixed;
//for (int y = 0; y < colorHeight; y += 1)
//{
// for (int x = 0; x < colorWidth; x += 1)
// {
// int srcIndex = x + y * colorWidth;
int len = colorWidth * colorHeight;
for (int i = 0; i < len; i++)
{
var coord = *mapPtr;
int cx = coord.X;
int cy = coord.Y;
if (cx >= 0 && cx < depthWidth &&
cy >= 0 && cy < depthHeight)
{
int targetIndex = (depthWidthMinusOne - cx) + cy * depthWidth;
*(mappedColorIntPtr + targetIndex) = *(colorIntPtr);
}
mapPtr++;
colorIntPtr++;
}
// }
//}
}
}
}
private void ReturnKinectFrameWorkItem(KinectFrameWorkItem workItem)
{
_kinectFrameWorkItemPool.Push(workItem);
}
public void ProcessFusionFrame(KinectFrameWorkItem workItem)
{
DepthImagePixel[] depthPixels = workItem.DepthImagePixels;
byte[] colorPixels = workItem.MappedColorImageData;
DepthImageFormat format = workItem.Format.DepthImageFormat;
ColorImageFormat color_format = workItem.Format.ColorImageFormat;
FaceFusion.Services.KinectFormat kinect_format = new FaceFusion.Services.KinectFormat();
kinect_format.DepthImageFormat = format;
kinect_format.ColorImageFormat = color_format;
if (kinect_format.DepthImageFormat != _currentFormat.DepthImageFormat || kinect_format.ColorImageFormat != _currentFormat.ColorImageFormat)
{
_currentFormat = kinect_format;
_fusionWorkItemPool.Format = kinect_format;
_residualImageData = new byte[depthPixels.Length * 4];
}
var fusionWorkItem = _fusionWorkItemPool.Pop();
if (fusionWorkItem == null)
{
Trace.WriteLine("Fusion Depth Pool empty");
return;
}
Array.Copy(depthPixels, fusionWorkItem.data, depthPixels.Length);
Array.Copy(colorPixels, fusionWorkItem.colordata, colorPixels.Length);
if (_fusionWorkQueue != null)
{
_fusionWorkQueue.AddWork(fusionWorkItem);
}
}