基本原理

ICP, 即Iterative Closest Point, 迭代点算法。

ICP算法有多种形式，其中最简单的思路就是比较点与点之间的距离，对于点云 P = { p i } , Q = { q i } P=\{p_i\}, Q=\{q_i\} P={pi​},Q={qi​}而言，如果二者是同一目标，通过旋转、平移等操作可以实现重合的话，那么只需要固定$Q$而不断地旋转或平移$P$，最终二者一定能最完美地重合。

设旋转$P$的矩阵为$R$，平移矩阵为$t$，在完美匹配的情况下，必有$q_i=R{p}_i+t$。

又因三维点云不具备栅格特征，故而很难保证$q_i$和$p_i$是同一点，所以要使得目标函数最小化

$$\underset{R,t}{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n\Vert q_i-R{p}_i-t{\Vert }^2$$

1992年Chen和Medioni对此方案进行了改进，提出了点对面的预估方法，其目标函数为

$$\underset{R,t}{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n[(q_i-R{p}_i)\cdot n_p{]}^2$$

其中$n_p$是点$p$的法线，这种方案显然效率更高。

open3d调用

open3d中实现了ICP算法，参数如下

registration_icp(source, target, max_correspondence_distance, init, estimation_method, criteria)

source为点云$P$，target为目标点云$Q$，max_correspondence_distance为匹配点在未匹配时的最大距离，init为初始变化矩阵，默认为单位矩阵；criteria为精度。

estimation_method可以理解为上面提到的两种方案，下面选择点对点ICP方法进行计算

import numpy as np
import open3d as o3d

pipreg = o3d.pipelines.registration

pcd = o3d.data.DemoICPPointClouds()
src = o3d.io.read_point_cloud(pcd.paths[0])
tar = o3d.io.read_point_cloud(pcd.paths[1])
th = 0.02
trans_init = np.array([
    [0.862, 0.011, -0.507, 0.5], [-0.139, 0.967, -0.215, 0.7],
    [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])

reg = pipreg.registration_icp(
    src, tar, th, trans_init,
    pipreg.TransformationEstimationPointToPoint())

print(reg.transformation)
''' 变换矩阵
[[ 0.83924644  0.01006041 -0.54390867  0.64639961]
 [-0.15102344  0.96521988 -0.21491604  0.75166079]
 [ 0.52191123  0.2616952   0.81146378 -1.50303533]
 [ 0.          0.          0.          1.        ]]
'''
print(reg)

print(reg)的返回信息如下，表示点云配准的拟合程度

RegistrationResult with fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056 Access transformation to get result.

绘图

为了对比配准前后的区别，对src和tar放在图中对比

import copy
srcDraw = copy.deepcopy(src)
tarDraw = copy.deepcopy(tar)
srcDraw.paint_uniform_color([1, 1, 0])
tarDraw.paint_uniform_color([0, 1, 1])
srcDraw.transform(tf)
o3d.visualization.draw_geometries([srcDraw, tarDraw])

此为原图，可以看到两组点云完全是错位的

srcDraw = copy.deepcopy(src)
tarDraw.paint_uniform_color([0, 1, 1])
srcDraw.transform(reg.transformation)
o3d.visualization.draw_geometries([srcDraw, tarDraw])

得到结果如下，可见两组不同颜色的点云已经几乎重合到了一起