Drawing the Point Cloud retrieved from Kinect v1 using Point Cloud Library without Grabber
#include "stdafx.h"
#define NOMINMAX
#include <Windows.h>
#include <NuiApi.h>
#include <pcl/visualization/cloud_viewer.h>
int _tmain( int argc, _TCHAR* argv[] )
{
// Create Sensor Instance
INuiSensor* sensor;
HRESULT result = S_OK;
result = NuiCreateSensorByIndex( 0, &sensor );
if( FAILED( result ) ){
std::cerr << "Error : NuiCreateSensorByIndex" << std::endl;
return -1;
}
// Initialize Sensor
result = sensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX );
if( FAILED( result ) ){
std::cerr << "Error : INuiSensor::NuiInitialize" << std::endl;
return -1;
}
// Open Color Stream
HANDLE colorEvent = INVALID_HANDLE_VALUE;
HANDLE colorHandle = INVALID_HANDLE_VALUE;
colorEvent = CreateEvent( nullptr, true, false, nullptr );
result = sensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, colorEvent, &colorHandle );
if( FAILED( result ) ){
std::cerr << "Error : INuiSensor::NuiImageStreamOpen( Color )" << std::endl;
return -1;
}
// Open Depth Stream
HANDLE depthEvent = INVALID_HANDLE_VALUE;
HANDLE depthHandle = INVALID_HANDLE_VALUE;
depthEvent = CreateEvent( nullptr, true, false, nullptr );
result = sensor->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_640x480, 0, 2, depthEvent, &depthHandle );
if( FAILED( result ) ){
std::cerr << "Error : INuiSensor::NuiImageStreamOpen( Depth )" << std::endl;
return -1;
}
// Retrieved Image Size from Stream Resolution
unsigned long refWidth = 0;
unsigned long refHeight = 0;
NuiImageResolutionToSize( NUI_IMAGE_RESOLUTION_640x480, refWidth, refHeight );
int width = static_cast<int>( refWidth );
int height = static_cast<int>( refHeight );
// Retrieved Coordinate Mapper
INuiCoordinateMapper* coordinateMapper;
result = sensor->NuiGetCoordinateMapper( &coordinateMapper );
if( FAILED( result ) ){
std::cerr << "Error : INuiSensor::NuiGetCoordinateMapper" << std::endl;
return -1;
}
HANDLE events[2] = { colorEvent, depthEvent };
// Create Cloud Viewer
pcl::visualization::CloudViewer viewer( "Point Cloud Viewer" );
while( !viewer.wasStopped() ){
// Waiting for Frame Update
ResetEvent( colorEvent );
ResetEvent( depthEvent );
WaitForMultipleObjects( ARRAYSIZE( events ), events, true, INFINITE );
// Retrieved Color Data from Kinect
NUI_IMAGE_FRAME colorImageFrame = { 0 };
result = sensor->NuiImageStreamGetNextFrame( colorHandle, 0, &colorImageFrame );
if( FAILED( result ) ){
std::cerr << "Error : INuiSensor::NuiImageStreamGetNextFrame( Color )" << std::endl;
return -1;
}
INuiFrameTexture* colorFrameTexture = colorImageFrame.pFrameTexture;
NUI_LOCKED_RECT colorLockedRect;
colorFrameTexture->LockRect( 0, &colorLockedRect, nullptr, 0 );
// Retrieved Depth Data from Kinect
NUI_IMAGE_FRAME depthPlayerImageFrame = { 0 };
result = sensor->NuiImageStreamGetNextFrame( depthHandle, 0, &depthPlayerImageFrame );
if( FAILED( result ) ){
std::cerr << "Error : INuiSensor::NuiImageStreamGetNextFrame( Depth )" << std::endl;
return -1;
}
BOOL nearMode = false;
INuiFrameTexture* depthFrameTexture = nullptr;
sensor->NuiImageFrameGetDepthImagePixelFrameTexture( depthHandle, &depthPlayerImageFrame, &nearMode, &depthFrameTexture );
NUI_LOCKED_RECT depthPlayerLockedRect;
depthFrameTexture->LockRect( 0, &depthPlayerLockedRect, nullptr, 0 );
NUI_DEPTH_IMAGE_PIXEL* depthPixel = reinterpret_cast<NUI_DEPTH_IMAGE_PIXEL*>( depthPlayerLockedRect.pBits );
// Create Point Cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud( new pcl::PointCloud<pcl::PointXYZRGB>() );
pointcloud->width = static_cast<uint32_t>( width );
pointcloud->height = static_cast<uint32_t>( height );
pointcloud->is_dense = false;
for( int y = 0; y < height; y++ ){
for( int x = 0; x < width; x++ ){
pcl::PointXYZRGB point;
NUI_DEPTH_IMAGE_POINT depthPoint;
depthPoint.x = x;
depthPoint.y = y;
depthPoint.depth = depthPixel[y * width + x].depth;
// Coordinate Mapping Depth to Real Space, and Setting PointCloud XYZ
Vector4 skeletonPoint;
coordinateMapper->MapDepthPointToSkeletonPoint( NUI_IMAGE_RESOLUTION_640x480, &depthPoint, &skeletonPoint );
point.x = skeletonPoint.x;
point.y = skeletonPoint.y;
point.z = skeletonPoint.z;
// Coordinate Mapping Depth to Color Space, and Setting PointCloud RGB
NUI_COLOR_IMAGE_POINT colorPoint;
coordinateMapper->MapDepthPointToColorPoint( NUI_IMAGE_RESOLUTION_640x480, &depthPoint, NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, &colorPoint );
if( 0 <= colorPoint.x && colorPoint.x < width && 0 <= colorPoint.y && colorPoint.y < height ){
unsigned int index = colorPoint.y * colorLockedRect.Pitch + colorPoint.x * 4;
point.b = colorLockedRect.pBits[index + 0];
point.g = colorLockedRect.pBits[index + 1];
point.r = colorLockedRect.pBits[index + 2];
}
pointcloud->push_back( point );
}
}
// Show Point Cloud on Cloud Viewer
viewer.showCloud( pointcloud );
// Release Frame
colorFrameTexture->UnlockRect( 0 );
depthFrameTexture->UnlockRect( 0 );
sensor->NuiImageStreamReleaseFrame( colorHandle, &colorImageFrame );
sensor->NuiImageStreamReleaseFrame( depthHandle, &depthPlayerImageFrame );
// Input Key ( Exit ESC key )
if( GetKeyState( VK_ESCAPE ) < 0 ){
break;
}
}
// End Processing
sensor->NuiShutdown();
coordinateMapper->Release();
CloseHandle( colorEvent );
CloseHandle( depthEvent );
return 0;
}