使用PCL配合cvFindExtrinsicCameraParams2实现棋盘格控制模型的三维运动

0. 前言

      之前在http://blog.csdn.net/lwx309025167/article/details/78140490文章的末尾提了一下使用cvFindExtrinsicCameraParams2实现棋盘格控制三维模型运动的效果，现在就这一块单独写一篇文章并给出实现代码。

1.PCL

      处理图像时有时会需要显示一些三维点云，类似于opencv在二维图像处理领域中的地位，PCL是针对三维点云处理的一款开源工具包，包括各种点云的读写与处理功能。自己之前需要进行三维重建，学习了一段时间的PCL。这里把PCL入门和一些经典的学习资料列出来供码友参考。

1.1 PCL的安装

      PCL依赖的库比较多，以前安装PCL时需要事先编译好很多依赖库，不过网上已经有大神针对vs2013和vs2015制作了all-in-one的安装包，真是造福大众~。

      PCL1.8 All-in-one安装包下载地址见http://download.csdn.net/download/lwx309025167/10016881（帮忙也给小弟一点分用于下载csdn资源~）

      安装完之后的配置，类似于opencv，主要配置头文件、库文件的包含目录，并把lib文件引入链接中，步骤很简单，就是lib依赖项太多了，需要花一点时间。百度搜索一下“PCL1.8 All-in-one安装包配置”可以搜到很多相关文章，这里推荐一篇http://blog.csdn.net/u014283958/article/details/52599457 

      这里我就个人经验给出两点建议：

    （1）设置附加依赖项时最好根据自己安装得到的文件夹中的.lib名称逐一添加，而不是从网上文章给的示例直接copy，以免后续编译出现问题时再查找就很麻烦；注意添加opengl32.lib和Glu32.lib

    （2）可以配置一下“属性”——“调试”——“环境”项，譬如我的PCL安装时选的地址是D:\PCL1.8.0\，那就配置为

Path=D:\PCL1.8.0\bin;D:\PCL1.8.0\3rdParty\FLANN\bin;D:\PCL1.8.0\3rdParty\OpenNI2\Samples\Bin;D:\PCL1.8.0\3rdParty\Qhull\bin;D:\PCL1.8.0\3rdParty\VTK\bin

      这一步的作用是在运行程序时VS知道从这些目录中寻找需要的dll文件，就不需要我们把链接时需要的dll文件拷贝到执行程序所在目录了

1.2 PCL的学习

      有关PCL的学习书籍很少，目前我知道的中文版教材只有一本《点云库PCL学习教程 》，下载链接http://download.csdn.net/download/lwx309025167/9921487（帮忙也给小弟一点分用于下载csdn资源~）。另外就是有一个PCL学习的中国官网http://www.pclcn.org/，提供的相关内容也比较有限。不过一般使用过程中遇到的问题网上大多都能搜到答案~

2. 摄像机标定相关

2.1 摄像机的标定

      找一台USB接口摄像机并完成标定，标定流程可以参考http://blog.csdn.net/lwx309025167/article/details/78086295

2.2 cvFindExtrinsicCameraParams2的使用

      完成摄像机的标定后，可以使用opencv提供的cvFindExtrinsicCameraParams2函数单独计算相机外参，具体流程可以参考http://blog.csdn.net/lwx309025167/article/details/78140490

      到此我们已经得到相机外参，也有了显示三维点云的能力，后面要做的就是把相机外参（棋盘格所在世界坐标系相对于相机坐标系的转换参数）中的旋转向量和平移向量组合成为变换矩阵，作用于一个三维模型上（结果就是其位置随棋盘格运动而变化）并实时显示其位置即可。

3. 三维点云的位置变换实现

3.1 变换矩阵

      有关三维空间中的变换矩阵，《学习opencv》的第十三章有详细讲解，这里不再重复造轮子。简单说明就是变换矩阵T（3X4）由旋转矩阵R(3X3)和平移向量t(3X1)组成，即T=[R t]。同时之前通过cvFindExtrinsicCameraParams2已经得到了旋转向量（这里还是3X1大小，和旋转矩阵不同）r和平移向量t，使用cvRodrigues2()将r转换为R后与t组合为T。

3.2 三维点云的位置变换

      测试demo中我随意读取了一个点云文件作为模型，得到了变换矩阵T后如何作用于它来改变其世界坐标？PCL提供了transformPointCloud()函数，输入原始点云和变换矩阵，即得到目标点云。该函数的使用可参考http://www.cnblogs.com/21207-iHome/p/6032691.html

4. 实现代码

#include 
#include 

#include "opencv2/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/core/ocl.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include"opencv2/flann.hpp"
#include"opencv2/xfeatures2d.hpp"
#include"opencv2/ml.hpp"

#include 
#include 
#include 
#include 
#include 
#include  
#include 

#include 
#include 

using namespace cv;
using namespace std;
using namespace cv::xfeatures2d;
using namespace cv::ml;


float squareSise = 31.f;// 棋盘网格边长

pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr cloud_output(new pcl::PointCloud);
pcl::visualization::PCLVisualizer pclViewer("Matrix transformation example");


