diff --git a/ICP/ICP.vcxproj b/ICP/ICP.vcxproj index 227df44..18f8595 100644 --- a/ICP/ICP.vcxproj +++ b/ICP/ICP.vcxproj @@ -29,46 +29,46 @@ {973EE923-B423-4BCD-AA08-B03DA40CB51F} ICP - 10.0.17763.0 + 10.0 Application true - v141 + v142 MultiByte Application true - v141 + v142 MultiByte Application false - v141 + v142 true MultiByte Application false - v141 + v142 true MultiByte DynamicLibrary false - v141 + v142 true MultiByte DynamicLibrary false - v141 + v142 true MultiByte diff --git a/LiveScanClient/KinectClient.vcxproj b/LiveScanClient/KinectClient.vcxproj index 9900606..eaf094b 100644 --- a/LiveScanClient/KinectClient.vcxproj +++ b/LiveScanClient/KinectClient.vcxproj @@ -60,32 +60,32 @@ {9B550BBA-EAFB-4D12-8B1C-8FDA39361F52} KinectClient LiveScanClient - 10.0.17763.0 + 10.0 Application true - v141 + v142 Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application false - v141 + v142 true Unicode @@ -193,12 +193,12 @@ - + 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/packages.config b/LiveScanClient/packages.config index 49caf13..bd5be72 100644 --- a/LiveScanClient/packages.config +++ b/LiveScanClient/packages.config @@ -1,4 +1,4 @@  - + \ No newline at end of file diff --git a/include/LiveScanClient/azureKinectCapture.h b/include/LiveScanClient/azureKinectCapture.h index facd9b0..e33144c 100644 --- a/include/LiveScanClient/azureKinectCapture.h +++ b/include/LiveScanClient/azureKinectCapture.h @@ -3,7 +3,10 @@ #include "stdafx.h" #include "ICapture.h" #include +#include #include "utils.h" +#include +#include class AzureKinectCapture : public ICapture { @@ -17,16 +20,22 @@ public: 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 pointCloudImage = NULL; + k4a_image_t transformedDepthImage = NULL; k4a_image_t colorImageInDepth = NULL; k4a_image_t depthImageInColor = NULL; + k4a_image_t colorImageDownscaled = NULL; + int colorImageDownscaledWidth; + int colorImageDownscaledHeight; k4a_transformation_t transformation = NULL; - + k4a_transformation_t transformationColorDownscaled = NULL; void UpdateDepthPointCloud(); + void UpdateDepthPointCloudForColorFrame(); }; \ No newline at end of file diff --git a/include/LiveScanClient/liveScanClient.h b/include/LiveScanClient/liveScanClient.h index 0fcc273..09785c1 100644 --- a/include/LiveScanClient/liveScanClient.h +++ b/include/LiveScanClient/liveScanClient.h @@ -73,7 +73,7 @@ private: DWORD m_nFramesSinceUpdate; 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 ce85698..4a72538 100644 --- a/include/LiveScanClient/utils.h +++ b/include/LiveScanClient/utils.h @@ -45,16 +45,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 33d9140..c9a9da8 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); } @@ -36,7 +41,7 @@ bool AzureKinectCapture::Initialize() 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; @@ -51,6 +56,54 @@ bool AzureKinectCapture::Initialize() } transformation = k4a_transformation_create(&calibration); + + //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); + std::chrono::time_point start = std::chrono::system_clock::now(); bool bTemp; do @@ -91,9 +144,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); if (colorImage == NULL || depthImage == NULL) @@ -102,10 +155,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]; } @@ -116,7 +182,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)); k4a_capture_release(capture); @@ -124,23 +192,40 @@ 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 (depthPointCloudImage == NULL) + if (pointCloudImage == NULL) { k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight, nDepthFrameWidth * 3 * (int)sizeof(int16_t), - &depthPointCloudImage); + &pointCloudImage); } - k4a_transformation_depth_image_to_point_cloud(transformation, depthImage, K4A_CALIBRATION_TYPE_DEPTH, depthPointCloudImage); + 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(depthPointCloudImage); + int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage); for (int i = 0; i < nDepthFrameHeight; i++) { @@ -153,87 +238,23 @@ void AzureKinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints) } } -// This mapping is much slower then the other ones, use with caution -void AzureKinectCapture::MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints) +void AzureKinectCapture::MapColorFrameToCameraSpace(Point3f* pCameraSpacePoints) { - UpdateDepthPointCloud(); + UpdateDepthPointCloudForColorFrame(); - // 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); + int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage); - 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; + 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; } } - - 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) @@ -243,12 +264,12 @@ void AzureKinectCapture::MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace) &depthImageInColor); } - k4a_transformation_depth_image_to_color_camera(transformation, depthImage, 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) +void AzureKinectCapture::MapColorFrameToDepthSpace(RGB* pColorInDepthSpace) { if (colorImageInDepth == NULL) { @@ -257,7 +278,9 @@ void AzureKinectCapture::MapColorFrameToDepthSpace(RGB *pColorInDepthSpace) &colorImageInDepth); } - k4a_transformation_color_image_to_depth_camera(transformation, depthImage, colorImage, 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)); -} \ No newline at end of file +} + + diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index 5ea01f7..a5d56be 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), @@ -107,10 +107,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) @@ -198,11 +198,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) { @@ -283,8 +283,8 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, 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]; + m_pCameraSpaceCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; + m_pColorInColorSpace = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; } else { @@ -682,17 +682,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]; @@ -705,12 +711,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; } } @@ -737,12 +752,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]; }