Kinect开发：使用PCL+RANSAC算法拟合三维点云平面

几个月之前做了一个小项目，今天有一点时间重新复习一下代码，虽然代码比较简单的，但是还是想分享出来，一是重新学习一下自己没接触过的知识，二是希望能为其他人提供一些思路。

开发环境是VS2015+PCL+OpenCV，PCL和OpenCV的配置过程大家可以自己百度一下，下面直接上代码了，在配置完环境后连接好Kinect就可以直接运行了，运行完直接显示拟合的平面和平面参数。

一共三个文件。

PointCloudGrabber.h

#pragma once

#define NOMINMAX

#include 
#include 
#include 

#include 
#include 
#include 
#include 

class PointCloudGrabber : public pcl::Grabber//数据获取驱动Grabber
{
public:
	PointCloudGrabber();
	~PointCloudGrabber();

	bool LoadConfigdata();
	bool OpenDevice();
	virtual void start();
	virtual void stop();
	virtual bool isRunning() const;
	virtual std::string getName() const;
	virtual float getFramesPerSecond() const;
	bool Grab(pcl::PointCloud::Ptr& out);
	typedef void (signal_Kinect2_PointXYZRGBA)(const boost::shared_ptr>&);
	void convert2rgb();
	double  calculatedistance(double a, double b, double c);
	cv::Mat ConvertMat(const UINT16* pBuffer, int nWidth, int nHeight, USHORT nMinDepth, USHORT nMaxDepth);
	cv::Mat ConvertMat(const RGBQUAD* pBuffer, int nWidth, int nHeight);


	UINT nBufferSize_depth = 0;
	UINT16 *pBuffer_depth = NULL;
	UINT nBufferSize_coloar = 0;
	RGBQUAD *pBuffer_color = NULL;

//	void camerspace2colorspace(CameraSpacePoint __camerspacepoint, ColorSpacePoint* colorpoint);


	char key = 0;
	int depth_width = 512; 
	int depth_height = 424;
	int color_widht = 1920; 
	int color_height = 1080;
	double values0 = -0.0204145;
	double values1 = 0.00798539;
	double values2 = 0.99976;
	double values3 = -1.4175;
	double distance;
	char filepath[260];
	int _colorWidth;
	int _colorHeight;

	void threadFunction();
	bool _running;
	//void daitimainhanshu();

	cv::Mat depthImg_show = cv::Mat::zeros(depth_height, depth_width, CV_8UC3);//原始UINT16 深度图像不适合用来显示，所以需要砍成8位的就可以了，但是显示出来也不是非常好，最好能用原始16位图像颜色编码，凑合着看了  
	cv::Mat depthImg = cv::Mat::zeros(depth_height, depth_width, CV_16UC1);//the depth image  
	cv::Mat colorImg = cv::Mat::zeros(color_height, color_widht, CV_8UC3);//the color image  
	
protected:
	boost::signals2::signal* signal_PointXYZRGBA;
	boost::thread _thread;
	mutable boost::mutex _mutex;

	
	HRESULT hr;//判断函数执行结果 
	IKinectSensor* _sensor;//初始化kinect对象  打开Sensor
	ICoordinateMapper* m_pCoordinateMapper; //坐标映射
	
	IColorFrameSource* _colorSource;//定义获取颜色的source		
	IColorFrameReader* _colorReader;//定义Reader
	IDepthFrameSource* _depthSource;//定义获取深度的source	
	IDepthFrameReader* _depthReader;//定义reader
	//ICoordinateMapper::MapCameraPointsToColorSpace();
	
	std::vector _colorBuffer;
	std::vector _colorSpacePoints;
	std::vector _cameraSpacePoints;

	
	
	int _depthWidth;//深度图像的宽
	int _depthHeight;//深度图像的高
	const int _depthPointCount;
	std::vector _depthBuffer;
};

template
inline void SafeRelease(Interface *& IRelease)//释放资源
{
	if (IRelease != NULL)
	{
		IRelease->Release();
		IRelease = NULL;
	}
}

 PointCloudGrabber.cpp

#include "PointCloudGrabber.h"
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include 
#include 

using namespace std; 
using namespace  cv;
PointCloudGrabber::PointCloudGrabber()
	: _sensor(nullptr)
	, m_pCoordinateMapper(nullptr)
	, _colorSource(nullptr)
	, _colorReader(nullptr)
	, _depthSource(nullptr)
	, _depthReader(nullptr)
	, hr(S_OK)
	, _colorWidth(1920)
	, _colorHeight(1080)
	, _colorBuffer()
	, _depthWidth(512)
	, _depthHeight(424)
	, _depthPointCount(512 * 424)
	, _depthBuffer(_depthPointCount)
	, _colorSpacePoints(512 * 424)
	, _cameraSpacePoints(512 * 424)//初始化
{
}