int main()
{
	// 读取点云文件
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	pcl::io::loadPCDFile("bunny.pcd", *cloud_output);

	// 设置显示窗口背景为黑色
	pclViewer.setBackgroundColor(0, 0, 0);
	// 增加坐标系
	pclViewer.addCoordinateSystem(1);
	CvMat *intrinsic, *distortion;
	CvMat* rotation_vector = cvCreateMat(3, 1, CV_32FC1);// 旋转向量
	CvMat* translation_vector = cvCreateMat(3, 1, CV_32FC1);// 平移向量
	CvMat* R = cvCreateMat(3, 3, CV_32FC1);

	Eigen::Matrix4f T = Eigen::Matrix4f::Identity();// 变换矩阵T，包含旋转和平移


	Mat M, D;
	// 读取标定参数 
	static string intrinsics_filename = "intrinsics.yml";
	FileStorage fs(intrinsics_filename, FileStorage::READ);
	if (fs.isOpened())
	{
		fs["M"] >> M;
		fs["D"] >> D;
		fs.release();
	}
	intrinsic = &((CvMat)M);
	distortion = &((CvMat)D);

	// 打开摄像头
	namedWindow("Camera");
	VideoCapture camera;
	camera.open(0);
	if (!camera.isOpened())
	{
		return -1;
	}

	Mat src, output;
	while (1)
	{
		camera >> src;
		if (src.empty())
		{
			return -1;
		}
		imshow("Camera", src);
		cvtColor(src, src, CV_BGR2GRAY);
		// 提取角点 
		Size boardSize(6, 4);

		std::vector imageCorners;
		std::vector objectCorners;

		bool found = cv::findChessboardCorners(src, boardSize, imageCorners);// 计算角点信息
		if (!found)
		{
			waitKey(10);
			continue;
		}
		cv::cornerSubPix(src, imageCorners, cv::Size(5, 5), cv::Size(-1, -1),
			cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 30, 0.1));// 计算亚像素级别角点信息

		CvMat* image_points = cvCreateMat(imageCorners.size(), 2, CV_32FC1);
		CvMat* object_points = cvCreateMat(imageCorners.size(), 3, CV_32FC1);


		if (imageCorners.size() == boardSize.area())
		{
			// 得到角点的世界坐标和像素坐标
			for (int i = 0; i < boardSize.height; i++)
			{
				for (int j = 0; j < boardSize.width; j++)
				{
					CV_MAT_ELEM(*image_points, float, i*boardSize.width + j, 0) = imageCorners[i*boardSize.width + j].x;
					CV_MAT_ELEM(*image_points, float, i*boardSize.width + j, 1) = imageCorners[i*boardSize.width + j].y;
					CV_MAT_ELEM(*object_points, float, i*boardSize.width + j, 0) = i;
					CV_MAT_ELEM(*object_points, float, i*boardSize.width + j, 1) = j;
					CV_MAT_ELEM(*object_points, float, i*boardSize.width + j, 2) = 0.0f;
				}
			}
			// 计算摄像头外参数，得到旋转向量（3X1）和平移向量（3X1）
			cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic, distortion, rotation_vector, translation_vector);
			float x = squareSise *CV_MAT_ELEM(*translation_vector, float, 0, 0);
			float y = squareSise *CV_MAT_ELEM(*translation_vector, float, 1, 0);
			float z = squareSise *CV_MAT_ELEM(*translation_vector, float, 2, 0);
			std::cout << "x: " << x << " y: " << y << " z: " << z << std::endl;
			
			// 将3X1的旋转向量转化为3X3的旋转矩阵
			cvRodrigues2(rotation_vector, R);

			// 将3X3的旋转矩阵和3X1的平移向量组合为3X4的变换矩阵T
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					T(i, j) = CV_MAT_ELEM(*R, float, i, j);
				}
			}
			T(0, 3) = CV_MAT_ELEM(*translation_vector, float, 0, 0) * squareSise /1000.f;
			T(1, 3) = CV_MAT_ELEM(*translation_vector, float, 1, 0) * squareSise / 1000.f;
			T(2, 3) = CV_MAT_ELEM(*translation_vector, float, 2, 0) * squareSise / 1000.f;
			
			// 将变换矩阵T作用于点云以修改其位姿
			pcl::transformPointCloud(*cloud, *cloud_output, T);
			// 显示点云
			pclViewer.removePointCloud("cloud");
			pclViewer.addPointCloud(cloud_output, "cloud");


		}

		// 调用该句用于渲染，否则点云无法显示
		pclViewer.spinOnce();
		if (waitKey(30) == 'q')
		{
			break;
		}
		

	}

	return 0;


}

5. 运行结果

      贴一张截图效果，受限于gif大小没有完全展示各种运动，实际操作中在保证cvFindExtrinsicCameraParams2能够计算到相机外参数的情况下可以随意控制三维模型的运动

6. 其他

    （1）完成该步骤之前需要按照文章要求先完成摄像机的标定与PCL的配置

    （2）代码中使用到的bunny.pcd文件，可以在PCL的github代码托管网址上下载https://github.com/PointCloudLibrary/pcl/tree/master/test