MRPT API 学习之——kinect图像获取，显示，2维图像某点深度信息

// kinect_test.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <mrpt/hwdrivers.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
#include <mrpt/system/filesystem.h>


using namespace mrpt;

using namespace mrpt::hwdrivers;
using namespace mrpt::system;
using namespace mrpt::gui;
using namespace mrpt::slam;

struct Point{
	int x;
	int y;
};
Point p[]={{50,50},{100,100}};//given two point in 2d image

void Preparation();


int _tmain(int argc, _TCHAR* argv[])
{
	Preparation();
	return 0;
}


void Preparation()
{
	CKinect kinect;
	CObservation3DRangeScanPtr obs = CObservation3DRangeScan::Create();
	CImage theImg;//image for show 

	CDisplayWindow3D  win3D("kinect",640,480);//window for show



	bool is_obs=true,hard_error=false;//kinect是否获取到图片，硬件是否出问题

	//openning;
	cout<<"kinect initializing...";
	kinect.initialize();
	cout<<"OK\n";	


	TPoint3D p3d;

	theImg.selectTextFont("10x20");

	/*
	grab images from kinect Continuously
	given some(two here) 2d points in the frame and get its 3d position from obs
	show images and positions*/	
	while(!hard_error){		
		kinect.getNextObservation(*obs,is_obs,hard_error);
		theImg = obs->intensityImage;

		if (is_obs)
		{
			p3d = TPoint3D(obs->points3D_x[obs->rangeImage.cols()*p[0].y+p[0].x],obs->points3D_y[obs->rangeImage.cols()*p[0].y+p[0].x],obs->points3D_z[obs->rangeImage.cols()*p[0].y+p[0].x]);

			theImg.textOut(p[0].x,p[0].y,format("(%f,%f,%f)", p3d.x,p3d.y,p3d.z),TColor(200,20,20) );

			p3d = TPoint3D(obs->points3D_x[obs->rangeImage.cols()*p[1].y+p[1].x],obs->points3D_y[obs->rangeImage.cols()*p[1].y+p[1].x],obs->points3D_z[obs->rangeImage.cols()*p[1].y+p[1].x]);

			theImg.textOut(p[1].x,p[1].y,format("(%f,%f,%f)", p3d.x,p3d.y,p3d.z),TColor(200,20,20) );
		}

		win3D.setImageView(theImg);
		win3D.repaint();
		sleep(100);
	}
}