点云配准算法---ICP（迭代最近邻点）详解 附C++代码_icp迭代最近邻点

一、概述

现在是2021.6.2晚21:24，闲着也是闲着，写个ICP吧，再从头到尾思考一遍，做一个详细的记录。ICP算法是一种点云配准时常用的方法，它是一种细配准方法，通常在配准时要配合其他粗配准方法进行使用，先将两帧差异非常大的点云进行粗配准，得到初步具有重合部分的点云，然后再利用ICP将其进一步配准，使得重合区域最大。

ICP的缺点就是容易陷入局部最优解当中，每次迭代找到的对应点对只是点云的局部，这样就会每次只在两帧点云重叠的部分进行迭代，而重叠的部分可能并不是两帧点云对应的重合部分。所以这也就是为什么要事先通过粗配准方法先将两帧点云大致进行重合配准，先确保重叠部分基本为两帧点云对应的重合部分。

二、ICP思想步骤

ICP：Iterative Closest Point，顾名思义，这是一种迭代的思想。通过不断地迭代，每次在前一次的计算结果之上再计算出新的变换矩阵，最终当迭代次数满足条件或者变换矩阵收敛时停止。

设已经过粗配准后待进行细配准的两帧点云分别为 P = { p 1 , p 2 , ⋯   , p m } , Q = { q 1 , q 2 , ⋯   , q n } P = \{p_1, p_2,\cdots, p_m\},\quad Q = \{q_1, q_2, \cdots, q_n\} P={p1​,p2​,⋯,pm​},Q={q1​,q2​,⋯,qn​}（通常情况下，由于深度相机视野不变，所以扫描出的每一帧点云中点集数量相等，即$m=n$），这里假设需要将$P$配准到$Q$上。

步骤：

1. 寻找$P$和$Q$上所有对应点对：遍历$P$（或者Q）中所有点进行如下操作：假设当前遍历到$p_i$点，将点$p_i$放入到$Q$的所有点所构成的kdtree空间中，查找$Q$中距离$p_i$的最近点$q_i$，如果$\Vert p_i-q_i\Vert <\xi$，则认为$(p_i,q_i)$是一对对应点，其中$\xi$为人为设定的距离阈值。(其实值得注意的是，在不同点$(p_i,q_i)$和$(p_j,q_j)$中，完全有可能会出现$q_i=q_j$的情况，但这并不影响)。遍历完$P$中的所有点后便找到了所有对应点对，对应点对集合为 C o r r = { ( p i , q i ) ∣ p i ∈ P , q i ∈ Q , ∥ p i − q i ∥ < ξ } Corr=\{(p_i, q_i)|p_i\in P, q_i\in Q,\|p_i-q_i\|<\xi\} Corr={(pi​,qi​)∣pi​∈P,qi​∈Q,∥pi​−qi​∥<ξ}，其中$q_i$为$Q$的kdtree空间中与$p_i$最近的点；
2. 设要求的变换矩阵为$R$，对于$\forall p\in Corr$，都可以构造一个误差$l_p=\Vert Rp-q\Vert$，那么对于所有的对应点对，则可以利用每个点的误差求和来构造一个整体的误差函数$l={\sum }_i^N\Vert R{p}_i-q_i\Vert$（其中$N$为对应点对的数量）这样，我们的目标就变成了让这个函数最小，这就转换成了一个最小二乘问题。在Eigen中，只需要输入两片点云中的对应点即可求得变换矩阵$R$；
3. 在第2步中求得变换矩阵$R$后，将点云$P$进行变换后得到更新位置后的点云$P^{\prime }$，然后重复步骤1~3，直至满足终止条件时停止迭代。

三、实验结果

下图为粗配准之后的两帧点云。

再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> 
#include <vector>
#include <omp.h>
using namespace std;

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.6f %6.6f %6.6f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.6f %6.6f %6.6f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.6f %6.6f %6.6f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.6f, %6.6f, %6.6f >\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;
}

void findcorrespondences(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& P, pcl::PointCloud<pcl::PointXYZ>::Ptr& Q)
{
	P->clear();
	Q->clear();
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(target);
//#pragma omp parallel for num_threads(200)
	for (int i = 0; i < source->points.size(); i++)
	{
		vector<int> indx;
		vector<float> dists;
		kdtree.nearestKSearch(source->points[i], 1, indx, dists);
		if (dists[0] < 0.02)
		{
			P->push_back(source->points[i]);
			Q->push_back(target->points[indx[0]]);
		}
	}
}

Eigen::Matrix<double, 4, 4> findRT(pcl::PointCloud<pcl::PointXYZ>::Ptr& P, pcl::PointCloud<pcl::PointXYZ>::Ptr& Q)
{
	int n = P->points.size();
	Eigen::Matrix<double, 3, Eigen::Dynamic> matrixP(3, n), matrixQ(3, n);
	for (int i = 0; i < n; i++)
	{
		matrixP(0, i) = P->points[i].x;
		matrixP(1, i) = P->points[i].y;
		matrixP(2, i) = P->points[i].z;
		matrixQ(0, i) = Q->points[i].x;
		matrixQ(1, i) = Q->points[i].y;
		matrixQ(2, i) = Q->points[i].z;
	}
	Eigen::Matrix4d RT = Eigen::umeyama(matrixP, matrixQ);
	print4x4Matrix(RT);
	return RT;
}

void myICP(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target)
{
	Eigen::Matrix<double, 4, 4> RT;
	pcl::PointCloud<pcl::PointXYZ>::Ptr P(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr Q(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::visualization::PCLVisualizer viewer("viewer");

	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_source(source, 255, 0, 0), cloud_target(target, 255, 255, 255);

	viewer.addPointCloud(source, cloud_source, "source");
	viewer.addPointCloud(target, cloud_target, "target");

	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
	while(!viewer.wasStopped())
	{
		viewer.spinOnce();
		if (next_iteration)
		{
			findcorrespondences(source, target, P, Q);
			RT = findRT(P, Q);
			pcl::transformPointCloud(*source, *source, RT);
			viewer.updatePointCloud(source, cloud_source,"source");
		}
		next_iteration = false;
	}	
}


int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PLYReader reader;
	
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	transformation_matrix(0, 0) = 0.965774;
	transformation_matrix(0, 1) = -0.249227;
	transformation_matrix(0, 2) = -0.0718807;
	transformation_matrix(0, 3) = 0.0268529;
	transformation_matrix(1, 0) = 0.259346;
	transformation_matrix(1, 1) = 0.92296;
	transformation_matrix(1, 2) = 0.284401;
	transformation_matrix(1, 3) = 0.2054;
	transformation_matrix(2, 0) = -0.00453762;
	transformation_matrix(2, 1) = -0.293309;
	transformation_matrix(2, 2) = 0.956007;
	transformation_matrix(2, 3) = -0.0292454;

	reader.read("fragment_000.ply", *source);
	reader.read("fragment_001.ply", *target);
	
	pcl::transformPointCloud(*source, *source, transformation_matrix);
	myICP(source, target);
	return 0;
}
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
121
122
123
124
125

代码中的粗配准变换矩阵$transformation\_matrix$事先已通过Ransac进行计算得到。后面有时间将记录Ransac算法以及其在点云配准中的应用。

6.2号开始写的，居然今天才发。。。我吐了。