mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-09 00:09:58 +00:00
Merge pull request #48 from ChristopherRemde/AzureKinect_Mapping_Fix
Changed Color Image Mapping to be more accurate
This commit is contained in:
Marek
+7
-7
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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();
|
||||
};
|
||||
@@ -73,7 +73,7 @@ private:
|
||||
DWORD m_nFramesSinceUpdate;
|
||||
|
||||
Point3f* m_pCameraSpaceCoordinates;
|
||||
RGB* m_pColorInDepthSpace;
|
||||
RGB* m_pColorInColorSpace;
|
||||
UINT16* m_pDepthInColorSpace;
|
||||
|
||||
// Direct2D
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
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));
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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<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_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<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];
|
||||
@@ -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);
|
||||
goodColorPoints.push_back(tempColor);
|
||||
AllVertices[vertexIndex] = temp;
|
||||
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];
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user