storytold/LiveScan3D
2
0
Fork 0
mirror of https://github.com/storytold/LiveScan3D.git synced 2026-10-09 00:09:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Finalized depth_image_to_Color_Camera mapping, added dynamic resize of the Color Image via OpenCV for improved performance and improved performance on the StoreFrame function by avoiding "vector.push_back"

Browse Source
This commit is contained in:
Christopher Remde
2020-07-02 13:50:24 +02:00
parent 4a4f9ae0ad
commit 00383d8867
25 changed files with 220 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ICP/ICP.vcxproj
+7 -7
View File
@@ -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
+7 -7
View File
@@ -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 -1
View File
@@ -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>
bin/ICP.dll
BIN
View File
Binary file not shown.
bin/ICP.exp
BIN
View File
Binary file not shown.
bin/ICP.iobj
BIN
View File
Binary file not shown.
bin/ICP.ipdb
BIN
View File
Binary file not shown.
bin/ICP.lib
BIN
View File
Binary file not shown.
bin/LiveScanClient.exe
BIN
View File
Binary file not shown.
bin/LiveScanClient.iobj
BIN
View File
Binary file not shown.
bin/LiveScanClient.ipdb
BIN
View File
Binary file not shown.
bin/LiveScanClientD.exe
BIN
View File
Binary file not shown.
bin/LiveScanPlayer.exe
BIN
View File
Binary file not shown.
bin/LiveScanPlayer.exe.config
+6
View File
@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="utf-8" ?>
<configuration>
<startup>
<supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.5" />
</startup>
</configuration>
bin/LiveScanServer.exe
BIN
View File
Binary file not shown.
bin/LiveScanServer.exe.config
+6
View File
@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="utf-8" ?>
<configuration>
<startup>
<supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.5" />
</startup>
</configuration>
bin/depthengine_2_0.dll
BIN
View File
Binary file not shown.
bin/k4a.dll
BIN
View File
Binary file not shown.
bin/k4arecord.dll
BIN
View File
Binary file not shown.
bin/settings.bin
BIN
View File
Binary file not shown.
include/LiveScanClient/azureKinectCapture.h
+14 -2
View File
@@ -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,25 @@ public:
void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints);
void MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace);
void MapColorFrameToDepthSpace(RGB *pColorInDepthSpace);
k4a_image_t Downscale_image_2x2_binning(const k4a_image_t color_image);
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 transformed_depth_image = NULL;
k4a_image_t colorImageInDepth = NULL;
k4a_image_t depthImageInColor = NULL;
k4a_image_t color_image_downscaled = NULL;
int color_image_downscaled_width;
int color_image_downscaled_height;
k4a_transformation_t transformation = NULL;
k4a_transformation_t transformation_color_downscaled = NULL;
k4a_image_t opencv_to_k4a_image(const cv::Mat cImg);
cv::Mat color_to_opencv(const k4a_image_t im);
void UpdateDepthPointCloud();
void UpdateDepthPointCloudForColorFrame();
};
include/LiveScanClient/liveScanClient.h
+1 -1
View File
@@ -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
+10
View File
@@ -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
+114 -81
View File
@@ -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(transformed_depth_image);
k4a_image_release(color_image_downscaled);
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;
color_image_downscaled_height = depth_camera_height;
color_image_downscaled_width = 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 calibration_color_downscaled;
memcpy(&calibration_color_downscaled, &calibration, sizeof(k4a_calibration_t));
calibration_color_downscaled.color_camera_calibration.resolution_width /= rescaleRatio;
calibration_color_downscaled.color_camera_calibration.resolution_height /= rescaleRatio;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.cx /= rescaleRatio;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.cy /= rescaleRatio;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.fx /= rescaleRatio;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.fy /= rescaleRatio;
transformation_color_downscaled = k4a_transformation_create(&calibration_color_downscaled);
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(color_image_downscaled);
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(color_image_downscaled_width, color_image_downscaled_height), cv::INTER_NEAREST);
//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), &color_image_downscaled);
memcpy(k4a_image_get_buffer(color_image_downscaled), &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(color_image_downscaled);
nColorFrameWidth = k4a_image_get_width_pixels(color_image_downscaled);
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(color_image_downscaled), 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 (transformed_depth_image == NULL)
{
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight, nColorFrameWidth * (int)sizeof(uint16_t), &transformed_depth_image);
}
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(transformation_color_downscaled, depthImage, transformed_depth_image);
k4a_transformation_depth_image_to_point_cloud(transformation_color_downscaled, transformed_depth_image, 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)
{
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,7 +264,7 @@ void AzureKinectCapture::MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace)
&depthImageInColor);
}
k4a_transformation_depth_image_to_color_camera(transformation, depthImage, depthImageInColor);
k4a_transformation_depth_image_to_color_camera(transformation_color_downscaled, depthImage, depthImageInColor);
memcpy(pDepthInColorSpace, k4a_image_get_buffer(depthImageInColor), nColorFrameHeight * nColorFrameWidth * (int)sizeof(uint16_t));
}
@@ -257,7 +278,19 @@ void AzureKinectCapture::MapColorFrameToDepthSpace(RGB *pColorInDepthSpace)
&colorImageInDepth);
}
k4a_transformation_color_image_to_depth_camera(transformation, depthImage, colorImage, colorImageInDepth);
k4a_transformation_color_image_to_depth_camera(transformation_color_downscaled, depthImage, colorImage, colorImageInDepth);
memcpy(pColorInDepthSpace, k4a_image_get_buffer(colorImageInDepth), nDepthFrameHeight * nDepthFrameWidth * 4 * (int)sizeof(uint8_t));
}
cv::Mat AzureKinectCapture::color_to_opencv(const k4a_image_t im)
{
cv::Mat cv_image_with_alpha(k4a_image_get_height_pixels(im), k4a_image_get_width_pixels(im), CV_8UC4, k4a_image_get_buffer(im));
cv::Mat cv_image_no_alpha;
cv::cvtColor(cv_image_with_alpha, cv_image_no_alpha, cv::COLOR_BGRA2BGR);
return cv_image_no_alpha;
}
src/LiveScanClient/liveScanClient.cpp
+49 -16
View File
@@ -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,10 @@ 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];
//CameraSpaceCoordinate sollen so groß sein wie der Color Frame
m_pCameraSpaceCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
//ColorInDepthSpace ebenso, nur es ist nicht mehr im Depth Space sonder Color Space
m_pColorInColorSpace = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
}
else
{
@@ -682,17 +684,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 +714,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 +754,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];
}