diff --git a/ICP/ICP.vcxproj b/ICP/ICP.vcxproj index b58d041..227df44 100644 --- a/ICP/ICP.vcxproj +++ b/ICP/ICP.vcxproj @@ -1,5 +1,5 @@  - + Debug @@ -29,46 +29,46 @@ {973EE923-B423-4BCD-AA08-B03DA40CB51F} ICP - 8.1 + 10.0.17763.0 Application true - v140 + v141 MultiByte Application true - v140 + v141 MultiByte Application false - v140 + v141 true MultiByte Application false - v140 + v141 true MultiByte DynamicLibrary false - v140 + v141 true MultiByte DynamicLibrary false - v140 + v141 true MultiByte diff --git a/LiveScanClient/KinectClient.vcxproj b/LiveScanClient/KinectClient.vcxproj index 7b28a9b..b77cedd 100644 --- a/LiveScanClient/KinectClient.vcxproj +++ b/LiveScanClient/KinectClient.vcxproj @@ -1,5 +1,5 @@  - + Debug @@ -19,13 +19,13 @@ + - @@ -35,13 +35,13 @@ + - @@ -53,36 +53,39 @@ + + + {9B550BBA-EAFB-4D12-8B1C-8FDA39361F52} KinectClient LiveScanClient - 8.1 + 10.0.17763.0 Application true - v140 + v141 Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application false - v140 + v141 true Unicode @@ -144,7 +147,7 @@ true $(KINECTSDK20_DIR)\lib\x64;$(SolutionDir)lib - opencv_world320d.lib;kinect20.lib;libzstd.lib;%(AdditionalDependencies) + opencv_world320d.lib;libzstd.lib;%(AdditionalDependencies) NotSet @@ -184,11 +187,18 @@ true true true - opencv_world320.lib;kinect20.lib;libzstd.lib;%(AdditionalDependencies) + opencv_world320.lib;libzstd.lib;%(AdditionalDependencies) $(KINECTSDK20_DIR)\lib\x64;$(SolutionDir)lib + + + + This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. + + + \ No newline at end of file diff --git a/LiveScanClient/KinectClient.vcxproj.filters b/LiveScanClient/KinectClient.vcxproj.filters index a1483aa..7dd0adf 100644 --- a/LiveScanClient/KinectClient.vcxproj.filters +++ b/LiveScanClient/KinectClient.vcxproj.filters @@ -42,9 +42,6 @@ Header Files - - Header Files - Header Files @@ -57,6 +54,9 @@ Header Files + + Header Files + @@ -77,9 +77,6 @@ Source Files - - Source Files - Source Files @@ -92,6 +89,9 @@ Source Files + + Source Files + @@ -103,4 +103,7 @@ Resource Files + + + \ No newline at end of file diff --git a/LiveScanClient/packages.config b/LiveScanClient/packages.config new file mode 100644 index 0000000..48e49af --- /dev/null +++ b/LiveScanClient/packages.config @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/include/LiveScanClient/azureKinectCapture.h b/include/LiveScanClient/azureKinectCapture.h new file mode 100644 index 0000000..c0c5352 --- /dev/null +++ b/include/LiveScanClient/azureKinectCapture.h @@ -0,0 +1,33 @@ +#pragma once + +#include "stdafx.h" +#include "ICapture.h" +#include +#include "utils.h" + +class AzureKinectCapture : public ICapture +{ +public: + AzureKinectCapture(); + ~AzureKinectCapture(); + + bool Initialize(); + bool Initialize(int deviceIdx); + bool AcquireFrame(); + void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints); + void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints); + void MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace); + void MapColorFrameToDepthSpace(RGB *pColorInDepthSpace); +private: + k4a_device_t kinectSensor = NULL; + int32_t captureTimeoutMs = 1000; + + k4a_image_t colorImage = NULL; + k4a_image_t depthImage = NULL; + k4a_image_t depthPointCloudImage = NULL; + k4a_image_t colorImageInDepth = NULL; + k4a_image_t depthImageInColor = NULL; + k4a_transformation_t transformation = NULL; + + void UpdateDepthPointCloud(); +}; \ No newline at end of file diff --git a/include/LiveScanClient/frameFileWriterReader.h b/include/LiveScanClient/frameFileWriterReader.h index 855d2ce..cb212c5 100644 --- a/include/LiveScanClient/frameFileWriterReader.h +++ b/include/LiveScanClient/frameFileWriterReader.h @@ -12,9 +12,9 @@ public: FrameFileWriterReader(); void openNewFileForWriting(); void openCurrentFileForReading(); - + // leave filename blank if you want the filename to be generated from the date - void setCurrentFilename(std::string filename = ""); + void setCurrentFilename(std::string filename = ""); void writeFrame(std::vector points, std::vector colors); bool readFrame(std::vector &outPoints, std::vector &outColors); @@ -32,7 +32,7 @@ private: int getRecordingTimeMilliseconds(); FILE *m_pFileHandle = nullptr; - bool m_bFileOpenedForWriting = false; + bool m_bFileOpenedForWriting = false; bool m_bFileOpenedForReading = false; std::string m_sFilename = ""; diff --git a/include/LiveScanClient/iCapture.h b/include/LiveScanClient/iCapture.h index 247b30a..e39a334 100644 --- a/include/LiveScanClient/iCapture.h +++ b/include/LiveScanClient/iCapture.h @@ -15,15 +15,19 @@ #pragma once #include "utils.h" -#include "Kinect.h" + +struct Joint +{ + +}; struct Body { Body() { bTracked = false; - vJoints.resize(JointType_Count); - vJointsInColorSpace.resize(JointType_Count); + vJoints.resize(5); + vJointsInColorSpace.resize(5); } bool bTracked; std::vector vJoints; @@ -40,8 +44,8 @@ public: virtual bool AcquireFrame() = 0; virtual void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0; virtual void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0; - virtual void MapDepthFrameToColorSpace(Point2f *pColorSpacePoints) = 0; - virtual void MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints) = 0; + virtual void MapDepthFrameToColorSpace(UINT16 *pColorSpacePoints) = 0; + virtual void MapColorFrameToDepthSpace(RGB *pDepthSpacePoints) = 0; bool bInitialized; diff --git a/include/LiveScanClient/imageRenderer.h b/include/LiveScanClient/imageRenderer.h index 95bef9d..87d18e5 100644 --- a/include/LiveScanClient/imageRenderer.h +++ b/include/LiveScanClient/imageRenderer.h @@ -52,7 +52,7 @@ private: UINT m_sourceWidth; LONG m_sourceStride; - // Direct2D + // Direct2D ID2D1Factory* m_pD2DFactory; ID2D1HwndRenderTarget* m_pRenderTarget; ID2D1Bitmap* m_pBitmap; @@ -64,12 +64,12 @@ private: HRESULT EnsureResources(); /// - /// Dispose of Direct2d resources + /// Dispose of Direct2d resources /// void DiscardResources(); - void DrawBody(Body &body); - void DrawBone(Body &body, JointType joint0, JointType joint1); + //void DrawBody(Body &body); + //void DrawBone(Body &body, JointType joint0, JointType joint1); ID2D1SolidColorBrush* m_pBrushJointTracked; ID2D1SolidColorBrush* m_pBrushJointInferred; diff --git a/include/LiveScanClient/kinectCapture.h b/include/LiveScanClient/kinectCapture.h deleted file mode 100644 index 56a3f8c..0000000 --- a/include/LiveScanClient/kinectCapture.h +++ /dev/null @@ -1,43 +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}, -// } -#pragma once - -#include "stdafx.h" -#include "ICapture.h" -#include "Kinect.h" -#include "utils.h" - -class KinectCapture : public ICapture -{ -public: - KinectCapture(); - ~KinectCapture(); - - bool Initialize(); - bool AcquireFrame(); - void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints); - void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints); - void MapDepthFrameToColorSpace(Point2f *pColorSpacePoints); - void MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints); -private: - ICoordinateMapper* pCoordinateMapper; - IKinectSensor* pKinectSensor; - IMultiSourceFrameReader* pMultiSourceFrameReader; - - void GetDepthFrame(IMultiSourceFrame* pMultiFrame); - void GetColorFrame(IMultiSourceFrame* pMultiFrame); - void GetBodyFrame(IMultiSourceFrame* pMultiFrame); - void GetBodyIndexFrame(IMultiSourceFrame* pMultiFrame); -}; diff --git a/include/LiveScanClient/liveScanClient.h b/include/LiveScanClient/liveScanClient.h index cec4563..0fcc273 100644 --- a/include/LiveScanClient/liveScanClient.h +++ b/include/LiveScanClient/liveScanClient.h @@ -19,7 +19,7 @@ #include "SocketCS.h" #include "calibration.h" #include "utils.h" -#include "KinectCapture.h" +#include "azureKinectCapture.h" #include "frameFileWriterReader.h" #include #include @@ -70,11 +70,11 @@ private: INT64 m_nLastCounter; double m_fFreq; INT64 m_nNextStatusTime; - DWORD m_nFramesSinceUpdate; + DWORD m_nFramesSinceUpdate; Point3f* m_pCameraSpaceCoordinates; - Point2f* m_pColorCoordinatesOfDepth; - Point2f* m_pDepthCoordinatesOfColor; + RGB* m_pColorInDepthSpace; + UINT16* m_pDepthInColorSpace; // Direct2D ImageRenderer* m_pDrawColor; @@ -82,8 +82,8 @@ private: RGB* m_pDepthRGBX; void UpdateFrame(); - void ProcessColor(RGB* pBuffer, int nWidth, int nHeight); - void ProcessDepth(const UINT16* pBuffer, int nHeight, int nWidth); + void ShowColor(); + void ShowDepth(); bool SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce); @@ -91,7 +91,7 @@ private: void SendFrame(vector vertices, vector RGB, vector body); void SocketThreadFunction(); - void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies, BYTE* bodyIndex); + void StoreFrame(Point3f *vertices, RGB *colorInDepth, vector &bodies, BYTE* bodyIndex); void ShowFPS(); void ReadIPFromFile(); void WriteIPToFile(); diff --git a/src/LiveScanClient/azureKinectCapture.cpp b/src/LiveScanClient/azureKinectCapture.cpp new file mode 100644 index 0000000..70aa04b --- /dev/null +++ b/src/LiveScanClient/azureKinectCapture.cpp @@ -0,0 +1,253 @@ +#include "azureKinectCapture.h" +#include + +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 start = std::chrono::system_clock::now(); + bool bTemp; + do + { + bTemp = AcquireFrame(); + + std::chrono::duration 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)); +} \ No newline at end of file diff --git a/src/LiveScanClient/calibration.cpp b/src/LiveScanClient/calibration.cpp index 4a2f3dd..e255209 100644 --- a/src/LiveScanClient/calibration.cpp +++ b/src/LiveScanClient/calibration.cpp @@ -14,7 +14,6 @@ // } #include "calibration.h" -#include "Kinect.h" #include "opencv\cv.h" #include @@ -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> Rcopy = worldR; @@ -262,7 +261,7 @@ bool Calibration::GetMarkerCorners3D(vector &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; diff --git a/src/LiveScanClient/imageRenderer.cpp b/src/LiveScanClient/imageRenderer.cpp index f074df5..9f03643 100644 --- a/src/LiveScanClient/imageRenderer.cpp +++ b/src/LiveScanClient/imageRenderer.cpp @@ -10,12 +10,12 @@ /// /// Constructor /// -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() } /// -/// Dispose of Direct2d resources +/// Dispose of Direct2d resources /// void ImageRenderer::DiscardResources() { @@ -149,7 +149,7 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vectorCopyFromMemory(NULL, pImage, m_sourceStride); @@ -157,17 +157,17 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vectorBeginDraw(); // 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 /// body to be drawn -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); +// } +// } +//} /// /// Draws one bone of a body (joint to joint) @@ -251,35 +251,35 @@ void ImageRenderer::DrawBody(Body &body) /// body to be drawn /// one joint of the bone to draw /// other joint of the bone to draw -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); - } -} \ No newline at end of file +//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); +// } +//} \ No newline at end of file diff --git a/src/LiveScanClient/kinectCapture.cpp b/src/LiveScanClient/kinectCapture.cpp deleted file mode 100644 index b9da5fd..0000000 --- a/src/LiveScanClient/kinectCapture.cpp +++ /dev/null @@ -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 - -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 start = std::chrono::system_clock::now(); - bool bTemp; - do - { - bTemp = AcquireFrame(); - - std::chrono::duration 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(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_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); -} \ No newline at end of file diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index 247694a..288fc1d 100644 --- a/src/LiveScanClient/liveScanClient.cpp +++ b/src/LiveScanClient/liveScanClient.cpp @@ -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(this)); // Show window @@ -185,8 +185,6 @@ int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow) return static_cast(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 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 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(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(depth % 256); - } + USHORT depth = m_pDepthInColorSpace[i]; + BYTE intensity = static_cast(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(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies); + m_pDrawColor->Draw(reinterpret_cast(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 points; - vector colors; + vector colors; bool res = m_framesFileWriterReader.readFrame(points, colors); if (res == false) { @@ -621,7 +609,7 @@ void LiveScanClient::SendFrame(vector vertices, vector 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 vertices, vector 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 vertices, vector 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 vertices, vector 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 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 vertices, vector RGB, vector m_pClientSocket->SendBytes(buffer.data(), size); } -void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies, BYTE* bodyIndex) +void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector &bodies, BYTE* bodyIndex) { std::vector goodVertices; std::vector 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 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);