迭代最近點(diǎn)算法（ICP）

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

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

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

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

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

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

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

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

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

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

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

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

第一步，計(jì)算X2中的每一個(gè)點(diǎn)在X1 點(diǎn)集中的對(duì)應(yīng)近點(diǎn)；

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

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

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

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

基本原理

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

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

?

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

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

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

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

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

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

1. 求使E最小的?
2. 求使?

平移中心點(diǎn)的 具體代碼為： [cpp]?view plain?copy?

1. //計(jì)算點(diǎn)云P的中心點(diǎn)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. }??

初始平移效果如下： ?

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

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

對(duì)于第i對(duì)點(diǎn)，計(jì)算點(diǎn)對(duì)的矩陣 Ai：

?

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

對(duì)于每一對(duì)矩陣Ai，計(jì)算矩陣B:?

[cpp]?view plain?copy?

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. ????????}??

原最優(yōu)化問題可以轉(zhuǎn)為求B的最小特征值和特征向量，具體代碼： [cpp]?view plain?copy?

1. //使用奇異值分解計(jì)算B的特征值和特征向量??
2. ????????double?eigen,?qr[4];??
3. ????????MatrixEigen(B,?&eigen,?qr,?4);??

[cpp]?view plain?copy?

1. //計(jì)算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開源庫(kù)中的矩陣操作求解矩陣特征值和特征向量??
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. //計(jì)算旋轉(zhuǎn)矩陣??
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. }??

平移矩陣計(jì)算

2.4中可以得到選擇矩陣的4元數(shù)表示，由于在"平行移動(dòng)和旋轉(zhuǎn)的分離"中，我們將最優(yōu)問題分解為：

1. 求使E最小的?
2. 求使?

因此還需要通過中心點(diǎn)計(jì)算平移矩陣。
[cpp]?view plain?copy?

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

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

迭代最近點(diǎn)

在初始匹配之后，所點(diǎn)集P`中所有點(diǎn)做平移變化，在比較點(diǎn)集合P`和Q`的匹配度，（或迭代次數(shù)）作為算法終止的條件。
具體為對(duì)點(diǎn)集P中每個(gè)點(diǎn)，找Q中離他最近的點(diǎn)作為對(duì)應(yīng)點(diǎn)。在某一步利用前一步得到的，求使下述函數(shù)最小的：
? 這里，? [cpp]?view plain?copy?

1. //計(jì)算誤差和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);??

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

? 封裝后的效果圖：

ICP算法（Iterative Closest Point迭代最近點(diǎn)算法）

2 ICP變種　　

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

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

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

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

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

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

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

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

總結(jié)

以上是生活随笔為你收集整理的ICP的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。