mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-11 21:41:24 +00:00
Initial commit
This commit is contained in:
marek
@@ -0,0 +1,735 @@
|
||||
// Copyright (C) 2015 Marek Kowalski (M.Kowalski@ire.pw.edu.pl), Jacek Naruniec (J.Naruniec@ire.pw.edu.pl)
|
||||
// License: MIT Software License See LICENSE.txt for the full license.
|
||||
|
||||
// If you use this software in your research, then please use the following citation:
|
||||
|
||||
// Kowalski, M.; Naruniec, J.; Daniluk, M.: "LiveScan3D: A Fast and Inexpensive 3D Data
|
||||
// Acquisition System for Multiple Kinect v2 Sensors". in 3D Vision (3DV), 2015 International Conference on, Lyon, France, 2015
|
||||
|
||||
// @INPROCEEDINGS{Kowalski15,
|
||||
// author={Kowalski, M. and Naruniec, J. and Daniluk, M.},
|
||||
// booktitle={3D Vision (3DV), 2015 International Conference on},
|
||||
// title={LiveScan3D: A Fast and Inexpensive 3D Data Acquisition System for Multiple Kinect v2 Sensors},
|
||||
// year={2015},
|
||||
// }
|
||||
#include "stdafx.h"
|
||||
#include "resource.h"
|
||||
#include "LiveScanClient.h"
|
||||
#include "filter.h"
|
||||
#include <chrono>
|
||||
#include <strsafe.h>
|
||||
#include <fstream>
|
||||
|
||||
std::mutex m_mSocketThreadMutex;
|
||||
|
||||
int APIENTRY wWinMain(
|
||||
_In_ HINSTANCE hInstance,
|
||||
_In_opt_ HINSTANCE hPrevInstance,
|
||||
_In_ LPWSTR lpCmdLine,
|
||||
_In_ int nShowCmd
|
||||
)
|
||||
{
|
||||
UNREFERENCED_PARAMETER(hPrevInstance);
|
||||
UNREFERENCED_PARAMETER(lpCmdLine);
|
||||
|
||||
LiveScanClient application;
|
||||
application.Run(hInstance, nShowCmd);
|
||||
}
|
||||
|
||||
LiveScanClient::LiveScanClient() :
|
||||
m_hWnd(NULL),
|
||||
m_nLastCounter(0),
|
||||
m_nFramesSinceUpdate(0),
|
||||
m_fFreq(0),
|
||||
m_nNextStatusTime(0LL),
|
||||
m_pD2DFactory(NULL),
|
||||
m_pDrawColor(NULL),
|
||||
m_pDepthRGBX(NULL),
|
||||
m_pCameraSpaceCoordinates(NULL),
|
||||
m_bCalibrate(false),
|
||||
m_bFilter(false),
|
||||
m_bCaptureFrame(false),
|
||||
m_bConnected(false),
|
||||
m_bConfirmCaptured(false),
|
||||
m_bConfirmCalibrated(false),
|
||||
m_bShowDepth(false),
|
||||
m_bSocketThread(true),
|
||||
m_pClientSocket(NULL),
|
||||
m_nFilterNeighbors(10),
|
||||
m_fFilterThreshold(0.01f)
|
||||
{
|
||||
pCapture = new KinectCapture();
|
||||
|
||||
LARGE_INTEGER qpf = {0};
|
||||
if (QueryPerformanceFrequency(&qpf))
|
||||
{
|
||||
m_fFreq = double(qpf.QuadPart);
|
||||
}
|
||||
|
||||
m_vBounds.push_back(-0.5);
|
||||
m_vBounds.push_back(-0.5);
|
||||
m_vBounds.push_back(-0.5);
|
||||
m_vBounds.push_back(0.5);
|
||||
m_vBounds.push_back(0.5);
|
||||
m_vBounds.push_back(0.5);
|
||||
}
|
||||
|
||||
LiveScanClient::~LiveScanClient()
|
||||
{
|
||||
// clean up Direct2D renderer
|
||||
if (m_pDrawColor)
|
||||
{
|
||||
delete m_pDrawColor;
|
||||
m_pDrawColor = NULL;
|
||||
}
|
||||
|
||||
if (pCapture)
|
||||
{
|
||||
delete pCapture;
|
||||
pCapture = NULL;
|
||||
}
|
||||
|
||||
if (m_pDepthRGBX)
|
||||
{
|
||||
delete[] m_pDepthRGBX;
|
||||
m_pDepthRGBX = NULL;
|
||||
}
|
||||
|
||||
if (m_pCameraSpaceCoordinates)
|
||||
{
|
||||
delete[] m_pCameraSpaceCoordinates;
|
||||
m_pCameraSpaceCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pColorCoordinates)
|
||||
{
|
||||
delete[] m_pColorCoordinates;
|
||||
m_pColorCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pClientSocket)
|
||||
{
|
||||
delete m_pClientSocket;
|
||||
m_pClientSocket = NULL;
|
||||
}
|
||||
// clean up Direct2D
|
||||
SafeRelease(m_pD2DFactory);
|
||||
}
|
||||
|
||||
int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow)
|
||||
{
|
||||
MSG msg = {0};
|
||||
WNDCLASS wc;
|
||||
|
||||
// Dialog custom window class
|
||||
ZeroMemory(&wc, sizeof(wc));
|
||||
wc.style = CS_HREDRAW | CS_VREDRAW;
|
||||
wc.cbWndExtra = DLGWINDOWEXTRA;
|
||||
wc.hCursor = LoadCursorW(NULL, IDC_ARROW);
|
||||
wc.hIcon = LoadIconW(hInstance, MAKEINTRESOURCE(IDI_APP));
|
||||
wc.lpfnWndProc = DefDlgProcW;
|
||||
wc.lpszClassName = L"LiveScanClientAppDlgWndClass";
|
||||
|
||||
if (!RegisterClassW(&wc))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Create main application window
|
||||
HWND hWndApp = CreateDialogParamW(
|
||||
NULL,
|
||||
MAKEINTRESOURCE(IDD_APP),
|
||||
NULL,
|
||||
(DLGPROC)LiveScanClient::MessageRouter,
|
||||
reinterpret_cast<LPARAM>(this));
|
||||
|
||||
// Show window
|
||||
ShowWindow(hWndApp, nCmdShow);
|
||||
|
||||
std::thread t1(&LiveScanClient::SocketThreadFunction, this);
|
||||
// Main message loop
|
||||
while (WM_QUIT != msg.message)
|
||||
{
|
||||
//HandleSocket();
|
||||
UpdateFrame();
|
||||
|
||||
while (PeekMessageW(&msg, NULL, 0, 0, PM_REMOVE))
|
||||
{
|
||||
// If a dialog message will be taken care of by the dialog proc
|
||||
if (hWndApp && IsDialogMessageW(hWndApp, &msg))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
TranslateMessage(&msg);
|
||||
DispatchMessageW(&msg);
|
||||
}
|
||||
}
|
||||
|
||||
m_bSocketThread = false;
|
||||
t1.join();
|
||||
return static_cast<int>(msg.wParam);
|
||||
}
|
||||
|
||||
void LiveScanClient::UpdateFrame()
|
||||
{
|
||||
if (!pCapture->bInitialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
bool bNewFrameAcquired = pCapture->AcquireFrame();
|
||||
|
||||
if (!bNewFrameAcquired)
|
||||
return;
|
||||
|
||||
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
|
||||
pCapture->MapDepthFrameToColorSpace(m_pColorCoordinates);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinates, pCapture->pColorRGBX);
|
||||
|
||||
if (m_bCaptureFrame)
|
||||
{
|
||||
m_vGatheredVertices.push_back(m_vLastFrameVertices);
|
||||
m_vGatheredRGBPoints.push_back(m_vLastFrameRGB);
|
||||
m_bConfirmCaptured = true;
|
||||
m_bCaptureFrame = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bCalibrate)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
Point3f *pCameraCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
pCapture->MapColorFrameToCameraSpace(pCameraCoordinates);
|
||||
|
||||
bool res = calibration.Calibrate(pCapture->pColorRGBX, pCameraCoordinates, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight);
|
||||
|
||||
delete[] pCameraCoordinates;
|
||||
|
||||
if (res)
|
||||
{
|
||||
m_bConfirmCalibrated = true;
|
||||
m_bCalibrate = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (!m_bShowDepth)
|
||||
ProcessColor(pCapture->pColorRGBX, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight);
|
||||
else
|
||||
ProcessDepth(pCapture->pDepth, pCapture->nDepthFrameWidth, pCapture->nDepthFrameHeight);
|
||||
|
||||
ShowFPS();
|
||||
}
|
||||
|
||||
LRESULT CALLBACK LiveScanClient::MessageRouter(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
|
||||
{
|
||||
LiveScanClient* pThis = NULL;
|
||||
|
||||
if (WM_INITDIALOG == uMsg)
|
||||
{
|
||||
pThis = reinterpret_cast<LiveScanClient*>(lParam);
|
||||
SetWindowLongPtr(hWnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(pThis));
|
||||
}
|
||||
else
|
||||
{
|
||||
pThis = reinterpret_cast<LiveScanClient*>(::GetWindowLongPtr(hWnd, GWLP_USERDATA));
|
||||
}
|
||||
|
||||
if (pThis)
|
||||
{
|
||||
return pThis->DlgProc(hWnd, uMsg, wParam, lParam);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
|
||||
{
|
||||
UNREFERENCED_PARAMETER(wParam);
|
||||
UNREFERENCED_PARAMETER(lParam);
|
||||
|
||||
switch (message)
|
||||
{
|
||||
case WM_INITDIALOG:
|
||||
{
|
||||
// Bind application window handle
|
||||
m_hWnd = hWnd;
|
||||
|
||||
// Init Direct2D
|
||||
D2D1CreateFactory(D2D1_FACTORY_TYPE_SINGLE_THREADED, &m_pD2DFactory);
|
||||
|
||||
// Get and initialize the default Kinect sensor
|
||||
bool res = pCapture->Initialize();
|
||||
if (res)
|
||||
{
|
||||
m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
|
||||
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pColorCoordinates = new Point2f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
}
|
||||
else
|
||||
{
|
||||
SetStatusMessage(L"Capture device failed to initialize!", 10000, true);
|
||||
}
|
||||
|
||||
// Create and initialize a new Direct2D image renderer (take a look at ImageRenderer.h)
|
||||
// We'll use this to draw the data we receive from the Kinect to the screen
|
||||
HRESULT hr;
|
||||
m_pDrawColor = new ImageRenderer();
|
||||
hr = m_pDrawColor->Initialize(GetDlgItem(m_hWnd, IDC_VIDEOVIEW), m_pD2DFactory, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight, pCapture->nColorFrameWidth * sizeof(RGB));
|
||||
if (FAILED(hr))
|
||||
{
|
||||
SetStatusMessage(L"Failed to initialize the Direct2D draw device.", 10000, true);
|
||||
}
|
||||
|
||||
ReadIPFromFile();
|
||||
}
|
||||
break;
|
||||
|
||||
// If the titlebar X is clicked, destroy app
|
||||
case WM_CLOSE:
|
||||
WriteIPToFile();
|
||||
DestroyWindow(hWnd);
|
||||
break;
|
||||
case WM_DESTROY:
|
||||
// Quit the main message pump
|
||||
PostQuitMessage(0);
|
||||
break;
|
||||
|
||||
// Handle button press
|
||||
case WM_COMMAND:
|
||||
if (IDC_BUTTON_CONNECT == LOWORD(wParam) && BN_CLICKED == HIWORD(wParam))
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
if (m_bConnected)
|
||||
{
|
||||
delete m_pClientSocket;
|
||||
m_pClientSocket = NULL;
|
||||
|
||||
m_bConnected = false;
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_CONNECT, "Connect");
|
||||
}
|
||||
else
|
||||
{
|
||||
m_vGatheredVertices.clear();
|
||||
m_vGatheredRGBPoints.clear();
|
||||
try
|
||||
{
|
||||
char address[20];
|
||||
GetDlgItemTextA(m_hWnd, IDC_IP, address, 20);
|
||||
m_pClientSocket = new SocketClient(address, 48001);
|
||||
|
||||
m_bConnected = true;
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_CONNECT, "Disconnect");
|
||||
//Clear the status bar so that the "Failed to connect..." disappears.
|
||||
SetStatusMessage(L"", 1, true);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
SetStatusMessage(L"Failed to connect. Did you start the server?", 10000, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (IDC_BUTTON_SWITCH == LOWORD(wParam) && BN_CLICKED == HIWORD(wParam))
|
||||
{
|
||||
m_bShowDepth = !m_bShowDepth;
|
||||
|
||||
if (m_bShowDepth)
|
||||
{
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_SWITCH, "Show color");
|
||||
}
|
||||
else
|
||||
{
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_SWITCH, "Show depth");
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
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))
|
||||
{
|
||||
// end pixel is start + width*height - 1
|
||||
const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
|
||||
|
||||
Point2f *pDepthSpacePoints = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
pCapture->MapColorFrameToDepthSpace(pDepthSpacePoints);
|
||||
|
||||
for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++)
|
||||
{
|
||||
Point2f depthPoint = pDepthSpacePoints[i];
|
||||
BYTE intensity = 0;
|
||||
|
||||
if (depthPoint.X >= 0 && depthPoint.Y >= 0)
|
||||
{
|
||||
int depthIdx = (int)(depthPoint.X + depthPoint.Y * pCapture->nDepthFrameWidth);
|
||||
USHORT depth = pBuffer[depthIdx];
|
||||
intensity = static_cast<BYTE>(depth % 256);
|
||||
}
|
||||
|
||||
m_pDepthRGBX[i].rgbRed = intensity;
|
||||
m_pDepthRGBX[i].rgbGreen = intensity;
|
||||
m_pDepthRGBX[i].rgbBlue = intensity;
|
||||
}
|
||||
|
||||
// Draw the data with Direct2D
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB));
|
||||
|
||||
delete[] pDepthSpacePoints;
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessColor(RGB* pBuffer, int nWidth, int nHeight)
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
bool LiveScanClient::SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce)
|
||||
{
|
||||
INT64 now = GetTickCount64();
|
||||
|
||||
if (m_hWnd && (bForce || (m_nNextStatusTime <= now)))
|
||||
{
|
||||
SetDlgItemText(m_hWnd, IDC_STATUS, szMessage);
|
||||
m_nNextStatusTime = now + nShowTimeMsec;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void LiveScanClient::SocketThreadFunction()
|
||||
{
|
||||
while (m_bSocketThread)
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
HandleSocket();
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::HandleSocket()
|
||||
{
|
||||
char byteToSend;
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
|
||||
if (!m_bConnected)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
string received = m_pClientSocket->ReceiveBytes();
|
||||
for (unsigned int i = 0; i < received.length(); i++)
|
||||
{
|
||||
//capture a frame
|
||||
if (received[i] == MSG_CAPTURE_FRAME)
|
||||
m_bCaptureFrame = true;
|
||||
//calibrate
|
||||
else if (received[i] == MSG_CALIBRATE)
|
||||
m_bCalibrate = true;
|
||||
//receive settings
|
||||
//TODO: what if packet is split?
|
||||
else if (received[i] == MSG_RECEIVE_SETTINGS)
|
||||
{
|
||||
vector<float> bounds(6);
|
||||
i++;
|
||||
int nBytes = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
|
||||
for (int j = 0; j < 6; j++)
|
||||
{
|
||||
bounds[j] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
m_bFilter = (received[i]!=0);
|
||||
i++;
|
||||
|
||||
m_nFilterNeighbors = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
|
||||
m_fFilterThreshold = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
|
||||
m_vBounds = bounds;
|
||||
|
||||
int nMarkers = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
|
||||
calibration.markerPoses.resize(nMarkers);
|
||||
|
||||
for (int j = 0; j < nMarkers; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
for (int l = 0; l < 3; l++)
|
||||
{
|
||||
calibration.markerPoses[j].R[k][l] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
}
|
||||
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
calibration.markerPoses[j].t[k] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
calibration.markerPoses[j].markerId = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
}
|
||||
|
||||
//so that we do not lose the next character in the stream
|
||||
i--;
|
||||
}
|
||||
//send stored frame
|
||||
else if (received[i] == MSG_REQUEST_STORED_FRAME)
|
||||
{
|
||||
byteToSend = MSG_STORED_FRAME;
|
||||
m_pClientSocket->SendBytes(&byteToSend, 1);
|
||||
|
||||
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;
|
||||
|
||||
m_vGatheredRGBPoints.erase(m_vGatheredRGBPoints.begin(), m_vGatheredRGBPoints.begin() + 1);
|
||||
m_vGatheredVertices.erase(m_vGatheredVertices.begin(), m_vGatheredVertices.begin() + 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
int size = 0;
|
||||
m_pClientSocket->SendBytes((char*)&size, 4);
|
||||
}
|
||||
}
|
||||
//send last frame
|
||||
else if (received[i] == MSG_REQUEST_LAST_FRAME)
|
||||
{
|
||||
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;
|
||||
}
|
||||
//receive calibration data
|
||||
else if (received[i] == MSG_RECEIVE_CALIBRATION)
|
||||
{
|
||||
i++;
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
calibration.worldR[j][k] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
calibration.worldT[j] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
calibration.cameraR[j][k] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
calibration.cameraT[j] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
//so that we do not lose the next character in the stream
|
||||
i--;
|
||||
}
|
||||
else if (received[i] == MSG_CLEAR_STORED_FRAMES)
|
||||
{
|
||||
m_vGatheredVertices.clear();
|
||||
m_vGatheredRGBPoints.clear();
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bConfirmCaptured)
|
||||
{
|
||||
byteToSend = MSG_CONFIRM_CAPTURED;
|
||||
m_pClientSocket->SendBytes(&byteToSend, 1);
|
||||
m_bConfirmCaptured = false;
|
||||
}
|
||||
|
||||
if (m_bConfirmCalibrated)
|
||||
{
|
||||
int size = 2 * (9 + 3) * sizeof(float) + sizeof(int) + 1;
|
||||
char *buffer = new char[size];
|
||||
buffer[0] = MSG_CONFIRM_CALIBRATED;
|
||||
int i = 1;
|
||||
|
||||
memcpy(buffer + i, &calibration.iUsedMarkerId, 1 * sizeof(int));
|
||||
i += 1 * sizeof(int);
|
||||
memcpy(buffer + i, calibration.worldR[0].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.worldR[1].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.worldR[2].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.worldT.data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
|
||||
memcpy(buffer + i, calibration.cameraR[0].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.cameraR[1].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.cameraR[2].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.cameraT.data(), 3 * sizeof(float));
|
||||
|
||||
m_pClientSocket->SendBytes(buffer, size);
|
||||
m_bConfirmCalibrated = false;
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color)
|
||||
{
|
||||
std::vector<Point3f> goodVertices;
|
||||
std::vector<RGB> goodColorPoints;
|
||||
|
||||
unsigned int nVertices = pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight;
|
||||
|
||||
for (unsigned int vertexIndex = 0; vertexIndex < nVertices; vertexIndex++)
|
||||
{
|
||||
if (vertices[vertexIndex].Z >= 0 && mapping[vertexIndex].Y >= 0 && mapping[vertexIndex].Y < pCapture->nColorFrameHeight)
|
||||
{
|
||||
Point3f temp = vertices[vertexIndex];
|
||||
RGB tempColor = color[(int)mapping[vertexIndex].X + (int)mapping[vertexIndex].Y * pCapture->nColorFrameWidth];
|
||||
if (calibration.bCalibrated)
|
||||
{
|
||||
temp.X += calibration.worldT[0];
|
||||
temp.Y += calibration.worldT[1];
|
||||
temp.Z += calibration.worldT[2];
|
||||
temp = RotatePoint(temp, calibration.worldR);
|
||||
|
||||
if (temp.X < m_vBounds[0] || temp.X > m_vBounds[3]
|
||||
|| temp.Y < m_vBounds[1] || temp.Y > m_vBounds[4]
|
||||
|| temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5])
|
||||
continue;
|
||||
}
|
||||
|
||||
goodVertices.push_back(temp);
|
||||
goodColorPoints.push_back(tempColor);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bFilter)
|
||||
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
|
||||
m_vLastFrameVertices = goodVertices;
|
||||
m_vLastFrameRGB = goodColorPoints;
|
||||
}
|
||||
|
||||
void LiveScanClient::ShowFPS()
|
||||
{
|
||||
if (m_hWnd)
|
||||
{
|
||||
double fps = 0.0;
|
||||
|
||||
LARGE_INTEGER qpcNow = { 0 };
|
||||
if (m_fFreq)
|
||||
{
|
||||
if (QueryPerformanceCounter(&qpcNow))
|
||||
{
|
||||
if (m_nLastCounter)
|
||||
{
|
||||
m_nFramesSinceUpdate++;
|
||||
fps = m_fFreq * m_nFramesSinceUpdate / double(qpcNow.QuadPart - m_nLastCounter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
WCHAR szStatusMessage[64];
|
||||
StringCchPrintf(szStatusMessage, _countof(szStatusMessage), L" FPS = %0.2f", fps);
|
||||
|
||||
if (SetStatusMessage(szStatusMessage, 1000, false))
|
||||
{
|
||||
m_nLastCounter = qpcNow.QuadPart;
|
||||
m_nFramesSinceUpdate = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::ReadIPFromFile()
|
||||
{
|
||||
ifstream file;
|
||||
file.open("lastIP.txt");
|
||||
if (file.is_open())
|
||||
{
|
||||
char lastUsedIPAddress[20];
|
||||
file.getline(lastUsedIPAddress, 20);
|
||||
file.close();
|
||||
SetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress);
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::WriteIPToFile()
|
||||
{
|
||||
ofstream file;
|
||||
file.open("lastIP.txt");
|
||||
char lastUsedIPAddress[20];
|
||||
GetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress, 20);
|
||||
file << lastUsedIPAddress;
|
||||
file.close();
|
||||
}
|
||||
Reference in New Issue
Block a user