當(dāng)前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

ICP

發(fā)布時間：2023/11/27 生活经验 55 豆豆

生活随笔收集整理的這篇文章主要介紹了 ICP 小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

迭代最近點算法（ICP）

　　在20世紀80年代中期，很多學(xué)者開始對點集數(shù)據(jù)的配準進行了大量研究。1987年，Horn[1]、Arun[2]等人用四元數(shù)法提出點集對點集配準方法。這種點集與點集坐標(biāo)系匹配算法通過實踐證明是一個解決復(fù)雜配準問題的關(guān)鍵方法。1992年，計算機視覺研究者Besl和Mckay[3]介紹了一種高層次的基于自由形態(tài)曲面的配準方法，也稱為迭代最近點法ICP（Iterative Closest Point）。以點集對點集（PSTPS）配準方法為基礎(chǔ)，他們闡述了一種曲面擬合算法，該算法是基于四元數(shù)的點集到點集配準方法。從測量點集中確定其對應(yīng)的最近點點集后，運用Faugera和Hebert提出的方法計算新的最近點點集。用該方法進行迭代計算，直到殘差平方和所構(gòu)成的目標(biāo)函數(shù)值不變，結(jié)束迭代過程。ICP配準法主要用于解決基于自由形態(tài)曲面的配準問題。　　迭代最近點法ICP最近點法經(jīng)過十幾年的發(fā)展，不斷地得到了完善和補充。Chen和Medioni[4]及Bergevin等人[5]提出了point-to-plane搜索最近點的精確配準方法。Rusinkiewicz和Levoy提出了point-to-p rojection搜索最近點的快速配準方法。Soon-Yong和Murali提出了Contractive-projection-point搜索最近點的配準方法。此外，Andrew和Sing[6]提取了基于彩色三維掃描數(shù)據(jù)點紋理信息的數(shù)據(jù)配準方法，主要在ICP算法中考慮三維掃描點的紋理色彩信息進行搜索最近點。Natasha等人[7]分析了ICP算法中的點云數(shù)據(jù)配準質(zhì)量問題。[8]

ICP目的：就是求解兩堆點云之間的變換關(guān)系

思路：既然不知道R和t(針對剛體運動)，就假設(shè)為未知量，然后通過某些方法求解。

具體求法：假設(shè)有兩堆點云，分別記為兩個集合X=x1,x2,...,xm 和Y=y1,y2,...,ym（m并不總是等于n）。然后呢，我們不失一般性的，假設(shè)兩個點云之間的變換為R（旋轉(zhuǎn)變換）和t（平移變換），這兩個就是我們要求的東西啦～那我們將求解這個問題描述成最小化均方誤差：e(X,Y)=∑i=1m(Rxi+t?yi)2　　

初級的ICP方法對上面的優(yōu)化問題的處理思路如下：　　

（1）初始化R 和 t ：確定初始的R 和 t 的方法很多，如果什么方法都不知道，那隨便賦一個R 和 t ，然后就迭代的算呀。隨便給一個值從原理上來說也可以得到最終的一個結(jié)果呀，但是準不準就不知道了。相信有基本的優(yōu)化概念的人都知道，初始值的選取很重要，如果初始值選的不好很容易收斂到一個局部最優(yōu)解，然后局部最優(yōu)解好不好那就另說了。ICP發(fā)展了這么多年了，當(dāng)然有很多的方法來估計初始的R和t了，像PCL給的SampleConsensusInitalAlignment函數(shù)以及TransformationEstimationSVD函數(shù)都可以得到較好的初始估計。　　

（2）迭代：得到初始的估計后，然后，對于X中的每一個點用當(dāng)前的R 和 t在Y中找最近的點（比如用歐式距離），然后這兩個點就成了一對了～就這樣，對所有的點都這么做一次，然后我們就得到了所有的匹配對了～然后呢，用每一對的坐標(biāo)列一個方程，就得到一系列的方程。然后就求解最優(yōu)的R和t最小化上面的誤差。如此循環(huán)往復(fù)。

