第七周PCL学习--点云配准（七）

目录
引言
一、点云配准
1.1、定义
1.2、含义
1.3、配准过程
1.4、算法原理
1.5、实验
二、总结
三、参考

引言

随着计算机辅助设计技术的发展，通过实物模型产生数字模型的逆向工程技术，由于它的独特魅力获得了越来越广泛的应用，与此同时，硬件设备的日趋完善也为数字模型操作提供了足够的技术支持。在逆向工程计算机视觉、文物数字化等领域中，由于点云的不完整、旋转错位、平移错位等问题，使得要得到完整的点云数据，就需要局部点云配准。

一、点云配准

1.1 定义

为了得到被测物体的完整数据模型，需要确定一个合适的坐标变换，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以
方便地进行可视化等操作，这就是点云数据的配准。

1.2 实质含义

点云配准有手动配准、依赖仪器的配准和自动配准。 通常我们所说的点云配准技术即是指最后一种自动配准。 点云自动配准技术是通过一定的算法或者统计学规律，
利用计算机计算两块点云之间的错位，从而达到把两片点云自动配准的效果。 其实质是把在不同的坐标系中测量得到的数据点云进行坐标变换，以得到整体的数据
模型。 问题的关键是如何求得坐标变换参数 R(旋转矩阵）和 T(平移向量），使得两视角下测得的三维数据测得的三维数据经坐标变换后的距离最小。

因此，点云配准基本的输入输出是：
两个刚性变换的点云：源点云（source）以及目标点云（target）形状、大小相同；
点云配准得到一个旋转平移变换矩阵RTMatrixRTMatrix，简称RTRT，该矩阵表示了这两个点云的位置变换关系，即通过RTRT可以
将源点云变换到目标点云的位置，使二者能够重合。

1.3 配准过程

点云配准按照初始条件与精确度等，可以分为粗略配准与精确配准两种配准方法。

1.3.1 粗略配准

粗略配准是在**源点云与目标点云完全不知道任何初始相对位置的情况下**，所进行的配准方法。该方法的主要目的是在初始条件未知的情况下，快速估算
一个大致的点云配准矩阵。整个计算过程要求比较高的计算速度，对于计算结果的精确度则不做过高的要求。

常见的粗略配准算法的思路包括了：基于局部特征描述的方法、基于全局搜索策略以及通过统计学概率等方法。
（a）基于局部特征描述的方法是通过提取source与target的邻域几何特征，通过几何特征快速确定二者之间的点对的对应关系，再计算此关系进而获得
变换矩阵。而点云的几何特征包括了很多种，比较常见的即为快速点特征直方图（Fast Point Feature Histgrams，简称FPFH）。
（b）基于全局搜索策略的代表算法是采样一致性算法（Sample Consensus Initial Alignment，简称SAC_IA），该算法在source与target之间随机
选取几何特征一致的点组成点对。通过计算对应点对的变换关系，得到最优解。
（c）正态分布算法（NDT）利用统计学概率的方法，根据点云正态分布情况，确定了对应点对从而计算target与source之间的变换关系。

1.3.2 精确配准

精确配准是利用已知的初始变换矩阵，通过迭代最近点算法（ICP算法）等计算得到较为精确的解。ICP算法通过计算source与target对应点距离，
构造旋转平移矩阵RT，通过RT对source变换，计算变换之后的均方差。若均方差满足阈值条件，则算法结束。否则则继续重复迭代直至误差满足
阈值条件或者迭代次数终止。

因此，ICP算法具有以下条件：
配准结果精确度较高，是一种精确配准算法；
对初始矩阵要求严格，差的初始矩阵严重影响算法性能，甚至会造成局部最优的情况；

1.4 点云配准的算法原理介绍

点云配准理论基础
（1）刚性变换矩阵
点云配准的最终目的是通过一定的旋转和平移变换将不同坐标系下的两组或者多组点云数据统一到同一参考坐标系下。这个过程，可以通过一组映射来完成。假设映射变换为H，这H可以用以下的公式来表示。

其中代表A旋转矩阵，T代表平移向量，V代表透视变换向量，S代表整体的比例因子。因为根据一系列图片得到的点云数据只存在旋转和平移变换，不存在形变，所以将V设为零向量，比例因子S=1。
映射变换H可以表示为：

其中，旋转矩阵R3X3和平移矩阵T3X1可以通过以下公式来表示：

其中α、β、γ分别表示点沿x、y、z轴的旋转角度，tx、ty、tz分别表示点沿
x、y、z轴的平移量。

（2）刚性变换矩阵的参数估计
将两个不同坐标系下的点 X和X’进行坐标变换时，可以通过以下公式来实现转换：
刚性变换矩阵中涉及到六个未知数α、β、γ、 tx、ty、tz。要唯一确定这六个未知参数，需要六个线性方程，即至少需要在待匹配点云重叠区域找到3组对应点对，且3组对应点对不能共线，才可以得到这几个未知数的值，进而完成刚性矩阵的参数估计。通常情况下，人们会选择尽可能多的对应点对，进一步提高刚性变换矩阵的参数估计精度。

（3）目标函数
在待匹配的两组点云数据的重叠区域内，分别选取两个点集来表示源点集和目标点集，其中P={pi|pi∈R3，i=1,2，……n}为源点集，Q ={qj|qj∈R3，j=1,2，……m}为目标点集，m和n分别代码两个点集的规模。设旋转矩阵为R，平移矩阵为t，用f（R，t）来表示源点集P在变换矩阵（R，t）下与目标点集Q之间的误差。则求解最优变换矩阵的问题就可以转化为求满足min（f（R，t））的最优解（R，t）。

1.4.1 ICP算法原理

ICP算法的基本原理是：分别在带匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点（pi，qi），然后计算出最优匹配参数R和t，使得误差函数最小。误差函数为E（R，t）为：

其中n为最邻近点对的个数，pi为目标点云 P 中的一点，qi 为源点云 Q 中与pi对应的最近点，R 为旋转矩阵，t为平移向量。

ICP算法步骤：
（1）在目标点云P中取点集pi∈P；
（2）找出源点云Q中的对应点集qi∈Q，使得||qi-pi||=min；
（3）计算旋转矩阵R和平移矩阵t，使得误差函数最小；
（4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的 对应点集pi’={pi’=Rpi+t,pi∈P}；
（5）计算pi’与对应点集qi的平均距离；
（6）如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。
否则返回第2步，直到满足收敛条件为止。

ICP算法的关键点：
（1）原始点集的采集
均匀采样、随机采样和法矢采样
（2）确定对应点集
点到点、点到投影、点到面
（3）计算变化矩阵
四元数法、SVD奇异值分解法

1.5 实验

1.5.1 ICP算法的简单使用

#include<pcl/registration/icp.h>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>


using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int 
main() {
	// 定义两个点云指针
	PointCloudT::Ptr cloud_in(new PointCloudT);
	PointCloudT::Ptr cloud_out(new PointCloudT);

	cloud_in->width = 5;
	cloud_in->height = 1;
	cloud_in->points.resize(cloud_in->width * cloud_in->height);

	for (size_t i = 0; i < cloud_in->points.size(); i++) {
		cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f); 
		cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f); 
		cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); 
	}

	cout << "源点云数据：" << endl;
	for (size_t i = 0; i < cloud_in->points.size(); ++i) 
		cout << "    " <<
		cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
		cloud_in->points[i].z << endl;

	*cloud_out = *cloud_in;

	for (size_t i = 0; i < cloud_out->points.size(); i++) {
		cloud_out->points[i].x += 0.7f;
	}
	
	cout << "目标点云数据：" << endl;
	for (size_t i = 0; i < cloud_out->points.size(); ++i)
		cout << "    " << cloud_out->points[i].x << " " <<
		cloud_out->points[i].y << " " << cloud_out->points[i].z << endl;

	// 定义ICP实例
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setInputSource(cloud_in);  // 源点云
	icp.setInputTarget(cloud_out);  // 目标点云

	// 定义点云数据，用来存放经过PCL后的点云数据
	PointCloudT final;
	icp.align(final);  // 执行配准存储变换后的点云到final

	cout << "收敛状态（1为收敛）:" << icp.hasConverged() << " 距离平方和: " <<
		icp.getFitnessScore() << endl; // 输出一些适应性评估
	cout << "转换矩阵为：" << endl;
	cout << icp.getFinalTransformation() << endl;

	cout << " 经过ICP后的矩阵final:" << endl;
	for (size_t i = 0; i < final.points.size(); i++) {
		std::cout << "    " << final.points[i].x << " " <<
			final.points[i].y << " " << final.points[i].z << std::endl;
	}

	return 0;
}

