Created
November 12, 2014 15:35
-
-
Save UnaNancyOwen/2b1fb050a8189fb35e7a to your computer and use it in GitHub Desktop.
Drawing the Point Cloud retrieved from Kinect v1 using Point Cloud Library without Grabber
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment