Kinect 距離による表示フィルター RGBと距離カメラの位置補正を含む

距離カメラの情報が1次元データなので、RGBカメラ座標に変換するときに少し工夫する。

	#include <iostream>
	#include <sstream>
	// この順番でインクルード
	#include <Windows.h>
	#include <NuiApi.h>
	#include <opencv2/opencv.hpp>
	#define ERROR_CHECK( ret ) \
	if ( ret != S_OK ) { \
	std::cout << "failed " #ret " " << ret << std::endl; \
	exit( 1 ); \
	}
	const NUI_IMAGE_RESOLUTION CAMERA_RESOLUTION = NUI_IMAGE_RESOLUTION_640x480;
	const int MAX_DISTANCE = 300;
	class KinectSample{
	private:
	INuiSensor* kinect;
	// RGBカメラ用ハンドル
	HANDLE imageStreamHandle;
	// 距離カメラ用ハンドル
	HANDLE depthStreamHandle;
	// RGB及び距離カメラのフレーム更新イベントを待つためのイベントハンドル
	HANDLE streamEvent;
	DWORD width;
	DWORD height;
	public:
	KinectSample();
	~KinectSample();
	public:
	void initialize();
	void run();
	private:
	void createInstance();
	void drawRgbImage(cv::Mat& image);
	void drawDepthImage(cv::Mat& image);
	};
	KinectSample::KinectSample(){
	}
	// デストラクタ
	KinectSample::~KinectSample(){
	if(kinect != 0){
	// Kinectの終了処理
	kinect->NuiShutdown();
	// Kinectのインスタンス開放処理
	kinect->Release();
	}
	}
	void KinectSample::initialize(){
	createInstance();
	// RGBカメラ、距離カメラの使用。パイプで区切って設定する。
	ERROR_CHECK(kinect->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH));
	/* RGB及び距離カメラをそれぞれ初期化する*/
	// RGBカメラ。NUI_IMAGE_TYPE_COLORはRGBカメラの意味。解像度にCAMERA_RESOLUTION、ストリームハンドルのポインタを渡す
	ERROR_CHECK(kinect->NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, CAMERA_RESOLUTION, 0,2,0, &imageStreamHandle));
	// 距離カメラ
	ERROR_CHECK(kinect->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH, CAMERA_RESOLUTION, 0,2,0, &depthStreamHandle));
	// フレーム更新イベントのハンドル作成(Win32ApiのCreateEventでハンドラーを作成)
	streamEvent = ::CreateEvent(0, TRUE, FALSE, 0);
	// kinectオブジェクトにフレーム更新イベントを設定する
	ERROR_CHECK(kinect->NuiSetFrameEndEvent(streamEvent, 0));
	// 指定解像度の画面サイズを取得
	::NuiImageResolutionToSize(CAMERA_RESOLUTION, width, height);
	}
	void KinectSample::createInstance(){
	int count = 0;
	// Kinect接続数
	ERROR_CHECK(::NuiGetSensorCount(&count));
	if(count == 0){
	throw std::runtime_error("Kinectを接続してください");
	}
	// 1台目 インデックス: 0 のキネクトインスタンスを作成
	ERROR_CHECK(::NuiCreateSensorByIndex(0, &kinect));
	HRESULT status = kinect->NuiStatus();
	if(status != S_OK){
	throw std::runtime_error("Kinect が利用可能ではありません");
	}
	}
	// RGBカメラの更新されたフレームデータの取得＆処理
	void KinectSample::drawRgbImage(cv::Mat& image){
	NUI_IMAGE_FRAME imageFrame = {0};
	// 新しいフレームの取得（runで更新待ちをしているので、待ち時間を 0 にして、すぐに取得する）
	ERROR_CHECK(kinect->NuiImageStreamGetNextFrame(imageStreamHandle, 0, &imageFrame));
	// フレームの画像データ（更新画像データはpFrameTextureが持っていて、それをLockRectで呼び出してNUI_LOCKED_RECT型で取得する）
	NUI_LOCKED_RECT colorData;
	imageFrame.pFrameTexture->LockRect(0, &colorData, 0, 0);
	// NUI_LOCKED_RECTの画像データをMat型にコピーする。データは1ピクセルあたり、8bitのRGBデータと無効値の4つであり、OpenCVではCV_8UC4と表現する。
	// RGBはKinectとOpenCVで共通なので、そのままコピー可能。
	image = cv::Mat(height, width, CV_8UC4, colorData.pBits);
	// フレームを開放する
	ERROR_CHECK(kinect->NuiImageStreamReleaseFrame(imageStreamHandle, &imageFrame));
	}
	// 距離カメラの更新されたフレームの取得＆処理
	void KinectSample::drawDepthImage(cv::Mat& image){
	NUI_IMAGE_FRAME depthFrame = {0};
	// 距離データはピクセル毎に16ビット値として取得することができる。ただし、実際の距離データは16ビットのうち上位13ビット。下位3ビットはプレイヤー認識に使用する。
	ERROR_CHECK(kinect->NuiImageStreamGetNextFrame(depthStreamHandle, 0, &depthFrame));
	NUI_LOCKED_RECT depthData = {0};
	depthFrame.pFrameTexture->LockRect(0, &depthData, 0, 0);
	USHORT* depth = (USHORT*)depthData.pBits;
	// depthDataは USHORT型の値を持つ。なので、depthDataの要素数はdepthDataのサイズ / USHORTのサイズ。cpp一般の配列と同じ。
	for(int i = 0; i < (depthData.size / sizeof(USHORT)); ++i){
	// depthDataは16bit値であり、そこから実際の距離を取得するのは、NuiDepthPixelToDepthでできる
	USHORT distance = ::NuiDepthPixelToDepth(depth[i]);
	// depthは1次元データでheight分格納されている（画像を横に割った状態）。iを擬似的に2次元データに変換する。横幅で割って、余りをx、商をyに割り当てる。
	LONG depthX = i % width;
	LONG depthY = i / width;
	LONG colorX = depthX;
	LONG colorY = depthY;
	// 引数 : RGB画像解像度、距離画像解像度、
	kinect->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(CAMERA_RESOLUTION, CAMERA_RESOLUTION, 0, depthX, depthY, depth[i], &colorX, &colorY);
	// 一定以上の距離を表示しない
	if(distance >= MAX_DISTANCE){
	// CV_8UC4のインデックス
	int index = ((colorY * width) + colorX) * 4;
	UCHAR* data = &image.data[index];
	data[0] = 255;
	data[1] = 255;
	data[2] = 255;
	}
	}
	// フレームを開放する
	ERROR_CHECK(kinect->NuiImageStreamReleaseFrame(depthStreamHandle, &depthFrame));
	}
	void KinectSample::run(){
	cv::Mat image;
	while(1){
	// イベントハンドルを使って、データの更新を待つ(INFINITEでタイムアウト無し（ずっと待つ）)
	DWORD ret = ::WaitForSingleObject(streamEvent, INFINITE);
	// 更新を受け取ったらリセットする
	::ResetEvent(streamEvent);
	drawRgbImage(image);
	drawDepthImage(image);
	cv::imshow("Kinect Sample", image);
	int key = cv::waitKey(10);
	if(key == 'q'){
	break;
	}
	}
	}
	void main(){
	//Mat a;
	try{
	KinectSample kinect;
	kinect.initialize();
	kinect.run();
	}catch(std::exception& ex){
	std::cout << ex.what() << std::endl;
	}
	}

view raw DistRGB.cpp hosted with ❤ by GitHub