PCL——从点云到网格（一）利用OpenNI2和深度相机生成点云

最近做毕设，学习了一下PCL的使用（C++）。这几篇博客就记录一下自己做毕设的时候利用深度相机得到点云，最后生成Mesh的过程。效果应该不是最好的，但是先把流程记录下来，自己下次看的时候就知道大体流程了。
我需要生成的是彩色点云，如果不需要颜色的话只用深度相机就够了。
我打算把整个流程分为三步：
（一）利用OpenNI2和深度相机生成点云
（二）点云预处理
（三）点云到网格Mesh

这篇博客先说一下第一步的流程。由于深度相机不在手边，之前做的时候忘了截图了（教训，工作记录得有），就不放图了。
感谢博客上和网上的各位大佬，提供了非常多的资料和帮助。
下面就开始吧。（win10/win7 + VS2015+PCL1.8.1）

配置PCL的链接：https://blog.csdn.net/uniqueyyc/article/details/79245009
PCL官网上也有些教程来教如何利用OpenNI获取点云的，但是官网上那个貌似是OpenNI而不是OpenNI2，比较旧了，而且用起来也不方便。

利用OpenNI2获得点云的方法（我查到的）有两种，一种是利用OpenNI2Grabber来直接生成点云（PCL官网上的用法），另一种是从OpenNI2获得深度流和RGB流计算得到点云（这个是自己写代码算，应该没有Grabber封装好的计算的效果好，但是可扩展性好）。

首先说一下OpenNI2Grabber

这个东西我其实也是只会用，但是并不是很理解里边的具体的东西。他用到了boost中的bind，注册回调啥的，并不是很理解。代码和注释如下：

#include 

#include  //时间 
#include 
#include 
#include 
#include 
#include               //boost指针相关头文件
#include   

class SimpleOpenNIViewer
{
public:
	SimpleOpenNIViewer() : viewer("PCL OpenNI Viewer") 
	{
			
	}   
	void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
	{
		if (!viewer.wasStopped())
		{	
			viewer.showCloud(cloud);
		}
	}
	void run()
	{
		m_interface = new pcl::io::OpenNI2Grabber(); //

		//绑定 function为函数模板，函数返回值为void 参数为  pcl::PointCloud::ConstPtr的引用
		//绑定到的函数为 SimpleOpenNIViewer::cloud_cb_  利用this调用cloud_cb_   _1为占位符，相当于第一个参数，也就是cloud_cb_的参数
		boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =
			boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);
		
		//回调f
		m_interface->registerCallback(f);
		
		m_interface->start();

		//只要这个viewer没有停止时就一直会有心的点云产生
		while (!viewer.wasStopped())
		{
			boost::this_thread::sleep(boost::posix_time::seconds(1));
		}
		
		m_interface->stop(); //这个OpenNI2Grabber打开之后一定得关闭。
	}
	pcl::visualization::CloudViewer  viewer;
}

boost::bind详解在这里：
https://www.cnblogs.com/benxintuzi/p/4862129.html
上述方法只要在main函数中生成一个新的SimpleOpenNIViewer对象然后调用他的run即可。

上述方法虽然方便，但是他直接获得了点云，相当于相机一打开就会一直在计算点云，不仅慢，而且浪费资源。我后面查资料用到了第二种方法，手动计算得到点云。

由于一些原因，我的下述方法都写在了自己建的类SimpleOpenNIViewer里面了。

通过数据流计算点云

一共大概也是有三步：

1. 初始化OpenNI；
2. 从深度流和RGB流中得到图像并配准
3. 计算点云

首先来看初始化OpenNI，主要工作是打开设备和设置相机的工作模式：