运行结果

  设置的目标点云是在源点云的x上加了0.7，然后做pcl变换，最后得到的final矩阵与目标点云极为相似，说明其配准效果很好。

1.5.2 ICP配准实例

#include <iostream>
#include <string>

#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool next_iteration = false;

void
print4x4Matrix(const Eigen::Matrix4d & matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}

void
keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
	void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

int
main(int argc, char* argv[])
{
	// 我们要使用的点云
	PointCloudT::Ptr cloud_in(new PointCloudT);  // 初始点云
	PointCloudT::Ptr cloud_tr(new PointCloudT);  // 转换点云
	PointCloudT::Ptr cloud_icp(new PointCloudT);  // 输出点云

	// 检查输入参数
	if (argc < 2)
	{
		printf("Usage :\n");
		printf("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
		PCL_ERROR("Provide one ply file.\n");
		return (-1);
	}

	int iterations = 1;  // 默认ICP配准的迭代次数
	if (argc > 2)
	{

		iterations = atoi(argv[2]);//将字符串变量转换为整数变量
		if (iterations < 1)
		{
			PCL_ERROR("Number of initial iterations must be >= 1\n");
			return (-1);
		}
	}

	pcl::console::TicToc time;  // 定义时间对象
	time.tic();  // 起始时间
	// 读取PLY文件
	if (pcl::io::loadPLYFile(argv[1], *cloud_in) < 0)
	{
		PCL_ERROR("Error loading cloud %s.\n", argv[1]);
		return (-1);
	}
	std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size() << " points) in " << time.toc() << " ms\n" << std::endl;

	// 定义旋转矩阵和平移矩阵
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

	// 旋转矩阵的具体定义 (请参考 https://en.wikipedia.org/wiki/Rotation_matrix)
	double theta = M_PI / 20;  // 设置旋转弧度的角度
	transformation_matrix(0, 0) = cos(theta);
	transformation_matrix(0, 1) = -sin(theta);
	transformation_matrix(1, 0) = sin(theta);
	transformation_matrix(1, 1) = cos(theta);

	// 设置平移矩阵
	transformation_matrix(0, 3) = 0.0;
	transformation_matrix(1, 3) = 0.0;
	transformation_matrix(2, 3) = 0.0;
	// 在终端输出转换矩阵
	std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
	print4x4Matrix(transformation_matrix);

	// 执行初始变换
	pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
	*cloud_tr = *cloud_icp;  // 将cloud_icp变量备份

	 // 设置ICP配准算法输入参数
	time.tic();
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaximumIterations(iterations);//设置迭代次数
	icp.setInputSource(cloud_icp);
	icp.setInputTarget(cloud_in);
	icp.align(*cloud_icp);
	icp.setMaximumIterations(1);  //  当再次调用.align ()函数时，我们设置该变量为1。
	std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;

	if (icp.hasConverged())
	{
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
		transformation_matrix = icp.getFinalTransformation().cast<double>();
		print4x4Matrix(transformation_matrix);
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return (-1);
	}

	// 可视化部分
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	// 创建两个独立的视口
	int v1(0);
	int v2(1);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

	// 我们将要使用的颜色
	float bckgr_gray_level = 0.0;  // 黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	// 设置初始点云为白色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
		(int)255 * txt_gray_lvl);//赋予显示点云的颜色
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

	// 设置初始转换后的点云为绿色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);
	viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

	//  设置ICP配准后的点云为绿色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);
	viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

	//  在两个视口，分别添加文字描述
	viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
	viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

	std::stringstream ss;
	ss << iterations;
	std::string iterations_cnt = "ICP iterations = " + ss.str();
	viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

	// 设置背景颜色
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

	// 设置相机位置和方向
	viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
	viewer.setSize(1280, 1024);  // 设置可视化窗口的尺寸

	// 通过键盘，调用回调函数
	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

	// 设置显示器
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();

		// 用户按下空格键
		if (next_iteration)
		{
			// 配准算法开始迭代
			time.tic();
			icp.align(*cloud_icp);
			std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;

			if (icp.hasConverged())
			{
				printf("\033[11A");
				printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
				std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
				transformation_matrix *= icp.getFinalTransformation().cast<double>();  // 
				print4x4Matrix(transformation_matrix);  // 输出初始矩阵和当前矩阵的转换矩阵

				ss.str("");
				ss << iterations;
				std::string iterations_cnt = "ICP iterations = " + ss.str();
				viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
				viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
			}
			else
			{
				PCL_ERROR("\nICP has not converged.\n");
				return (-1);
			}
		}
		next_iteration = false;
	}
	return (0);
}

