diff --git a/include/LiveScanClient/azureKinectCapture.h b/include/LiveScanClient/azureKinectCapture.h index 1a99141..fd18c43 100644 --- a/include/LiveScanClient/azureKinectCapture.h +++ b/include/LiveScanClient/azureKinectCapture.h @@ -3,9 +3,10 @@ #include "stdafx.h" #include "ICapture.h" #include -#include -using namespace std; -//#include "utils.h" +#include +#include "utils.h" +#include +#include class AzureKinectCapture : public ICapture { @@ -25,17 +26,22 @@ public: int GetDeviceIndex(); void SetExposureState(bool enableAutoExposure, int exposureStep); - 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 pointCloudImage = NULL; + k4a_image_t transformedDepthImage = NULL; k4a_image_t colorImageInDepth = NULL; k4a_image_t depthImageInColor = NULL; - k4a_transformation_t transformation = NULL; + k4a_image_t colorImageDownscaled = NULL; + k4a_transformation_t transformationColorDownscaled = NULL; + k4a_transformation_t transformation = NULL; + + int colorImageDownscaledWidth; + int colorImageDownscaledHeight; + bool syncInConnected = false; bool syncOutConnected = false; uint64_t currentTimeStamp = 0; @@ -44,6 +50,7 @@ private: int restartAttempts = 0; bool autoExposureEnabled = true; int exposureTimeStep = 0; + void UpdateDepthPointCloud(); - + void UpdateDepthPointCloudForColorFrame(); }; \ No newline at end of file diff --git a/include/LiveScanClient/liveScanClient.h b/include/LiveScanClient/liveScanClient.h index 99be1f3..da7d3c6 100644 --- a/include/LiveScanClient/liveScanClient.h +++ b/include/LiveScanClient/liveScanClient.h @@ -86,7 +86,7 @@ private: int frameRecordCounter; Point3f* m_pCameraSpaceCoordinates; - RGB* m_pColorInDepthSpace; + RGB* m_pColorInColorSpace; UINT16* m_pDepthInColorSpace; // Direct2D diff --git a/include/LiveScanClient/utils.h b/include/LiveScanClient/utils.h index e7918e7..7ef502c 100644 --- a/include/LiveScanClient/utils.h +++ b/include/LiveScanClient/utils.h @@ -59,16 +59,26 @@ typedef struct Point3f this->X = 0; this->Y = 0; this->Z = 0; + this->Invalid = false; + } + Point3f(float X, float Y, float Z, bool invalid) + { + this->X = X; + this->Y = Y; + this->Z = Z; + this->Invalid = invalid; } Point3f(float X, float Y, float Z) { this->X = X; this->Y = Y; this->Z = Z; + this->Invalid = false; } float X; float Y; float Z; + bool Invalid = false; } Point3f; typedef struct Point3s diff --git a/src/LiveScanClient/azureKinectCapture.cpp b/src/LiveScanClient/azureKinectCapture.cpp index 8d7a4d4..0222cda 100644 --- a/src/LiveScanClient/azureKinectCapture.cpp +++ b/src/LiveScanClient/azureKinectCapture.cpp @@ -1,4 +1,7 @@ #include "azureKinectCapture.h" +#include +#include +#include #include AzureKinectCapture::AzureKinectCapture() @@ -10,9 +13,11 @@ AzureKinectCapture::~AzureKinectCapture() { k4a_image_release(colorImage); k4a_image_release(depthImage); - k4a_image_release(depthPointCloudImage); + k4a_image_release(pointCloudImage); k4a_image_release(colorImageInDepth); k4a_image_release(depthImageInColor); + k4a_image_release(transformedDepthImage); + k4a_image_release(colorImageDownscaled); k4a_transformation_destroy(transformation); k4a_device_close(kinectSensor); } @@ -74,7 +79,7 @@ bool AzureKinectCapture::Initialize(SYNC_STATE state, int syncOffsetMultiplier) 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.color_resolution = K4A_COLOR_RESOLUTION_720P; config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED; config.synchronized_images_only = true; @@ -122,9 +127,57 @@ bool AzureKinectCapture::Initialize(SYNC_STATE state, int syncOffsetMultiplier) transformation = k4a_transformation_create(&calibration); - //If this device is a subordinate, it is expected to start capturing at a later time (When the master has started), so we skip this check - if (state != Subordinate) + + //No way to get the depth pixel values from the SDK at the moment, so this is hardcoded + int depth_camera_width; + int depth_camera_height; + + switch (config.depth_mode) { + case K4A_DEPTH_MODE_NFOV_UNBINNED: + depth_camera_width = 640; + depth_camera_height = 576; + break; + case K4A_DEPTH_MODE_NFOV_2X2BINNED: + depth_camera_width = 320; + depth_camera_height = 288; + break; + case K4A_DEPTH_MODE_WFOV_UNBINNED: + depth_camera_width = 1024; + depth_camera_height = 1024; + case K4A_DEPTH_MODE_WFOV_2X2BINNED: + depth_camera_width = 512; + depth_camera_height = 512; + break; + default: + break; + } + + //It's crucial for this program to output accurately mapped Pointclouds. The highest accuracy mapping is achieved + //by using the k4a_transformation_depth_image_to_color_camera function. However this converts a small depth image + //to a larger size, equivalent to the the color image size. This means more points to process and higher processing costs + //We can however scale the color image to the depth images size beforehand, to reduce proccesing power. + + //We calculate the minimum size that the color Image can be, while preserving its aspect ration + float rescaleRatio = (float)calibration.color_camera_calibration.resolution_height / (float)depth_camera_height; + colorImageDownscaledHeight = depth_camera_height; + colorImageDownscaledWidth = calibration.color_camera_calibration.resolution_width / rescaleRatio; + + //We don't only need the size in pixels of the downscaled color image, but also a new k4a_calibration_t which fits the new + //sizes + k4a_calibration_t calibrationColorDownscaled; + memcpy(&calibrationColorDownscaled, &calibration, sizeof(k4a_calibration_t)); + calibrationColorDownscaled.color_camera_calibration.resolution_width /= rescaleRatio; + calibrationColorDownscaled.color_camera_calibration.resolution_height /= rescaleRatio; + calibrationColorDownscaled.color_camera_calibration.intrinsics.parameters.param.cx /= rescaleRatio; + calibrationColorDownscaled.color_camera_calibration.intrinsics.parameters.param.cy /= rescaleRatio; + calibrationColorDownscaled.color_camera_calibration.intrinsics.parameters.param.fx /= rescaleRatio; + calibrationColorDownscaled.color_camera_calibration.intrinsics.parameters.param.fy /= rescaleRatio; + transformationColorDownscaled = k4a_transformation_create(&calibrationColorDownscaled); + +//If this device is a subordinate, it is expected to start capturing at a later time (When the master has started), so we skip this check +if (state != Subordinate) +{ std::chrono::time_point start = std::chrono::system_clock::now(); bool bTemp; do @@ -184,9 +237,9 @@ bool AzureKinectCapture::AcquireFrame() } k4a_image_release(colorImage); + k4a_image_release(colorImageDownscaled); k4a_image_release(depthImage); - colorImage = k4a_capture_get_color_image(capture); depthImage = k4a_capture_get_depth_image(capture); @@ -198,10 +251,23 @@ bool AzureKinectCapture::AcquireFrame() return false; } + //We need to resize the color image, so that it's height fits the depth camera height, while preserving the aspect ratio of the color camera: + + //Convert the k4a_image to an OpenCV Mat + cv::Mat cImg = cv::Mat(k4a_image_get_height_pixels(colorImage), k4a_image_get_width_pixels(colorImage), CV_8UC4, k4a_image_get_buffer(colorImage)); + + //Resize the k4a_image to the precalculated size. Takes quite along time, maybe there is a faster algorithm? + cv::resize(cImg, cImg, cv::Size(colorImageDownscaledWidth, colorImageDownscaledHeight), cv::INTER_LINEAR); + + //Create a k4a_image from the resized OpenCV Mat. Code taken from here: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/978#issuecomment-566002061 + k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32, cImg.cols, cImg.rows, cImg.cols * 4 * (int)sizeof(uint8_t), &colorImageDownscaled); + memcpy(k4a_image_get_buffer(colorImageDownscaled), &cImg.ptr(0)[0], cImg.rows * cImg.cols * sizeof(cv::Vec4b)); + + if (pColorRGBX == NULL) { - nColorFrameHeight = k4a_image_get_height_pixels(colorImage); - nColorFrameWidth = k4a_image_get_width_pixels(colorImage); + nColorFrameHeight = k4a_image_get_height_pixels(colorImageDownscaled); + nColorFrameWidth = k4a_image_get_width_pixels(colorImageDownscaled); pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight]; } @@ -212,7 +278,9 @@ bool AzureKinectCapture::AcquireFrame() pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth]; } - memcpy(pColorRGBX, k4a_image_get_buffer(colorImage), nColorFrameWidth * nColorFrameHeight * sizeof(RGB)); + + + memcpy(pColorRGBX, k4a_image_get_buffer(colorImageDownscaled), nColorFrameWidth * nColorFrameHeight * sizeof(RGB)); memcpy(pDepth, k4a_image_get_buffer(depthImage), nDepthFrameHeight * nDepthFrameWidth * sizeof(UINT16)); @@ -221,6 +289,98 @@ bool AzureKinectCapture::AcquireFrame() return true; } + +void AzureKinectCapture::UpdateDepthPointCloudForColorFrame() +{ + if (transformedDepthImage == NULL) + { + k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight, nColorFrameWidth * (int)sizeof(uint16_t), &transformedDepthImage); + } + + if (pointCloudImage == NULL) + { + k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM, nColorFrameWidth, nColorFrameHeight, nColorFrameWidth * 3 * (int)sizeof(int16_t), &pointCloudImage); + } + + k4a_transformation_depth_image_to_color_camera(transformationColorDownscaled, depthImage, transformedDepthImage); + + k4a_transformation_depth_image_to_point_cloud(transformationColorDownscaled, transformedDepthImage, K4A_CALIBRATION_TYPE_COLOR, pointCloudImage); +} + +void AzureKinectCapture::UpdateDepthPointCloud() +{ + if (pointCloudImage == NULL) + { + k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight, + nDepthFrameWidth * 3 * (int)sizeof(int16_t), + &pointCloudImage); + } + + k4a_transformation_depth_image_to_point_cloud(transformation, depthImage, K4A_CALIBRATION_TYPE_DEPTH, pointCloudImage); +} + +void AzureKinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints) +{ + UpdateDepthPointCloud(); + + int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage); + + 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; + } + } +} + +void AzureKinectCapture::MapColorFrameToCameraSpace(Point3f* pCameraSpacePoints) +{ + UpdateDepthPointCloudForColorFrame(); + + int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage); + + for (int i = 0; i < nColorFrameHeight; i++) + { + for (int j = 0; j < nColorFrameWidth; j++) + { + pCameraSpacePoints[j + i * nColorFrameWidth].X = pointCloudData[3 * (j + i * nColorFrameWidth) + 0] / 1000.0f; + pCameraSpacePoints[j + i * nColorFrameWidth].Y = pointCloudData[3 * (j + i * nColorFrameWidth) + 1] / 1000.0f; + pCameraSpacePoints[j + i * nColorFrameWidth].Z = pointCloudData[3 * (j + i * nColorFrameWidth) + 2] / 1000.0f; + } + } +} + +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(transformationColorDownscaled, 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(transformationColorDownscaled, depthImage, colorImage, colorImageInDepth); + + memcpy(pColorInDepthSpace, k4a_image_get_buffer(colorImageInDepth), nDepthFrameHeight * nDepthFrameWidth * 4 * (int)sizeof(uint8_t)); +} + /// /// Enables/Disables Auto Exposure and/or sets the exposure to a step value between -11 and -5 /// The kinect supports exposure values up to 1, but these are only available in lower FPS modes (5 or 15 FPS) @@ -253,145 +413,7 @@ void AzureKinectCapture::SetExposureState(bool enableAutoExposure, int exposureS autoExposureEnabled = false; exposureTimeStep = exposureStep; } - } -} - -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)); } @@ -435,4 +457,3 @@ int AzureKinectCapture::GetDeviceIndex() { return deviceIDForRestart; } - diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index 36eb510..5227306 100644 --- a/src/LiveScanClient/liveScanClient.cpp +++ b/src/LiveScanClient/liveScanClient.cpp @@ -47,7 +47,7 @@ LiveScanClient::LiveScanClient() : m_pDrawColor(NULL), m_pDepthRGBX(NULL), m_pCameraSpaceCoordinates(NULL), - m_pColorInDepthSpace(NULL), + m_pColorInColorSpace(NULL), m_pDepthInColorSpace(NULL), m_bCalibrate(false), m_bFilter(false), @@ -111,10 +111,10 @@ LiveScanClient::~LiveScanClient() m_pCameraSpaceCoordinates = NULL; } - if (m_pColorInDepthSpace) + if (m_pColorInColorSpace) { - delete[] m_pColorInDepthSpace; - m_pColorInDepthSpace = NULL; + delete[] m_pColorInColorSpace; + m_pColorInColorSpace = NULL; } if (m_pDepthInColorSpace) @@ -202,11 +202,11 @@ void LiveScanClient::UpdateFrame() if (!bNewFrameAcquired) return; - pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates); - pCapture->MapColorFrameToDepthSpace(m_pColorInDepthSpace); + pCapture->MapColorFrameToCameraSpace(m_pCameraSpaceCoordinates); + { std::lock_guard lock(m_mSocketThreadMutex); - StoreFrame(m_pCameraSpaceCoordinates, m_pColorInDepthSpace, pCapture->vBodies, pCapture->pBodyIndex); + StoreFrame(m_pCameraSpaceCoordinates, pCapture->pColorRGBX, pCapture->vBodies, pCapture->pBodyIndex); if (m_bCaptureFrame) { @@ -287,11 +287,9 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, calibration.LoadCalibration(pCapture->serialNumber); 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_pColorInDepthSpace = new RGB[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; - - pCapture->SetExposureState(true, 0); + m_pCameraSpaceCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; + m_pColorInColorSpace = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; + pCapture->SetExposureState(true, 0); } else { @@ -854,17 +852,23 @@ void LiveScanClient::SendFrame(vector vertices, vector RGB, vector void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector &bodies, BYTE* bodyIndex) { - std::vector goodVertices; - std::vector goodColorPoints; + unsigned int nVertices = pCapture->nColorFrameHeight * pCapture->nColorFrameWidth; - unsigned int nVertices = pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight; + //To save some processing cost, we allocate a full frame size (nVertices) of a Point3f Vector beforehand + //instead of using push_back for each vertice. Even though we have to copy the vertices into a clean array + //later and it uses a little bit more RAM, this gives us a nice speed increase for this function, around 25-50% + Point3f invalidPoint = Point3f(0, 0, 0, true); + vector AllVertices(nVertices); + int goodVerticesCount = 0; for (unsigned int vertexIndex = 0; vertexIndex < nVertices; vertexIndex++) { if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size()) continue; - if (vertices[vertexIndex].Z >= 0 && colorInDepth[vertexIndex].rgbReserved == 255) + //As the resizing function doesn't return a valid RGB-Reserved value which indicates that this pixel is invalid, + //we cut all vertices under a distance of 0.0001mm, as the invalid vertices always have a Z-Value of 0 + if (vertices[vertexIndex].Z >= 0.0001 && colorInDepth[vertexIndex].rgbReserved == 255) { Point3f temp = vertices[vertexIndex]; RGB tempColor = colorInDepth[vertexIndex]; @@ -877,12 +881,21 @@ void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector m_vBounds[3] || temp.Y < m_vBounds[1] || temp.Y > m_vBounds[4] - || temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5]) + || temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5]) + { + AllVertices[vertexIndex] = invalidPoint; continue; + } + } - goodVertices.push_back(temp); - goodColorPoints.push_back(tempColor); + AllVertices[vertexIndex] = temp; + goodVerticesCount++; + } + + else + { + AllVertices[vertexIndex] = invalidPoint; } } @@ -909,12 +922,28 @@ void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector goodVertices(goodVerticesCount); + vector goodColorPoints(goodVerticesCount); + int goodVerticesShortCounter = 0; + + //Copy all valid vertices into a clean vector + for (unsigned int i = 0; i < AllVertices.size(); i++) + { + if (!AllVertices[i].Invalid) + { + goodVertices[goodVerticesShortCounter] = AllVertices[i]; + goodColorPoints[goodVerticesShortCounter] = colorInDepth[i]; + goodVerticesShortCounter++; + } + } + if (m_bFilter) filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold); - vector goodVerticesShort(goodVertices.size()); - for (unsigned int i = 0; i < goodVertices.size(); i++) + vector goodVerticesShort(goodVertices.size()); + + for (size_t i = 0; i < goodVertices.size(); i++) { goodVerticesShort[i] = goodVertices[i]; }