kinectColorDepth.cpp

#include <kinect.h>
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

int main(int argc, char* argv[]) {

	// (1) Sensorを開く
	IKinectSensor* kinect;
	GetDefaultKinectSensor(&kinect);
	kinect->Open();
	
	// (2-1) ColorFrameSourceを得る
	IColorFrameSource* p_color_source;
	kinect->get_ColorFrameSource(&p_color_source);
	
	// (2-2) DepthFrameSourceを得る
	IDepthFrameSource* p_depth_source;
	kinect->get_DepthFrameSource(&p_depth_source);

	// (3-1) ColorFrameReaderを開く
	IColorFrameReader* p_color_reader;
	p_color_source->OpenReader(&p_color_reader);

	// (3-2) DepthFrameReaderを開く
	IDepthFrameReader* p_depth_reader;
	p_depth_source->OpenReader(&p_depth_reader);

	// (4) 画像サイズの取得
	int color_width, color_height, depth_width, depth_height;
	IFrameDescription* p_frame_desc;
	p_color_source->get_FrameDescription(&p_frame_desc);
	p_frame_desc->get_Width(&color_width);
	p_frame_desc->get_Height(&color_height);
	p_depth_source->get_FrameDescription(&p_frame_desc);
	p_frame_desc->get_Width(&depth_width);
	p_frame_desc->get_Height(&depth_height);

	cout << "color_width : " << color_width << endl;
	cout << "color_height : " << color_height << endl;
	cout << "depth_width : " << depth_width << endl;
	cout << "depth_height : " << depth_height << endl;

	// フレーム数カウント用
	int frame_counter = 0;

	// ウィンドウの用意
	cv::namedWindow("Color");
	cv::namedWindow("Depth");

	cout << endl << "--------------------------------------------------------" << endl;
	cout << " [Esc]:終了" << endl;
	cout << " [c]:カラー画像の保存" << endl;
	cout << " [d]:デプス画像の保存" << endl;
	cout << "--------------------------------------------------------" << endl;

	while (1) {	
		// (5-1) kinect v2からのカラー画像の取得
		IColorFrame* p_color_frame = nullptr;

		Mat color_image(color_height, color_width, CV_8UC4);
		int color_buffer_size = color_width * color_height * 4 * sizeof(unsigned char);

		HRESULT color_result = p_color_reader->AcquireLatestFrame(&p_color_frame);
		if (SUCCEEDED(color_result)) {
			color_result = p_color_frame->CopyConvertedFrameDataToArray(color_buffer_size, reinterpret_cast<BYTE*>(color_image.data), ColorImageFormat_Bgra);
			if (SUCCEEDED(color_result)) {
				resize(color_image, color_image, cv::Size(), 0.5, 0.5);
				flip(color_image, color_image, 1); // 水平反転
			}
		}

		if (p_color_frame != nullptr) {
			p_color_frame->Release();
		}

		if (SUCCEEDED(color_result)) {
			cv::imshow("Color", color_image);
		}

		// (5-2) kinect v2からのデプス画像の取得
		IDepthFrame* p_depth_frame = nullptr;

		Mat depth_image(depth_height, depth_width, CV_8UC1);
		int depth_buffer_size = depth_width * depth_height;
		unsigned short *depth_buffer;
		depth_buffer = new unsigned short[depth_buffer_size];

		HRESULT depth_result = p_depth_reader->AcquireLatestFrame(&p_depth_frame);
		if (SUCCEEDED(depth_result)) {
			p_depth_frame->CopyFrameDataToArray(depth_buffer_size, depth_buffer); 
			
			// 深度0～8000[mm]で取得されるので、0～255で正規化
			for (int y = 0; y < depth_buffer_size; y++) {
				depth_image.data[y] = (unsigned int)(depth_buffer[y] * 255.0f / 8000.0f);
			}

			flip(depth_image, depth_image, 1); // 水平反転
		}

		// 使わないので解放
		if (p_depth_frame != nullptr) {
			p_depth_frame->Release();
		}

		if (SUCCEEDED(depth_result)) {	
			cv::imshow("Depth", depth_image);
		}
		
		// (6) キーが押されたときの処理
		char key = cv::waitKey(30);
		if (key == VK_ESCAPE) {
			break;
		}
		else if (key == 'c') {
			string filename = "color_" + to_string(frame_counter) + ".png";
			imwrite(filename, color_image);
			cout << filename << "を保存しました" << endl;
		} else if (key == 'd') {
			string filename = "depth_" + to_string(frame_counter) + ".png";
			imwrite(filename, depth_image);
			cout << filename << "を保存しました" << endl;
		}
		frame_counter++;
	}
	
	kinect->Close();

	return 0;
}