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

一、点云配准

点云配准有很多的用途，比如说在三维重建中会将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$进行改写，就是在后边加一项又减一项
J = 1 2 Σ i = 1 n ∣ ∣ q i − ( R p i + t ) ∣ ∣ 2 = 1 2 Σ i = 1 n ∣ ∣ q i − R p i − t − ( μ q − R μ p ) + ( μ q − R μ p ) ∣ ∣ 2 = 1 2 Σ i = 1 n ( ∣ ∣ q i − μ q − R ( p i − μ p ) ∣ ∣ 2 + ∣ ∣ μ q − R μ p − t ∣ ∣ 2 ) + Σ i = 1 n ( q i − μ q − R ( p i − μ p ) ) T ( μ q − R μ p − t )

$$\begin{aligned} J&=\frac{1}{2}\Sigma_{i=1}^n \left||q_i-(Rp_i+t)|\right|^2\\ &=\frac{1}{2}\Sigma_{i=1}^n \left||q_i-Rp_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{aligned}$$ J​=21​Σi=1n​∣∣qi​−(Rpi​+t)∣∣2=21​Σi=1n​∣∣qi​−Rpi​−t−(μq​−Rμp​)+(μq​−Rμp​)∣∣2=21​Σi=1n​(∣∣qi​−μq​−R(pi​−μp​)∣∣2+∣∣μq​−Rμp​−t∣∣2)+Σi=1n​(qi​−μq​−R(pi​−μp​))T(μq​−Rμp​−t)​

        其中，对最后一项进行化简，可以发现前半部分为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 }$:
R ∗ = a r g m i n R Σ i = 1 n ( q i ′ T q i ′ + p i ′ T R T R p i ′ − 2 q i ′ T R p i ′ )

$$\begin{aligned} R^*&=argmin_{R}\Sigma_{i=1}^n(q_i'^{T}q_i'+p_i'^{T}R^TRp_i'-2q_i'^{T}Rp_i') \end{aligned}$$ R∗​=argminR​Σi=1n​(qi′T​qi′​+pi′T​RTRpi′​−2qi′T​Rpi′​)​
        因为$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$
∴ t r ( R H ) = t r ( R U Σ V T ) = t r ( Σ V T R U )

$$\begin{aligned} \therefore tr(RH)&=tr(RU\Sigma V^T)\\ &=tr(\Sigma V^TRU) \end{aligned}$$ ∴tr(RH)​=tr(RUΣVT)=tr(ΣVTRU)​

        设$M=V^{T}RU$，由SVD的定义可知$M$仍为正交阵
∴ t r ( Σ M ) = t r ( ( σ 1 0 0 0 σ 2 0 0 0 σ 3 ) ( m 11 m 12 m 13 m 21 m 22 m 23 m 31 m 32 m 33 ) ) \therefore tr(\Sigma M)=tr(

$$\begin{pmatrix} \sigma_{1} &0 &0 \\ 0 &\sigma_{2} &0 \\ 0 &0 &\sigma_{3} \end{pmatrix}$$ $$\begin{pmatrix} m_{11} &m_{12} &m_{13} \\ m_{21} &m_{22} &m_{23} \\ m_{31} &m_{32} &m_{33} \end{pmatrix}$$ ) ∴tr(ΣM)=tr( ​σ1​00​0σ2​0​00σ3​​ ​ ​m11​m21​m31​​m12​m22​m32​​m13​m23​m33​​ ​)
        上式等于${\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;
			}
		}
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120