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

Merge branch 'AzureKinect' into Azure_Kinect_Temp_Sync

Browse Source
This commit is contained in:
Christopher Remde
2020-10-20 17:55:08 +02:00
committed by GitHub
parent 76bf5e9dba 13e634f76a
commit 6b983126f3
5 changed files with 244 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/LiveScanClient/azureKinectCapture.h
+15 -8
View File
@@ -3,9 +3,10 @@
#include "stdafx.h" #include "stdafx.h"
#include "ICapture.h" #include "ICapture.h"
#include <k4a/k4a.h> #include <k4a/k4a.h>
#include <iostream> #include <opencv2/opencv.hpp>
using namespace std; #include "utils.h"
//#include "utils.h" #include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
class AzureKinectCapture : public ICapture class AzureKinectCapture : public ICapture
{ {
@@ -25,17 +26,22 @@ public:
int GetDeviceIndex(); int GetDeviceIndex();
void SetExposureState(bool enableAutoExposure, int exposureStep); void SetExposureState(bool enableAutoExposure, int exposureStep);
private: private:
k4a_device_t kinectSensor = NULL; k4a_device_t kinectSensor = NULL;
int32_t captureTimeoutMs = 1000; int32_t captureTimeoutMs = 1000;
k4a_image_t colorImage = NULL; k4a_image_t colorImage = NULL;
k4a_image_t depthImage = 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 colorImageInDepth = NULL;
k4a_image_t depthImageInColor = NULL; k4a_image_t depthImageInColor = NULL;
k4a_transformation_t transformation = NULL; k4a_image_t colorImageDownscaled = NULL;
k4a_transformation_t transformationColorDownscaled = NULL;
k4a_transformation_t transformation = NULL;
int colorImageDownscaledWidth;
int colorImageDownscaledHeight;
bool syncInConnected = false; bool syncInConnected = false;
bool syncOutConnected = false; bool syncOutConnected = false;
uint64_t currentTimeStamp = 0; uint64_t currentTimeStamp = 0;
@@ -44,6 +50,7 @@ private:
int restartAttempts = 0; int restartAttempts = 0;
bool autoExposureEnabled = true; bool autoExposureEnabled = true;
int exposureTimeStep = 0; int exposureTimeStep = 0;
void UpdateDepthPointCloud(); void UpdateDepthPointCloud();
void UpdateDepthPointCloudForColorFrame();
}; };
include/LiveScanClient/liveScanClient.h
+1 -1
View File
@@ -86,7 +86,7 @@ private:
int frameRecordCounter; int frameRecordCounter;
Point3f* m_pCameraSpaceCoordinates; Point3f* m_pCameraSpaceCoordinates;
RGB* m_pColorInDepthSpace; RGB* m_pColorInColorSpace;
UINT16* m_pDepthInColorSpace; UINT16* m_pDepthInColorSpace;
// Direct2D // Direct2D
include/LiveScanClient/utils.h
+10
View File
@@ -59,16 +59,26 @@ typedef struct Point3f
this->X = 0; this->X = 0;
this->Y = 0; this->Y = 0;
this->Z = 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) Point3f(float X, float Y, float Z)
{ {
this->X = X; this->X = X;
this->Y = Y; this->Y = Y;
this->Z = Z; this->Z = Z;
this->Invalid = false;
} }
float X; float X;
float Y; float Y;
float Z; float Z;
bool Invalid = false;
} Point3f; } Point3f;
typedef struct Point3s typedef struct Point3s
src/LiveScanClient/azureKinectCapture.cpp
+168 -147
View File
@@ -1,4 +1,7 @@
#include "azureKinectCapture.h" #include "azureKinectCapture.h"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <chrono> #include <chrono>
AzureKinectCapture::AzureKinectCapture() AzureKinectCapture::AzureKinectCapture()
@@ -10,9 +13,11 @@ AzureKinectCapture::~AzureKinectCapture()
{ {
k4a_image_release(colorImage); k4a_image_release(colorImage);
k4a_image_release(depthImage); k4a_image_release(depthImage);
k4a_image_release(depthPointCloudImage); k4a_image_release(pointCloudImage);
k4a_image_release(colorImageInDepth); k4a_image_release(colorImageInDepth);
k4a_image_release(depthImageInColor); k4a_image_release(depthImageInColor);
k4a_image_release(transformedDepthImage);
k4a_image_release(colorImageDownscaled);
k4a_transformation_destroy(transformation); k4a_transformation_destroy(transformation);
k4a_device_close(kinectSensor); 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; k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
config.camera_fps = K4A_FRAMES_PER_SECOND_30; config.camera_fps = K4A_FRAMES_PER_SECOND_30;
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32; 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.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
config.synchronized_images_only = true; config.synchronized_images_only = true;
@@ -122,9 +127,57 @@ bool AzureKinectCapture::Initialize(SYNC_STATE state, int syncOffsetMultiplier)
transformation = k4a_transformation_create(&calibration); transformation = k4a_transformation_create(&calibration);
//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) //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)
{
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now(); std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
bool bTemp; bool bTemp;
do do
@@ -184,9 +237,9 @@ bool AzureKinectCapture::AcquireFrame()
} }
k4a_image_release(colorImage); k4a_image_release(colorImage);
k4a_image_release(colorImageDownscaled);
k4a_image_release(depthImage); k4a_image_release(depthImage);
colorImage = k4a_capture_get_color_image(capture); colorImage = k4a_capture_get_color_image(capture);
depthImage = k4a_capture_get_depth_image(capture); depthImage = k4a_capture_get_depth_image(capture);
@@ -198,10 +251,23 @@ bool AzureKinectCapture::AcquireFrame()
return false; 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) 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]; pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight];
} }
@@ -212,7 +278,9 @@ bool AzureKinectCapture::AcquireFrame()
pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth]; 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)); memcpy(pDepth, k4a_image_get_buffer(depthImage), nDepthFrameHeight * nDepthFrameWidth * sizeof(UINT16));
@@ -221,6 +289,98 @@ bool AzureKinectCapture::AcquireFrame()
return true; 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> /// <summary>
/// Enables/Disables Auto Exposure and/or sets the exposure to a step value between -11 and -5 /// 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) /// The kinect supports exposure values up to 1, but these are only available in lower FPS modes (5 or 15 FPS)
@@ -253,145 +413,7 @@ void AzureKinectCapture::SetExposureState(bool enableAutoExposure, int exposureS
autoExposureEnabled = false; autoExposureEnabled = false;
exposureTimeStep = exposureStep; exposureTimeStep = exposureStep;
} }
}
}
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));
} }
@@ -435,4 +457,3 @@ int AzureKinectCapture::GetDeviceIndex()
{ {
return deviceIDForRestart; return deviceIDForRestart;
} }
src/LiveScanClient/liveScanClient.cpp
+50 -21
View File
@@ -47,7 +47,7 @@ LiveScanClient::LiveScanClient() :
m_pDrawColor(NULL), m_pDrawColor(NULL),
m_pDepthRGBX(NULL), m_pDepthRGBX(NULL),
m_pCameraSpaceCoordinates(NULL), m_pCameraSpaceCoordinates(NULL),
m_pColorInDepthSpace(NULL), m_pColorInColorSpace(NULL),
m_pDepthInColorSpace(NULL), m_pDepthInColorSpace(NULL),
m_bCalibrate(false), m_bCalibrate(false),
m_bFilter(false), m_bFilter(false),
@@ -111,10 +111,10 @@ LiveScanClient::~LiveScanClient()
m_pCameraSpaceCoordinates = NULL; m_pCameraSpaceCoordinates = NULL;
} }
if (m_pColorInDepthSpace) if (m_pColorInColorSpace)
{ {
delete[] m_pColorInDepthSpace; delete[] m_pColorInColorSpace;
m_pColorInDepthSpace = NULL; m_pColorInColorSpace = NULL;
} }
if (m_pDepthInColorSpace) if (m_pDepthInColorSpace)
@@ -202,11 +202,11 @@ void LiveScanClient::UpdateFrame()
if (!bNewFrameAcquired) if (!bNewFrameAcquired)
return; return;
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates); pCapture->MapColorFrameToCameraSpace(m_pCameraSpaceCoordinates);
pCapture->MapColorFrameToDepthSpace(m_pColorInDepthSpace);
{ {
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex); 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) if (m_bCaptureFrame)
{ {
@@ -287,11 +287,9 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
calibration.LoadCalibration(pCapture->serialNumber); calibration.LoadCalibration(pCapture->serialNumber);
m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
m_pDepthInColorSpace = new UINT16[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight]; m_pDepthInColorSpace = new UINT16[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
m_pCameraSpaceCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; m_pColorInColorSpace = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
m_pColorInDepthSpace = new RGB[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight]; pCapture->SetExposureState(true, 0);
pCapture->SetExposureState(true, 0);
} }
else 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) 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++) for (unsigned int vertexIndex = 0; vertexIndex < nVertices; vertexIndex++)
{ {
if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size()) if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size())
continue; 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]; Point3f temp = vertices[vertexIndex];
RGB tempColor = colorInDepth[vertexIndex]; RGB tempColor = colorInDepth[vertexIndex];
@@ -877,12 +881,21 @@ void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Bod
if (temp.X < m_vBounds[0] || temp.X > m_vBounds[3] if (temp.X < m_vBounds[0] || temp.X > m_vBounds[3]
|| temp.Y < m_vBounds[1] || temp.Y > m_vBounds[4] || temp.Y < m_vBounds[1] || temp.Y > m_vBounds[4]
|| temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5]) || temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5])
{
AllVertices[vertexIndex] = invalidPoint;
continue; continue;
}
} }
goodVertices.push_back(temp); AllVertices[vertexIndex] = temp;
goodColorPoints.push_back(tempColor); 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) if (m_bFilter)
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold); filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
vector<Point3s> goodVerticesShort(goodVertices.size());
for (unsigned int i = 0; i < goodVertices.size(); i++) vector<Point3s> goodVerticesShort(goodVertices.size());
for (size_t i = 0; i < goodVertices.size(); i++)
{ {
goodVerticesShort[i] = goodVertices[i]; goodVerticesShort[i] = goodVertices[i];
} }