生活随笔
收集整理的這篇文章主要介紹了
迭代最近点算法 Iterative Closest Points
小編覺得挺不錯的,現在分享給大家,幫大家做個參考.
研究生課程系列文章參見索引《在信科的那些課》
基本原理
假定已給兩個數據集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 plaincopy
?? 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 plaincopy
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 plaincopy
?? ????????double?eigen,?qr[4];?? ????????MatrixEigen(B,?&eigen,?qr,?4);?? [cpp]?view plaincopy
?? 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);?? ?????? ????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 plaincopy
?? ????????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 plaincopy
?? ????????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);?? 循環結束后能得到較好的匹配效果：
封裝后的效果圖：
from:?http://blog.csdn.net/xiaowei_cqu/article/details/8470376
總結
以上是生活随笔為你收集整理的迭代最近点算法 Iterative Closest Points的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
