PCL ICP配准【点到点】（Registration_ICP）

PCL专栏目录及须知

注意：ICP配准为最常用的精配准方法，需掌握原理及代码。

ICP配准即不停的迭代计算两点云之间相应点对的最小距离，如果点对之间的距离小于某个阈值或者达到最大迭代次数，即停止计算。

1.点云配准

1.1 点云配准解释

在实际点云处理过程中，由于被测物体尺寸过大，物体表面被遮挡或者三维扫描设备的扫描角度等因素，单次扫描往往得不到物体完整的几何信息。因此，对于多次在不同视角即不同参考坐标下扫描得到的两组或者多组点云进行合并，统一到统一坐标系下，还原原始完整点云信息，即为点云配准。

1.2 刚性变换矩阵

点云配准一般通过对各输入点云进行一定的旋转和平移变换（不包括形变），以将不同坐标系下的两组或者多组点云数据统一到同一参考坐标系下。

由于不包含形变，因此该类只包含平移和旋转的变换矩阵即为刚性变换矩阵。

1.2.1 刚性变换矩阵之平移矩阵

刚性变换由平移和旋转两个矩阵构成。其中平移矩阵即将点云从一个位置移动到另一个位置的矩阵：

平移矩阵通过向量t=(tx,ty,tz)对点云进行平移操作。

写为Eigen常用的4X4矩阵形式为：

1.2.2 刚性变换矩阵之旋转矩阵

旋转矩阵即为对点云进行位姿旋转的矩阵：

我们假设三个旋转矩阵 Rx(Θ)、Ry(Θ)、Rz(Θ)分别表示将物体绕x,y,z轴进行旋转。

旋转矩阵的三维推导

三个单个轴的旋转矩阵相乘，得到最终的旋转矩阵（α代表x轴，β代表y轴，γ代表z轴）：

相乘最终得到的包含xyz三个轴旋转分量的旋转矩阵公式为：

1.2.3 合并平移及旋转矩阵

最终得到的刚性变换矩阵，即合并平移矩阵T(t)和旋转矩阵M(α,β,γ)，得到最终的刚性变换矩阵。

如将点云点A，用如上矩阵进行刚性变换，得到点云点B，计算公式为：

1.3 ICP配准流程

1.3.1 ICP算法基本原理

（1）取待匹配的目标点云P，以及源点云Q。

（2）在点云P和点云Q中，按照一定的约束条件，找到最邻近的点对(Pi,Qi)。

（3）迭代计算得出使得误差函数最小的，最优的匹配参数R(旋转矩阵)、T(平移矩阵)。

误差函数公式：

n：最邻近点对个数。

Pi、Qi：即最邻近的点对(Pi,Qi)。

R：旋转矩阵。

T：平移矩阵。

1.3.2 ICP算法具体流程

（1）在目标点云P中取点集Pi∈P；

（2）找目标点云及源点云之间的最邻近点对(Pi,Qi)，即找出源点云Q中的对应点集Qi∈Q，使得| Qi-Pi |=min；

（3）计算得到旋转矩阵R和平移矩阵t，使得误差函数最小；

（4）对目标点云的点集Pi，使用上一步得到的旋转矩阵和平移矩阵进行刚体变换，得到新的目标点云点集：

（5）计算Pi'与对应点集Qi的平均距离。计算公式：

（6）如果满足收敛条件：1.d小于某一给定的阈值或者2.大于预设的最大迭代次数，则停止迭代计算。如不符合上述两个条件，则返回第二步，重新计算，直到满足收敛条件为止。

2.关键函数

（1）为终止条件设置最小转换差异，即最优点对距离的阈值（本值一般都设很小）

registration.setTransformationEpsilon(1e-10);							// 为终止条件设置最小转换差异

（2）设置对应点对之间的最大距离，即pcl::registration::CorrespondenceEstimation调用determineCorrespondences(*correspondences, dis)时的dis的值。

registration.setMaxCorrespondenceDistance(1.5);							// 设置对应点对之间的最大距离

（3）设置收敛条件是均方误差和小于阈值， 停止迭代；

registration.setEuclideanFitnessEpsilon(0.01);

（4）最大迭代次数

registration.setMaximumIterations(500);

（5）设置为true,则使用点对对应关系【需设置为false】

registration.setUseReciprocalCorrespondences(false);					// 设置为true,则使用点对对应关系

3.完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>

using namespace std;

int
main(int argc, char** argv)
{
	/****************ICP配准********************/
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);			// 源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云
	pcl::io::loadPCDFile("D:/code/csdn/data/tree0.pcd", *source);
	pcl::io::loadPCDFile("D:/code/csdn/data/tree1.pcd", *target);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云转换后的结果

	// ICP配准
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration;	// icp对象
	registration.setInputSource(source);									// 源点云
	registration.setInputTarget(target);									// 目标点云
	registration.setTransformationEpsilon(1e-10);							// 为终止条件设置最小转换差异
	registration.setMaxCorrespondenceDistance(1.5);							// 设置对应点对之间的最大距离
	registration.setEuclideanFitnessEpsilon(0.01);							// 设置收敛条件是均方误差和小于阈值， 停止迭代；
	registration.setMaximumIterations(500);									// 最大迭代次数
	registration.setUseReciprocalCorrespondences(false);					// 设置为true,则使用点对对应关系
	registration.align(*result);

	std::cout << "点对匹配分数：" << registration.getFitnessScore() << std::endl;
	std::cout << "刚体变换矩阵：" << registration.getFinalTransformation() << std::endl;

	/****************展示********************/
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP"));
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0);		// 目标点云
	viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>result_color(result, 0, 255, 0);		// 配准结果点云
	viewer->addPointCloud<pcl::PointXYZ>(result, result_color, "result cloud");

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return (0);
}

4.结果展示

（1）配准前两棵树的位置

（2）两棵树配准后的结果点云