diff --git a/LiveScanServer/KinectServer.cs b/LiveScanServer/KinectServer.cs index 13beede..eb30e02 100644 --- a/LiveScanServer/KinectServer.cs +++ b/LiveScanServer/KinectServer.cs @@ -277,10 +277,11 @@ namespace KinectServer } } - public void GetLatestFrame(List> lFramesRGB, List> lFramesVerts) + public void GetLatestFrame(List> lFramesRGB, List> lFramesVerts, List> lFramesBody) { lFramesRGB.Clear(); lFramesVerts.Clear(); + lFramesBody.Clear(); lock (oFrameRequestLock) { @@ -317,6 +318,7 @@ namespace KinectServer { lFramesRGB.Add(new List(lClientSockets[i].lFrameRGB)); lFramesVerts.Add(new List(lClientSockets[i].lFrameVerts)); + lFramesBody.Add(new List(lClientSockets[i].lBodies)); } } } diff --git a/LiveScanServer/KinectSocket.cs b/LiveScanServer/KinectSocket.cs index a091fe6..9d264a7 100644 --- a/LiveScanServer/KinectSocket.cs +++ b/LiveScanServer/KinectSocket.cs @@ -19,6 +19,7 @@ using System.Text; using System.Threading.Tasks; using System.Net.Sockets; + namespace KinectServer { public delegate void SocketChangedHandler(); @@ -40,6 +41,7 @@ namespace KinectServer public List lFrameRGB = new List(); public List lFrameVerts = new List(); + public List lBodies = new List(); public event SocketChangedHandler eChanged; @@ -150,6 +152,7 @@ namespace KinectServer { lFrameRGB.Clear(); lFrameVerts.Clear(); + lBodies.Clear(); int nToRead; byte[] buffer = new byte[1024]; @@ -160,7 +163,6 @@ namespace KinectServer return; } - oSocket.Receive(buffer, 4, SocketFlags.None); //string result = System.Text.Encoding.UTF8.GetString(buffer); @@ -185,16 +187,64 @@ namespace KinectServer nAlreadyRead += oSocket.Receive(buffer, nAlreadyRead, nToRead - nAlreadyRead, SocketFlags.None); } - int point_size = 3 + 3 * 4; - int n_vertices = nToRead / point_size; + //Receive depth and color data + int startIdx = 0; + + int n_vertices = BitConverter.ToInt32(buffer, startIdx); + startIdx += 4; for (int i = 0; i < n_vertices; i++) { for (int j = 0; j < 3; j++) { - lFrameRGB.Add(buffer[i * point_size + j]); - lFrameVerts.Add(BitConverter.ToSingle(buffer, i * point_size + j * 4 + 3)); + lFrameRGB.Add(buffer[startIdx++]); } + for (int j = 0; j < 3; j++) + { + lFrameVerts.Add(BitConverter.ToSingle(buffer, startIdx)); + startIdx += 4; + } + } + + //Receive body data + int nBodies = BitConverter.ToInt32(buffer, startIdx); + startIdx += 4; + for (int i = 0; i < nBodies; i++) + { + Body tempBody = new Body(); + tempBody.bTracked = BitConverter.ToBoolean(buffer, startIdx++); + int nJoints = BitConverter.ToInt32(buffer, startIdx); + startIdx += 4; + + tempBody.lJoints = new List(nJoints); + tempBody.lJointsInColorSpace = new List(nJoints); + + for (int j = 0; j < nJoints; j++) + { + Joint tempJoint = new Joint(); + Point2f tempPoint = new Point2f(); + + tempJoint.jointType = (JointType)BitConverter.ToInt32(buffer, startIdx); + startIdx += 4; + tempJoint.trackingState = (TrackingState)BitConverter.ToInt32(buffer, startIdx); + startIdx += 4; + tempJoint.position.X = BitConverter.ToSingle(buffer, startIdx); + startIdx += 4; + tempJoint.position.Y = BitConverter.ToSingle(buffer, startIdx); + startIdx += 4; + tempJoint.position.Z = BitConverter.ToSingle(buffer, startIdx); + startIdx += 4; + + tempPoint.X = BitConverter.ToSingle(buffer, startIdx); + startIdx += 4; + tempPoint.Y = BitConverter.ToSingle(buffer, startIdx); + startIdx += 4; + + tempBody.lJoints.Add(tempJoint); + tempBody.lJointsInColorSpace.Add(tempPoint); + } + + lBodies.Add(tempBody); } } diff --git a/LiveScanServer/MainWindowForm.cs b/LiveScanServer/MainWindowForm.cs index 432cc96..968dd08 100644 --- a/LiveScanServer/MainWindowForm.cs +++ b/LiveScanServer/MainWindowForm.cs @@ -31,6 +31,8 @@ using System.Net; using System.Net.Sockets; using System.Timers; +using System.Diagnostics; + namespace KinectServer { @@ -48,6 +50,8 @@ namespace KinectServer List lAllColors = new List(); //Sensor poses from all of the sensors List lAllCameraPoses = new List(); + //Body data from all of the sensors + List lAllBodies = new List(); bool bServerRunning = false; bool bRecording = false; @@ -161,6 +165,7 @@ namespace KinectServer oOpenGLWindow.vertices = lAllVertices; oOpenGLWindow.colors = lAllColors; oOpenGLWindow.cameraPoses = lAllCameraPoses; + oOpenGLWindow.bodies = lAllBodies; oOpenGLWindow.settings = oSettings; } oOpenGLWindow.Run(); @@ -245,26 +250,30 @@ namespace KinectServer { List> lFramesRGB = new List>(); List> lFramesVerts = new List>(); + List> lFramesBody = new List>(); BackgroundWorker worker = (BackgroundWorker)sender; while (!worker.CancellationPending) { Thread.Sleep(1); - oServer.GetLatestFrame(lFramesRGB, lFramesVerts); + oServer.GetLatestFrame(lFramesRGB, lFramesVerts, lFramesBody); //Update the vertex and color lists that are common between this class and the OpenGLWindow. lock (lAllVertices) { lAllVertices.Clear(); lAllColors.Clear(); + lAllBodies.Clear(); lAllCameraPoses.Clear(); for (int i = 0; i < lFramesRGB.Count; i++) { lAllVertices.AddRange(lFramesVerts[i]); lAllColors.AddRange(lFramesRGB[i]); - lAllCameraPoses.Add(oServer.lCameraPoses[i]); + lAllBodies.AddRange(lFramesBody[i]); } + + lAllCameraPoses.AddRange(oServer.lCameraPoses); } //Notes the fact that a new frame was downloaded, this is used to estimate the FPS. @@ -285,7 +294,8 @@ namespace KinectServer //Download a frame from each client. List> lAllFrameVertices = new List>(); List> lAllFrameColors = new List>(); - oServer.GetLatestFrame(lAllFrameColors, lAllFrameVertices); + List> lAllFrameBody = new List>(); + oServer.GetLatestFrame(lAllFrameColors, lAllFrameVertices, lAllFrameBody); //Initialize containers for the poses. List Rs = new List(); diff --git a/LiveScanServer/OpenGLWindow.cs b/LiveScanServer/OpenGLWindow.cs index 220a086..2ce54e2 100644 --- a/LiveScanServer/OpenGLWindow.cs +++ b/LiveScanServer/OpenGLWindow.cs @@ -19,6 +19,8 @@ using System.Text; using System.Threading.Tasks; using System.Globalization; +using System.Diagnostics; + using OpenTK; using OpenTK.Graphics; using OpenTK.Input; @@ -58,6 +60,7 @@ namespace KinectServer public List vertices = new List(); public List colors = new List(); public List cameraPoses = new List(); + public List bodies = new List(); public KinectSettings settings = new KinectSettings(); DateTime tFPSUpdateTimer = DateTime.Now; @@ -298,6 +301,7 @@ namespace KinectServer LineCount += settings.lMarkerPoses.Count * 3; //cameras LineCount += cameraPoses.Count * 3; + LineCount += 24 * bodies.Count; VBO = new VertexC4ubV3f[PointCount + 2 * LineCount]; @@ -322,6 +326,7 @@ namespace KinectServer { iCurLineCount += AddCamera(PointCount + 2 * iCurLineCount, cameraPoses[i]); } + iCurLineCount += AddBodies(PointCount + 2 * iCurLineCount); } } } @@ -544,6 +549,68 @@ namespace KinectServer return nLinesBeingAdded; } + private int AddBone(int bodyIdx, JointType jointType0, JointType jointType1, int startIdx) + { + Point3f joint0 = bodies[bodyIdx].lJoints[(int)jointType0].position; + Point3f joint1 = bodies[bodyIdx].lJoints[(int)jointType1].position; + AddLine(startIdx, joint0.X, joint0.Y, joint0.Z, joint1.X, joint1.Y, joint1.Z); + return 2; + } + + private int AddBodies(int startIdx) + { + int nLinesToAdd = 24 * bodies.Count; + int nPointsToAdd = nLinesToAdd * 2; + + for (int i = startIdx; i < startIdx + nPointsToAdd; i++) + { + VBO[i].R = 0; + VBO[i].G = 255; + VBO[i].B = 0; + VBO[i].A = 0; + } + + int n = 0; + + for (int bodyIdx = 0; bodyIdx < bodies.Count; bodyIdx++) + { + //Torso + n += AddBone(bodyIdx, JointType.JointType_Head, JointType.JointType_Neck, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_Neck, JointType.JointType_SpineShoulder, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_SpineShoulder, JointType.JointType_SpineMid, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_SpineMid, JointType.JointType_SpineBase, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_SpineShoulder, JointType.JointType_ShoulderRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_SpineShoulder, JointType.JointType_ShoulderLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_SpineBase, JointType.JointType_HipRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_SpineBase, JointType.JointType_HipLeft, startIdx + n); + + // Right Arm + n += AddBone(bodyIdx, JointType.JointType_ShoulderRight, JointType.JointType_ElbowRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_ElbowRight, JointType.JointType_WristRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_WristRight, JointType.JointType_HandRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_HandRight, JointType.JointType_HandTipRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_WristRight, JointType.JointType_ThumbRight, startIdx + n); + + // Left Arm + n += AddBone(bodyIdx, JointType.JointType_ShoulderLeft, JointType.JointType_ElbowLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_ElbowLeft, JointType.JointType_WristLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_WristLeft, JointType.JointType_HandLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_HandLeft, JointType.JointType_HandTipLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_WristLeft, JointType.JointType_ThumbLeft, startIdx + n); + + // Right Leg + n += AddBone(bodyIdx, JointType.JointType_HipRight, JointType.JointType_KneeRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_KneeRight, JointType.JointType_AnkleRight, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_AnkleRight, JointType.JointType_FootRight, startIdx + n); + + // Left Leg + n += AddBone(bodyIdx, JointType.JointType_HipLeft, JointType.JointType_KneeLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_KneeLeft, JointType.JointType_AnkleLeft, startIdx + n); + n += AddBone(bodyIdx, JointType.JointType_AnkleLeft, JointType.JointType_FootLeft, startIdx + n); + } + return nLinesToAdd; + } + private void AddLine(int startIdx, float x0, float y0, float z0, float x1, float y1, float z1) { diff --git a/LiveScanServer/Utils.cs b/LiveScanServer/Utils.cs index 9c5ae78..f3e600e 100644 --- a/LiveScanServer/Utils.cs +++ b/LiveScanServer/Utils.cs @@ -13,9 +13,23 @@ // year={2015}, // } using System; +using System.Collections.Generic; namespace KinectServer { + public struct Point2f + { + public float X; + public float Y; + } + + public struct Point3f + { + public float X; + public float Y; + public float Z; + } + [Serializable] public class AffineTransform { @@ -92,4 +106,55 @@ namespace KinectServer private float[] r = new float[3]; } + + public enum TrackingState + { + TrackingState_NotTracked = 0, + TrackingState_Inferred = 1, + TrackingState_Tracked = 2 + } + + public enum JointType + { + JointType_SpineBase = 0, + JointType_SpineMid = 1, + JointType_Neck = 2, + JointType_Head = 3, + JointType_ShoulderLeft = 4, + JointType_ElbowLeft = 5, + JointType_WristLeft = 6, + JointType_HandLeft = 7, + JointType_ShoulderRight = 8, + JointType_ElbowRight = 9, + JointType_WristRight = 10, + JointType_HandRight = 11, + JointType_HipLeft = 12, + JointType_KneeLeft = 13, + JointType_AnkleLeft = 14, + JointType_FootLeft = 15, + JointType_HipRight = 16, + JointType_KneeRight = 17, + JointType_AnkleRight = 18, + JointType_FootRight = 19, + JointType_SpineShoulder = 20, + JointType_HandTipLeft = 21, + JointType_ThumbLeft = 22, + JointType_HandTipRight = 23, + JointType_ThumbRight = 24, + JointType_Count = (JointType_ThumbRight + 1) + } + + public struct Joint + { + public Point3f position; + public JointType jointType; + public TrackingState trackingState; + } + + public struct Body + { + public bool bTracked; + public List lJoints; + public List lJointsInColorSpace; + } } \ No newline at end of file diff --git a/include/LiveScanClient/iCapture.h b/include/LiveScanClient/iCapture.h index 5d55710..736f658 100644 --- a/include/LiveScanClient/iCapture.h +++ b/include/LiveScanClient/iCapture.h @@ -15,6 +15,14 @@ #pragma once #include "utils.h" +#include "Kinect.h" + +struct Body +{ + bool bTracked; + std::vector vJoints; + std::vector vJointsInColorSpace; +}; class ICapture { @@ -36,4 +44,5 @@ public: UINT16 *pDepth; RGB *pColorRGBX; + std::vector vBodies; }; \ No newline at end of file diff --git a/include/LiveScanClient/imageRenderer.h b/include/LiveScanClient/imageRenderer.h index 65282d6..95bef9d 100644 --- a/include/LiveScanClient/imageRenderer.h +++ b/include/LiveScanClient/imageRenderer.h @@ -9,6 +9,7 @@ #pragma once #include +#include "iCapture.h" class ImageRenderer { @@ -41,7 +42,7 @@ public: /// image data in RGBX format /// size of image data in bytes /// indicates success or failure - HRESULT Draw(BYTE* pImage, unsigned long cbImage); + HRESULT Draw(BYTE* pImage, unsigned long cbImage, std::vector &bodies); private: HWND m_hWnd; @@ -66,4 +67,15 @@ private: /// Dispose of Direct2d resources /// void DiscardResources(); + + void DrawBody(Body &body); + void DrawBone(Body &body, JointType joint0, JointType joint1); + + ID2D1SolidColorBrush* m_pBrushJointTracked; + ID2D1SolidColorBrush* m_pBrushJointInferred; + ID2D1SolidColorBrush* m_pBrushBoneTracked; + ID2D1SolidColorBrush* m_pBrushBoneInferred; + ID2D1SolidColorBrush* m_pBrushHandClosed; + ID2D1SolidColorBrush* m_pBrushHandOpen; + ID2D1SolidColorBrush* m_pBrushHandLasso; }; \ No newline at end of file diff --git a/include/LiveScanClient/kinectCapture.h b/include/LiveScanClient/kinectCapture.h index 3df9fcc..733a44a 100644 --- a/include/LiveScanClient/kinectCapture.h +++ b/include/LiveScanClient/kinectCapture.h @@ -35,4 +35,8 @@ private: ICoordinateMapper* pCoordinateMapper; IKinectSensor* pKinectSensor; IMultiSourceFrameReader* pMultiSourceFrameReader; + + void GetDepthFrame(IMultiSourceFrame* pMultiFrame); + void GetColorFrame(IMultiSourceFrame* pMultiFrame); + void GetBodyFrame(IMultiSourceFrame* pMultiFrame); }; diff --git a/include/LiveScanClient/liveScanClient.h b/include/LiveScanClient/liveScanClient.h index 5225097..9bc9842 100644 --- a/include/LiveScanClient/liveScanClient.h +++ b/include/LiveScanClient/liveScanClient.h @@ -57,6 +57,7 @@ private: std::vector m_vLastFrameVertices; std::vector m_vLastFrameRGB; + std::vector m_vLastFrameBody; std::vector> m_vGatheredVertices; std::vector> m_vGatheredRGBPoints; @@ -67,7 +68,8 @@ private: DWORD m_nFramesSinceUpdate; Point3f* m_pCameraSpaceCoordinates; - Point2f* m_pColorCoordinates; + Point2f* m_pColorCoordinatesOfDepth; + Point2f* m_pDepthCoordinatesOfColor; // Direct2D ImageRenderer* m_pDrawColor; @@ -81,8 +83,10 @@ private: bool SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce); void HandleSocket(); + void SendFrame(vector vertices, vector RGB, vector body); + void SocketThreadFunction(); - void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color); + void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies); void ShowFPS(); void ReadIPFromFile(); void WriteIPToFile(); diff --git a/src/LiveScanClient/imageRenderer.cpp b/src/LiveScanClient/imageRenderer.cpp index 6757457..72eae75 100644 --- a/src/LiveScanClient/imageRenderer.cpp +++ b/src/LiveScanClient/imageRenderer.cpp @@ -58,6 +58,16 @@ HRESULT ImageRenderer::EnsureResources() return hr; } + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(0.27f, 0.75f, 0.27f), &m_pBrushJointTracked); + + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Yellow, 1.0f), &m_pBrushJointInferred); + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Green, 1.0f), &m_pBrushBoneTracked); + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Gray, 1.0f), &m_pBrushBoneInferred); + + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Red, 0.5f), &m_pBrushHandClosed); + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Green, 0.5f), &m_pBrushHandOpen); + m_pRenderTarget->CreateSolidColorBrush(D2D1::ColorF(D2D1::ColorF::Blue, 0.5f), &m_pBrushHandLasso); + // Create a bitmap that we can copy image data into and then render to the target hr = m_pRenderTarget->CreateBitmap( size, @@ -121,8 +131,9 @@ HRESULT ImageRenderer::Initialize(HWND hWnd, ID2D1Factory* pD2DFactory, int sour /// /// image data in RGBX format /// size of image data in bytes +/// vector of bodies to draw /// indicates success or failure -HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage) +HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector &vBodies) { // incorrectly sized image data passed in if (cbImage < ((m_sourceHeight - 1) * m_sourceStride) + (m_sourceWidth * 4)) @@ -151,7 +162,15 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage) // Draw the bitmap stretched to the size of the window m_pRenderTarget->DrawBitmap(m_pBitmap); - + + for (int i = 0; i < vBodies.size(); i++) + { + if (vBodies[i].bTracked) + { + DrawBody(vBodies[i]); + } + } + hr = m_pRenderTarget->EndDraw(); // Device lost, need to recreate the render target @@ -163,4 +182,104 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage) } return hr; +} + +/// +/// Draws all the body to the associated hwnd, assumes that BeginDraw has been called +/// +/// body to be drawn +void ImageRenderer::DrawBody(Body &body) +{ + // Draw the bones + + // Torso + DrawBone(body, JointType_Head, JointType_Neck); + DrawBone(body, JointType_Neck, JointType_SpineShoulder); + DrawBone(body, JointType_SpineShoulder, JointType_SpineMid); + DrawBone(body, JointType_SpineMid, JointType_SpineBase); + DrawBone(body, JointType_SpineShoulder, JointType_ShoulderRight); + DrawBone(body, JointType_SpineShoulder, JointType_ShoulderLeft); + DrawBone(body, JointType_SpineBase, JointType_HipRight); + DrawBone(body, JointType_SpineBase, JointType_HipLeft); + + // Right Arm + DrawBone(body, JointType_ShoulderRight, JointType_ElbowRight); + DrawBone(body, JointType_ElbowRight, JointType_WristRight); + DrawBone(body, JointType_WristRight, JointType_HandRight); + DrawBone(body, JointType_HandRight, JointType_HandTipRight); + DrawBone(body, JointType_WristRight, JointType_ThumbRight); + + // Left Arm + DrawBone(body, JointType_ShoulderLeft, JointType_ElbowLeft); + DrawBone(body, JointType_ElbowLeft, JointType_WristLeft); + DrawBone(body, JointType_WristLeft, JointType_HandLeft); + DrawBone(body, JointType_HandLeft, JointType_HandTipLeft); + DrawBone(body, JointType_WristLeft, JointType_ThumbLeft); + + // Right Leg + DrawBone(body, JointType_HipRight, JointType_KneeRight); + DrawBone(body, JointType_KneeRight, JointType_AnkleRight); + DrawBone(body, JointType_AnkleRight, JointType_FootRight); + + // Left Leg + DrawBone(body, JointType_HipLeft, JointType_KneeLeft); + DrawBone(body, JointType_KneeLeft, JointType_AnkleLeft); + DrawBone(body, JointType_AnkleLeft, JointType_FootLeft); + + for (int i = 0; i < body.vJoints.size(); i++) + { + D2D1_POINT_2F tempPoint; + tempPoint.x = body.vJointsInColorSpace[i].X; + tempPoint.y = body.vJointsInColorSpace[i].Y; + + D2D1_ELLIPSE ellipse = D2D1::Ellipse(tempPoint, 6.0f, 6.0f); + + if (body.vJoints[i].TrackingState == TrackingState_Inferred) + { + m_pRenderTarget->FillEllipse(ellipse, m_pBrushJointInferred); + } + else if (body.vJoints[i].TrackingState == TrackingState_Tracked) + { + m_pRenderTarget->FillEllipse(ellipse, m_pBrushJointTracked); + } + } +} + +/// +/// Draws one bone of a body (joint to joint) +/// +/// body to be drawn +/// one joint of the bone to draw +/// other joint of the bone to draw +void ImageRenderer::DrawBone(Body &body, JointType joint0, JointType joint1) +{ + TrackingState joint0State = body.vJoints[joint0].TrackingState; + TrackingState joint1State = body.vJoints[joint1].TrackingState; + + // If we can't find either of these joints, exit + if ((joint0State == TrackingState_NotTracked) || (joint1State == TrackingState_NotTracked)) + { + return; + } + + // Don't draw if both points are inferred + if ((joint0State == TrackingState_Inferred) && (joint1State == TrackingState_Inferred)) + { + return; + } + + D2D1_POINT_2F joint0Point, joint1Point; + joint0Point.x = body.vJointsInColorSpace[joint0].X; + joint0Point.y = body.vJointsInColorSpace[joint0].Y; + joint1Point.x = body.vJointsInColorSpace[joint1].X; + joint1Point.y = body.vJointsInColorSpace[joint1].Y; + // We assume all drawn bones are inferred unless BOTH joints are tracked + if ((joint0State == TrackingState_Tracked) && (joint1State == TrackingState_Tracked)) + { + m_pRenderTarget->DrawLine(joint0Point, joint1Point, m_pBrushBoneTracked, 10.0f); + } + else + { + m_pRenderTarget->DrawLine(joint0Point, joint1Point, m_pBrushBoneInferred, 3); + } } \ No newline at end of file diff --git a/src/LiveScanClient/kinectCapture.cpp b/src/LiveScanClient/kinectCapture.cpp index c3ffa9f..651228b 100644 --- a/src/LiveScanClient/kinectCapture.cpp +++ b/src/LiveScanClient/kinectCapture.cpp @@ -47,7 +47,11 @@ bool KinectCapture::Initialize() if (SUCCEEDED(hr)) { - pKinectSensor->OpenMultiSourceFrameReader(FrameSourceTypes::FrameSourceTypes_Color | FrameSourceTypes::FrameSourceTypes_Depth, &pMultiSourceFrameReader); + pKinectSensor->OpenMultiSourceFrameReader(FrameSourceTypes::FrameSourceTypes_Color | + FrameSourceTypes::FrameSourceTypes_Depth | + FrameSourceTypes::FrameSourceTypes_Body | + FrameSourceTypes::FrameSourceTypes_BodyIndex, + &pMultiSourceFrameReader); } } @@ -90,54 +94,10 @@ bool KinectCapture::AcquireFrame() return false; } - //Depth frame - IDepthFrameReference* pDepthFrameReference = NULL; - IDepthFrame* pDepthFrame = NULL; - pMultiFrame->get_DepthFrameReference(&pDepthFrameReference); - hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); + GetDepthFrame(pMultiFrame); + GetColorFrame(pMultiFrame); + GetBodyFrame(pMultiFrame); - if (SUCCEEDED(hr)) - { - if (pDepth == NULL) - { - IFrameDescription* pFrameDescription = NULL; - hr = pDepthFrame->get_FrameDescription(&pFrameDescription); - pFrameDescription->get_Width(&nDepthFrameWidth); - pFrameDescription->get_Height(&nDepthFrameHeight); - pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth]; - SafeRelease(pFrameDescription); - } - - UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth; - hr = pDepthFrame->CopyFrameDataToArray(nBufferSize, pDepth); - } - SafeRelease(pDepthFrame); - SafeRelease(pDepthFrameReference); - - //Color frame - IColorFrameReference* pColorFrameReference = NULL; - IColorFrame* pColorFrame = NULL; - pMultiFrame->get_ColorFrameReference(&pColorFrameReference); - hr = pColorFrameReference->AcquireFrame(&pColorFrame); - - if (SUCCEEDED(hr)) - { - if (pColorRGBX == NULL) - { - IFrameDescription* pFrameDescription = NULL; - hr = pColorFrame->get_FrameDescription(&pFrameDescription); - hr = pFrameDescription->get_Width(&nColorFrameWidth); - hr = pFrameDescription->get_Height(&nColorFrameHeight); - pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight]; - SafeRelease(pFrameDescription); - } - - UINT nBufferSize = nColorFrameWidth * nColorFrameHeight * sizeof(RGB); - hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast(pColorRGBX), ColorImageFormat_Bgra); - } - - SafeRelease(pColorFrame); - SafeRelease(pColorFrameReference); return true; } @@ -160,4 +120,101 @@ void KinectCapture::MapDepthFrameToColorSpace(Point2f *pColorSpacePoints) void KinectCapture::MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints) { pCoordinateMapper->MapColorFrameToDepthSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (DepthSpacePoint*)pDepthSpacePoints);; +} + +void KinectCapture::GetDepthFrame(IMultiSourceFrame* pMultiFrame) +{ + IDepthFrameReference* pDepthFrameReference = NULL; + IDepthFrame* pDepthFrame = NULL; + pMultiFrame->get_DepthFrameReference(&pDepthFrameReference); + HRESULT hr = pDepthFrameReference->AcquireFrame(&pDepthFrame); + + if (SUCCEEDED(hr)) + { + if (pDepth == NULL) + { + IFrameDescription* pFrameDescription = NULL; + hr = pDepthFrame->get_FrameDescription(&pFrameDescription); + pFrameDescription->get_Width(&nDepthFrameWidth); + pFrameDescription->get_Height(&nDepthFrameHeight); + pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth]; + SafeRelease(pFrameDescription); + } + + UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth; + hr = pDepthFrame->CopyFrameDataToArray(nBufferSize, pDepth); + } + + SafeRelease(pDepthFrame); + SafeRelease(pDepthFrameReference); +} + +void KinectCapture::GetColorFrame(IMultiSourceFrame* pMultiFrame) +{ + IColorFrameReference* pColorFrameReference = NULL; + IColorFrame* pColorFrame = NULL; + pMultiFrame->get_ColorFrameReference(&pColorFrameReference); + HRESULT hr = pColorFrameReference->AcquireFrame(&pColorFrame); + + if (SUCCEEDED(hr)) + { + if (pColorRGBX == NULL) + { + IFrameDescription* pFrameDescription = NULL; + hr = pColorFrame->get_FrameDescription(&pFrameDescription); + hr = pFrameDescription->get_Width(&nColorFrameWidth); + hr = pFrameDescription->get_Height(&nColorFrameHeight); + pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight]; + SafeRelease(pFrameDescription); + } + + UINT nBufferSize = nColorFrameWidth * nColorFrameHeight * sizeof(RGB); + hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast(pColorRGBX), ColorImageFormat_Bgra); + } + + SafeRelease(pColorFrame); + SafeRelease(pColorFrameReference); +} + +void KinectCapture::GetBodyFrame(IMultiSourceFrame* pMultiFrame) +{ + IBodyFrameReference* pBodyFrameReference = NULL; + IBodyFrame* pBodyFrame = NULL; + pMultiFrame->get_BodyFrameReference(&pBodyFrameReference); + HRESULT hr = pBodyFrameReference->AcquireFrame(&pBodyFrame); + + + if (SUCCEEDED(hr)) + { + IBody* bodies[6] = { NULL }; + pBodyFrame->GetAndRefreshBodyData(6, bodies); + + vBodies = std::vector(BODY_COUNT); + for (int i = 0; i < 6; i++) + { + if (bodies[i]) + { + Joint joints[JointType_Count]; + BOOLEAN isTracked; + + bodies[i]->get_IsTracked(&isTracked); + bodies[i]->GetJoints(JointType_Count, joints); + + vBodies[i].vJoints.assign(joints, joints + JointType_Count); + vBodies[i].bTracked = isTracked; + vBodies[i].vJointsInColorSpace.resize(JointType_Count); + + for (int j = 0; j < JointType_Count; j++) + { + ColorSpacePoint tempPoint; + pCoordinateMapper->MapCameraPointToColorSpace(joints[j].Position, &tempPoint); + vBodies[i].vJointsInColorSpace[j].X = tempPoint.X; + vBodies[i].vJointsInColorSpace[j].Y = tempPoint.Y; + } + } + } + } + + SafeRelease(pBodyFrame); + SafeRelease(pBodyFrameReference); } \ No newline at end of file diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index 078e8fa..c4e4176 100644 --- a/src/LiveScanClient/liveScanClient.cpp +++ b/src/LiveScanClient/liveScanClient.cpp @@ -46,6 +46,8 @@ LiveScanClient::LiveScanClient() : m_pDrawColor(NULL), m_pDepthRGBX(NULL), m_pCameraSpaceCoordinates(NULL), + m_pColorCoordinatesOfDepth(NULL), + m_pDepthCoordinatesOfColor(NULL), m_bCalibrate(false), m_bFilter(false), m_bCaptureFrame(false), @@ -101,10 +103,16 @@ LiveScanClient::~LiveScanClient() m_pCameraSpaceCoordinates = NULL; } - if (m_pColorCoordinates) + if (m_pColorCoordinatesOfDepth) { - delete[] m_pColorCoordinates; - m_pColorCoordinates = NULL; + delete[] m_pColorCoordinatesOfDepth; + m_pColorCoordinatesOfDepth = NULL; + } + + if (m_pDepthCoordinatesOfColor) + { + delete[] m_pDepthCoordinatesOfColor; + m_pDepthCoordinatesOfColor = NULL; } if (m_pClientSocket) @@ -184,10 +192,10 @@ void LiveScanClient::UpdateFrame() return; pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates); - pCapture->MapDepthFrameToColorSpace(m_pColorCoordinates); + pCapture->MapDepthFrameToColorSpace(m_pColorCoordinatesOfDepth); { std::lock_guard lock(m_mSocketThreadMutex); - StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinates, pCapture->pColorRGBX); + StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinatesOfDepth, pCapture->pColorRGBX, pCapture->vBodies); if (m_bCaptureFrame) { @@ -267,7 +275,8 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; - m_pColorCoordinates = new Point2f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; + m_pColorCoordinatesOfDepth = new Point2f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; + m_pDepthCoordinatesOfColor = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; } else { @@ -354,17 +363,16 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight) { // Make sure we've received valid data - if (m_pDepthRGBX && pBuffer && (nWidth == pCapture->nDepthFrameWidth) && (nHeight == pCapture->nDepthFrameHeight)) + if (m_pDepthRGBX && m_pDepthCoordinatesOfColor && pBuffer && (nWidth == pCapture->nDepthFrameWidth) && (nHeight == pCapture->nDepthFrameHeight)) { // end pixel is start + width*height - 1 const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight); - Point2f *pDepthSpacePoints = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; - pCapture->MapColorFrameToDepthSpace(pDepthSpacePoints); + pCapture->MapColorFrameToDepthSpace(m_pDepthCoordinatesOfColor); for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++) { - Point2f depthPoint = pDepthSpacePoints[i]; + Point2f depthPoint = m_pDepthCoordinatesOfColor[i]; BYTE intensity = 0; if (depthPoint.X >= 0 && depthPoint.Y >= 0) @@ -380,9 +388,7 @@ void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight } // Draw the data with Direct2D - m_pDrawColor->Draw(reinterpret_cast(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB)); - - delete[] pDepthSpacePoints; + m_pDrawColor->Draw(reinterpret_cast(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies); } } @@ -392,7 +398,7 @@ void LiveScanClient::ProcessColor(RGB* pBuffer, int nWidth, int nHeight) if (pBuffer && (nWidth == pCapture->nColorFrameWidth) && (nHeight == pCapture->nColorFrameHeight)) { // Draw the data with Direct2D - m_pDrawColor->Draw(reinterpret_cast(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB)); + m_pDrawColor->Draw(reinterpret_cast(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies); } } @@ -502,29 +508,7 @@ void LiveScanClient::HandleSocket() if (m_vGatheredRGBPoints.size() > 0) { - int size = m_vGatheredRGBPoints[0].size() * (3 + 3 * sizeof(float)); - //m_pClientSocket->SendLine(to_string(size)); - m_pClientSocket->SendBytes((char*)&size, 4); - - char *to_send = new char[size]; - char *ptr1 = (char*)m_vGatheredRGBPoints[0].data(); - char *ptr2 = (char*)m_vGatheredVertices[0].data(); - int pos = 0, initial_pos; - for (unsigned int i = 0; i < m_vGatheredRGBPoints[0].size(); i++) - { - initial_pos = pos; - to_send[pos++] = m_vGatheredRGBPoints[0][i].rgbRed; - to_send[pos++] = m_vGatheredRGBPoints[0][i].rgbGreen; - to_send[pos++] = m_vGatheredRGBPoints[0][i].rgbBlue; - - - memcpy(to_send + pos, ptr2, sizeof(float)* 3); - ptr2 += sizeof(float)* 3; - pos += sizeof(float)* 3; - } - - m_pClientSocket->SendBytes(to_send, size); - delete[]to_send; + SendFrame(m_vGatheredVertices[0], m_vGatheredRGBPoints[0], m_vLastFrameBody); m_vGatheredRGBPoints.erase(m_vGatheredRGBPoints.begin(), m_vGatheredRGBPoints.begin() + 1); m_vGatheredVertices.erase(m_vGatheredVertices.begin(), m_vGatheredVertices.begin() + 1); @@ -541,28 +525,7 @@ void LiveScanClient::HandleSocket() byteToSend = MSG_LAST_FRAME; m_pClientSocket->SendBytes(&byteToSend, 1); - int size = m_vLastFrameRGB.size() * (3 + 3 * sizeof(float)); - //m_pClientSocket->SendLine(to_string(size)); - m_pClientSocket->SendBytes((char*)&size, 4); - - char *buffer = new char[size]; - char *ptr2 = (char*)m_vLastFrameVertices.data(); - int pos = 0, initial_pos; - for (unsigned int i = 0; i < m_vLastFrameRGB.size(); i++) - { - initial_pos = pos; - buffer[pos++] = m_vLastFrameRGB[i].rgbRed; - buffer[pos++] = m_vLastFrameRGB[i].rgbGreen; - buffer[pos++] = m_vLastFrameRGB[i].rgbBlue; - - - memcpy(buffer + pos, ptr2, sizeof(float)* 3); - ptr2 += sizeof(float)* 3; - pos += sizeof(float)* 3; - } - - m_pClientSocket->SendBytes(buffer, size); - delete[]buffer; + SendFrame(m_vLastFrameVertices, m_vLastFrameRGB, m_vLastFrameBody); } //receive calibration data else if (received[i] == MSG_RECEIVE_CALIBRATION) @@ -644,7 +607,83 @@ void LiveScanClient::HandleSocket() } } -void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color) +void LiveScanClient::SendFrame(vector vertices, vector RGB, vector body) +{ + int size = RGB.size() * (3 + 3 * sizeof(float)) + sizeof(int); + + vector buffer(size); + char *ptr2 = (char*)vertices.data(); + int pos = 0; + + int nVertices = RGB.size(); + memcpy(buffer.data() + pos, &nVertices, sizeof(nVertices)); + pos += sizeof(nVertices); + + for (unsigned int i = 0; i < RGB.size(); i++) + { + buffer[pos++] = RGB[i].rgbRed; + buffer[pos++] = RGB[i].rgbGreen; + buffer[pos++] = RGB[i].rgbBlue; + + + memcpy(buffer.data() + pos, ptr2, sizeof(float)* 3); + ptr2 += sizeof(float)* 3; + pos += sizeof(float)* 3; + } + + int nBodies = body.size(); + size += sizeof(nBodies); + for (int i = 0; i < nBodies; i++) + { + size += sizeof(body[i].bTracked); + int nJoints = body[i].vJoints.size(); + size += sizeof(nJoints); + size += nJoints * (3 * sizeof(float) + 2 * sizeof(int)); + size += nJoints * 2 * sizeof(float); + } + buffer.resize(size); + + memcpy(buffer.data() + pos, &nBodies, sizeof(nBodies)); + pos += sizeof(nBodies); + + for (int i = 0; i < nBodies; i++) + { + memcpy(buffer.data() + pos, &body[i].bTracked, sizeof(body[i].bTracked)); + pos += sizeof(body[i].bTracked); + + int nJoints = body[i].vJoints.size(); + memcpy(buffer.data() + pos, &nJoints, sizeof(nJoints)); + pos += sizeof(nJoints); + + for (int j = 0; j < nJoints; j++) + { + //Joint + memcpy(buffer.data() + pos, &body[i].vJoints[j].JointType, sizeof(JointType)); + pos += sizeof(JointType); + memcpy(buffer.data() + pos, &body[i].vJoints[j].TrackingState, sizeof(TrackingState)); + pos += sizeof(TrackingState); + //Joint position + memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.X, sizeof(float)); + pos += sizeof(float); + memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.Y, sizeof(float)); + pos += sizeof(float); + memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.Z, sizeof(float)); + pos += sizeof(float); + + //JointInColorSpace + memcpy(buffer.data() + pos, &body[i].vJointsInColorSpace[j].X, sizeof(float)); + pos += sizeof(float); + memcpy(buffer.data() + pos, &body[i].vJointsInColorSpace[j].Y, sizeof(float)); + pos += sizeof(float); + } + } + + m_pClientSocket->SendBytes((char*)&size, sizeof(int)); + m_pClientSocket->SendBytes(buffer.data(), size); + +} + +void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies) { std::vector goodVertices; std::vector goodColorPoints; @@ -675,8 +714,31 @@ void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color) } } + for (unsigned int i = 0; i < bodies.size(); i++) + { + for (unsigned int j = 0; j < bodies[i].vJoints.size(); j++) + { + if (calibration.bCalibrated) + { + bodies[i].vJoints[j].Position.X += calibration.worldT[0]; + bodies[i].vJoints[j].Position.Y += calibration.worldT[1]; + bodies[i].vJoints[j].Position.Z += calibration.worldT[2]; + + Point3f tempPoint(bodies[i].vJoints[j].Position.X, bodies[i].vJoints[j].Position.Y, bodies[i].vJoints[j].Position.Z); + + tempPoint = RotatePoint(tempPoint, calibration.worldR); + + bodies[i].vJoints[j].Position.X = tempPoint.X; + bodies[i].vJoints[j].Position.Y = tempPoint.Y; + bodies[i].vJoints[j].Position.Z = tempPoint.Z; + } + } + } + if (m_bFilter) filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold); + + m_vLastFrameBody = bodies; m_vLastFrameVertices = goodVertices; m_vLastFrameRGB = goodColorPoints; } @@ -732,4 +794,4 @@ void LiveScanClient::WriteIPToFile() GetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress, 20); file << lastUsedIPAddress; file.close(); -} \ No newline at end of file +}