storytold/LiveScan3D
2
0
Fork 0
mirror of https://github.com/storytold/LiveScan3D.git synced 2026-10-18 07:34:02 +00:00
Code Issues Packages Projects Releases Wiki Activity

Skeleton data can now be seen in the client and in the live window. No skeleton merging/saving yet.

Browse Source
This commit is contained in:
marek
2015-11-05 18:56:53 +01:00
parent 6df7380372
commit 948eee769e
12 changed files with 584 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
LiveScanServer/KinectServer.cs
+3 -1
View File
@@ -277,10 +277,11 @@ namespace KinectServer
} }
} }
public void GetLatestFrame(List<List<byte>> lFramesRGB, List<List<Single>> lFramesVerts) public void GetLatestFrame(List<List<byte>> lFramesRGB, List<List<Single>> lFramesVerts, List<List<Body>> lFramesBody)
{ {
lFramesRGB.Clear(); lFramesRGB.Clear();
lFramesVerts.Clear(); lFramesVerts.Clear();
lFramesBody.Clear();
lock (oFrameRequestLock) lock (oFrameRequestLock)
{ {
@@ -317,6 +318,7 @@ namespace KinectServer
{ {
lFramesRGB.Add(new List<byte>(lClientSockets[i].lFrameRGB)); lFramesRGB.Add(new List<byte>(lClientSockets[i].lFrameRGB));
lFramesVerts.Add(new List<Single>(lClientSockets[i].lFrameVerts)); lFramesVerts.Add(new List<Single>(lClientSockets[i].lFrameVerts));
lFramesBody.Add(new List<Body>(lClientSockets[i].lBodies));
} }
} }
} }
LiveScanServer/KinectSocket.cs
+55 -5
View File
@@ -19,6 +19,7 @@ using System.Text;
using System.Threading.Tasks; using System.Threading.Tasks;
using System.Net.Sockets; using System.Net.Sockets;
namespace KinectServer namespace KinectServer
{ {
public delegate void SocketChangedHandler(); public delegate void SocketChangedHandler();
@@ -40,6 +41,7 @@ namespace KinectServer
public List<byte> lFrameRGB = new List<byte>(); public List<byte> lFrameRGB = new List<byte>();
public List<Single> lFrameVerts = new List<Single>(); public List<Single> lFrameVerts = new List<Single>();
public List<Body> lBodies = new List<Body>();
public event SocketChangedHandler eChanged; public event SocketChangedHandler eChanged;
@@ -150,6 +152,7 @@ namespace KinectServer
{ {
lFrameRGB.Clear(); lFrameRGB.Clear();
lFrameVerts.Clear(); lFrameVerts.Clear();
lBodies.Clear();
int nToRead; int nToRead;
byte[] buffer = new byte[1024]; byte[] buffer = new byte[1024];
@@ -160,7 +163,6 @@ namespace KinectServer
return; return;
} }
oSocket.Receive(buffer, 4, SocketFlags.None); oSocket.Receive(buffer, 4, SocketFlags.None);
//string result = System.Text.Encoding.UTF8.GetString(buffer); //string result = System.Text.Encoding.UTF8.GetString(buffer);
@@ -185,16 +187,64 @@ namespace KinectServer
nAlreadyRead += oSocket.Receive(buffer, nAlreadyRead, nToRead - nAlreadyRead, SocketFlags.None); nAlreadyRead += oSocket.Receive(buffer, nAlreadyRead, nToRead - nAlreadyRead, SocketFlags.None);
} }
int point_size = 3 + 3 * 4; //Receive depth and color data
int n_vertices = nToRead / point_size; int startIdx = 0;
int n_vertices = BitConverter.ToInt32(buffer, startIdx);
startIdx += 4;
for (int i = 0; i < n_vertices; i++) for (int i = 0; i < n_vertices; i++)
{ {
for (int j = 0; j < 3; j++) for (int j = 0; j < 3; j++)
{ {
lFrameRGB.Add(buffer[i * point_size + j]); lFrameRGB.Add(buffer[startIdx++]);
lFrameVerts.Add(BitConverter.ToSingle(buffer, i * point_size + j * 4 + 3));
} }
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<Joint>(nJoints);
tempBody.lJointsInColorSpace = new List<Point2f>(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);
} }
} }
LiveScanServer/MainWindowForm.cs
+13 -3
View File
@@ -31,6 +31,8 @@ using System.Net;
using System.Net.Sockets; using System.Net.Sockets;
using System.Timers; using System.Timers;
using System.Diagnostics;
namespace KinectServer namespace KinectServer
{ {
@@ -48,6 +50,8 @@ namespace KinectServer
List<byte> lAllColors = new List<byte>(); List<byte> lAllColors = new List<byte>();
//Sensor poses from all of the sensors //Sensor poses from all of the sensors
List<AffineTransform> lAllCameraPoses = new List<AffineTransform>(); List<AffineTransform> lAllCameraPoses = new List<AffineTransform>();
//Body data from all of the sensors
List<Body> lAllBodies = new List<Body>();
bool bServerRunning = false; bool bServerRunning = false;
bool bRecording = false; bool bRecording = false;
@@ -161,6 +165,7 @@ namespace KinectServer
oOpenGLWindow.vertices = lAllVertices; oOpenGLWindow.vertices = lAllVertices;
oOpenGLWindow.colors = lAllColors; oOpenGLWindow.colors = lAllColors;
oOpenGLWindow.cameraPoses = lAllCameraPoses; oOpenGLWindow.cameraPoses = lAllCameraPoses;
oOpenGLWindow.bodies = lAllBodies;
oOpenGLWindow.settings = oSettings; oOpenGLWindow.settings = oSettings;
} }
oOpenGLWindow.Run(); oOpenGLWindow.Run();
@@ -245,26 +250,30 @@ namespace KinectServer
{ {
List<List<byte>> lFramesRGB = new List<List<byte>>(); List<List<byte>> lFramesRGB = new List<List<byte>>();
List<List<Single>> lFramesVerts = new List<List<Single>>(); List<List<Single>> lFramesVerts = new List<List<Single>>();
List<List<Body>> lFramesBody = new List<List<Body>>();
BackgroundWorker worker = (BackgroundWorker)sender; BackgroundWorker worker = (BackgroundWorker)sender;
while (!worker.CancellationPending) while (!worker.CancellationPending)
{ {
Thread.Sleep(1); 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. //Update the vertex and color lists that are common between this class and the OpenGLWindow.
lock (lAllVertices) lock (lAllVertices)
{ {
lAllVertices.Clear(); lAllVertices.Clear();
lAllColors.Clear(); lAllColors.Clear();
lAllBodies.Clear();
lAllCameraPoses.Clear(); lAllCameraPoses.Clear();
for (int i = 0; i < lFramesRGB.Count; i++) for (int i = 0; i < lFramesRGB.Count; i++)
{ {
lAllVertices.AddRange(lFramesVerts[i]); lAllVertices.AddRange(lFramesVerts[i]);
lAllColors.AddRange(lFramesRGB[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. //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. //Download a frame from each client.
List<List<float>> lAllFrameVertices = new List<List<float>>(); List<List<float>> lAllFrameVertices = new List<List<float>>();
List<List<byte>> lAllFrameColors = new List<List<byte>>(); List<List<byte>> lAllFrameColors = new List<List<byte>>();
oServer.GetLatestFrame(lAllFrameColors, lAllFrameVertices); List<List<Body>> lAllFrameBody = new List<List<Body>>();
oServer.GetLatestFrame(lAllFrameColors, lAllFrameVertices, lAllFrameBody);
//Initialize containers for the poses. //Initialize containers for the poses.
List<float[]> Rs = new List<float[]>(); List<float[]> Rs = new List<float[]>();
LiveScanServer/OpenGLWindow.cs
+67
View File
@@ -19,6 +19,8 @@ using System.Text;
using System.Threading.Tasks; using System.Threading.Tasks;
using System.Globalization; using System.Globalization;
using System.Diagnostics;
using OpenTK; using OpenTK;
using OpenTK.Graphics; using OpenTK.Graphics;
using OpenTK.Input; using OpenTK.Input;
@@ -58,6 +60,7 @@ namespace KinectServer
public List<float> vertices = new List<float>(); public List<float> vertices = new List<float>();
public List<byte> colors = new List<byte>(); public List<byte> colors = new List<byte>();
public List<AffineTransform> cameraPoses = new List<AffineTransform>(); public List<AffineTransform> cameraPoses = new List<AffineTransform>();
public List<Body> bodies = new List<Body>();
public KinectSettings settings = new KinectSettings(); public KinectSettings settings = new KinectSettings();
DateTime tFPSUpdateTimer = DateTime.Now; DateTime tFPSUpdateTimer = DateTime.Now;
@@ -298,6 +301,7 @@ namespace KinectServer
LineCount += settings.lMarkerPoses.Count * 3; LineCount += settings.lMarkerPoses.Count * 3;
//cameras //cameras
LineCount += cameraPoses.Count * 3; LineCount += cameraPoses.Count * 3;
LineCount += 24 * bodies.Count;
VBO = new VertexC4ubV3f[PointCount + 2 * LineCount]; VBO = new VertexC4ubV3f[PointCount + 2 * LineCount];
@@ -322,6 +326,7 @@ namespace KinectServer
{ {
iCurLineCount += AddCamera(PointCount + 2 * iCurLineCount, cameraPoses[i]); iCurLineCount += AddCamera(PointCount + 2 * iCurLineCount, cameraPoses[i]);
} }
iCurLineCount += AddBodies(PointCount + 2 * iCurLineCount);
} }
} }
} }
@@ -544,6 +549,68 @@ namespace KinectServer
return nLinesBeingAdded; 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, private void AddLine(int startIdx, float x0, float y0, float z0,
float x1, float y1, float z1) float x1, float y1, float z1)
{ {
LiveScanServer/Utils.cs
+65
View File
@@ -13,9 +13,23 @@
// year={2015}, // year={2015},
// } // }
using System; using System;
using System.Collections.Generic;
namespace KinectServer namespace KinectServer
{ {
public struct Point2f
{
public float X;
public float Y;
}
public struct Point3f
{
public float X;
public float Y;
public float Z;
}
[Serializable] [Serializable]
public class AffineTransform public class AffineTransform
{ {
@@ -92,4 +106,55 @@ namespace KinectServer
private float[] r = new float[3]; 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<Joint> lJoints;
public List<Point2f> lJointsInColorSpace;
}
} }
include/LiveScanClient/iCapture.h
+9
View File
@@ -15,6 +15,14 @@
#pragma once #pragma once
#include "utils.h" #include "utils.h"
#include "Kinect.h"
struct Body
{
bool bTracked;
std::vector<Joint> vJoints;
std::vector<Point2f> vJointsInColorSpace;
};
class ICapture class ICapture
{ {
@@ -36,4 +44,5 @@ public:
UINT16 *pDepth; UINT16 *pDepth;
RGB *pColorRGBX; RGB *pColorRGBX;
std::vector<Body> vBodies;
}; };
include/LiveScanClient/imageRenderer.h
+13 -1
View File
@@ -9,6 +9,7 @@
#pragma once #pragma once
#include <d2d1.h> #include <d2d1.h>
#include "iCapture.h"
class ImageRenderer class ImageRenderer
{ {
@@ -41,7 +42,7 @@ public:
/// <param name="pImage">image data in RGBX format</param> /// <param name="pImage">image data in RGBX format</param>
/// <param name="cbImage">size of image data in bytes</param> /// <param name="cbImage">size of image data in bytes</param>
/// <returns>indicates success or failure</returns> /// <returns>indicates success or failure</returns>
HRESULT Draw(BYTE* pImage, unsigned long cbImage); HRESULT Draw(BYTE* pImage, unsigned long cbImage, std::vector<Body> &bodies);
private: private:
HWND m_hWnd; HWND m_hWnd;
@@ -66,4 +67,15 @@ private:
/// Dispose of Direct2d resources /// Dispose of Direct2d resources
/// </summary> /// </summary>
void DiscardResources(); 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;
}; };
include/LiveScanClient/kinectCapture.h
+4
View File
@@ -35,4 +35,8 @@ private:
ICoordinateMapper* pCoordinateMapper; ICoordinateMapper* pCoordinateMapper;
IKinectSensor* pKinectSensor; IKinectSensor* pKinectSensor;
IMultiSourceFrameReader* pMultiSourceFrameReader; IMultiSourceFrameReader* pMultiSourceFrameReader;
void GetDepthFrame(IMultiSourceFrame* pMultiFrame);
void GetColorFrame(IMultiSourceFrame* pMultiFrame);
void GetBodyFrame(IMultiSourceFrame* pMultiFrame);
}; };
include/LiveScanClient/liveScanClient.h
+6 -2
View File
@@ -57,6 +57,7 @@ private:
std::vector<Point3f> m_vLastFrameVertices; std::vector<Point3f> m_vLastFrameVertices;
std::vector<RGB> m_vLastFrameRGB; std::vector<RGB> m_vLastFrameRGB;
std::vector<Body> m_vLastFrameBody;
std::vector<std::vector<Point3f>> m_vGatheredVertices; std::vector<std::vector<Point3f>> m_vGatheredVertices;
std::vector<std::vector<RGB>> m_vGatheredRGBPoints; std::vector<std::vector<RGB>> m_vGatheredRGBPoints;
@@ -67,7 +68,8 @@ private:
DWORD m_nFramesSinceUpdate; DWORD m_nFramesSinceUpdate;
Point3f* m_pCameraSpaceCoordinates; Point3f* m_pCameraSpaceCoordinates;
Point2f* m_pColorCoordinates; Point2f* m_pColorCoordinatesOfDepth;
Point2f* m_pDepthCoordinatesOfColor;
// Direct2D // Direct2D
ImageRenderer* m_pDrawColor; ImageRenderer* m_pDrawColor;
@@ -81,8 +83,10 @@ private:
bool SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce); bool SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce);
void HandleSocket(); void HandleSocket();
void SendFrame(vector<Point3f> vertices, vector<RGB> RGB, vector<Body> body);
void SocketThreadFunction(); void SocketThreadFunction();
void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color); void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector<Body> &bodies);
void ShowFPS(); void ShowFPS();
void ReadIPFromFile(); void ReadIPFromFile();
void WriteIPToFile(); void WriteIPToFile();
src/LiveScanClient/imageRenderer.cpp
+121 -2
View File
@@ -58,6 +58,16 @@ HRESULT ImageRenderer::EnsureResources()
return hr; 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 // Create a bitmap that we can copy image data into and then render to the target
hr = m_pRenderTarget->CreateBitmap( hr = m_pRenderTarget->CreateBitmap(
size, size,
@@ -121,8 +131,9 @@ HRESULT ImageRenderer::Initialize(HWND hWnd, ID2D1Factory* pD2DFactory, int sour
/// </summary> /// </summary>
/// <param name="pImage">image data in RGBX format</param> /// <param name="pImage">image data in RGBX format</param>
/// <param name="cbImage">size of image data in bytes</param> /// <param name="cbImage">size of image data in bytes</param>
/// <param name="vBodies">vector of bodies to draw</param>
/// <returns>indicates success or failure</returns> /// <returns>indicates success or failure</returns>
HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage) HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector<Body> &vBodies)
{ {
// incorrectly sized image data passed in // incorrectly sized image data passed in
if (cbImage < ((m_sourceHeight - 1) * m_sourceStride) + (m_sourceWidth * 4)) 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 // Draw the bitmap stretched to the size of the window
m_pRenderTarget->DrawBitmap(m_pBitmap); m_pRenderTarget->DrawBitmap(m_pBitmap);
for (int i = 0; i < vBodies.size(); i++)
{
if (vBodies[i].bTracked)
{
DrawBody(vBodies[i]);
}
}
hr = m_pRenderTarget->EndDraw(); hr = m_pRenderTarget->EndDraw();
// Device lost, need to recreate the render target // Device lost, need to recreate the render target
@@ -163,4 +182,104 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage)
} }
return hr; return hr;
}
/// <summary>
/// Draws all the body to the associated hwnd, assumes that BeginDraw has been called
/// </summary>
/// <param name="body">body to be drawn</param>
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);
}
}
}
/// <summary>
/// Draws one bone of a body (joint to joint)
/// </summary>
/// <param name="body">body to be drawn</param>
/// <param name="joint0">one joint of the bone to draw</param>
/// <param name="joint1">other joint of the bone to draw</param>
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);
}
} }
src/LiveScanClient/kinectCapture.cpp
+105 -48
View File
@@ -47,7 +47,11 @@ bool KinectCapture::Initialize()
if (SUCCEEDED(hr)) 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; return false;
} }
//Depth frame GetDepthFrame(pMultiFrame);
IDepthFrameReference* pDepthFrameReference = NULL; GetColorFrame(pMultiFrame);
IDepthFrame* pDepthFrame = NULL; GetBodyFrame(pMultiFrame);
pMultiFrame->get_DepthFrameReference(&pDepthFrameReference);
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);
//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<BYTE*>(pColorRGBX), ColorImageFormat_Bgra);
}
SafeRelease(pColorFrame);
SafeRelease(pColorFrameReference);
return true; return true;
} }
@@ -160,4 +120,101 @@ void KinectCapture::MapDepthFrameToColorSpace(Point2f *pColorSpacePoints)
void KinectCapture::MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints) void KinectCapture::MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints)
{ {
pCoordinateMapper->MapColorFrameToDepthSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (DepthSpacePoint*)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<BYTE*>(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>(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);
} }
src/LiveScanClient/liveScanClient.cpp
+123 -61
View File
@@ -46,6 +46,8 @@ LiveScanClient::LiveScanClient() :
m_pDrawColor(NULL), m_pDrawColor(NULL),
m_pDepthRGBX(NULL), m_pDepthRGBX(NULL),
m_pCameraSpaceCoordinates(NULL), m_pCameraSpaceCoordinates(NULL),
m_pColorCoordinatesOfDepth(NULL),
m_pDepthCoordinatesOfColor(NULL),
m_bCalibrate(false), m_bCalibrate(false),
m_bFilter(false), m_bFilter(false),
m_bCaptureFrame(false), m_bCaptureFrame(false),
@@ -101,10 +103,16 @@ LiveScanClient::~LiveScanClient()
m_pCameraSpaceCoordinates = NULL; m_pCameraSpaceCoordinates = NULL;
} }
if (m_pColorCoordinates) if (m_pColorCoordinatesOfDepth)
{ {
delete[] m_pColorCoordinates; delete[] m_pColorCoordinatesOfDepth;
m_pColorCoordinates = NULL; m_pColorCoordinatesOfDepth = NULL;
}
if (m_pDepthCoordinatesOfColor)
{
delete[] m_pDepthCoordinatesOfColor;
m_pDepthCoordinatesOfColor = NULL;
} }
if (m_pClientSocket) if (m_pClientSocket)
@@ -184,10 +192,10 @@ void LiveScanClient::UpdateFrame()
return; return;
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates); pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
pCapture->MapDepthFrameToColorSpace(m_pColorCoordinates); pCapture->MapDepthFrameToColorSpace(m_pColorCoordinatesOfDepth);
{ {
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex); std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinates, pCapture->pColorRGBX); StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinatesOfDepth, pCapture->pColorRGBX, pCapture->vBodies);
if (m_bCaptureFrame) 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_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; 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 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) void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight)
{ {
// Make sure we've received valid data // 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 // end pixel is start + width*height - 1
const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight); const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
Point2f *pDepthSpacePoints = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; pCapture->MapColorFrameToDepthSpace(m_pDepthCoordinatesOfColor);
pCapture->MapColorFrameToDepthSpace(pDepthSpacePoints);
for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++) for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++)
{ {
Point2f depthPoint = pDepthSpacePoints[i]; Point2f depthPoint = m_pDepthCoordinatesOfColor[i];
BYTE intensity = 0; BYTE intensity = 0;
if (depthPoint.X >= 0 && depthPoint.Y >= 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 // Draw the data with Direct2D
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB)); m_pDrawColor->Draw(reinterpret_cast<BYTE*>(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies);
delete[] pDepthSpacePoints;
} }
} }
@@ -392,7 +398,7 @@ void LiveScanClient::ProcessColor(RGB* pBuffer, int nWidth, int nHeight)
if (pBuffer && (nWidth == pCapture->nColorFrameWidth) && (nHeight == pCapture->nColorFrameHeight)) if (pBuffer && (nWidth == pCapture->nColorFrameWidth) && (nHeight == pCapture->nColorFrameHeight))
{ {
// Draw the data with Direct2D // Draw the data with Direct2D
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB)); m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies);
} }
} }
@@ -502,29 +508,7 @@ void LiveScanClient::HandleSocket()
if (m_vGatheredRGBPoints.size() > 0) if (m_vGatheredRGBPoints.size() > 0)
{ {
int size = m_vGatheredRGBPoints[0].size() * (3 + 3 * sizeof(float)); SendFrame(m_vGatheredVertices[0], m_vGatheredRGBPoints[0], m_vLastFrameBody);
//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;
m_vGatheredRGBPoints.erase(m_vGatheredRGBPoints.begin(), m_vGatheredRGBPoints.begin() + 1); m_vGatheredRGBPoints.erase(m_vGatheredRGBPoints.begin(), m_vGatheredRGBPoints.begin() + 1);
m_vGatheredVertices.erase(m_vGatheredVertices.begin(), m_vGatheredVertices.begin() + 1); m_vGatheredVertices.erase(m_vGatheredVertices.begin(), m_vGatheredVertices.begin() + 1);
@@ -541,28 +525,7 @@ void LiveScanClient::HandleSocket()
byteToSend = MSG_LAST_FRAME; byteToSend = MSG_LAST_FRAME;
m_pClientSocket->SendBytes(&byteToSend, 1); m_pClientSocket->SendBytes(&byteToSend, 1);
int size = m_vLastFrameRGB.size() * (3 + 3 * sizeof(float)); SendFrame(m_vLastFrameVertices, m_vLastFrameRGB, m_vLastFrameBody);
//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;
} }
//receive calibration data //receive calibration data
else if (received[i] == MSG_RECEIVE_CALIBRATION) 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<Point3f> vertices, vector<RGB> RGB, vector<Body> body)
{
int size = RGB.size() * (3 + 3 * sizeof(float)) + sizeof(int);
vector<char> 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<Body> &bodies)
{ {
std::vector<Point3f> goodVertices; std::vector<Point3f> goodVertices;
std::vector<RGB> goodColorPoints; std::vector<RGB> 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) if (m_bFilter)
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold); filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
m_vLastFrameBody = bodies;
m_vLastFrameVertices = goodVertices; m_vLastFrameVertices = goodVertices;
m_vLastFrameRGB = goodColorPoints; m_vLastFrameRGB = goodColorPoints;
} }
@@ -732,4 +794,4 @@ void LiveScanClient::WriteIPToFile()
GetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress, 20); GetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress, 20);
file << lastUsedIPAddress; file << lastUsedIPAddress;
file.close(); file.close();
} }