PointCloudGrabber::~PointCloudGrabber()
{

}

bool PointCloudGrabber::OpenDevice()
{
	// Create Sensor Instance
	hr = GetDefaultKinectSensor(&_sensor);
	if (FAILED(hr)){
		throw std::exception("Exception : GetDefaultKinectSensor()");
	}

	// Open Sensor
	hr = _sensor->Open();
	if (FAILED(hr)){
		throw std::exception("Exception : IKinectSensor::Open()");
	}

	// Retrieved Coordinate Mapper
	hr = _sensor->get_CoordinateMapper(&m_pCoordinateMapper);
	if (FAILED(hr)){
		throw std::exception("Exception : IKinectSensor::get_CoordinateMapper()");
	}

	// Retrieved Color Frame Source
	hr = _sensor->get_ColorFrameSource(&_colorSource);
	if (FAILED(hr)){
		throw std::exception("Exception : IKinectSensor::get_ColorFrameSource()");
	}

	// Retrieved Depth Frame Source
	hr = _sensor->get_DepthFrameSource(&_depthSource);
	if (FAILED(hr)){
		throw std::exception("Exception : IKinectSensor::get_DepthFrameSource()");
	}

	// Retrieved Color Frame Size
	IFrameDescription* colorDescription;
	hr = _colorSource->get_FrameDescription(&colorDescription);
	if (FAILED(hr)){
		throw std::exception("Exception : IColorFrameSource::get_FrameDescription()");
	}

	hr = colorDescription->get_Width(&_colorWidth); // 1920
	if (FAILED(hr)){
		throw std::exception("Exception : IFrameDescription::get_Width()");
	}

	hr = colorDescription->get_Height(&_colorHeight); // 1080
	if (FAILED(hr)){
		throw std::exception("Exception : IFrameDescription::get_Height()");
	}

	SafeRelease(colorDescription);

	// To Reserve Color Frame Buffer
	_colorBuffer.resize(_colorWidth * _colorHeight);

	// Retrieved Depth Frame Size
	IFrameDescription* depthDescription;
	hr = _depthSource->get_FrameDescription(&depthDescription);
	if (FAILED(hr)){
		throw std::exception("Exception : IDepthFrameSource::get_FrameDescription()");
	}

	hr = depthDescription->get_Width(&_depthWidth); // 512
	if (FAILED(hr)){
		throw std::exception("Exception : IFrameDescription::get_Width()");
	}

	hr = depthDescription->get_Height(&_depthHeight); // 424
	if (FAILED(hr)){
		throw std::exception("Exception : IFrameDescription::get_Height()");
	}

	SafeRelease(depthDescription);

	// To Reserve Depth Frame Buffer
	_depthBuffer.resize(_depthWidth * _depthHeight);

	// Open Color Frame Reader
	hr = _colorSource->OpenReader(&_colorReader);
	if (FAILED(hr)){
		throw std::exception("Exception : IColorFrameSource::OpenReader()");
	}
	
	// Open Depth Frame Reader
	hr = _depthSource->OpenReader(&_depthReader);
	if (FAILED(hr)){
		throw std::exception("Exception : IDepthFrameSource::OpenReader()");
	}

	signal_PointXYZRGBA = createSignal();

	return true;
}

void PointCloudGrabber::start()
{
	boost::mutex::scoped_lock lock(_mutex);

	_running = true;

	_thread = boost::thread(&PointCloudGrabber::threadFunction, this);
}

void PointCloudGrabber::stop()
{
	boost::mutex::scoped_lock lock(_mutex);

	_running = false;
}



bool PointCloudGrabber::isRunning() const
{
	boost::mutex::scoped_lock lock(_mutex);

	return _running;
}


std::string PointCloudGrabber::getName() const
{
	return std::string("Kinect2Grabber");
}


float PointCloudGrabber::getFramesPerSecond() const
{
	return 30.0f;
}

void PointCloudGrabber::threadFunction()
{
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud());
	while (_running)
	{
		if (Grab(cloud))
		{
			signal_PointXYZRGBA->operator()(cloud);
			//std::cout << "grab succeed." << std::endl;
		}
		//else
		//	std::cout << "grab failed." << std::endl;
	}
}


