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(); } }