一種類似于ICP算法的三維點(diǎn)自動(dòng)配準(zhǔn)方法

<問題描述>

傳統(tǒng)經(jīng)典的方法就是ICP算法，Besl于1992年提出，稱為ICP（最近點(diǎn)迭代算法）。該方法將輸入的兩片點(diǎn)云分別記為源點(diǎn)云P和目標(biāo)點(diǎn)云Q，通過遍歷搜索找到P、Q之間所有點(diǎn)的對(duì)應(yīng)關(guān)系得出變換矩陣H，最后以兩點(diǎn)距離的平方和作為目標(biāo)誤差函數(shù)，不斷迭代計(jì)算，直至滿足一定的誤差收斂條件。

這個(gè)問題在很多方面都會(huì)遇到，比如相機(jī)的外參標(biāo)定，在世界坐標(biāo)系放置幾個(gè)點(diǎn)，相機(jī)a在自己的坐標(biāo)系下面看到的坐標(biāo)是a1,a2,a3...an，相機(jī)b在自己坐標(biāo)系下面看到的是b1,b2,b3...bn，如何將將相機(jī)a兩個(gè)相機(jī)統(tǒng)一到a的坐標(biāo)系下面。此時(shí)就會(huì)用到點(diǎn)的配準(zhǔn)。

使用三維掃描進(jìn)行物體掃描，要將一個(gè)物體完整的建模需要多個(gè)視角連續(xù)采集，每個(gè)視角相機(jī)的姿態(tài)都會(huì)存在變化，視角之間需要配準(zhǔn)，通常的方法是在物體表面貼Marker點(diǎn)，通過識(shí)別Marker點(diǎn)的坐標(biāo)，快速配準(zhǔn)然后再局部?jī)?yōu)化。

Nonlinear Curve Fit 最適合輸入數(shù)據(jù)點(diǎn)集（X，Y）的參數(shù)集，如非線性函數(shù)Y＝f（X，a）所示，其中a是參數(shù)集。該方法使用Levenberg-Marquardt（列文伯格-馬夸爾特）算法，是使用最廣泛的非線性最小二乘算法。

使用這個(gè)函數(shù)來優(yōu)化迭代找到最優(yōu)的變換矩陣，核心是設(shè)置這個(gè)迭代的VI。

輸入點(diǎn)先進(jìn)行旋轉(zhuǎn)計(jì)算，再進(jìn)行平移計(jì)算，最后輸出變換后的點(diǎn)集，通過迭代使得輸出變換后的點(diǎn)集Q與目標(biāo)點(diǎn)集P的差值最小。

將源點(diǎn)云和目標(biāo)點(diǎn)云變換為1D數(shù)組，通過Nonlinear Curve Fit 迭代，最后輸出變換后的RT（旋轉(zhuǎn)矩陣和平移向量）。

<結(jié)果驗(yàn)證>

白色點(diǎn)是源點(diǎn)云P，紅色線是目標(biāo)點(diǎn)云Q，綠色線是配準(zhǔn)后的Q，結(jié)果配準(zhǔn)后的Q與源點(diǎn)集P重合。

這里提供了一種類似于ICP算法的一種方法，希望對(duì)大家有所啟發(fā)！

審核編輯：劉清