Merge branch 'AzureKinect' into Azure_Kinect_Temp_Sync

include/LiveScanClient/azureKinectCapture.h

@@ -3,9 +3,10 @@
 #include "stdafx.h"
 #include "ICapture.h"
 #include <k4a/k4a.h>
-#include <iostream>
-using namespace std;
-//#include "utils.h"
+#include <opencv2/opencv.hpp>
+#include "utils.h"
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
 
 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_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 UpdateDepthPointCloud();
+	void UpdateDepthPointCloudForColorFrame();
 };

include/LiveScanClient/liveScanClient.h

@@ -86,7 +86,7 @@ private:
 	int frameRecordCounter;
 
 	Point3f* m_pCameraSpaceCoordinates;
-	RGB* m_pColorInDepthSpace;
+	RGB* m_pColorInColorSpace;
 	UINT16* m_pDepthInColorSpace;
 
     // Direct2D

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

src/LiveScanClient/azureKinectCapture.cpp

@@ -1,4 +1,7 @@
 #include "azureKinectCapture.h"
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/opencv.hpp>
 #include <chrono>
 
 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,6 +127,54 @@ bool AzureKinectCapture::Initialize(SYNC_STATE state, int syncOffsetMultiplier)
 
 	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);
+  
 //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) 
 {
@@ -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<cv::Vec4b>(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));
+}
+
 /// <summary>
 /// 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)
@@ -256,144 +416,6 @@ void AzureKinectCapture::SetExposureState(bool enableAutoExposure, int exposureS
 	}
 }
 
-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));
-}
-
 
 /// <summary>
 /// Determines if this camera is configured as a Master, Subordinate or Standalone. 
@@ -435,4 +457,3 @@ int AzureKinectCapture::GetDeviceIndex()
 {
 	return deviceIDForRestart;
 }
-

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<std::mutex> 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,10 +287,8 @@ 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];
-
+				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<Point3s> vertices, vector<RGB> RGB, vector
 
 void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Body> &bodies, BYTE* bodyIndex)
 {
-	std::vector<Point3f> goodVertices;
-	std::vector<RGB> 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<Point3f> 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];
@@ -878,11 +882,20 @@ void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Bod
 				if (temp.X < m_vBounds[0] || temp.X > m_vBounds[3]
 					|| temp.Y < m_vBounds[1] || temp.Y > m_vBounds[4]
 					|| temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5]) 
+				{
+					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<Bod
 	//	}
 	//}
 
+	vector<Point3f> goodVertices(goodVerticesCount);
+	vector<RGB> 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<Point3s> goodVerticesShort(goodVertices.size());
 	
-	for (unsigned int i = 0; i < goodVertices.size(); i++)
+	for (size_t i = 0; i < goodVertices.size(); i++)
 	{
 		goodVerticesShort[i] = goodVertices[i];
 	}