網(wǎng)站建設(shè)外包合同seo外鏈發(fā)布
文章目錄
- 基本原理
- open3d調(diào)用
- 繪圖
基本原理
ICP, 即Iterative Closest Point, 迭代點(diǎn)算法。
ICP算法有多種形式,其中最簡(jiǎn)單的思路就是比較點(diǎn)與點(diǎn)之間的距離,對(duì)于點(diǎn)云P={pi},Q={qi}P=\{p_i\}, Q=\{q_i\}P={pi?},Q={qi?}而言,如果二者是同一目標(biāo),通過(guò)旋轉(zhuǎn)、平移等操作可以實(shí)現(xiàn)重合的話,那么只需要固定QQQ而不斷地旋轉(zhuǎn)或平移PPP,最終二者一定能最完美地重合。
設(shè)旋轉(zhuǎn)PPP的矩陣為RRR,平移矩陣為ttt,在完美匹配的情況下,必有qi=Rpi+tq_i = Rp_i + tqi?=Rpi?+t。
又因三維點(diǎn)云不具備柵格特征,故而很難保證qiq_iqi?和pip_ipi?是同一點(diǎn),所以要使得目標(biāo)函數(shù)最小化
arg?min?R,t12∑i=1n∥qi?Rpi?t∥2\argmin_{R,t}\frac{1}{2}\sum^n_{i=1}\Vert q_i-Rp_i-t\Vert^2 R,targmin?21?i=1∑n?∥qi??Rpi??t∥2
1992年Chen和Medioni對(duì)此方案進(jìn)行了改進(jìn),提出了點(diǎn)對(duì)面的預(yù)估方法,其目標(biāo)函數(shù)為
arg?min?R,t12∑i=1n[(qi?Rpi)?np]2\argmin_{R,t}\frac{1}{2}\sum^n_{i=1}[(q_i-Rp_i)\cdot n_p]^2 R,targmin?21?i=1∑n?[(qi??Rpi?)?np?]2
其中npn_pnp?是點(diǎn)ppp的法線,這種方案顯然效率更高。
open3d調(diào)用
open3d中實(shí)現(xiàn)了ICP算法,參數(shù)如下
registration_icp(source, target, max_correspondence_distance, init, estimation_method, criteria)
source
為點(diǎn)云PPP,target
為目標(biāo)點(diǎn)云QQQ,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 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.
繪圖
為了對(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)幾乎重合到了一起