迭代最近點演算法 Iterative Closest Points

查志強發表於2016-09-05
演算法

【原文：http://www.cnblogs.com/GarfieldEr007/p/5116328.html】

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

基本原理

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

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

 

迭代最近點法目標函式

三維空間中兩個3D點， ，他們的歐式距離表示為：
三維點雲匹配問題的目的是找到P和Q變化的矩陣R和T，對於 ，，利用最小二乘法求解最優解使：
最小時的R和T。
 

資料預處理

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

平行移動和旋轉的分離

先對平移向量T進行初始的估算，具體方法是分別得到點集P和Q的中心：
 
 
分別將點集P和Q平移至中心點處：
則上述最優化目標函式可以轉化為：
 
最優化問題分解為：
  1. 求使E最小的 
  2. 求使 
平移中心點的 具體程式碼為：
  1. //計算點雲P的中心點mean  
  2. void CalculateMeanPoint3D(vector<Point3D> &P, Point3D &mean)  
  3. {  
  4.     vector<Point3D>::iterator it;  
  5.     mean.x = 0;  
  6.     mean.y = 0;  
  7.     mean.z = 0;  
  8.     for(it=P.begin(); it!=P.end(); it++){  
  9.         mean.x += it->x;  
  10.         mean.y += it->y;  
  11.         mean.z += it->z;  
  12.     }  
  13.     mean.x = mean.x/P.size();  
  14.     mean.y = mean.y/P.size();  
  15.     mean.z = mean.z/P.size();  
  16. }  
初始平移效果如下：
 

利用控制點求初始旋轉矩陣

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


對於第i對點，計算點對的矩陣 Ai：
 
 ，為的轉置矩陣。
（*這裡在査老師的課上給了一個錯誤的矩陣變換公式）
對於每一對矩陣Ai，計算矩陣B: 
  1. double B[16];  
  2.         for(int i=0;i<16;i++)  
  3.             B[i]=0;  
  4.         for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){  
  5.             double divpq[3]={itp->x,itp->y,itp->z};  
  6.             double addpq[3]={itp->x,itp->y,itp->z};  
  7.             double q[3]={itq->x,itq->y,itq->z};  
  8.             MatrixDiv(divpq,q,3,1);  
  9.             MatrixAdd(addpq,q,3,1);  
  10.             double A[16];  
  11.             for(int i=0;i<16;i++)  
  12.                 A[i]=0;  
  13.             for(int i=0;i<3;i++){  
  14.                 A[i+1]=divpq[i];  
  15.                 A[i*4+4]=divpq[i];  
  16.                 A[i+13]=addpq[i];  
  17.             }  
  18.             double AT[16],AMul[16];  
  19.             MatrixTran(A,AT,4,4);  
  20.             MatrixMul(A,AT,AMul,4,4,4,4);  
  21.             MatrixAdd(B,AMul,4,4);  
  22.         }  

原最優化問題可以轉為求B的最小特徵值和特徵向量，具體程式碼：
  1. //使用奇異值分解計算B的特徵值和特徵向量  
  2.         double eigen, qr[4];  
  3.         MatrixEigen(B, &eigen, qr, 4);  

  1. //計算n階正定陣m的特徵值分解：eigen為特徵值，q為特徵向量  
  2. void MatrixEigen(double *m, double *eigen, double *q, int n)  
  3. {  
  4.     double *vec, *eig;  
  5.     vec = new double[n*n];  
  6.     eig = new double[n];  
  7.     CvMat _m = cvMat(n, n, CV_64F, m);  
  8.     CvMat _vec = cvMat(n, n, CV_64F, vec);  
  9.     CvMat _eig = cvMat(n, 1, CV_64F, eig);  
  10.     //使用OpenCV開源庫中的矩陣操作求解矩陣特徵值和特徵向量  
  11.     cvEigenVV(&_m, &_vec, &_eig);  
  12.     *eigen = eig[0];  
  13.     for(int i=0; i<n; i++)  
  14.         q[i] = vec[i];  
  15.     delete[] vec;  
  16.     delete[] eig;  
  17. }  
  18.   
  19. //計算旋轉矩陣  
  20. void CalculateRotation(double *q, double *R)  
  21. {  
  22.     R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];  
  23.     R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);  
  24.     R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);  
  25.     R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);  
  26.     R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];  
  27.     R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);  
  28.     R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);  
  29.     R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);  
  30.     R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];  
  31. }  

平移矩陣計算

2.4中可以得到選擇矩陣的4元數表示，由於在"平行移動和旋轉的分離"中，我們將最優問題分解為：
  1. 求使E最小的 
  2. 求使 
因此還需要通過中心點計算平移矩陣。
  1. //通過特徵向量計算旋轉矩陣R1和平移矩陣T1  
  2.         CalculateRotation(qr, R1);  
  3.         double mean_Q[3]={_mean_Q.x,_mean_Q.y,_mean_Q.z};  
  4.         double mean_P[3]={_mean_P.x,_mean_P.y,_mean_P.z};  
  5.         double meanT[3]={0,0,0};  
  6.         int nt=0;  
  7.         for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){  
  8.             double tmpP[3]={itp->x,itp->y,itp->z};  
  9.             double tmpQ[3]={itq->x,itq->y,itq->z};  
  10.             double tmpMul[3];  
  11.             MatrixMul(R1, mean_P, tmpMul, 3, 3, 3, 1);  
  12.             MatrixDiv(tmpQ,tmpMul,3,1);  
  13.             MatrixAdd(meanT,tmpQ,3,1);  
  14.             nt++;  
  15.         }  
  16.         for(int i=0; i<3; i++)  
  17.             T1[i] = meanT[i]/(double)(nt);  

一次旋轉計算得到的矩陣如下：

效果在Geomagic Studio中顯示如圖：
 

迭代最近點

在初始匹配之後，所點集P`中所有點做平移變化，在比較點集合P`和Q`的匹配度，（或迭代次數）作為演算法終止的條件。
具體為對點集P中每個點，找Q中離他最近的點作為對應點。在某一步利用前一步得到的，求使下述函式最小的：
 
這裡， 
  1. //計算誤差和d  
  2.         d = 0.0;  
  3.         if(round==1){  
  4.             FindClosestPointSet(data,P,Q);  
  5.         }  
  6.         int pcount=0;  
  7.         for(itp = P.begin(),itq=Q.begin();itp!=P.end(); itp++, itq++){  
  8.             double sum = (itp->x - itq->x)*(itp->x - itq->x) + (itp->y - itq->y)*(itp->y - itq->y)   
  9.                 + (itp->z - itq->z)*(itp->z - itq->z);  
  10.             d += sum;  
  11.             pcount++;  
  12.         }  
  13.         d=d/(double)(pcount);  

迴圈結束後能得到較好的匹配效果：
 
封裝後的效果圖：

相關文章