mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-12 23:48:55 +00:00
Merge branch 'AzureKinect' into Azure_Kinect_Temp_Sync
This commit is contained in:
Christopher Remde
@@ -1,4 +1,7 @@
|
||||
#include "azureKinectCapture.h"
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <chrono>
|
||||
|
||||
AzureKinectCapture::AzureKinectCapture()
|
||||
@@ -10,9 +13,11 @@ AzureKinectCapture::~AzureKinectCapture()
|
||||
{
|
||||
k4a_image_release(colorImage);
|
||||
k4a_image_release(depthImage);
|
||||
k4a_image_release(depthPointCloudImage);
|
||||
k4a_image_release(pointCloudImage);
|
||||
k4a_image_release(colorImageInDepth);
|
||||
k4a_image_release(depthImageInColor);
|
||||
k4a_image_release(transformedDepthImage);
|
||||
k4a_image_release(colorImageDownscaled);
|
||||
k4a_transformation_destroy(transformation);
|
||||
k4a_device_close(kinectSensor);
|
||||
}
|
||||
@@ -74,7 +79,7 @@ bool AzureKinectCapture::Initialize(SYNC_STATE state, int syncOffsetMultiplier)
|
||||
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
|
||||
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
|
||||
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
|
||||
config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
|
||||
config.color_resolution = K4A_COLOR_RESOLUTION_720P;
|
||||
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
|
||||
config.synchronized_images_only = true;
|
||||
|
||||
@@ -122,9 +127,57 @@ bool AzureKinectCapture::Initialize(SYNC_STATE state, int syncOffsetMultiplier)
|
||||
|
||||
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();
|
||||
bool bTemp;
|
||||
do
|
||||
@@ -184,9 +237,9 @@ bool AzureKinectCapture::AcquireFrame()
|
||||
}
|
||||
|
||||
k4a_image_release(colorImage);
|
||||
k4a_image_release(colorImageDownscaled);
|
||||
k4a_image_release(depthImage);
|
||||
|
||||
|
||||
colorImage = k4a_capture_get_color_image(capture);
|
||||
depthImage = k4a_capture_get_depth_image(capture);
|
||||
|
||||
@@ -198,10 +251,23 @@ bool AzureKinectCapture::AcquireFrame()
|
||||
return false;
|
||||
}
|
||||
|
||||
//We need to resize the color image, so that it's height fits the depth camera height, while preserving the aspect ratio of the color camera:
|
||||
|
||||
//Convert the k4a_image to an OpenCV Mat
|
||||
cv::Mat cImg = cv::Mat(k4a_image_get_height_pixels(colorImage), k4a_image_get_width_pixels(colorImage), CV_8UC4, k4a_image_get_buffer(colorImage));
|
||||
|
||||
//Resize the k4a_image to the precalculated size. Takes quite along time, maybe there is a faster algorithm?
|
||||
cv::resize(cImg, cImg, cv::Size(colorImageDownscaledWidth, colorImageDownscaledHeight), cv::INTER_LINEAR);
|
||||
|
||||
//Create a k4a_image from the resized OpenCV Mat. Code taken from here: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/978#issuecomment-566002061
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32, cImg.cols, cImg.rows, cImg.cols * 4 * (int)sizeof(uint8_t), &colorImageDownscaled);
|
||||
memcpy(k4a_image_get_buffer(colorImageDownscaled), &cImg.ptr<cv::Vec4b>(0)[0], cImg.rows * cImg.cols * sizeof(cv::Vec4b));
|
||||
|
||||
|
||||
if (pColorRGBX == NULL)
|
||||
{
|
||||
nColorFrameHeight = k4a_image_get_height_pixels(colorImage);
|
||||
nColorFrameWidth = k4a_image_get_width_pixels(colorImage);
|
||||
nColorFrameHeight = k4a_image_get_height_pixels(colorImageDownscaled);
|
||||
nColorFrameWidth = k4a_image_get_width_pixels(colorImageDownscaled);
|
||||
pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight];
|
||||
}
|
||||
|
||||
@@ -212,7 +278,9 @@ bool AzureKinectCapture::AcquireFrame()
|
||||
pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth];
|
||||
}
|
||||
|
||||
memcpy(pColorRGBX, k4a_image_get_buffer(colorImage), nColorFrameWidth * nColorFrameHeight * sizeof(RGB));
|
||||
|
||||
|
||||
memcpy(pColorRGBX, k4a_image_get_buffer(colorImageDownscaled), nColorFrameWidth * nColorFrameHeight * sizeof(RGB));
|
||||
memcpy(pDepth, k4a_image_get_buffer(depthImage), nDepthFrameHeight * nDepthFrameWidth * sizeof(UINT16));
|
||||
|
||||
|
||||
@@ -221,6 +289,98 @@ bool AzureKinectCapture::AcquireFrame()
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void AzureKinectCapture::UpdateDepthPointCloudForColorFrame()
|
||||
{
|
||||
if (transformedDepthImage == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight, nColorFrameWidth * (int)sizeof(uint16_t), &transformedDepthImage);
|
||||
}
|
||||
|
||||
if (pointCloudImage == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM, nColorFrameWidth, nColorFrameHeight, nColorFrameWidth * 3 * (int)sizeof(int16_t), &pointCloudImage);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_color_camera(transformationColorDownscaled, depthImage, transformedDepthImage);
|
||||
|
||||
k4a_transformation_depth_image_to_point_cloud(transformationColorDownscaled, transformedDepthImage, K4A_CALIBRATION_TYPE_COLOR, pointCloudImage);
|
||||
}
|
||||
|
||||
void AzureKinectCapture::UpdateDepthPointCloud()
|
||||
{
|
||||
if (pointCloudImage == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * 3 * (int)sizeof(int16_t),
|
||||
&pointCloudImage);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_point_cloud(transformation, depthImage, K4A_CALIBRATION_TYPE_DEPTH, pointCloudImage);
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
UpdateDepthPointCloud();
|
||||
|
||||
int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage);
|
||||
|
||||
for (int i = 0; i < nDepthFrameHeight; i++)
|
||||
{
|
||||
for (int j = 0; j < nDepthFrameWidth; j++)
|
||||
{
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].X = pointCloudData[3 * (j + i * nDepthFrameWidth) + 0] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].Y = pointCloudData[3 * (j + i * nDepthFrameWidth) + 1] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].Z = pointCloudData[3 * (j + i * nDepthFrameWidth) + 2] / 1000.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapColorFrameToCameraSpace(Point3f* pCameraSpacePoints)
|
||||
{
|
||||
UpdateDepthPointCloudForColorFrame();
|
||||
|
||||
int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(pointCloudImage);
|
||||
|
||||
for (int i = 0; i < nColorFrameHeight; i++)
|
||||
{
|
||||
for (int j = 0; j < nColorFrameWidth; j++)
|
||||
{
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].X = pointCloudData[3 * (j + i * nColorFrameWidth) + 0] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].Y = pointCloudData[3 * (j + i * nColorFrameWidth) + 1] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].Z = pointCloudData[3 * (j + i * nColorFrameWidth) + 2] / 1000.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace)
|
||||
{
|
||||
if (depthImageInColor == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(uint16_t),
|
||||
&depthImageInColor);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_color_camera(transformationColorDownscaled, depthImage, depthImageInColor);
|
||||
|
||||
memcpy(pDepthInColorSpace, k4a_image_get_buffer(depthImageInColor), nColorFrameHeight * nColorFrameWidth * (int)sizeof(uint16_t));
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapColorFrameToDepthSpace(RGB* pColorInDepthSpace)
|
||||
{
|
||||
if (colorImageInDepth == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * 4 * (int)sizeof(uint8_t),
|
||||
&colorImageInDepth);
|
||||
}
|
||||
|
||||
k4a_transformation_color_image_to_depth_camera(transformationColorDownscaled, depthImage, colorImage, colorImageInDepth);
|
||||
|
||||
memcpy(pColorInDepthSpace, k4a_image_get_buffer(colorImageInDepth), nDepthFrameHeight * nDepthFrameWidth * 4 * (int)sizeof(uint8_t));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Enables/Disables Auto Exposure and/or sets the exposure to a step value between -11 and -5
|
||||
/// The kinect supports exposure values up to 1, but these are only available in lower FPS modes (5 or 15 FPS)
|
||||
@@ -253,145 +413,7 @@ void AzureKinectCapture::SetExposureState(bool enableAutoExposure, int exposureS
|
||||
autoExposureEnabled = false;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@@ -47,7 +47,7 @@ LiveScanClient::LiveScanClient() :
|
||||
m_pDrawColor(NULL),
|
||||
m_pDepthRGBX(NULL),
|
||||
m_pCameraSpaceCoordinates(NULL),
|
||||
m_pColorInDepthSpace(NULL),
|
||||
m_pColorInColorSpace(NULL),
|
||||
m_pDepthInColorSpace(NULL),
|
||||
m_bCalibrate(false),
|
||||
m_bFilter(false),
|
||||
@@ -111,10 +111,10 @@ LiveScanClient::~LiveScanClient()
|
||||
m_pCameraSpaceCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pColorInDepthSpace)
|
||||
if (m_pColorInColorSpace)
|
||||
{
|
||||
delete[] m_pColorInDepthSpace;
|
||||
m_pColorInDepthSpace = NULL;
|
||||
delete[] m_pColorInColorSpace;
|
||||
m_pColorInColorSpace = NULL;
|
||||
}
|
||||
|
||||
if (m_pDepthInColorSpace)
|
||||
@@ -202,11 +202,11 @@ void LiveScanClient::UpdateFrame()
|
||||
if (!bNewFrameAcquired)
|
||||
return;
|
||||
|
||||
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
|
||||
pCapture->MapColorFrameToDepthSpace(m_pColorInDepthSpace);
|
||||
pCapture->MapColorFrameToCameraSpace(m_pCameraSpaceCoordinates);
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorInDepthSpace, pCapture->vBodies, pCapture->pBodyIndex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, pCapture->pColorRGBX, pCapture->vBodies, pCapture->pBodyIndex);
|
||||
|
||||
if (m_bCaptureFrame)
|
||||
{
|
||||
@@ -287,11 +287,9 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
|
||||
calibration.LoadCalibration(pCapture->serialNumber);
|
||||
m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
m_pDepthInColorSpace = new UINT16[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
|
||||
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pColorInDepthSpace = new RGB[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
|
||||
pCapture->SetExposureState(true, 0);
|
||||
m_pCameraSpaceCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
m_pColorInColorSpace = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
pCapture->SetExposureState(true, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -854,17 +852,23 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Body> &bodies, BYTE* bodyIndex)
|
||||
{
|
||||
std::vector<Point3f> goodVertices;
|
||||
std::vector<RGB> goodColorPoints;
|
||||
unsigned int nVertices = pCapture->nColorFrameHeight * pCapture->nColorFrameWidth;
|
||||
|
||||
unsigned int nVertices = pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight;
|
||||
//To save some processing cost, we allocate a full frame size (nVertices) of a Point3f Vector beforehand
|
||||
//instead of using push_back for each vertice. Even though we have to copy the vertices into a clean array
|
||||
//later and it uses a little bit more RAM, this gives us a nice speed increase for this function, around 25-50%
|
||||
Point3f invalidPoint = Point3f(0, 0, 0, true);
|
||||
vector<Point3f> AllVertices(nVertices);
|
||||
int goodVerticesCount = 0;
|
||||
|
||||
for (unsigned int vertexIndex = 0; vertexIndex < nVertices; vertexIndex++)
|
||||
{
|
||||
if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size())
|
||||
continue;
|
||||
|
||||
if (vertices[vertexIndex].Z >= 0 && colorInDepth[vertexIndex].rgbReserved == 255)
|
||||
//As the resizing function doesn't return a valid RGB-Reserved value which indicates that this pixel is invalid,
|
||||
//we cut all vertices under a distance of 0.0001mm, as the invalid vertices always have a Z-Value of 0
|
||||
if (vertices[vertexIndex].Z >= 0.0001 && colorInDepth[vertexIndex].rgbReserved == 255)
|
||||
{
|
||||
Point3f temp = vertices[vertexIndex];
|
||||
RGB tempColor = colorInDepth[vertexIndex];
|
||||
@@ -877,12 +881,21 @@ 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])
|
||||
|| temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5])
|
||||
{
|
||||
AllVertices[vertexIndex] = invalidPoint;
|
||||
continue;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
goodVertices.push_back(temp);
|
||||
goodColorPoints.push_back(tempColor);
|
||||
AllVertices[vertexIndex] = temp;
|
||||
goodVerticesCount++;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
AllVertices[vertexIndex] = invalidPoint;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -909,12 +922,28 @@ void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Bod
|
||||
// }
|
||||
//}
|
||||
|
||||
vector<Point3f> goodVertices(goodVerticesCount);
|
||||
vector<RGB> goodColorPoints(goodVerticesCount);
|
||||
int goodVerticesShortCounter = 0;
|
||||
|
||||
//Copy all valid vertices into a clean vector
|
||||
for (unsigned int i = 0; i < AllVertices.size(); i++)
|
||||
{
|
||||
if (!AllVertices[i].Invalid)
|
||||
{
|
||||
goodVertices[goodVerticesShortCounter] = AllVertices[i];
|
||||
goodColorPoints[goodVerticesShortCounter] = colorInDepth[i];
|
||||
goodVerticesShortCounter++;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bFilter)
|
||||
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
|
||||
|
||||
vector<Point3s> goodVerticesShort(goodVertices.size());
|
||||
|
||||
for (unsigned int i = 0; i < goodVertices.size(); i++)
|
||||
vector<Point3s> goodVerticesShort(goodVertices.size());
|
||||
|
||||
for (size_t i = 0; i < goodVertices.size(); i++)
|
||||
{
|
||||
goodVerticesShort[i] = goodVertices[i];
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user