open3d快速上手

基本原理

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

ICP算法有多種形式，其中最簡(jiǎn)單的思路就是比較點(diǎn)與點(diǎn)之間的距離，對(duì)于點(diǎn)云 P = { p i } , Q = { q i } P=\{p_i\}, Q=\{q_i\} P={pi?},Q={qi?}而言，如果二者是同一目標(biāo)，通過(guò)旋轉(zhuǎn)、平移等操作可以實(shí)現(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對(duì)此方案進(jìn)行了改進(jìn)，提出了點(diǎn)對(duì)面的預(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中實(shí)現(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)在未匹配時(shí)的最大距離，init為初始變化矩陣，默認(rèn)為單位矩陣；criteria為精度。

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

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)的返回信息如下，表示點(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.

繪圖

為了對(duì)比配準(zhǔn)前后的區(qū)別，對(duì)src和tar放在圖中對(duì)比

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)云完全是錯(cuò)位的

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

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

文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-418386.html

到了這里，關(guān)于open3d點(diǎn)云配準(zhǔn)函數(shù)registration_icp的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！