基本原理

ICP, 即Iterative Closest Point, 迭代點(diǎn)算法。

ICP算法有多種形式，其中最簡單的思路就是比較點(diǎn)與點(diǎn)之間的距離，對于點(diǎn)云P={pi},Q={qi}P=\{p_i\}, Q=\{q_i\}P={pi?},Q={qi?}而言，如果二者是同一目標(biāo)，通過旋轉(zhuǎn)、平移等操作可以實現(xiàn)重合的話，那么只需要固定$Q$而不斷地旋轉(zhuǎn)或平移$P$，最終二者一定能最完美地重合。

設(shè)旋轉(zhuǎn)$P$的矩陣為$R$，平移矩陣為$t$，在完美匹配的情況下，必有$q_i=R{p}_i+t$。

又因三維點(diǎn)云不具備柵格特征，故而很難保證$q_i$和$p_i$是同一點(diǎn)，所以要使得目標(biāo)函數(shù)最小化

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

1992年Chen和Medioni對此方案進(jìn)行了改進(jìn)，提出了點(diǎn)對面的預(yù)估方法，其目標(biāo)函數(shù)為

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

其中$n_p$是點(diǎn)$p$的法線，這種方案顯然效率更高。

open3d調(diào)用

open3d中實現(xiàn)了ICP算法，參數(shù)如下

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

source為點(diǎn)云$P$，target為目標(biāo)點(diǎn)云$Q$，max_correspondence_distance為匹配點(diǎn)在未匹配時的最大距離，init為初始變化矩陣，默認(rèn)為單位矩陣；criteria為精度。

estimation_method可以理解為上面提到的兩種方案，下面選擇點(diǎn)對點(diǎn)ICP方法進(jìn)行計算

import numpy as np
import open3d as o3dpipreg = o3d.pipelines.registrationpcd = 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)的返回信息如下，表示點(diǎn)云配準(zhǔn)的擬合程度

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

繪圖

為了對比配準(zhǔn)前后的區(qū)別，對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])

此為原圖，可以看到兩組點(diǎn)云完全是錯位的

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

得到結(jié)果如下，可見兩組不同顏色的點(diǎn)云已經(jīng)幾乎重合到了一起