点云配准经典方法——ICP 原理推导及PCL实现

一、点云配准

点云配准有很多的用途，比如说在三维重建中会将2.5D的点云通过点云配准技术获得准确的3D点云；再比如点云配准可以用来做位姿估计；除此以外在自动驾驶、考古、地图测绘等领域都有应用到这项技术。这篇文章我们聊聊最经典的ICP（Iterative Closest Point）算法。

二、ICP原理

2.1 简介

        ICP算法需要输入两个点云，一个是目标点云，另一个是源点云，输出的是从源点云到目标点云的位姿变换。目标点云不会移动，而源点云则会在迭代的过程中不断接近目标点云，直到收敛。

        但是ICP算法很容易陷入局部最优解，通常需要目标点云和源点云在配准前有比较高的重合率，因此在执行ICP之前通常会用其他方法进行一次粗配准，来获得比较好的初值，然后再使用ICP进行精配准。

2.2 原理推导

        经典的ICP算法会对下面的目标函数进行优化：
$$J=\frac{1}{2}{\Sigma }_{i=1}^n{\left||q_i-(R{p}_i+t)|\right|}^2$$

        式中的 $p$ 和 $q$ 分别是源点云和目标点云中的点，$n$是源点云的总点数。这个目标函数很容易理解，就是要找到由$R$、$t$组成的刚体变换，来使得源点云和目标点云之间的差异尽可能小。

        如何衡量这两个点云之间的差异呢？经典的ICP会遍历 $<R,t>$变换后的源点云中的所有点$p_i^{\prime }$，从目标点云中搜索出每个$p_i^{\prime }$的最近邻$q_i$，$(p_i^{\prime },q_i)$形成一个匹配点对，计算二者之间距离的平方。 对所有点对的距离平方求和，总和最小则认为差异最小。

        当匹配点对确定好之后，ICP的优化函数其实是一个凸函数，可以找到最优解，求解过程如下：

        定义源点云和目标点云的质心：
$${\mu }_p=\frac{1}{n}{\Sigma }_{i=1}^n{p}_i,{\mu }_q=\frac{1}{n}{\Sigma }_{i=1}^n{q}_i$$

        然后对$J$进行改写，就是在后边加一项又减一项
$$\begin{array}{rl}J& =\frac{1}{2}{\Sigma }_{i=1}^n{\left||q_i-(R{p}_i+t)|\right|}^2\\ & =\frac{1}{2}{\Sigma }_{i=1}^n{\left||q_i-R{p}_i-t-({\mu }_q-R{\mu }_p)+({\mu }_q-R{\mu }_p)|\right|}^2\\ & =\frac{1}{2}{\Sigma }_{i=1}^n({\left||q_i-{\mu }_q-R(p_i-{\mu }_p)|\right|}^2+{\left||{\mu }_q-R{\mu }_p-t|\right|}^2)\\ & +{\Sigma }_{i=1}^n(q_i-{\mu }_q-R(p_i-{\mu }_p){)}^T({\mu }_q-R{\mu }_p-t)\end{array}$$

        其中，对最后一项进行化简，可以发现前半部分为0，因为${\Sigma }_{i=1}^n{q}_i={\Sigma }_{i=1}^n{\mu }_q$，${\Sigma }_{i=1}^n{p}_i={\Sigma }_{i=1}^n{\mu }_p$，所以$J$可以改写为：
$$J=\frac{1}{2}{\Sigma }_{i=1}^n({\left||q_i-{\mu }_q-R(p_i-{\mu }_p)|\right|}^2+{\left||{\mu }_q-R{\mu }_p-t|\right|}^2)$$

        观察一下这个式子，对于后一项${\Sigma }_{i=1}^n{\left||{\mu }_q-R{\mu }_p-t|\right|}^2$，很明显，不管选什么$R$，都可以找到对应的$t$使得这一项为0。因此我们只需要对前一项进行优化，找出使得其值最小的$R$，然后再用后一项求解$t$，即可获得最小的$J$值。
        将点云去中心化，$q_i^{\prime }=q_i-{\mu }_q$，$p_i^{\prime }=p_i-{\mu }_p$，然后对第一部分进行简化，最优的旋转矩阵$R^{\ast }$:
$$\begin{array}{rl}{R}^{\ast }& =argmi{n}_R{\Sigma }_{i=1}^n(q_i^{\prime T}{q}_i^{\prime }+p_i^{\prime T}{R}^{T}R{p}_i^{\prime }-2q_i^{\prime T}R{p}_i^{\prime })\end{array}$$
        因为$R^{T}R=E$，所以前两项与$R$无关，因此
$$\begin{array}{r}{R}^{\ast }=argmi{n}_R{\Sigma }_{i=1}^n(-2q_i^{\prime T}R{p}_i^{\prime })\end{array}$$

        相当于求使得下式最大的$R$，取迹成立是因为$E$是一个标量
$$E={\Sigma }_{i=1}^n(q_i^{\prime T}R{p}_i^{\prime })=tr({\Sigma }_{i=1}^n(q_i^{\prime T}R{p}_i^{\prime }))$$
        然后利用迹的性质，对于满足矩阵乘法规则的$AB$和$BA$，$tr(AB)=tr(BA)$
