mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-12 23:48:55 +00:00
Working with Azure Kinect but without body and only single sensor per machine
This commit is contained in:
Marek Kowalski
@@ -0,0 +1,253 @@
|
||||
#include "azureKinectCapture.h"
|
||||
#include <chrono>
|
||||
|
||||
AzureKinectCapture::AzureKinectCapture()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
AzureKinectCapture::~AzureKinectCapture()
|
||||
{
|
||||
k4a_image_release(colorImage);
|
||||
k4a_image_release(depthImage);
|
||||
k4a_image_release(depthPointCloudImage);
|
||||
k4a_image_release(colorImageInDepth);
|
||||
k4a_image_release(depthImageInColor);
|
||||
k4a_transformation_destroy(transformation);
|
||||
k4a_device_close(kinectSensor);
|
||||
}
|
||||
|
||||
bool AzureKinectCapture::Initialize()
|
||||
{
|
||||
return Initialize(K4A_DEVICE_DEFAULT);
|
||||
}
|
||||
|
||||
bool AzureKinectCapture::Initialize(int deviceIdx)
|
||||
{
|
||||
uint32_t count = k4a_device_get_installed_count();
|
||||
|
||||
kinectSensor = NULL;
|
||||
if (K4A_FAILED(k4a_device_open(deviceIdx, &kinectSensor)))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
|
||||
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
|
||||
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
|
||||
config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
|
||||
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
|
||||
|
||||
// Start the camera with the given configuration
|
||||
bInitialized = K4A_SUCCEEDED(k4a_device_start_cameras(kinectSensor, &config));
|
||||
|
||||
k4a_calibration_t calibration;
|
||||
if (K4A_FAILED(k4a_device_get_calibration(kinectSensor, config.depth_mode, config.color_resolution, &calibration)))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
transformation = k4a_transformation_create(&calibration);
|
||||
|
||||
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
|
||||
bool bTemp;
|
||||
do
|
||||
{
|
||||
bTemp = AcquireFrame();
|
||||
|
||||
std::chrono::duration<double> elapsedSeconds = std::chrono::system_clock::now() - start;
|
||||
if (elapsedSeconds.count() > 5.0)
|
||||
{
|
||||
bInitialized = false;
|
||||
break;
|
||||
}
|
||||
} while (!bTemp);
|
||||
|
||||
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
bool AzureKinectCapture::AcquireFrame()
|
||||
{
|
||||
if (!bInitialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
k4a_capture_t capture = NULL;
|
||||
|
||||
k4a_wait_result_t captureResult = k4a_device_get_capture(kinectSensor, &capture, captureTimeoutMs);
|
||||
if (captureResult != K4A_WAIT_RESULT_SUCCEEDED)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
k4a_image_release(colorImage);
|
||||
k4a_image_release(depthImage);
|
||||
|
||||
colorImage = k4a_capture_get_color_image(capture);
|
||||
depthImage = k4a_capture_get_depth_image(capture);
|
||||
if (colorImage == NULL || depthImage == NULL)
|
||||
return false;
|
||||
|
||||
|
||||
if (pColorRGBX == NULL)
|
||||
{
|
||||
nColorFrameHeight = k4a_image_get_height_pixels(colorImage);
|
||||
nColorFrameWidth = k4a_image_get_width_pixels(colorImage);
|
||||
pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight];
|
||||
}
|
||||
|
||||
if (pDepth == NULL)
|
||||
{
|
||||
nDepthFrameHeight = k4a_image_get_height_pixels(depthImage);
|
||||
nDepthFrameWidth = k4a_image_get_width_pixels(depthImage);
|
||||
pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth];
|
||||
}
|
||||
|
||||
memcpy(pColorRGBX, k4a_image_get_buffer(colorImage), nColorFrameWidth * nColorFrameHeight * sizeof(RGB));
|
||||
memcpy(pDepth, k4a_image_get_buffer(depthImage), nDepthFrameHeight * nDepthFrameWidth * sizeof(UINT16));
|
||||
|
||||
k4a_capture_release(capture);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AzureKinectCapture::UpdateDepthPointCloud()
|
||||
{
|
||||
if (depthPointCloudImage == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * 3 * (int)sizeof(int16_t),
|
||||
&depthPointCloudImage);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_point_cloud(transformation, depthImage, K4A_CALIBRATION_TYPE_DEPTH, depthPointCloudImage);
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
UpdateDepthPointCloud();
|
||||
|
||||
int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(depthPointCloudImage);
|
||||
|
||||
for (int i = 0; i < nDepthFrameHeight; i++)
|
||||
{
|
||||
for (int j = 0; j < nDepthFrameWidth; j++)
|
||||
{
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].X = pointCloudData[3 * (j + i * nDepthFrameWidth) + 0] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].Y = pointCloudData[3 * (j + i * nDepthFrameWidth) + 1] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].Z = pointCloudData[3 * (j + i * nDepthFrameWidth) + 2] / 1000.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// This mapping is much slower then the other ones, use with caution
|
||||
void AzureKinectCapture::MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
UpdateDepthPointCloud();
|
||||
|
||||
// Initializing temporary images
|
||||
k4a_image_t colorPointCloudImageX, colorPointCloudImageY, colorPointCloudImageZ, dummy_output_image;
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&colorPointCloudImageX);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&colorPointCloudImageY);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&colorPointCloudImageZ);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&dummy_output_image);
|
||||
|
||||
k4a_image_t depthPointCloudX, depthPointCloudY, depthPointCloudZ;
|
||||
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * (int)sizeof(int16_t),
|
||||
&depthPointCloudX);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * (int)sizeof(int16_t),
|
||||
&depthPointCloudY);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * (int)sizeof(int16_t),
|
||||
&depthPointCloudZ);
|
||||
|
||||
int16_t* depthPointCloudImage_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudImage);
|
||||
int16_t* depthPointCloudImageX_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudX);
|
||||
int16_t* depthPointCloudImageY_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudY);
|
||||
int16_t* depthPointCloudImageZ_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudZ);
|
||||
for (int i = 0; i < nDepthFrameWidth * nDepthFrameHeight; i++)
|
||||
{
|
||||
depthPointCloudImageX_buffer[i] = depthPointCloudImage_buffer[3 * i + 0];
|
||||
depthPointCloudImageY_buffer[i] = depthPointCloudImage_buffer[3 * i + 1];
|
||||
depthPointCloudImageZ_buffer[i] = depthPointCloudImage_buffer[3 * i + 2];
|
||||
}
|
||||
|
||||
int test = 1 << 16;
|
||||
|
||||
// Transforming per-depth-pixel point cloud to per-color-pixel point cloud
|
||||
k4a_transformation_depth_image_to_color_camera_custom(transformation, depthImage, depthPointCloudX,
|
||||
dummy_output_image, colorPointCloudImageX,
|
||||
K4A_TRANSFORMATION_INTERPOLATION_TYPE_LINEAR, 0);
|
||||
k4a_transformation_depth_image_to_color_camera_custom(transformation, depthImage, depthPointCloudY,
|
||||
dummy_output_image, colorPointCloudImageY,
|
||||
K4A_TRANSFORMATION_INTERPOLATION_TYPE_LINEAR, 0);
|
||||
k4a_transformation_depth_image_to_color_camera_custom(transformation, depthImage, depthPointCloudZ,
|
||||
dummy_output_image, colorPointCloudImageZ,
|
||||
K4A_TRANSFORMATION_INTERPOLATION_TYPE_LINEAR, 0);
|
||||
|
||||
|
||||
int16_t* pointCloudDataX = (int16_t*)k4a_image_get_buffer(colorPointCloudImageX);
|
||||
int16_t* pointCloudDataY = (int16_t*)k4a_image_get_buffer(colorPointCloudImageY);
|
||||
int16_t* pointCloudDataZ = (int16_t*)k4a_image_get_buffer(colorPointCloudImageZ);
|
||||
for (int i = 0; i < nColorFrameHeight; i++)
|
||||
{
|
||||
for (int j = 0; j < nColorFrameWidth; j++)
|
||||
{
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].X = pointCloudDataX[j + i * nColorFrameWidth + 0] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].Y = pointCloudDataY[j + i * nColorFrameWidth + 1] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].Z = pointCloudDataZ[j + i * nColorFrameWidth + 2] / 1000.0f;
|
||||
}
|
||||
}
|
||||
|
||||
k4a_image_release(colorPointCloudImageX);
|
||||
k4a_image_release(colorPointCloudImageY);
|
||||
k4a_image_release(colorPointCloudImageZ);
|
||||
|
||||
k4a_image_release(dummy_output_image);
|
||||
k4a_image_release(depthPointCloudX);
|
||||
k4a_image_release(depthPointCloudY);
|
||||
k4a_image_release(depthPointCloudZ);
|
||||
}
|
||||
|
||||
|
||||
void AzureKinectCapture::MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace)
|
||||
{
|
||||
if (depthImageInColor == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(uint16_t),
|
||||
&depthImageInColor);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_color_camera(transformation, depthImage, depthImageInColor);
|
||||
|
||||
memcpy(pDepthInColorSpace, k4a_image_get_buffer(depthImageInColor), nColorFrameHeight * nColorFrameWidth * (int)sizeof(uint16_t));
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapColorFrameToDepthSpace(RGB *pColorInDepthSpace)
|
||||
{
|
||||
if (colorImageInDepth == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * 4 * (int)sizeof(uint8_t),
|
||||
&colorImageInDepth);
|
||||
}
|
||||
|
||||
k4a_transformation_color_image_to_depth_camera(transformation, depthImage, colorImage, colorImageInDepth);
|
||||
|
||||
memcpy(pColorInDepthSpace, k4a_image_get_buffer(colorImageInDepth), nDepthFrameHeight * nDepthFrameWidth * 4 * (int)sizeof(uint8_t));
|
||||
}
|
||||
@@ -14,7 +14,6 @@
|
||||
// }
|
||||
|
||||
#include "calibration.h"
|
||||
#include "Kinect.h"
|
||||
#include "opencv\cv.h"
|
||||
|
||||
#include <fstream>
|
||||
@@ -92,7 +91,7 @@ bool Calibration::Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColo
|
||||
marker3D[i].Z += marker3DSamples[j][i].Z / (float)nRequiredSamples;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Procrustes(marker, marker3D, worldT, worldR);
|
||||
|
||||
vector<vector<float>> Rcopy = worldR;
|
||||
@@ -262,7 +261,7 @@ bool Calibration::GetMarkerCorners3D(vector<Point3f> &marker3D, MarkerInfo &mark
|
||||
Point3f pointXMinYMax = pCameraCoordinates[minX + maxY * cColorWidth];
|
||||
Point3f pointMax = pCameraCoordinates[maxX + maxY * cColorWidth];
|
||||
|
||||
if (pointMin.Z < 0 || pointXMaxYMin.Z < 0 || pointXMinYMax.Z < 0 || pointMax.Z < 0)
|
||||
if (pointMin.Z <= 0 || pointXMaxYMin.Z <= 0 || pointXMinYMax.Z <= 0 || pointMax.Z <= 0)
|
||||
return false;
|
||||
|
||||
marker3D[i].X = (1 - dx) * (1 - dy) * pointMin.X + dx * (1 - dy) * pointXMaxYMin.X + (1 - dx) * dy * pointXMinYMax.X + dx * dy * pointMax.X;
|
||||
|
||||
@@ -10,12 +10,12 @@
|
||||
/// <summary>
|
||||
/// Constructor
|
||||
/// </summary>
|
||||
ImageRenderer::ImageRenderer() :
|
||||
ImageRenderer::ImageRenderer() :
|
||||
m_hWnd(0),
|
||||
m_sourceWidth(0),
|
||||
m_sourceHeight(0),
|
||||
m_sourceStride(0),
|
||||
m_pD2DFactory(NULL),
|
||||
m_pD2DFactory(NULL),
|
||||
m_pRenderTarget(NULL),
|
||||
m_pBitmap(0)
|
||||
{
|
||||
@@ -70,9 +70,9 @@ HRESULT ImageRenderer::EnsureResources()
|
||||
|
||||
// Create a bitmap that we can copy image data into and then render to the target
|
||||
hr = m_pRenderTarget->CreateBitmap(
|
||||
size,
|
||||
size,
|
||||
D2D1::BitmapProperties(D2D1::PixelFormat(DXGI_FORMAT_B8G8R8A8_UNORM, D2D1_ALPHA_MODE_IGNORE)),
|
||||
&m_pBitmap
|
||||
&m_pBitmap
|
||||
);
|
||||
|
||||
if (FAILED(hr))
|
||||
@@ -86,7 +86,7 @@ HRESULT ImageRenderer::EnsureResources()
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dispose of Direct2d resources
|
||||
/// Dispose of Direct2d resources
|
||||
/// </summary>
|
||||
void ImageRenderer::DiscardResources()
|
||||
{
|
||||
@@ -149,7 +149,7 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector<Bod
|
||||
{
|
||||
return hr;
|
||||
}
|
||||
|
||||
|
||||
// Copy the image that was passed in into the direct2d bitmap
|
||||
hr = m_pBitmap->CopyFromMemory(NULL, pImage, m_sourceStride);
|
||||
|
||||
@@ -157,17 +157,17 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector<Bod
|
||||
{
|
||||
return hr;
|
||||
}
|
||||
|
||||
|
||||
m_pRenderTarget->BeginDraw();
|
||||
|
||||
// Draw the bitmap stretched to the size of the window
|
||||
m_pRenderTarget->DrawBitmap(m_pBitmap);
|
||||
|
||||
|
||||
for (unsigned int i = 0; i < vBodies.size(); i++)
|
||||
{
|
||||
if (vBodies[i].bTracked)
|
||||
{
|
||||
DrawBody(vBodies[i]);
|
||||
//DrawBody(vBodies[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -188,62 +188,62 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector<Bod
|
||||
/// 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 (unsigned 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
//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 (unsigned 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)
|
||||
@@ -251,35 +251,35 @@ void ImageRenderer::DrawBody(Body &body)
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
//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);
|
||||
// }
|
||||
//}
|
||||
@@ -1,250 +0,0 @@
|
||||
// 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 "KinectCapture.h"
|
||||
#include <chrono>
|
||||
|
||||
KinectCapture::KinectCapture()
|
||||
{
|
||||
pKinectSensor = NULL;
|
||||
pCoordinateMapper = NULL;
|
||||
pMultiSourceFrameReader = NULL;
|
||||
}
|
||||
|
||||
KinectCapture::~KinectCapture()
|
||||
{
|
||||
SafeRelease(pKinectSensor);
|
||||
SafeRelease(pCoordinateMapper);
|
||||
SafeRelease(pMultiSourceFrameReader);
|
||||
}
|
||||
|
||||
bool KinectCapture::Initialize()
|
||||
{
|
||||
HRESULT hr;
|
||||
|
||||
hr = GetDefaultKinectSensor(&pKinectSensor);
|
||||
if (FAILED(hr))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
if (pKinectSensor)
|
||||
{
|
||||
pKinectSensor->get_CoordinateMapper(&pCoordinateMapper);
|
||||
hr = pKinectSensor->Open();
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
pKinectSensor->OpenMultiSourceFrameReader(FrameSourceTypes::FrameSourceTypes_Color |
|
||||
FrameSourceTypes::FrameSourceTypes_Depth |
|
||||
FrameSourceTypes::FrameSourceTypes_Body |
|
||||
FrameSourceTypes::FrameSourceTypes_BodyIndex,
|
||||
&pMultiSourceFrameReader);
|
||||
}
|
||||
}
|
||||
|
||||
bInitialized = SUCCEEDED(hr);
|
||||
|
||||
if (bInitialized)
|
||||
{
|
||||
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
|
||||
bool bTemp;
|
||||
do
|
||||
{
|
||||
bTemp = AcquireFrame();
|
||||
|
||||
std::chrono::duration<double> elapsedSeconds = std::chrono::system_clock::now() - start;
|
||||
if (elapsedSeconds.count() > 5.0)
|
||||
{
|
||||
bInitialized = false;
|
||||
break;
|
||||
}
|
||||
|
||||
} while (!bTemp);
|
||||
}
|
||||
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
bool KinectCapture::AcquireFrame()
|
||||
{
|
||||
if (!bInitialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
//Multi frame
|
||||
IMultiSourceFrame* pMultiFrame = NULL;
|
||||
HRESULT hr = pMultiSourceFrameReader->AcquireLatestFrame(&pMultiFrame);
|
||||
|
||||
if (!SUCCEEDED(hr))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
GetDepthFrame(pMultiFrame);
|
||||
GetColorFrame(pMultiFrame);
|
||||
GetBodyFrame(pMultiFrame);
|
||||
GetBodyIndexFrame(pMultiFrame);
|
||||
|
||||
SafeRelease(pMultiFrame);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void KinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapDepthFrameToCameraSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nDepthFrameWidth * nDepthFrameHeight, (CameraSpacePoint*)pCameraSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapColorFrameToCameraSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (CameraSpacePoint*)pCameraSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapDepthFrameToColorSpace(Point2f *pColorSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapDepthFrameToColorSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nDepthFrameWidth * nDepthFrameHeight, (ColorSpacePoint*)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[BODY_COUNT] = { NULL };
|
||||
pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, bodies);
|
||||
|
||||
vBodies = std::vector<Body>(BODY_COUNT);
|
||||
for (int i = 0; i < BODY_COUNT; 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);
|
||||
|
||||
if (isTracked == TRUE)
|
||||
vBodies[i].bTracked = true;
|
||||
else
|
||||
vBodies[i].bTracked = false;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void KinectCapture::GetBodyIndexFrame(IMultiSourceFrame* pMultiFrame)
|
||||
{
|
||||
IBodyIndexFrameReference* pBodyIndexFrameReference = NULL;
|
||||
IBodyIndexFrame* pBodyIndexFrame = NULL;
|
||||
pMultiFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference);
|
||||
HRESULT hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame);
|
||||
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
if (pBodyIndex == NULL)
|
||||
{
|
||||
pBodyIndex = new BYTE[nDepthFrameHeight * nDepthFrameWidth];
|
||||
}
|
||||
|
||||
UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth;
|
||||
hr = pBodyIndexFrame->CopyFrameDataToArray(nBufferSize, pBodyIndex);
|
||||
}
|
||||
|
||||
SafeRelease(pBodyIndexFrame);
|
||||
SafeRelease(pBodyIndexFrameReference);
|
||||
}
|
||||
@@ -23,7 +23,7 @@
|
||||
|
||||
std::mutex m_mSocketThreadMutex;
|
||||
|
||||
int APIENTRY wWinMain(
|
||||
int APIENTRY wWinMain(
|
||||
_In_ HINSTANCE hInstance,
|
||||
_In_opt_ HINSTANCE hPrevInstance,
|
||||
_In_ LPWSTR lpCmdLine,
|
||||
@@ -47,8 +47,8 @@ LiveScanClient::LiveScanClient() :
|
||||
m_pDrawColor(NULL),
|
||||
m_pDepthRGBX(NULL),
|
||||
m_pCameraSpaceCoordinates(NULL),
|
||||
m_pColorCoordinatesOfDepth(NULL),
|
||||
m_pDepthCoordinatesOfColor(NULL),
|
||||
m_pColorInDepthSpace(NULL),
|
||||
m_pDepthInColorSpace(NULL),
|
||||
m_bCalibrate(false),
|
||||
m_bFilter(false),
|
||||
m_bStreamOnlyBodies(false),
|
||||
@@ -64,7 +64,7 @@ LiveScanClient::LiveScanClient() :
|
||||
m_nFilterNeighbors(10),
|
||||
m_fFilterThreshold(0.01f)
|
||||
{
|
||||
pCapture = new KinectCapture();
|
||||
pCapture = new AzureKinectCapture();
|
||||
|
||||
LARGE_INTEGER qpf = {0};
|
||||
if (QueryPerformanceFrequency(&qpf))
|
||||
@@ -81,7 +81,7 @@ LiveScanClient::LiveScanClient() :
|
||||
|
||||
calibration.LoadCalibration();
|
||||
}
|
||||
|
||||
|
||||
LiveScanClient::~LiveScanClient()
|
||||
{
|
||||
// clean up Direct2D renderer
|
||||
@@ -109,16 +109,16 @@ LiveScanClient::~LiveScanClient()
|
||||
m_pCameraSpaceCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pColorCoordinatesOfDepth)
|
||||
if (m_pColorInDepthSpace)
|
||||
{
|
||||
delete[] m_pColorCoordinatesOfDepth;
|
||||
m_pColorCoordinatesOfDepth = NULL;
|
||||
delete[] m_pColorInDepthSpace;
|
||||
m_pColorInDepthSpace = NULL;
|
||||
}
|
||||
|
||||
if (m_pDepthCoordinatesOfColor)
|
||||
if (m_pDepthInColorSpace)
|
||||
{
|
||||
delete[] m_pDepthCoordinatesOfColor;
|
||||
m_pDepthCoordinatesOfColor = NULL;
|
||||
delete[] m_pDepthInColorSpace;
|
||||
m_pDepthInColorSpace = NULL;
|
||||
}
|
||||
|
||||
if (m_pClientSocket)
|
||||
@@ -154,7 +154,7 @@ int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow)
|
||||
NULL,
|
||||
MAKEINTRESOURCE(IDD_APP),
|
||||
NULL,
|
||||
(DLGPROC)LiveScanClient::MessageRouter,
|
||||
(DLGPROC)LiveScanClient::MessageRouter,
|
||||
reinterpret_cast<LPARAM>(this));
|
||||
|
||||
// Show window
|
||||
@@ -185,8 +185,6 @@ int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow)
|
||||
return static_cast<int>(msg.wParam);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void LiveScanClient::UpdateFrame()
|
||||
{
|
||||
if (!pCapture->bInitialized)
|
||||
@@ -200,10 +198,10 @@ void LiveScanClient::UpdateFrame()
|
||||
return;
|
||||
|
||||
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
|
||||
pCapture->MapDepthFrameToColorSpace(m_pColorCoordinatesOfDepth);
|
||||
pCapture->MapColorFrameToDepthSpace(m_pColorInDepthSpace);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinatesOfDepth, pCapture->pColorRGBX, pCapture->vBodies, pCapture->pBodyIndex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorInDepthSpace, pCapture->vBodies, pCapture->pBodyIndex);
|
||||
|
||||
if (m_bCaptureFrame)
|
||||
{
|
||||
@@ -214,7 +212,7 @@ void LiveScanClient::UpdateFrame()
|
||||
}
|
||||
|
||||
if (m_bCalibrate)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
Point3f *pCameraCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
pCapture->MapColorFrameToCameraSpace(pCameraCoordinates);
|
||||
@@ -231,9 +229,9 @@ void LiveScanClient::UpdateFrame()
|
||||
}
|
||||
|
||||
if (!m_bShowDepth)
|
||||
ProcessColor(pCapture->pColorRGBX, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight);
|
||||
ShowColor();
|
||||
else
|
||||
ProcessDepth(pCapture->pDepth, pCapture->nDepthFrameWidth, pCapture->nDepthFrameHeight);
|
||||
ShowDepth();
|
||||
|
||||
ShowFPS();
|
||||
}
|
||||
@@ -241,7 +239,7 @@ void LiveScanClient::UpdateFrame()
|
||||
LRESULT CALLBACK LiveScanClient::MessageRouter(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
|
||||
{
|
||||
LiveScanClient* pThis = NULL;
|
||||
|
||||
|
||||
if (WM_INITDIALOG == uMsg)
|
||||
{
|
||||
pThis = reinterpret_cast<LiveScanClient*>(lParam);
|
||||
@@ -280,10 +278,10 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
|
||||
if (res)
|
||||
{
|
||||
m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
m_pDepthInColorSpace = new UINT16[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
|
||||
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pColorCoordinatesOfDepth = new Point2f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pDepthCoordinatesOfColor = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
m_pColorInDepthSpace = new RGB[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -305,15 +303,15 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
|
||||
break;
|
||||
|
||||
// If the titlebar X is clicked, destroy app
|
||||
case WM_CLOSE:
|
||||
case WM_CLOSE:
|
||||
WriteIPToFile();
|
||||
DestroyWindow(hWnd);
|
||||
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))
|
||||
@@ -368,27 +366,17 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight)
|
||||
void LiveScanClient::ShowDepth()
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
if (m_pDepthRGBX && m_pDepthCoordinatesOfColor && pBuffer && (nWidth == pCapture->nDepthFrameWidth) && (nHeight == pCapture->nDepthFrameHeight))
|
||||
if (m_pDepthRGBX && m_pDepthInColorSpace)
|
||||
{
|
||||
// end pixel is start + width*height - 1
|
||||
const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
|
||||
|
||||
pCapture->MapColorFrameToDepthSpace(m_pDepthCoordinatesOfColor);
|
||||
pCapture->MapDepthFrameToColorSpace(m_pDepthInColorSpace);
|
||||
|
||||
for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++)
|
||||
{
|
||||
Point2f depthPoint = m_pDepthCoordinatesOfColor[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);
|
||||
}
|
||||
USHORT depth = m_pDepthInColorSpace[i];
|
||||
BYTE intensity = static_cast<BYTE>(depth % 256);
|
||||
|
||||
m_pDepthRGBX[i].rgbRed = intensity;
|
||||
m_pDepthRGBX[i].rgbGreen = intensity;
|
||||
@@ -400,13 +388,13 @@ void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessColor(RGB* pBuffer, int nWidth, int nHeight)
|
||||
void LiveScanClient::ShowColor()
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
if (pBuffer && (nWidth == pCapture->nColorFrameWidth) && (nHeight == pCapture->nColorFrameHeight))
|
||||
if (pCapture->pColorRGBX)
|
||||
{
|
||||
// Draw the data with Direct2D
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies);
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pCapture->pColorRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -467,7 +455,7 @@ void LiveScanClient::HandleSocket()
|
||||
bounds[j] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
|
||||
m_bFilter = (received[i]!=0);
|
||||
i++;
|
||||
|
||||
@@ -525,7 +513,7 @@ void LiveScanClient::HandleSocket()
|
||||
m_pClientSocket->SendBytes(&byteToSend, 1);
|
||||
|
||||
vector<Point3s> points;
|
||||
vector<RGB> colors;
|
||||
vector<RGB> colors;
|
||||
bool res = m_framesFileWriterReader.readFrame(points, colors);
|
||||
if (res == false)
|
||||
{
|
||||
@@ -621,7 +609,7 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
ptr2 += sizeof(short) * 3;
|
||||
pos += sizeof(short) * 3;
|
||||
}
|
||||
|
||||
|
||||
int nBodies = body.size();
|
||||
size += sizeof(nBodies);
|
||||
for (int i = 0; i < nBodies; i++)
|
||||
@@ -633,7 +621,7 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
size += nJoints * 2 * sizeof(float);
|
||||
}
|
||||
buffer.resize(size);
|
||||
|
||||
|
||||
memcpy(buffer.data() + pos, &nBodies, sizeof(nBodies));
|
||||
pos += sizeof(nBodies);
|
||||
|
||||
@@ -648,24 +636,24 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
|
||||
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);
|
||||
////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);
|
||||
////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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -673,12 +661,12 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
|
||||
if (m_bFrameCompression)
|
||||
{
|
||||
// *2, because according to zstd documentation, increasing the size of the output buffer above a
|
||||
// *2, because according to zstd documentation, increasing the size of the output buffer above a
|
||||
// bound should speed up the compression.
|
||||
int cBuffSize = ZSTD_compressBound(size) * 2;
|
||||
int cBuffSize = ZSTD_compressBound(size) * 2;
|
||||
vector<char> compressedBuffer(cBuffSize);
|
||||
int cSize = ZSTD_compress(compressedBuffer.data(), cBuffSize, buffer.data(), size, m_iCompressionLevel);
|
||||
size = cSize;
|
||||
size = cSize;
|
||||
buffer = compressedBuffer;
|
||||
}
|
||||
char header[8];
|
||||
@@ -689,7 +677,7 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
m_pClientSocket->SendBytes(buffer.data(), size);
|
||||
}
|
||||
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector<Body> &bodies, BYTE* bodyIndex)
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Body> &bodies, BYTE* bodyIndex)
|
||||
{
|
||||
std::vector<Point3f> goodVertices;
|
||||
std::vector<RGB> goodColorPoints;
|
||||
@@ -701,10 +689,10 @@ void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color,
|
||||
if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size())
|
||||
continue;
|
||||
|
||||
if (vertices[vertexIndex].Z >= 0 && mapping[vertexIndex].Y >= 0 && mapping[vertexIndex].Y < pCapture->nColorFrameHeight)
|
||||
if (vertices[vertexIndex].Z >= 0 && colorInDepth[vertexIndex].rgbReserved == 255)
|
||||
{
|
||||
Point3f temp = vertices[vertexIndex];
|
||||
RGB tempColor = color[(int)mapping[vertexIndex].X + (int)mapping[vertexIndex].Y * pCapture->nColorFrameWidth];
|
||||
RGB tempColor = colorInDepth[vertexIndex];
|
||||
if (calibration.bCalibrated)
|
||||
{
|
||||
temp.X += calibration.worldT[0];
|
||||
@@ -725,26 +713,26 @@ void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color,
|
||||
|
||||
vector<Body> tempBodies = bodies;
|
||||
|
||||
for (unsigned int i = 0; i < tempBodies.size(); i++)
|
||||
{
|
||||
for (unsigned int j = 0; j < tempBodies[i].vJoints.size(); j++)
|
||||
{
|
||||
if (calibration.bCalibrated)
|
||||
{
|
||||
tempBodies[i].vJoints[j].Position.X += calibration.worldT[0];
|
||||
tempBodies[i].vJoints[j].Position.Y += calibration.worldT[1];
|
||||
tempBodies[i].vJoints[j].Position.Z += calibration.worldT[2];
|
||||
//for (unsigned int i = 0; i < tempBodies.size(); i++)
|
||||
//{
|
||||
// for (unsigned int j = 0; j < tempBodies[i].vJoints.size(); j++)
|
||||
// {
|
||||
// if (calibration.bCalibrated)
|
||||
// {
|
||||
// tempBodies[i].vJoints[j].Position.X += calibration.worldT[0];
|
||||
// tempBodies[i].vJoints[j].Position.Y += calibration.worldT[1];
|
||||
// tempBodies[i].vJoints[j].Position.Z += calibration.worldT[2];
|
||||
|
||||
Point3f tempPoint(tempBodies[i].vJoints[j].Position.X, tempBodies[i].vJoints[j].Position.Y, tempBodies[i].vJoints[j].Position.Z);
|
||||
// Point3f tempPoint(tempBodies[i].vJoints[j].Position.X, tempBodies[i].vJoints[j].Position.Y, tempBodies[i].vJoints[j].Position.Z);
|
||||
|
||||
tempPoint = RotatePoint(tempPoint, calibration.worldR);
|
||||
// tempPoint = RotatePoint(tempPoint, calibration.worldR);
|
||||
|
||||
tempBodies[i].vJoints[j].Position.X = tempPoint.X;
|
||||
tempBodies[i].vJoints[j].Position.Y = tempPoint.Y;
|
||||
tempBodies[i].vJoints[j].Position.Z = tempPoint.Z;
|
||||
}
|
||||
}
|
||||
}
|
||||
// tempBodies[i].vJoints[j].Position.X = tempPoint.X;
|
||||
// tempBodies[i].vJoints[j].Position.Y = tempPoint.Y;
|
||||
// tempBodies[i].vJoints[j].Position.Z = tempPoint.Z;
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
if (m_bFilter)
|
||||
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
|
||||
|
||||
Reference in New Issue
Block a user