storytold/LiveScan3D
2
0
Fork 0
mirror of https://github.com/storytold/LiveScan3D.git synced 2026-10-09 00:09:58 +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();
lFramesVerts.Clear();
lFramesBody.Clear();
lock (oFrameRequestLock)
{
@@ -317,6 +318,7 @@ namespace KinectServer
{
lFramesRGB.Add(new List<byte>(lClientSockets[i].lFrameRGB));
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.Net.Sockets;
namespace KinectServer
{
public delegate void SocketChangedHandler();
@@ -40,6 +41,7 @@ namespace KinectServer
public List<byte> lFrameRGB = new List<byte>();
public List<Single> lFrameVerts = new List<Single>();
public List<Body> lBodies = new List<Body>();
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<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.Timers;
using System.Diagnostics;
namespace KinectServer
{
@@ -48,6 +50,8 @@ namespace KinectServer
List<byte> lAllColors = new List<byte>();
//Sensor poses from all of the sensors
List<AffineTransform> lAllCameraPoses = new List<AffineTransform>();
//Body data from all of the sensors
List<Body> lAllBodies = new List<Body>();
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<List<byte>> lFramesRGB = new List<List<byte>>();
List<List<Single>> lFramesVerts = new List<List<Single>>();
List<List<Body>> lFramesBody = new List<List<Body>>();
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<List<float>> lAllFrameVertices = new List<List<float>>();
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.
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.Globalization;
using System.Diagnostics;
using OpenTK;
using OpenTK.Graphics;
using OpenTK.Input;
@@ -58,6 +60,7 @@ namespace KinectServer
public List<float> vertices = new List<float>();
public List<byte> colors = new List<byte>();
public List<AffineTransform> cameraPoses = new List<AffineTransform>();
public List<Body> bodies = new List<Body>();
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)
{
LiveScanServer/Utils.cs
+65
View File
@@ -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<Joint> lJoints;
public List<Point2f> lJointsInColorSpace;
}
}
include/LiveScanClient/iCapture.h
+9
View File
@@ -15,6 +15,14 @@
#pragma once
#include "utils.h"
#include "Kinect.h"
struct Body
{
bool bTracked;
std::vector<Joint> vJoints;
std::vector<Point2f> vJointsInColorSpace;
};
class ICapture
{
@@ -36,4 +44,5 @@ public:
UINT16 *pDepth;
RGB *pColorRGBX;
std::vector<Body> vBodies;
};
include/LiveScanClient/imageRenderer.h
+13 -1
View File
@@ -9,6 +9,7 @@
#pragma once
#include <d2d1.h>
#include "iCapture.h"
class ImageRenderer
{
@@ -41,7 +42,7 @@ public:
/// <param name="pImage">image data in RGBX format</param>
/// <param name="cbImage">size of image data in bytes</param>
/// <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:
HWND m_hWnd;
@@ -66,4 +67,15 @@ private:
/// Dispose of Direct2d resources
/// </summary>
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;
IKinectSensor* pKinectSensor;
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<RGB> m_vLastFrameRGB;
std::vector<Body> m_vLastFrameBody;
std::vector<std::vector<Point3f>> m_vGatheredVertices;
std::vector<std::vector<RGB>> 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<Point3f> vertices, vector<RGB> RGB, vector<Body> body);
void SocketThreadFunction();
void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color);
void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector<Body> &bodies);
void ShowFPS();
void ReadIPFromFile();
void WriteIPToFile();
src/LiveScanClient/imageRenderer.cpp
+121 -2
View File
@@ -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
/// </summary>
/// <param name="pImage">image data in RGBX format</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>
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
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;
}
/// <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))
{
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<BYTE*>(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<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_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<std::mutex> 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<BYTE*>(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB));
delete[] pDepthSpacePoints;
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(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<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)
{
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<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<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)
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();
}
}