bool SimpleOpenNIViewer::OpenniInit()
{
	// Initial OpenNI
	if (openni::OpenNI::initialize() != openni::STATUS_OK) {
		std::cerr << "OpenNI Initial Error: " << openni::OpenNI::getExtendedError() << std::endl;
		return false;
	}
	// Open Device
	if (mDevice.open(openni::ANY_DEVICE) != openni::STATUS_OK) {
		std::cerr << "Can't Open Device: " << openni::OpenNI::getExtendedError() << std::endl;
		return false;
	}

	if (!createColorStream())
	{
		std::cerr << "create ColorStream error!" << std::endl;
		viewer.~CloudViewer();
		CloseOpenni();
		return false;
	}
	if (!createDepthStream())
	{
		std::cerr << "create DepthStream error!!" << std::endl;
		viewer.~CloudViewer();
		CloseOpenni();
		return false;
	}

	std::cout << "init openni ok!" << std::endl;
	return true;
}

//创建颜色视频流
bool SimpleOpenNIViewer::createColorStream()
{
	if (mDevice.hasSensor(openni::SENSOR_COLOR)) {
		if (mColorStream.create(mDevice, openni::SENSOR_COLOR) == openni::STATUS_OK) {
			// set video mode
			openni::VideoMode mMode;
			//mMode.setResolution(640, 480);
			mMode.setResolution(1280, 720);  //分辨率 (1280, 720) 第一块彩色
			mMode.setFps(30);
			mMode.setPixelFormat(openni::PIXEL_FORMAT_RGB888);  //8_8_8 都是八位色

			if (mColorStream.setVideoMode(mMode) != openni::STATUS_OK) {
				std::cout << "Can't apply VideoMode: " << openni::OpenNI::getExtendedError() << std::endl;
				return false;
			}
		}
		else {
			std::cerr << "Can't create color stream on device: " << openni::OpenNI::getExtendedError() << std::endl;
			return false;
		}

		// start color stream
		mColorStream.start();
		return true;
	}	
}

//创建深度流
bool SimpleOpenNIViewer::createDepthStream()
{
	if (mDevice.hasSensor(openni::SENSOR_DEPTH)) {
		if (mDepthStream.create(mDevice, openni::SENSOR_DEPTH) == openni::STATUS_OK) {
			// set video mode
			openni::VideoMode mMode;
			//mMode.setResolution(640, 480); //第二块 深度设置
			mMode.setResolution(1280, 720); //第二块 深度设置
			mMode.setFps(30);
			mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
			

			if (mDepthStream.setVideoMode(mMode) != openni::STATUS_OK) {
				std::cout << "Can't apply VideoMode to depth stream: " << openni::OpenNI::getExtendedError() << std::endl;
				return false;
			}
		}
		else {
			std::cerr << "Can't create depth stream on device: " << openni::OpenNI::getExtendedError() << std::endl;
			return false;
		}
		// start depth stream
		if (mDepthStream.start() == openni::STATUS_ERROR)
			std::cerr << "mDepthStream.start() ERROR" << std::endl;
		else
			return true;		
	}
	else {
		std::cerr << "ERROR: This device does not have depth sensor" << std::endl;
		return false;
	}
}

其中函数名也都很清楚功能是干啥的了，大体流程就是，初始化OpenNI、打开任意的设备，初始化彩色流和深度流。这两个流的分辨率要一致。
利用OpenNI调用深度相机（打开ANYDEVICE），需要在 PCL路径\3rdParty\OpenNI2\OpenNI\Redist\OpenNI2\Drivers 里面把驱动的dll放进去。

接着开始从视频流中得到数据：
把视频流中的每帧拿出来，得到的每帧数据放到 openni::VideoFrameRef 类型的变量里面。
代码中 mColorStream 和 mDepthStream 是我自定的两个变量，变量类型为 openni::VideoStream

//从流中获得每帧数据
bool SimpleOpenNIViewer::getColorAndDepthFrame(openni::VideoFrameRef & colorFrame, openni::VideoFrameRef & depthFrame)
{
	if(mColorStream.readFrame(&colorFrame) == openni::STATUS_OK)
	{
		if (mDepthStream.readFrame(&depthFrame) == openni::STATUS_OK)
			return true;
	}
	return false;
}

