一種類似于ICP算法的三維點自動配準方法

<問題描述>

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

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

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

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

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

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

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

<結(jié)果驗證>

白色點是源點云P，紅色線是目標點云Q，綠色線是配準后的Q，結(jié)果配準后的Q與源點集P重合。

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

審核編輯：劉清