mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-12 23:48:55 +00:00
Skeleton data can now be seen in the client and in the live window. No skeleton merging/saving yet.
This commit is contained in:
marek
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user