如果还想看一下深度图和RGB图片，使用OpenCV，有：

	void GetMatFromFrame()
	{
		//RGB -> BGR
		this->rgbMat = cv::Mat(this->colorFrame.getHeight(), this->colorFrame.getWidth(), CV_8UC3, (void *)this->colorFrame.getData());
		cv::cvtColor(this->rgbMat, this->rgbMat, CV_RGB2BGR); //Realsense相机获得的RGB在OpenCV中用BGR存
		this->depthMat = cv::Mat(this->depthFrame.getHeight(), this->depthFrame.getWidth(), CV_16UC1, (void *)this->depthFrame.getData());
		this->depthMat.convertTo(this->depthMat, CV_16UC1, 65535 / mDepthStream.getMaxPixelValue());
		//cv::cvtColor(this->depthMat, this->depthMat, CV_GRAY2RGB);
	}

其中深度图的存储类型需要看相机的类型确定。代码中的rgbMat和depthMat都是自己定 cv::Mat 变量，colorFrame 和 depthFrame 都是从视频流中拿出来每帧的数据 openni::VideoFrameRef 类型。

最后，在得到每一帧的数据之后就可以开始计算点云了。
我的代码通过两个 openni::VideoFrameRef 变量，彩色帧和深度帧来计算点云，返回一个点云的Ptr：

pcl::PointCloud<pcl::PointXYZRGB>::Ptr SimpleOpenNIViewer::generatePointCloud(openni::VideoFrameRef & colorFrame, openni::VideoFrameRef & depthFrame)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

	cloud->points.resize(1280*720);  // 点云大小，里面点的个数 和深度图中像素个数相同
	
	//24位存一个点的RGB，每个颜色8位
	openni::RGB888Pixel *pColor = (openni::RGB888Pixel*)colorFrame.getData();
	
	float fx, fy, fz;
	int i = 0;
	//以米为单位 0.001;深度图中某个点的值需要*0.001才能变为以m为单位
	double fScale = 0.001;
	openni::DepthPixel *pDepthArray = (openni::DepthPixel*)depthFrame.getData(); 
	//获得深度图
	for (int y = 0; y < depthFrame.getHeight(); y++) 
	{
		for (int x = 0; x < depthFrame.getWidth(); x++) 
		{
			//按照深度图像的行列来寻找某点的深度值
			int idx = x + y*depthFrame.getWidth();
			//uint16_t 深度值
			const openni::DepthPixel rDepth = pDepthArray[idx];
			
			//将深度信息转化到RGB坐标下
	        openni::CoordinateConverter::convertDepthToWorld(mDepthStream, x, y, rDepth, &fx, &fy, &fz);
								
			//变成负的 这里不换的话世界坐标会颠倒  不知道为啥- -
			fx = -fx;
			fy = -fy;
			fz = -fz;

			//m为单位
			cloud->points[i].x = fx * fScale;
			cloud->points[i].y = fy * fScale;
			cloud->points[i].z = fz * fScale;
			
			//点云颜色是反的			
			cloud->points[i].b = pColor[i].r;
			cloud->points[i].g = pColor[i].g;
			cloud->points[i].r = pColor[i].b;
				
			i++;  //下一个点
		}
	}
	return cloud;	
}

我认为 convertDepthToWorld 函数相当于是将深度相机拍摄的图片和RGB相机对齐（由于深度相机和RGB相机不在同一个位置，导致像素不是严格按位置对应的），才能让深度值和彩色值能够对应（我不知道对不对，如果哪位dalao看到了能够指点一下，蟹蟹~）。

通过上面的三步就可以得到一个初始的点云了。大部分都是OpenNI封装好的函数，调用就行，最后一步注意那个单位，如果不用0.001去乘的话会在后面点云滤波上会出现单位不同的一些问题。