標籤：矩陣計算 date category vector 技術分享 精度 points tool 地址
標籤： 映像匹配ICP演算法機器視覺 2015-12-01 21:09 2217人閱讀 評論(0) 收藏 舉報 分類： Computer Vision
（27）
著作權聲明：本文為博主原創文章，未經博主允許不得轉載。
最近在做點雲匹配，需要用c++實現ICP演算法，下面是簡單理解，期待高手指正。
ICP演算法能夠使不同的座標下的點雲資料合併到同一個座標系統中，首先是找到一個可用的變換，配准操作實際是要找到從座標系1到座標系2的一個剛性變換。
ICP演算法本質上是基於最小二乘法的最優配准方法。該演算法重複進行選擇對應關係點對， 計算最優剛體變換，直到滿足正確配準的收斂精度要求。
ICP 演算法的目的是要找到待配準點雲資料與參考雲資料之間的旋轉參數R和平移參數 T，使得兩點資料之間滿足某種度量準則下的最優匹配。
假設給兩個三維點集 X1 和 X2，ICP方法的配准步驟如下：
第一步，計算X2中的每一個點在X1 點集中的對應近點；
第二步，求得使上述對應點對平均距離最小的剛體變換，求得平移參數和旋轉參數；
第三步，對X2使用上一步求得的平移和旋轉參數，得到新的變換點集；
第四步， 如果新的變換點集與參考點集滿足兩點集的平均距離小於某一給定閾值，則停止迭代計算，否則新的變換點集作為新的X2繼續迭代，直到達到目標函數的要求。
最近點對尋找：對應點的計算是整個配准過程中耗費時間最長的步驟，尋找最近點，利用 k－d tree提高尋找速度 K－d tree 法建立點的拓撲關係是基於二叉樹的座標軸分割，構造 k－d tree 的過程就是按照二叉樹法則產生，首先按 X 軸尋找分割線，即計算所有點的x值的平均值，以最接近這個平均值的點的x值將空間分成兩部分，然後在分成的子空間中按 Y 軸尋找分割線，將其各分成兩部分，分割好的子空間在按X軸分割……依此類推，最後直到分割的地區內只有一個點。這樣的分割過程就對應於一個二叉樹，二叉樹的分節點就對應一條分割線，而二叉樹的每個葉子節點就對應一個點。這樣點的拓撲關係就建立了。
******************
hao_09
時間：2015/12/1
文章地址：http://blog.csdn.net/lsh_2013/article/details/50135045
===================================================
研究生課程系列文章參見索引《在信科的那些課》
基本原理
假定已給兩個資料集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;
- for(int i=0;i<3;i++){
- A[i+1]=divpq[i];
- A[i*4+4]=divpq[i];
- A[i+13]=addpq[i];
- }
- double AT[16],AMul[16];
- MatrixTran(A,AT,4,4);
- MatrixMul(A,AT,AMul,4,4,4,4);
- MatrixAdd(B,AMul,4,4);
- }
原最佳化問題可以轉為求B的最小特徵值和特徵向量，具體代碼：
[cpp] view plain copy
- //使用奇異值分解計算B的特徵值和特徵向量
- double eigen, qr[4];
- MatrixEigen(B, &eigen, qr, 4);
[cpp] view plain copy
- //計算n階正定陣m的特徵值分解：eigen為特徵值，q為特徵向量
- void MatrixEigen(double *m, double *eigen, double *q, int n)
- {
- double *vec, *eig;
- vec = new double[n*n];
- eig = new double[n];
- CvMat _m = cvMat(n, n, CV_64F, m);
- CvMat _vec = cvMat(n, n, CV_64F, vec);
- CvMat _eig = cvMat(n, 1, CV_64F, eig);
- //使用OpenCV開源庫中的矩陣操作求解矩陣特徵值和特徵向量
- cvEigenVV(&_m, &_vec, &_eig);
- *eigen = eig[0];
- for(int i=0; i<n; i++)
- q[i] = vec[i];
- delete[] vec;
- delete[] eig;
- }
-
- //計算旋轉矩陣
- void CalculateRotation(double *q, double *R)
- {
- R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
- R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
- R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
- R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
- R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
- R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
- R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
- R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
- R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
- }
平移矩陣計算2.4中可以得到選擇矩陣的4元數表示，由於在"平行移動和旋轉的分離"中，我們將最優問題分解為：
- 求使E最小的
- 求使
因此還需要通過中心點計算平移矩陣。
[cpp] view plain copy
- //通過特徵向量計算旋轉矩陣R1和平移矩陣T1
- CalculateRotation(qr, R1);
- double mean_Q[3]={_mean_Q.x,_mean_Q.y,_mean_Q.z};
- double mean_P[3]={_mean_P.x,_mean_P.y,_mean_P.z};
- double meanT[3]={0,0,0};
- int nt=0;
- for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){
- double tmpP[3]={itp->x,itp->y,itp->z};
- double tmpQ[3]={itq->x,itq->y,itq->z};
- double tmpMul[3];
- MatrixMul(R1, mean_P, tmpMul, 3, 3, 3, 1);
- MatrixDiv(tmpQ,tmpMul,3,1);
- MatrixAdd(meanT,tmpQ,3,1);
- nt++;
- }
- for(int i=0; i<3; i++)
- T1[i] = meanT[i]/(double)(nt);
一次旋轉計算得到的矩陣如下：
效果在Geomagic Studio中顯示 迭代最近點在初始匹配之後，所點集P`中所有點做平移變化，在比較點集合P`和Q`的匹配度，（或迭代次數）作為演算法終止的條件。
具體為對點集P中每個點，找Q中離他最近的點作為對應點。在某一步利用前一步得到的，求使下述函數最小的：
這裡，
[cpp] view plain copy
- //計算誤差和d
- d = 0.0;
- if(round==1){
- FindClosestPointSet(data,P,Q);
- }
- int pcount=0;
- for(itp = P.begin(),itq=Q.begin();itp!=P.end(); itp++, itq++){
- double sum = (itp->x - itq->x)*(itp->x - itq->x) + (itp->y - itq->y)*(itp->y - itq->y)
- + (itp->z - itq->z)*(itp->z - itq->z);
- d += sum;
- pcount++;
- }
- d=d/(double)(pcount);
迴圈結束後能得到較好的匹配效果：
封裝後的：
ICP演算法（Iterative Closest Point迭代最近點演算法）
