迭代最近點演算法 Iterative Closest Points__演算法

  【3D】迭代最近點演算法 Iterative Closest Points http://blog.csdn.net/xiaowei_cqu/article/details/8470376
分類： 【演算法分析】 【機器視覺】 2013-01-21 13:06  17173人閱讀  評論(47)  收藏  舉報

目錄(?)[+]

研究生課程系列文章參見索引《在信科的那些課》 基本原理

假定已給兩個資料集P、Q， ，給出兩個點集的空間變換f使他們能進行空間匹配。這裡的問題是，f為一未知函數，而且兩點集中的點數不一定相同。解決這個問題使用的最多的方法是迭代最近點法（Iterative Closest Points Algorithm）。

基本思想是：根據某種幾何特性對資料進行匹配，並設這些匹配點為假想的對應點，然後根據這種對應關係求解運動參數。再利用這些運動參數對資料進行變換。並利用同一幾何特徵，確定新的對應關係，重複上述過程。

迭代最近點法目標函數 三維空間中兩個3D點，  ，他們的歐式距離表示為：

三維點雲匹配問題的目的是找到P和Q變化的矩陣R和T，對於  ， ，利用最小二乘法求解最優解使：

最小時的R和T。

資料預先處理 實驗中採集了五個面的點如下所示： 由於第一組（第一排第1個）和第三組（第一排第三個）採集均為模型正面點雲，所以選用一和三做後續的實驗。 首先利用Geomagic Studio中刪除點的工具，去除未經處理資料中的一些隔離的噪點，效果如下：


平行移動和旋轉的分離 先對平移向量T進行初始的估算，具體方法是分別得到點集P和Q的中心：
   
分別將點集P和Q平移至中心點處：
則上述最佳化目標函數可以轉化為：  
最佳化問題分解為：
求使E最小的  求使  平移中心點的 具體代碼為：
[cpp]  view plain copy //計算點雲P的中心點mean   void CalculateMeanPoint3D(vector<Point3D> &P, Point3D &mean)   {       vector<Point3D>::iterator it;       mean.x = 0;       mean.y = 0;       mean.z = 0;       for(it=P.begin(); it!=P.end(); it++){           mean.x += it->x;           mean.y += it->y;           mean.z += it->z;       }       mean.x = mean.x/P.size();       mean.y = mean.y/P.size();       mean.z = mean.z/P.size();   }   初始平移效果如下：

利用控制點求初始旋轉矩陣 在確定對應關係時，所使用的幾何特徵是空間中位置最近的點。這裡，我們甚至不需要兩個點集中的所有點。可以指用從某一點集中選取一部分點，一般稱這些點為 控制點（Control Points）。這時，配准問題轉化為： 這裡，pi，qi為最近匹配點。
在Geomagic Studio中利用三個點就可以進行兩個模型的“手動註冊”（感覺這裡翻譯的不好，Registration，應該為“手動匹配”）。

我們將手動選擇的三個點匯出，作為實驗初始的控制點：

對於第i對點，計算點對的矩陣 Ai：
   ， 為 的轉置矩陣。
（*這裡在査老師的課上給了一個錯誤的矩陣變換公式） 對於每一對矩陣Ai，計算矩陣B: 
[cpp]  view plain copy double B[16];           for(int i=0;i<16;i++)               B[i]=0;           for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){               double divpq[3]={itp->x,itp->y,itp->z};               double addpq[3]={itp->x,itp->y,itp->z};               double q[3]={itq->x,itq->y,itq->z};               MatrixDiv(divpq,q,3,1);               MatrixAdd(addpq,q,3,1);               double A[16];               for(int i=0;i<16;i++)                   A[i]=0;