private void FrameReader_FrameArrived(MediaFrameReader sender, MediaFrameArrivedEventArgs args)
{
if (!frameProcessingSemaphore.Wait(0))
{
return;
}
try {
var frame = sender.TryAcquireLatestFrame();
if (frame != null)
{
this.frames[frame.SourceKind] = frame;
}
if (this.frames[MediaFrameSourceKind.Color] != null && this.frames[MediaFrameSourceKind.Depth] != null)
{
var colorDesc = this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.SoftwareBitmap.LockBuffer(BitmapBufferAccessMode.Read).GetPlaneDescription(0);
var depthDesc = this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.SoftwareBitmap.LockBuffer(BitmapBufferAccessMode.Read).GetPlaneDescription(0);
// get points in 3d space
DepthCorrelatedCoordinateMapper coordinateMapper = this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.DepthMediaFrame.TryCreateCoordinateMapper(
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics, this.frames[MediaFrameSourceKind.Color].CoordinateSystem);
// get color information
var bitmap = SoftwareBitmap.Convert(this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.SoftwareBitmap, BitmapPixelFormat.Bgra8, BitmapAlphaMode.Ignore);
byte[] colorBytes = new byte[bitmap.PixelWidth * bitmap.PixelHeight * 4];
bitmap.CopyToBuffer(colorBytes.AsBuffer());
Vector3[] depthPoints = new Vector3[this.mapPoints.Length];
coordinateMapper.UnprojectPoints(this.mapPoints, this.frames[MediaFrameSourceKind.Color].CoordinateSystem, depthPoints);
// resize image 1920x1080 -> 480x270 and apply depth filter
byte[] resizedGrayColorBytes = new byte[240 * 135];
int row = 0;
int at = 0;
for (int idx = 0; idx < resizedGrayColorBytes.Length; ++idx)
{
var depth = depthPoints[idx];
// get depth bound for pixel idx
float depthBound = 10.0f;
if (float.IsNaN(depth.X))
{
depthBound = 0.0f;
}
else
{
for (int i = 0; i < this.depthMap.Count; ++i)
{
if (this.depthMap[i].xMin < depth.X && depth.X < this.depthMap[i].xMax)
{
depthBound = this.depthMap[i].F(depth.X);
break;
}
}
}
// get color of pixel idx
// topLeft : at; topRight : at + strideX - 4;
// bottomLeft : at + rgbWidth * (strideY - 1); bottomRight : at + rgbWidth * (strideY - 1) + strideX - 4;
float bgr = 255;
if (depth.Z < depthBound)
{
bgr =
(Convert.ToInt16(colorBytes[at] + colorBytes[at + 28] + colorBytes[at + 53760] + colorBytes[at + 53788]
+ colorBytes[at + 1] + colorBytes[at + 29] + colorBytes[at + 53761] + colorBytes[at + 53789]
+ colorBytes[at + 2] + colorBytes[at + 30] + colorBytes[at + 53762] + colorBytes[at + 53790])) * 0.0833333f;
}
resizedGrayColorBytes[idx] = BitConverter.GetBytes(Convert.ToInt32(bgr))[0];
// iterate
at += 32;
if (at - row * 7680 > 7648) // at - row * rgbWidth > rgbWidth - strideX
{
row += 8;
at = row * 7680;
}
}
var faces = this.cascadeClassifier.DetectMultiScale(resizedGrayColorBytes, 240, 135);
// debug image (optional)
//byte[] dbg = new byte[240 * 135 + 4];
//Buffer.BlockCopy(BitConverter.GetBytes(240), 0, dbg, 0, 2);
//Buffer.BlockCopy(BitConverter.GetBytes(135), 0, dbg, 2, 2);
//Buffer.BlockCopy(resizedGrayColorBytes, 0, dbg, 4, 32400);
//this.client.Publish("/kinect/face/debug", dbg);
// reset face found status
foreach (var log in this.faceLog)
{
log.SetFoundFalse();
}
// create byte array from each face
uint totalSize = 0;
List <byte[]> faceBytesList = new List <byte[]>();
at = 1;
int numDetectFaces = faces[0];
for (int j = 0; j < numDetectFaces; ++j)
{
// parse result from C++
uint xj = Convert.ToUInt32(faces[at++]) * 8;
uint yj = Convert.ToUInt32(faces[at++]) * 8;
uint width = Convert.ToUInt32(faces[at++]) * 8;
// result is head + shoulder -> multiply 0.6 to get head only region
uint height = Convert.ToUInt32(Convert.ToInt32(faces[at++]) * 8 * 0.6);
// center crop image
xj += Convert.ToUInt32(width * 0.2);
width = Convert.ToUInt32(width * 0.6);
uint size = width * height * 3 + 20;
byte[] faceBytes = new byte[size];
totalSize += size;
// get face 3d position
var centerPoint = new Point(xj + Convert.ToUInt32(width * 0.5), yj + Convert.ToUInt32(height * 0.5));
var positionVector = coordinateMapper.UnprojectPoint(centerPoint, this.frames[MediaFrameSourceKind.Color].CoordinateSystem);
// get likely face id from face position
int likelyEnum = -1;
float likeliness = float.MaxValue;
for (int i = 0; i < this.faceLog.Count; ++i)
{
if (this.faceLog[i].isTracked && !this.faceLog[i].foundInThisFrame)
{
var dist = Vector3.Distance(positionVector, this.faceLog[i].facePosition);
if (dist < 0.3 && dist < likeliness) // it is unlikely for a face to jump 30cm between frames
{
likelyEnum = i;
likeliness = dist;
}
}
}
if (likelyEnum < 0) // if no likely face was found
{
for (int i = 0; i < this.faceLog.Count; ++i)
{
if (!this.faceLog[i].isTracked) // a new track is registered (will switch to isTracked when called Update)
{
likelyEnum = i;
break;
}
}
}
if (likelyEnum < 0) // trackable number of faces already occupied, cannot track new face
{
continue; // id will be free once existing track is lost
}
this.faceLog[likelyEnum].Update(positionVector);
// first 4 bytes is size of face image
Array.Copy(BitConverter.GetBytes(width), 0, faceBytes, 0, 2);
Array.Copy(BitConverter.GetBytes(height), 0, faceBytes, 2, 2);
// next 12 bytes is 3d position of face
var position = new float[] { positionVector.X, positionVector.Y, positionVector.Z };
Buffer.BlockCopy(position, 0, faceBytes, 4, 12);
// next 4 bytes is face id
Buffer.BlockCopy(BitConverter.GetBytes(this.faceLog[likelyEnum].id), 0, faceBytes, 16, 4);
// copy rgb image
for (int y = 0; y < height; ++y)
{
var srcIdx = Convert.ToInt32(((y + yj) * colorDesc.Width + xj) * 4);
var destIdx = Convert.ToInt32((y * width) * 3 + 20);
for (int x = 0; x < width; ++x)
{
Buffer.BlockCopy(colorBytes, srcIdx + x * 4, faceBytes, destIdx + x * 3, 3);
}
}
faceBytesList.Add(faceBytes);
}
// for faces that were not found in current frame, release track state
foreach (var log in this.faceLog)
{
if (log.isTracked && !log.foundInThisFrame)
{
++log.lostTrackCount;
// get fps, note, clock is always running when frame is being captured
int fps = Convert.ToInt32(kinectFrameCount / this.appClock.Elapsed.TotalSeconds);
if (log.lostTrackCount > 2 * fps) // lost for two seconds
{
log.Free(this.nextReservedFaceId);
++this.nextReservedFaceId;
}
}
}
// concatenate byte arrays to send (post-processed as totalSize not known in first foreach)
int head = 1; // first 1 byte is number of faces
byte[] bytes = new byte[totalSize + 1];
Array.Copy(BitConverter.GetBytes(faceBytesList.Count), 0, bytes, 0, head);
foreach (byte[] faceByte in faceBytesList)
{
Array.Copy(faceByte, 0, bytes, head, faceByte.Length);
head += faceByte.Length;
}
this.client.Publish("/kinect/detected/face", bytes);
++this.kinectFrameCount;
bitmap.Dispose();
coordinateMapper.Dispose();
this.frames[MediaFrameSourceKind.Color].Dispose();
this.frames[MediaFrameSourceKind.Depth].Dispose();
this.frames[MediaFrameSourceKind.Color] = null;
this.frames[MediaFrameSourceKind.Depth] = null;
}
} catch (Exception ex) {
// TODO
} finally {
frameProcessingSemaphore.Release();
}
}
private void FrameReader_FrameArrived(MediaFrameReader sender, MediaFrameArrivedEventArgs args)
{
if (!frameProcessingSemaphore.Wait(0))
{
return;
}
try {
var frame = sender.TryAcquireLatestFrame();
if (frame != null)
{
this.frames[frame.SourceKind] = frame;
}
if (this.frames[MediaFrameSourceKind.Color] != null && this.frames[MediaFrameSourceKind.Depth] != null)
{
var colorDesc = this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.SoftwareBitmap.LockBuffer(BitmapBufferAccessMode.Read).GetPlaneDescription(0);
var depthDesc = this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.SoftwareBitmap.LockBuffer(BitmapBufferAccessMode.Read).GetPlaneDescription(0);
// get points in 3d space
DepthCorrelatedCoordinateMapper coordinateMapper = this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.DepthMediaFrame.TryCreateCoordinateMapper(
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics, this.frames[MediaFrameSourceKind.Color].CoordinateSystem);
// get camera intrinsics info
var cameraInfo = new float[] {
// this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.CameraIntrinsics.FocalLength.X,
// this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.CameraIntrinsics.FocalLength.Y,
// this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.CameraIntrinsics.PrincipalPoint.X,
// this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.CameraIntrinsics.PrincipalPoint.Y
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics.FocalLength.X * 3.0f,
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics.FocalLength.Y * 3.0f,
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics.PrincipalPoint.X / 3.0f,
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics.PrincipalPoint.Y / 3.0f
};
// get color information
var bitmap = SoftwareBitmap.Convert(this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.SoftwareBitmap, BitmapPixelFormat.Bgra8, BitmapAlphaMode.Ignore);
byte[] colorBytes = new byte[bitmap.PixelWidth * bitmap.PixelHeight * 4];
bitmap.CopyToBuffer(colorBytes.AsBuffer());
// get time info
var time = this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.FrameReference.SystemRelativeTime;
// map depth to color
// we will only create a reduced size map as original is too large : 1920 * 1080 -> 640 * 360
Vector3[] points = new Vector3[this.colorPoints.Length];
coordinateMapper.UnprojectPoints(this.colorPoints, this.frames[MediaFrameSourceKind.Color].CoordinateSystem, points);
// stream point clouds
byte[] streamBytes = new byte[points.Length * 16];
Parallel.ForEach(System.Collections.Concurrent.Partitioner.Create(0, points.Length),
(range) => {
int j = range.Item1 * 16;
for (int i = range.Item1; i < range.Item2; ++i)
{
var values = new float[] { points[i].X, points[i].Y, points[i].Z };
Buffer.BlockCopy(values, 0, streamBytes, j, 12); j += 12;
Buffer.BlockCopy(colorBytes, Convert.ToInt32((this.colorPoints[i].Y * colorDesc.Width + this.colorPoints[i].X) * 4), streamBytes, j, 4); j += 4;
}
});
this.client.Publish("/kinect/stream/points", streamBytes);
// stream camera intrinsics
byte[] camIntr = new byte[16];
Buffer.BlockCopy(cameraInfo, 0, camIntr, 0, 16);
this.client.Publish("/kinect/stream/camerainfo", camIntr);
// other requested queues (only one is processed at each frame)
if (this.sendImageFlag) // send full image if requested (full image should usually not be requested)
{
byte[] bgrColorBytes = new byte[colorDesc.Width * colorDesc.Height * 3];
Parallel.ForEach(System.Collections.Concurrent.Partitioner.Create(0, colorDesc.Height),
(range) => {
for (int i = range.Item1; i < range.Item2; ++i)
{
int srcIdx = i * colorDesc.Width * 4;
int destIdx = i * colorDesc.Width * 3;
for (int x = 0; x < colorDesc.Width; ++x)
{
Buffer.BlockCopy(colorBytes, srcIdx + x * 4, bgrColorBytes, destIdx + x * 3, 3);
}
}
});
this.client.Publish("/kinect/stream/image", bgrColorBytes);
this.sendImageFlag = false;
}
else if (this.sendImageCenters) // send image centers if requested
// get image centers from center pixels
{
Vector3[] positionVectors = new Vector3[this.centersInPixel.Length];
coordinateMapper.UnprojectPoints(this.centersInPixel, this.frames[MediaFrameSourceKind.Color].CoordinateSystem, positionVectors);
// Vector3 -> float[]
int numResults = positionVectors.Length;
float[] positions = new float[positionVectors.Length * 3];
for (int i = 0; i < positionVectors.Length; ++i)
{
positionVectors[i].CopyTo(positions, i * 3);
}
// float[] -> byte[]
byte[] positionBytes = new byte[positions.Length * 4 + 2];
Buffer.BlockCopy(BitConverter.GetBytes(numResults), 0, positionBytes, 0, 2);
Buffer.BlockCopy(positions, 0, positionBytes, 2, positions.Length * 4);
this.client.Publish("/kinect/stream/centers", positionBytes);
this.sendImageCenters = false;
}
#if PRINT_STATUS_MESSAGE
++this.kinectFrameCount;
#endif
bitmap.Dispose();
coordinateMapper.Dispose();
this.frames[MediaFrameSourceKind.Color].Dispose();
this.frames[MediaFrameSourceKind.Depth].Dispose();
this.frames[MediaFrameSourceKind.Color] = null;
this.frames[MediaFrameSourceKind.Depth] = null;
}
} catch (Exception ex) {
// TODO
} finally {
frameProcessingSemaphore.Release();
}
}
private void FrameReader_FrameArrived(MediaFrameReader sender, MediaFrameArrivedEventArgs args)
{
if (!frameProcessingSemaphore.Wait(0))
{
return;
}
try {
var frame = sender.TryAcquireLatestFrame();
if (frame != null)
{
this.frames[frame.SourceKind] = frame;
}
if (this.frames[MediaFrameSourceKind.Color] != null && this.frames[MediaFrameSourceKind.Depth] != null)
{
var colorDesc = this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.SoftwareBitmap.LockBuffer(BitmapBufferAccessMode.Read).GetPlaneDescription(0);
var depthDesc = this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.SoftwareBitmap.LockBuffer(BitmapBufferAccessMode.Read).GetPlaneDescription(0);
// get points in 3d space
DepthCorrelatedCoordinateMapper coordinateMapper = this.frames[MediaFrameSourceKind.Depth].VideoMediaFrame.DepthMediaFrame.TryCreateCoordinateMapper(
this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.CameraIntrinsics, this.frames[MediaFrameSourceKind.Color].CoordinateSystem);
// get color information
var bitmap = SoftwareBitmap.Convert(this.frames[MediaFrameSourceKind.Color].VideoMediaFrame.SoftwareBitmap, BitmapPixelFormat.Bgra8, BitmapAlphaMode.Ignore);
byte[] colorBytes = new byte[bitmap.PixelWidth * bitmap.PixelHeight * 4];
bitmap.CopyToBuffer(colorBytes.AsBuffer());
Vector3[] depthPoints = new Vector3[this.mapPoints.Length];
coordinateMapper.UnprojectPoints(this.mapPoints, this.frames[MediaFrameSourceKind.Color].CoordinateSystem, depthPoints);
// resize image 1920x1080 -> 480x270 and apply depth filter
byte[] resizedGrayColorBytes = new byte[240 * 135];
int row = 0;
int at = 0;
for (int idx = 0; idx < resizedGrayColorBytes.Length; ++idx)
{
var depth = depthPoints[idx];
// get depth bound for pixel idx
float depthBound = 10.0f;
if (float.IsNaN(depth.X))
{
depthBound = 0.0f;
}
else
{
for (int i = 0; i < this.depthMap.Count; ++i)
{
if (this.depthMap[i].xMin < depth.X && depth.X < this.depthMap[i].xMax)
{
depthBound = this.depthMap[i].F(depth.X);
break;
}
}
}
// get color of pixel idx
// topLeft : at; topRight : at + strideX - 4;
// bottomLeft : at + rgbWidth * (strideY - 1); bottomRight : at + rgbWidth * (strideY - 1) + strideX - 4;
float bgr = 255;
if (depth.Z < depthBound)
{
bgr =
(Convert.ToInt16(colorBytes[at] + colorBytes[at + 28] + colorBytes[at + 53760] + colorBytes[at + 53788]
+ colorBytes[at + 1] + colorBytes[at + 29] + colorBytes[at + 53761] + colorBytes[at + 53789]
+ colorBytes[at + 2] + colorBytes[at + 30] + colorBytes[at + 53762] + colorBytes[at + 53790])) * 0.0833333f;
}
resizedGrayColorBytes[idx] = BitConverter.GetBytes(Convert.ToInt32(bgr))[0];
// iterate
at += 32;
if (at - row * 7680 > 7648) // at - row * rgbWidth > rgbWidth - strideX
{
row += 8;
at = row * 7680;
}
}
var faces = this.cascadeClassifier.DetectMultiScale(resizedGrayColorBytes, 240, 135);
// debug image (optional)
//byte[] dbg = new byte[240 * 135 + 4];
//Buffer.BlockCopy(BitConverter.GetBytes(240), 0, dbg, 0, 2);
//Buffer.BlockCopy(BitConverter.GetBytes(135), 0, dbg, 2, 2);
//Buffer.BlockCopy(resizedGrayColorBytes, 0, dbg, 4, 32400);
//this.client.Publish("/kinect/face/debug", dbg);
#if TRACKING_ON
// reset face found status
foreach (var log in this.faceLog)
{
log.SetFoundFalse();
}
// parse data from cascadeClassifier
List <Vector3> facePositionList = new List <Vector3>();
List <Tuple <uint, uint, uint, uint> > faceBoundsList = new List <Tuple <uint, uint, uint, uint> >();
at = 1;
int numDetectFaces = faces[0];
for (int j = 0; j < numDetectFaces; ++j)
{
// parse result from C++
uint xj = Convert.ToUInt32(faces[at++]) * 8;
uint yj = Convert.ToUInt32(faces[at++]) * 8;
uint width = Convert.ToUInt32(faces[at++]) * 8;
// result is head + shoulder -> multiply 0.6 to get head only region
uint height = Convert.ToUInt32(Convert.ToInt32(faces[at++]) * 8 * 0.6);
// center crop image
xj += Convert.ToUInt32(width * 0.2);
width = Convert.ToUInt32(width * 0.6);
// get face 3d position
var centerPoint = new Point(xj + Convert.ToUInt32(width * 0.5), yj + Convert.ToUInt32(height * 0.5));
var positionVector = coordinateMapper.UnprojectPoint(centerPoint, this.frames[MediaFrameSourceKind.Color].CoordinateSystem);
facePositionList.Add(positionVector);
faceBoundsList.Add(new Tuple <uint, uint, uint, uint>(xj, yj, width, height));
}
// find face in current frame that matches log (only one face is linked to each log)
int[] faceCorrespondingLog = Enumerable.Repeat(-1, facePositionList.Count).ToArray();
float[] faceCorrespondenceScore = Enumerable.Repeat(float.MaxValue, facePositionList.Count).ToArray();
for (int i = 0; i < this.faceLog.Count; ++i)
{
if (!faceLog[i].isTracked)
{
continue; // log is currently not used
}
int likelyEnum = -1;
float likeliness = float.MaxValue;
for (int j = 0; j < facePositionList.Count; ++j)
{
var dist = Vector3.Distance(facePositionList[j], this.faceLog[i].facePosition);
if (dist < 0.3 && dist < likeliness) // it is unlikely for a face to jump 30cm between frames
{
likelyEnum = j;
likeliness = dist;
}
}
if (likelyEnum >= 0 && likeliness < faceCorrespondenceScore[likelyEnum])
{
if (faceCorrespondingLog[likelyEnum] >= 0)
{
this.faceLog[faceCorrespondingLog[likelyEnum]].foundInThisFrame = false; // last log was not right match, undo match
}
faceCorrespondingLog[likelyEnum] = i;
this.faceLog[i].foundInThisFrame = true; // this log is now matched with face
}
}
// check faceCorrespondingLog and create byte array from each face
uint totalSize = 0;
List <byte[]> faceBytesList = new List <byte[]>();
for (int j = 0; j < faceCorrespondingLog.Length; ++j)
{
int likelyEnum = -1;
Vector3 positionVector = facePositionList[j];
if (faceCorrespondingLog[j] < 0) // corresponding log was not yet found
// find likely face log from logs
{
float likeliness = float.MaxValue;
for (int i = 0; i < this.faceLog.Count; ++i)
{
if (this.faceLog[i].isTracked)
{
var dist = Vector3.Distance(positionVector, this.faceLog[i].facePosition);
if (dist < 0.3 && dist < likeliness) // it is unlikely for a face to jump 30cm between frames
{
likelyEnum = i;
likeliness = dist;
}
}
}
if (likelyEnum >= 0 && this.faceLog[likelyEnum].foundInThisFrame)
{
continue; // detected face was somehow a duplicate of an existing region, ignore
}
if (likelyEnum < 0) // if no likely face was found
{
for (int i = 0; i < this.faceLog.Count; ++i)
{
if (!this.faceLog[i].isTracked) // a new track is registered (will switch to isTracked when called Update)
{
likelyEnum = i;
break;
}
}
}
if (likelyEnum < 0) // trackable number of faces already occupied, cannot track new face
{
continue; // id will be free once existing track is lost
}
}
else // corresponding log is already found
{
likelyEnum = faceCorrespondingLog[j];
}
this.faceLog[likelyEnum].Update(positionVector);
uint xj = faceBoundsList[j].Item1;
uint yj = faceBoundsList[j].Item2;
uint width = faceBoundsList[j].Item3;
uint height = faceBoundsList[j].Item4;
uint size = width * height * 3 + 20;
byte[] faceBytes = new byte[size];
totalSize += size;
// first 4 bytes is size of face image
Array.Copy(BitConverter.GetBytes(width), 0, faceBytes, 0, 2);
Array.Copy(BitConverter.GetBytes(height), 0, faceBytes, 2, 2);
// next 12 bytes is 3d position of face
var position = new float[] { positionVector.X, positionVector.Y, positionVector.Z };
Buffer.BlockCopy(position, 0, faceBytes, 4, 12);
// next 4 bytes is face id
Buffer.BlockCopy(BitConverter.GetBytes(this.faceLog[likelyEnum].id), 0, faceBytes, 16, 4);
// copy rgb image
for (int y = 0; y < height; ++y)
{
var srcIdx = Convert.ToInt32(((y + yj) * colorDesc.Width + xj) * 4);
var destIdx = Convert.ToInt32((y * width) * 3 + 20);
for (int x = 0; x < width; ++x)
{
Buffer.BlockCopy(colorBytes, srcIdx + x * 4, faceBytes, destIdx + x * 3, 3);
}
}
faceBytesList.Add(faceBytes);
}
// for faces that were not found in current frame, release track state
foreach (var log in this.faceLog)
{
if (log.isTracked && !log.foundInThisFrame)
{
++log.lostTrackCount;
// get fps, note, clock is always running when frame is being captured
int fps = Convert.ToInt32(kinectFrameCount / this.appClock.Elapsed.TotalSeconds);
if (log.lostTrackCount > 10 * fps) // lost for ten seconds
{
log.Free(this.nextReservedFaceId);
++this.nextReservedFaceId;
}
}
}
#else
// parse data from cascadeClassifier
uint totalSize = 0;
List <byte[]> faceBytesList = new List <byte[]>();
at = 1;
int numDetectFaces = faces[0];
for (int j = 0; j < numDetectFaces; ++j)
{
#if VALID_FACE_CHECK
// parse result from C++
uint xjRaw = Convert.ToUInt32(faces[at++]);
uint yjRaw = Convert.ToUInt32(faces[at++]);
uint widthRaw = Convert.ToUInt32(faces[at++]);
// result is head + shoulder -> multiply 0.6 to get head only region
uint heightRaw = Convert.ToUInt32(Convert.ToInt32(faces[at++]) * 0.6);
// center crop image
xjRaw += Convert.ToUInt32(widthRaw * 0.2);
widthRaw = Convert.ToUInt32(widthRaw * 0.6);
uint xj = xjRaw * 8;
uint yj = yjRaw * 8;
uint width = widthRaw * 8;
uint height = heightRaw * 8;
#else
// parse result from C++
uint xj = Convert.ToUInt32(faces[at++]) * 8;
uint yj = Convert.ToUInt32(faces[at++]) * 8;
uint width = Convert.ToUInt32(faces[at++]) * 8;
// result is head + shoulder -> multiply 0.6 to get head only region
uint height = Convert.ToUInt32(Convert.ToInt32(faces[at++]) * 8 * 0.6);
// center crop image
xj += Convert.ToUInt32(width * 0.2);
width = Convert.ToUInt32(width * 0.6);
#endif
// get face 3d position
var centerPoint = new Point(xj + Convert.ToUInt32(width * 0.5), yj + Convert.ToUInt32(height * 0.5));
var positionVector = coordinateMapper.UnprojectPoint(centerPoint, this.frames[MediaFrameSourceKind.Color].CoordinateSystem);
#if VALID_FACE_CHECK
// find y height
uint yjMax = yjRaw + heightRaw;
uint xjMax = xjRaw + widthRaw;
float threshold_z = positionVector.Z + 0.25f;
float minimumY = float.MaxValue;
float maximumY = float.MinValue;
for (uint y = yjRaw; y < yjMax; ++y)
{
int rowPoints = 0;
float averageY = 0.0f;
uint point = xjRaw + y * 240;
var pxy = depthPoints[point];
for (uint x = xjRaw; x < xjMax; ++x)
{
if (!float.IsNaN(pxy.Y) && !float.IsInfinity(pxy.Y) && (pxy.Z < threshold_z))
{
averageY += pxy.Y;
++rowPoints;
}
pxy = depthPoints[++point];
}
averageY /= rowPoints;
if (rowPoints != 0)
{
if (averageY < minimumY)
{
minimumY = averageY;
}
else if (averageY > maximumY)
{
maximumY = averageY;
}
}
}
if ((maximumY - minimumY) > 0.35 || (maximumY - minimumY) < 0.1) // unlikely a face
{
continue;
}
#endif
uint size = width * height * 3 + 20;
byte[] faceBytes = new byte[size];
totalSize += size;
// first 4 bytes is size of face image
Array.Copy(BitConverter.GetBytes(width), 0, faceBytes, 0, 2);
Array.Copy(BitConverter.GetBytes(height), 0, faceBytes, 2, 2);
// next 12 bytes is 3d position of face
var position = new float[] { positionVector.X, positionVector.Y, positionVector.Z };
Buffer.BlockCopy(position, 0, faceBytes, 4, 12);
// next 4 bytes is face id (dummy)
Buffer.BlockCopy(BitConverter.GetBytes(j), 0, faceBytes, 16, 4);
// copy rgb image
for (int y = 0; y < height; ++y)
{
var srcIdx = Convert.ToInt32(((y + yj) * colorDesc.Width + xj) * 4);
var destIdx = Convert.ToInt32((y * width) * 3 + 20);
for (int x = 0; x < width; ++x)
{
Buffer.BlockCopy(colorBytes, srcIdx + x * 4, faceBytes, destIdx + x * 3, 3);
}
}
faceBytesList.Add(faceBytes);
}
#endif
// concatenate byte arrays to send (post-processed as totalSize not known in first foreach)
int head = 1; // first 1 byte is number of faces
byte[] bytes = new byte[totalSize + 1];
Array.Copy(BitConverter.GetBytes(faceBytesList.Count), 0, bytes, 0, head);
foreach (byte[] faceByte in faceBytesList)
{
Array.Copy(faceByte, 0, bytes, head, faceByte.Length);
head += faceByte.Length;
}
this.client.Publish("/kinect/detected/face", bytes);
++this.kinectFrameCount;
bitmap.Dispose();
coordinateMapper.Dispose();
this.frames[MediaFrameSourceKind.Color].Dispose();
this.frames[MediaFrameSourceKind.Depth].Dispose();
this.frames[MediaFrameSourceKind.Color] = null;
this.frames[MediaFrameSourceKind.Depth] = null;
}
} catch (Exception ex) {
// TODO
} finally {
frameProcessingSemaphore.Release();
}
}