最近在做點云匹配，需要用c++實現(xiàn)ICP算法，下面是簡單理解，期待高手指正。

ICP算法能夠使不同的坐標(biāo)下的點云數(shù)據(jù)合并到同一個坐標(biāo)系統(tǒng)中，首先是找到一個可用的變換，配準操作實際是要找到從坐標(biāo)系1到坐標(biāo)系2的一個剛性變換。

ICP算法本質(zhì)上是基于最小二乘法的最優(yōu)配準方法。該算法重復(fù)進行選擇對應(yīng)關(guān)系點對，?計算最優(yōu)剛體變換，直到滿足正確配準的收斂精度要求。

ICP 算法的目的是要找到待配準點云數(shù)據(jù)與參考云數(shù)據(jù)之間的旋轉(zhuǎn)參數(shù)R和平移參數(shù) T，使得兩點數(shù)據(jù)之間滿足某種度量準則下的最優(yōu)匹配。

假設(shè)給兩個三維點集 X1 和 X2，ICP方法的配準步驟如下：

第一步，計算X2中的每一個點在X1 點集中的對應(yīng)近點；

第二步，求得使上述對應(yīng)點對平均距離最小的剛體變換，求得平移參數(shù)和旋轉(zhuǎn)參數(shù)；

第三步，對X2使用上一步求得的平移和旋轉(zhuǎn)參數(shù)，得到新的變換點集；

第四步，如果新的變換點集與參考點集滿足兩點集的平均距離小于某一給定閾值，則停止迭代計算，否則新的變換點集作為新的X2繼續(xù)迭代，直到達到目標(biāo)函數(shù)的要求。

?最近點對查找：對應(yīng)點的計算是整個配準過程中耗費時間最長的步驟，查找最近點，利用 k－d tree提高查找速度 K－d tree 法建立點的拓撲關(guān)系是基于二叉樹的坐標(biāo)軸分割，構(gòu)造 k－d tree 的過程就是按照二叉樹法則生成，首先按 X 軸尋找分割線，即計算所有點的x值的平均值，以最接近這個平均值的點的x值將空間分成兩部分，然后在分成的子空間中按 Y 軸尋找分割線，將其各分成兩部分，分割好的子空間在按X軸分割……依此類推，最后直到分割的區(qū)域內(nèi)只有一個點。這樣的分割過程就對應(yīng)于一個二叉樹，二叉樹的分節(jié)點就對應(yīng)一條分割線，而二叉樹的每個葉子節(jié)點就對應(yīng)一個點。這樣點的拓撲關(guān)系就建立了。

基本原理

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

基本思想是：根據(jù)某種幾何特性對數(shù)據(jù)進行匹配，并設(shè)這些匹配點為假想的對應(yīng)點，然后根據(jù)這種對應(yīng)關(guān)系求解運動參數(shù)。再利用這些運動參數(shù)對數(shù)據(jù)進行變換。并利用同一幾何特征，確定新的對應(yīng)關(guān)系，重復(fù)上述過程。

迭代最近點法目標(biāo)函數(shù)

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

數(shù)據(jù)預(yù)處理

實驗中采集了五個面的點如下所示：由于第一組（第一排第1個）和第三組（第一排第三個）采集均為模型正面點云，所以選用一和三做后續(xù)的實驗。首先利用Geomagic Studio中刪除點的工具，去除原始數(shù)據(jù)中的一些隔離的噪點，效果如下： ?

平行移動和旋轉(zhuǎn)的分離

先對平移向量T進行初始的估算，具體方法是分別得到點集P和Q的中心：
? ?
分別將點集P和Q平移至中心點處：則上述最優(yōu)化目標(biāo)函數(shù)可以轉(zhuǎn)化為： ?
最優(yōu)化問題分解為：

求使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();??
}??

初始平移效果如下： ?

利用控制點求初始旋轉(zhuǎn)矩陣

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

對于第i對點，計算點對的矩陣 Ai：

?，為的轉(zhuǎn)置矩陣。