实验结果：

	实验分析：再精准配准ICP算法中，必须先进行矩阵初始化操作（包括旋转矩阵和平移矩阵），剩下的步骤就比较简单，设置源点云和目标点云，
	最大迭代次数，一次次的迭代，使计算得到的误差score越来越小，直到匹配完成。

1.5.3 利用正态分布交换进行配准

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include<pcl/kdtree/kdtree_flann.h>
#include<vector>

int
main(int argc, char** argv)
{
	// 读取文件
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/capture0001.pcd", *target_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file capture0001.pcd \n");
		return (-1);
	}
	std::vector<int> indices;  // 保存去除的点的索引
	pcl::removeNaNFromPointCloud(*target_cloud, *target_cloud, indices);  // 去掉NAN的点

	std::cout << "Loaded " << target_cloud->size() << " data points from capture0001.pcd" << std::endl;

	// 读取输入文件
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/capture0002.pcd", *input_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file capture0002.pcd \n");
		return (-1);
	}
	pcl::removeNaNFromPointCloud(*input_cloud, *input_cloud, indices);  
	std::cout << "Loaded " << input_cloud->size() << " data points from capture0002.pcd" << std::endl;

	// 将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度。
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;  // 过滤方法
	approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);  // 设置体素
	approximate_voxel_filter.setInputCloud(input_cloud);
	approximate_voxel_filter.filter(*filtered_cloud);
	std::cout << "Filtered cloud contains " << filtered_cloud->size()
		<< " data points from capture0002.pcd" << std::endl;

	//初始化正态分布变换（NDT）
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
	//设置依赖尺度NDT参数
	//为终止条件设置最小转换差异
	ndt.setTransformationEpsilon(0.01);
	//为More-Thuente线搜索设置最大步长
	ndt.setStepSize(0.1);
	//设置NDT网格结构的分辨率（VoxelGridCovariance）
	ndt.setResolution(1.0);
	//设置匹配迭代的最大次数
	ndt.setMaximumIterations(100);
	// 设置要配准的点云
	ndt.setInputSource(filtered_cloud);
	//设置点云配准目标
	ndt.setInputTarget(target_cloud);
	//设置使用机器人测距法得到的初始对准估计结果
	Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
	Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
	Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
	//计算需要的刚体变换以便将输入的点云匹配到目标点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	ndt.align(*output_cloud, init_guess);
	std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
		<< " score: " << ndt.getFitnessScore() << std::endl;
	//使用创建的变换对未过滤的输入点云进行变换
	pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());

	cout << "transform matrix: " << endl;
	cout << ndt.getFinalTransformation() << endl;
	//保存转换的输入点云
	pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);
	// 初始化点云可视化界面
	boost::shared_ptr<pcl::visualization::PCLVisualizer>
		viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer_final->setBackgroundColor(0, 0, 0);
	//对目标点云着色（红色）并可视化
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		target_color(target_cloud, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "target cloud");
	//对转换后的目标点云着色（绿色）并可视化
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		output_color(output_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "output cloud");
	// 启动可视化
	viewer_final->addCoordinateSystem(1.0);
	viewer_final->initCameraParameters();
	//等待直到可视化窗口关闭。
	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return (0);
}

