c_cpp 使用不带Grabber的Point Cloud Library绘制从Kinect v1检索到的Point Cloud

Posted 2021-05-14

#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;
}