Merge pull request #48 from ChristopherRemde/AzureKinect_Mapping_Fix

Changed Color Image Mapping to be more accurate

ICP/ICP.vcxproj

@@ -29,46 +29,46 @@
   <PropertyGroup Label="Globals">
     <ProjectGuid>{973EE923-B423-4BCD-AA08-B03DA40CB51F}</ProjectGuid>
     <RootNamespace>ICP</RootNamespace>
-    <WindowsTargetPlatformVersion>10.0.17763.0</WindowsTargetPlatformVersion>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
   </PropertyGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>true</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <CharacterSet>MultiByte</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>true</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <CharacterSet>MultiByte</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>MultiByte</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>MultiByte</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release DLL|Win32'" Label="Configuration">
     <ConfigurationType>DynamicLibrary</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>MultiByte</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release DLL|x64'" Label="Configuration">
     <ConfigurationType>DynamicLibrary</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>MultiByte</CharacterSet>
   </PropertyGroup>

LiveScanClient/KinectClient.vcxproj

@@ -60,32 +60,32 @@
     <ProjectGuid>{9B550BBA-EAFB-4D12-8B1C-8FDA39361F52}</ProjectGuid>
     <RootNamespace>KinectClient</RootNamespace>
     <ProjectName>LiveScanClient</ProjectName>
-    <WindowsTargetPlatformVersion>10.0.17763.0</WindowsTargetPlatformVersion>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
   </PropertyGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>true</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>true</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
     <ConfigurationType>Application</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v141</PlatformToolset>
+    <PlatformToolset>v142</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>
@@ -193,12 +193,12 @@
   </ItemDefinitionGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">
-    <Import Project="..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0\build\native\Microsoft.Azure.Kinect.Sensor.targets" Condition="Exists('..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0\build\native\Microsoft.Azure.Kinect.Sensor.targets')" />
+    <Import Project="..\packages\Microsoft.Azure.Kinect.Sensor.1.4.1\build\native\Microsoft.Azure.Kinect.Sensor.targets" Condition="Exists('..\packages\Microsoft.Azure.Kinect.Sensor.1.4.1\build\native\Microsoft.Azure.Kinect.Sensor.targets')" />
   </ImportGroup>
   <Target Name="EnsureNuGetPackageBuildImports" BeforeTargets="PrepareForBuild">
     <PropertyGroup>
       <ErrorText>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}.</ErrorText>
     </PropertyGroup>
-    <Error Condition="!Exists('..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0\build\native\Microsoft.Azure.Kinect.Sensor.targets')" Text="$([System.String]::Format('$(ErrorText)', '..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0\build\native\Microsoft.Azure.Kinect.Sensor.targets'))" />
+    <Error Condition="!Exists('..\packages\Microsoft.Azure.Kinect.Sensor.1.4.1\build\native\Microsoft.Azure.Kinect.Sensor.targets')" Text="$([System.String]::Format('$(ErrorText)', '..\packages\Microsoft.Azure.Kinect.Sensor.1.4.1\build\native\Microsoft.Azure.Kinect.Sensor.targets'))" />
   </Target>
 </Project>

LiveScanClient/packages.config

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="utf-8"?>
 <packages>
-  <package id="Microsoft.Azure.Kinect.Sensor" version="1.2.0" targetFramework="native" />
+  <package id="Microsoft.Azure.Kinect.Sensor" version="1.4.1" targetFramework="native" />
 </packages>

include/LiveScanClient/azureKinectCapture.h

@@ -3,7 +3,10 @@
 #include "stdafx.h"
 #include "ICapture.h"
 #include <k4a/k4a.h>
+#include <opencv2/opencv.hpp>
 #include "utils.h"
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
 
 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();
 };

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

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

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);
 }
@@ -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<std::chrono::system_clock> 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<cv::Vec4b>(0)[0], cImg.rows * cImg.cols * sizeof(cv::Vec4b));
+
+
 	if (pColorRGBX == NULL)
 	{
-		nColorFrameHeight = k4a_image_get_height_pixels(colorImage);
+		nColorFrameHeight = k4a_image_get_height_pixels(colorImageDownscaled);
-		nColorFrameWidth = k4a_image_get_width_pixels(colorImage);
+		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
+	int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage);
-	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].X = pointCloudData[3 * (j + i * nColorFrameWidth) + 0] / 1000.0f;
-			pCameraSpacePoints[j + i * nColorFrameWidth].Y = pointCloudDataY[j + i * nColorFrameWidth + 1] / 1000.0f;
+			pCameraSpacePoints[j + i * nColorFrameWidth].Y = pointCloudData[3 * (j + i * nColorFrameWidth) + 1] / 1000.0f;
-			pCameraSpacePoints[j + i * nColorFrameWidth].Z = pointCloudDataZ[j + i * nColorFrameWidth + 2] / 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));
 }
+
+

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;
+		delete[] m_pColorInColorSpace;
-		m_pColorInDepthSpace = NULL;
+		m_pColorInColorSpace = NULL;
 	}
 
 	if (m_pDepthInColorSpace)
@@ -198,11 +198,11 @@ void LiveScanClient::UpdateFrame()
 	if (!bNewFrameAcquired)
 		return;
 
-	pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
+	pCapture->MapColorFrameToCameraSpace(m_pCameraSpaceCoordinates);
-	pCapture->MapColorFrameToDepthSpace(m_pColorInDepthSpace);
+
 	{
 		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)
 		{
@@ -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_pCameraSpaceCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
-				m_pColorInDepthSpace = new RGB[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
+				m_pColorInColorSpace = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
 			}
 			else
 			{
@@ -682,17 +682,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;
+	unsigned int nVertices = pCapture->nColorFrameHeight * pCapture->nColorFrameWidth;
-	std::vector<RGB> goodColorPoints;
 
-	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];
@@ -706,11 +712,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);
+			AllVertices[vertexIndex] = temp;
-			goodColorPoints.push_back(tempColor);
+			goodVerticesCount++;
+		}
+
+		else 
+		{
+			AllVertices[vertexIndex] = invalidPoint;
 		}
 	}
 
@@ -737,12 +752,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];
 	}