<問題描述>

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

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

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

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

使用這個函數來優化迭代找到最優的變換矩陣，核心是設置這個迭代的VI。

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

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

<結果驗證>

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

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

審核編輯：劉清