$$E=tr({\Sigma }_{i=1}^n(R{p}_i^{\prime }{q}_i^{\prime T}))=tr(RH)$$
        其中$H={\Sigma }_{i=1}^n(p_i^{\prime }{q}_i^{\prime T})=P^{\prime }{Q}^{\prime T}$，对其进行SVD分解可得$H=U\Sigma V^T$
$$\begin{array}{rl}\therefore tr(RH)& =tr(RU\Sigma V^T)\\ & =tr(\Sigma V^{T}RU)\end{array}$$

        设$M=V^{T}RU$，由SVD的定义可知$M$仍为正交阵
$$\therefore tr(\Sigma M)=tr(\left(\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right)\left(\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right))$$
        上式等于${\sigma }_1{m}_{11}+{\sigma }_2{m}_{22}+{\sigma }_3{m}_{33}$，因为正交矩阵元素值小于等于1，所以当$M$为单位阵时，$tr(\Sigma M)$有最大值，所以$R^{\ast }=V{U}^T$。然后令${\mu }_q-R{\mu }_p-t=0$，可得$t^{\ast }={\mu }_q-R^{\ast }{\mu }_p$。

        对视角点云施加$（R^{\ast },t^{\ast })$的变换，然后重新进行点对匹配，再求解最优变换，直到满足收敛条件。

三、PCL中对于ICP的实现

简单总结一下ICP的流程

1. 输入源点云与目标点云，二者需要有比较好的相对位姿作为ICP的初值，$R=E$，$t=0$
2. 形成匹配点对：遍历源点云中的点$p_i$，从目标点云中找到最近的点$q_i$，二者配对
3. 求质心${\mu }_p$${\mu }_q$，将点云去中心化，并表示为矩阵形式$P^{\prime }$，$Q^{\prime T}$
4. 构造$H=P^{\prime }{Q}^{\prime T}$并做奇异值分解，得到$H=U\Sigma V^T$
5. $R^{\ast }=V{U}^T$，$t^{\ast }={\mu }_q-R\ast {\mu }_p$，$R=R^{\ast }R$，$t=t+t^{\ast }$
6. 对源点云做$<R,t>$变换，重新回到第2步进行迭代，直到满足迭代条件，输出最终的$<R,t>$

网上有很多对源码的分析，我直接把文章引过来，就不再赘述了，【PCL】ICP 源码分析

四、测试代码

通过W,A,S,D,Z,C,R,P,Y，可以对源点云进行移动，来实现粗配准，最后按Space来执行ICP

#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/filters\voxel_grid.h>

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

PointCloud::Ptr cloud_target(new PointCloud);
PointCloud::Ptr cloud_source(new PointCloud);
PointCloud::Ptr cloud_icp(new PointCloud);
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("global_visualizer"));

const double translation_step = 5;  // 设定一个平移的步长
const double rotation_step = M_PI / 24;  // 设定一个旋转的步长
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void);
Eigen::Affine3f G = Eigen::Affine3f::Identity();

int main() {
	//加载点云
	string target_path = "自己的目标点云路径";
	string source_path = "自己的源点云路径";
	pcl::io::loadPLYFile(target_path, *cloud_target);
	pcl::io::loadPLYFile(source_path, *cloud_source);

	//点云降采样
	pcl::VoxelGrid<pcl::PointXYZ> VG;
	VG.setInputCloud(cloud_target);
	VG.setLeafSize(5.0f, 5.0f, 5.0f);
	VG.filter(*cloud_target);
	VG.setInputCloud(cloud_source);
	VG.setLeafSize(5.0f, 5.0f, 5.0f);
	VG.filter(*cloud_source);

	//可视化相关
	int v1 = 1;
	viewer->createViewPort(0.0, 0.0, 1.0, 1.0, v1);
	viewer->setBackgroundColor(255, 255, 255, v1);
	viewer->addPointCloud(cloud_target, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);
	viewer->addPointCloud(cloud_source, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
	viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());


	while (!viewer->wasStopped()) {
		// 应用变换
		pcl::transformPointCloud(*cloud_source, *cloud_source, G);
		G = Eigen::Affine3f::Identity();

		// 更新视图
		viewer->removePointCloud("cloud_source");
		viewer->addPointCloud<pcl::PointXYZ>(cloud_source, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
		viewer->removePointCloud("cloud_target");
		viewer->addPointCloud<pcl::PointXYZ>(cloud_target, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);

		viewer->spinOnce(10);  // 每次更新等待10ms
	}

}

//键盘回调函数
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void)
{
	if (event.keyDown())
	{
		if (event.getKeySym() == "space") {
			pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
			icp.setInputSource(cloud_source);
			icp.setInputTarget(cloud_target);
			icp.setTransformationEpsilon(1e-10);
			icp.setMaxCorrespondenceDistance(100);
			icp.setEuclideanFitnessEpsilon(0.001);
			icp.setMaximumIterations(10000);
			icp.setUseReciprocalCorrespondences(false);

			icp.align(*cloud_icp);
			G = icp.getFinalTransformation();

		}
		else {
			switch (event.getKeySym()[0])
			{
			case 'w':
				G.translation() += Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 's':
				G.translation() -= Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 'a':
				G.translation() -= Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'd':
				G.translation() += Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'z':
				G.translation() += Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'c':
				G.translation() -= Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'r':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitX()));
				break;  // Roll
			case 'p':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitY()));
				break;  // Pitch
			case 'y':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitZ()));
				break;  // Yaw
			default: break;
			}
		}
	}
}