运行结果

1.5.4 位姿估计

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimation<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudT::Ptr object_aligned (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);
  FeatureCloudT::Ptr object_features (new FeatureCloudT);
  FeatureCloudT::Ptr scene_features (new FeatureCloudT);
  
  // Get input object and scene
  if (argc != 3)
  {
    pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
    return (1);
  }
  
  //  加载目标物体和场景点云
  pcl::console::print_highlight ("Loading point clouds...\n");
  if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
      pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (1);
  }
  
  // 下采样
  pcl::console::print_highlight ("Downsampling...\n");
  pcl::VoxelGrid<PointNT> grid;
  const float leaf = 0.005f;
  grid.setLeafSize (leaf, leaf, leaf);
  grid.setInputCloud (object);
  grid.filter (*object);
  grid.setInputCloud (scene);
  grid.filter (*scene);
  
  // 估计场景法线
  pcl::console::print_highlight ("Estimating scene normals...\n");
  pcl::NormalEstimation<PointNT,PointNT> nest;
  nest.setRadiusSearch (0.01);
  nest.setInputCloud (scene);
  nest.compute (*scene);
  
  // 特征估计
  pcl::console::print_highlight ("Estimating features...\n");
  FeatureEstimationT fest;
  fest.setRadiusSearch (0.025);
  fest.setInputCloud (object);
  fest.setInputNormals (object);
  fest.compute (*object_features);
  fest.setInputCloud (scene);
  fest.setInputNormals (scene);
  fest.compute (*scene_features);
  
  // 实施配准
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
  align.setInputSource (object);
  align.setSourceFeatures (object_features);
  align.setInputTarget (scene);
  align.setTargetFeatures (scene_features);
    align.setMaximumIterations (50000); //  采样一致性迭代次数
  align.setNumberOfSamples (3); //  创建假设所需的样本数
  align.setCorrespondenceRandomness (5); //  使用的临近特征点的数目
  align.setSimilarityThreshold (0.9f); // 多边形边长度相似度阈值
  align.setMaxCorrespondenceDistance (2.5f * 0.005); //  判断是否为内点的距离阈值
  align.setInlierFraction (0.25f); //接受位姿假设所需的内点比例
  {
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned);
  }
  
  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
    
    // Show alignment
    pcl::visualization::PCLVisualizer visu("鲁棒位姿估计");
	int v1(0),v2(0);
	visu.createViewPort(0,0,0.5,1,v1);
	visu.createViewPort(0.5,0,1,1,v2);
	visu.setBackgroundColor(255,255,255,v1);
    visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene",v1);
    visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned",v1);
	
	visu.addPointCloud(object,ColorHandlerT (object, 0.0, 255.0, 0.0), "object_before_aligned",v2);
	visu.addPointCloud(scene,ColorHandlerT (scene, 0.0, 0.0, 255.0), "scene_v2",v2);
	visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene");
	visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_aligned");
	visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_before_aligned");
	visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene_v2");
    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
    return (1);
  }
  
  return (0);
}

运行结果

二、总结

	本周学习了点云配准，明白了点云配准中的ICP原理。

三、参考

点云配准算法的综述（这主要讲解了一下点云配准的过程和分类）
ICP原理（说明了ICP的原理）
ICP面试