對于每一對矩陣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);??
????????}??

原最優(yōu)化問題可以轉(zhuǎn)為求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;??
}??
??
//計算旋轉(zhuǎn)矩陣??
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元數(shù)表示，由于在"平行移動和旋轉(zhuǎn)的分離"中，我們將最優(yōu)問題分解為：

求使E最小的?
求使?

因此還需要通過中心點計算平移矩陣。
[cpp]?view plain?copy?

//通過特征向量計算旋轉(zhuǎn)矩陣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);??

一次旋轉(zhuǎn)計算得到的矩陣如下：
效果在Geomagic Studio中顯示如圖： ?

迭代最近點

在初始匹配之后，所點集P`中所有點做平移變化，在比較點集合P`和Q`的匹配度，（或迭代次數(shù)）作為算法終止的條件。
具體為對點集P中每個點，找Q中離他最近的點作為對應(yīng)點。在某一步利用前一步得到的，求使下述函數(shù)最小的：
? 這里，? [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);??

循環(huán)結(jié)束后能得到較好的匹配效果：

? 封裝后的效果圖：

ICP算法（Iterative Closest Point迭代最近點算法）

2 ICP變種　　

除了經(jīng)典的ICP方法外，還有一些變種，如point-to-point的，point-to-plane的以及plane-to-plane。

三種方法區(qū)別：就是上面的誤差函數(shù)的定義不一樣而已。在上面講經(jīng)典ICP的時候，求和的每一項是X中的每一個點到Y(jié)中的每一個點的距離，那就是point-to-point了，那么將求和的每一項變成X中的每一個點到Y(jié)中的平面的距離，那就是point-to-plane。同理，如果把求和的每一項變成X中的平面到Y(jié)中的平面的距離，那就是plane-to-plane了。我們說了這么久的平面，那么平面到時是怎么定義的呢？　

　point-to-plane的誤差函數(shù)定義為：Mopt=argminR,t∑i((R?xi+t?yi)?ni)

　　一、基本原理[3] 　　三維空間R3存在兩組含有n個坐標(biāo)點的點集PL和PR，分別為：三維空間點集PL中各點經(jīng)過三維空間變換后與點集PR中點一一對應(yīng)，其單點變換關(guān)系式為：　　(0-1)上式中，R為三維旋轉(zhuǎn)矩陣，t為平移向量。在ICP配準方法中，空間變換參數(shù)向量X可表示為[9] 。參數(shù)向量中四元數(shù)參數(shù)滿足約束條件為：　　(0-2)根據(jù)迭代的初值X0，由式(0-1)計算新點集Pi為：　　(0-3)式中，P表示原始未修改過的點集，Pi的下標(biāo)i表示迭代次數(shù)，參數(shù)向量X的初始值X0為。根據(jù)以上數(shù)據(jù)處理方法，ICP配準算法可以概括為以下七個步驟：　　1）根據(jù)點集Plk中的點坐標(biāo)，在曲面S上搜索相應(yīng)最近點點集Prk；　　2）計算兩個點集的重心位置坐標(biāo)，并進行點集中心化生成新的點集；　　3）由新的點集計算正定矩陣N，并計算N的最大特征值及其最大特征向量；　　4）由于最大特征向量等價于殘差平方和最小時的旋轉(zhuǎn)四元數(shù)，將四元數(shù)轉(zhuǎn)換為旋轉(zhuǎn)矩陣R；　　5）在旋轉(zhuǎn)矩陣R被確定后，由平移向量t僅僅是兩個點集的重心差異，可以通過兩個坐標(biāo)系中的重心點和旋轉(zhuǎn)矩陣確定；　　6）根據(jù)式(0-3)，由點集Plk計算旋轉(zhuǎn)后的點集P’lk。通過Plk與P’lk計算距離平方和值為fk+1。以連續(xù)兩次距離平方和之差絕對值作為迭代判斷數(shù)值；　　7）當(dāng) 時，ICP配準算法就停止迭代，否則重復(fù)1至6步，直到滿足條件后停止迭代。　