bool PointCloudGrabber::Grab(pcl::PointCloud::Ptr& out)
{
	// Acquire Latest Color Frame
	IColorFrame* colorFrame = nullptr;
	hr = _colorReader->AcquireLatestFrame(&colorFrame);
	if (SUCCEEDED(hr)){
		// Retrieved Color Data
		hr = colorFrame->CopyConvertedFrameDataToArray(_colorBuffer.size() * sizeof(RGBQUAD), reinterpret_cast(&_colorBuffer[0]), ColorImageFormat::ColorImageFormat_Bgra);
		if (FAILED(hr)){
			throw std::exception("Exception : IColorFrame::CopyConvertedFrameDataToArray()");
		}
	}
	else
		return false;
	SafeRelease(colorFrame);


	// Acquire Latest Depth Frame
	IDepthFrame* depthFrame = nullptr;
	hr = _depthReader->AcquireLatestFrame(&depthFrame);
	if (SUCCEEDED(hr))
	{
		// Retrieved Depth Data
		hr = depthFrame->CopyFrameDataToArray(_depthBuffer.size(), &_depthBuffer[0]);
		if (FAILED(hr)){
			throw std::exception("Exception : IDepthFrame::CopyFrameDataToArray()");
		}
	}
	else
		return false;
	SafeRelease(depthFrame);


	pcl::PointCloud::Ptr cloud = out;
	cloud->clear();
	cloud->width = static_cast(_depthWidth);
	cloud->height = static_cast(_depthHeight);
	cloud->is_dense = false;

	cloud->points.resize(cloud->height * cloud->width);

	pcl::PointXYZRGBA point;

	pcl::PointXYZRGBA* pt = &cloud->points[0];

	RGBQUAD color;

	UINT depthPointCount = _depthHeight * _depthWidth;
	UINT colorPointCount = _colorHeight * _colorWidth;

	//分别根据深度数据和彩色数据映射获得深度数据矩阵与彩色值以及三维信息之间的对应关系
	hr = m_pCoordinateMapper->MapDepthFrameToColorSpace(depthPointCount, _depthBuffer.data(), depthPointCount, _colorSpacePoints.data());
	if (FAILED(hr))
		return false;
	
	hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(depthPointCount, _depthBuffer.data(), depthPointCount, _cameraSpacePoints.data());
	if (FAILED(hr))
		return false;

	int colorX;
	int colorY;

	ColorSpacePoint* pColorSpacePoints = _colorSpacePoints.data();
	CameraSpacePoint* pCameraSpacePoints = _cameraSpacePoints.data();
	RGBQUAD* pColorBuffer = _colorBuffer.data();
	//遍历深度矩阵，将其对应的彩色信息以及三维坐标信息融合后作为点云数据写入点云中

	for (int y = 0; y < _depthHeight; y++)
	{
		for (int x = 0; x < _depthWidth; x++, pt++)
		{
			colorX = static_cast(pColorSpacePoints->X + 0.5f);
			colorY = static_cast(pColorSpacePoints->Y + 0.5f);
			if ((0 <= colorX) && (colorX < _colorWidth) && (0 <= colorY) && (colorY < _colorHeight))
			{
				color = _colorBuffer[colorY * _colorWidth + colorX];
				pt->b = color.rgbBlue;
				pt->g = color.rgbGreen;
				pt->r = color.rgbRed;
				pt->a = color.rgbReserved;
				pt->x = pCameraSpacePoints->X;
				pt->y = pCameraSpacePoints->Y;
				pt->z = pCameraSpacePoints->Z;
			}



			pCameraSpacePoints++;
			pColorSpacePoints++;
		}
	}
	return true;
}

//深度图转换
cv::Mat   PointCloudGrabber::ConvertMat(const UINT16* pBuffer, int nWidth, int nHeight, USHORT nMinDepth, USHORT nMaxDepth)
{
	cv::Mat img(nHeight, nWidth, CV_8UC3);

	uchar* p_mat = img.data;

	const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);

	while (pBuffer < pBufferEnd)
	{
		USHORT depth = *pBuffer;
		//	cout << depth << endl;
		BYTE intensity = static_cast((depth >= nMinDepth) && (depth <= nMaxDepth) ? (depth % 256) : 0);
		*p_mat = intensity;
		p_mat++;
		*p_mat = intensity;
		p_mat++;
		*p_mat = intensity;
		p_mat++;
		++pBuffer;

	}
	return img;
}


//彩色图转换
cv::Mat    PointCloudGrabber::ConvertMat(const RGBQUAD* pBuffer, int nWidth, int nHeight)
{
	cv::Mat img(nHeight, nWidth, CV_8UC3);
	uchar* p_mat = img.data;

	const RGBQUAD* pBufferEnd = pBuffer + (nWidth * nHeight);

	while (pBuffer < pBufferEnd)
	{
		*p_mat = pBuffer->rgbBlue;
		p_mat++;
		*p_mat = pBuffer->rgbGreen;
		p_mat++;
		*p_mat = pBuffer->rgbRed;
		p_mat++;

		++pBuffer;
	}
	return img;
}

 PointCloudScanner.cpp

#include "PointCloudGrabber.h"
#include 
#include 
#include 
#include    
#include 
#include   
#include   
#include   
#include 
#include     //关于深度图像的头文件
#include    //深度图可视化的头文件
//#include 
#include  
#include


#ifndef WINSOCK2_H 
#define WINSOCK2_H 
#endif  

#pragma comment(lib,"ws2_32.lib")

using namespace std;
using namespace cv;

int g_AddRangleSize = 3;
SOCKET g_Socketscreen_urg;
bool g_connected_urg = false;
int  g_hz_count = 0;

int main(int argc, char* argv[])
{
	ofstream outfile;
	outfile.open("depthparameter.ini");//在文件中输出平面参数
//	ofstream outfile1;
//	outfile1.open("depth.txt");

	PointCloudGrabber grabber;
	bool done = false;

	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);//定义初始输入的点云数据
	pcl::PointCloud::Ptr cloud_target(new pcl::PointCloud);//定义目标点云集合
	pcl::PointCloud::Ptr cloud_source_filtered(new pcl::PointCloud);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	pcl::PointCloud::Ptr cloud_depth(new pcl::PointCloud);//定义包含距离的点云
	

	pcl::PassThrough pt;//pcl直通滤波器pt
	pcl::ExtractIndices ei;//ExtractIndices 滤波器 ei 用来提取平面
	pcl::visualization::PCLVisualizer p;//PCLVisualizer类彩色显示点云
	pcl::visualization::PCLVisualizer p2;
	pcl::SACSegmentation sac;


	pcl::visualization::PointCloudColorHandlerCustom tgt_h(cloud_target, 255, 0, 0);
	//对输入为pcl::PointXYZRGBA类型的点云，着色为红色。其中，cloud_target表示真正处理的点云，tgt_h表示处理结果.
	pcl::visualization::PointCloudColorHandlerCustom src_h(cloud, 0, 255, 0);
	//对输入为pcl::PointXYZRGBA类型的点云，着色为绿色。其中，cloud表示真正处理的点云，src_h表示处理结果.
	
	bool dataFlag = false;
	// PCL Visualizer
	boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));
	viewer->setCameraPosition(0.0, 0.0, -2.5, 0.0, 0.0, 0.0);
	// Retrieved Point Cloud Callback Function
	boost::mutex mutex;
	//数据获取槽
	boost::function::ConstPtr&)> function =
		[&cloud, &mutex, &dataFlag](const pcl::PointCloud::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex);
		pcl::copyPointCloud(*ptr, *cloud);
		dataFlag = true;
	};
	//初始化设备参数
	try
	{
		if (!grabber.OpenDevice())
		{
			//std::cout << "open device failed." << std::endl;
			return -1;
		}
	}
	catch (std::exception e)
	{
		return -1;
	}
	//绑定信号槽
	boost::signals2::connection connection = grabber.registerCallback(function);
	grabber.start();
//	CameraSpacePoint _camerapoint;
//	ColorSpacePoint* _colorpoint;

	//当槽收到数据的时候在viewer中进行显示

	while (!done)
	{
	   viewer->spinOnce();

		if (dataFlag)
		{
			boost::mutex::scoped_lock lock(mutex);
			
            int i = 0;
			if (cloud->size() != 0)
			{
		    	//	cout << 1<< endl;
				
				if (!viewer->updatePointCloud(cloud, "cloud"))
				{
					viewer->addPointCloud(cloud, "cloud");	
					pt.setInputCloud(cloud); //输入点云
					pt.setFilterFieldName("y");
					pt.setFilterLimits(-0.7, 0.7);
					pt.filter(*cloud_source_filtered);
					sac.setInputCloud(cloud_source_filtered);
					sac.setMethodType(pcl::SAC_RANSAC); //设置模型估计方法RANSAC
					sac.setModelType(pcl::SACMODEL_PLANE);//设置模型类型(平面)
					sac.setDistanceThreshold(0.01);//距离阈值（离散点到模型表面距离）
					sac.setMaxIterations(300);//设置最大迭代次数
					sac.setProbability(0.95); //计算模型参数和得到符合此模型的局内点索引
					sac.segment(*inliers, *coefficients);
					ei.setIndices(inliers); //存放局内点索引
					ei.setInputCloud(cloud_source_filtered); // cloud_source_filtered 为提取桌子表面 cloud_source 为提取地面
					ei.filter(*cloud_target);



					p.addPointCloud(cloud_target, tgt_h, "target");
					p.setWindowName("目标数据平面");
					p.spinOnce();

				    p2.addPointCloud(cloud, src_h, "source");
		            p2.setWindowName("cloud_source原始数据");
                    p2.spinOnce();//no spin()


				   //输出平面模型的四个参数   公式模型 ax+by+cz+d=0;
				   cout << "平面模型参数" << endl;
				   std::cout<<*coefficients <