迭代最近點算法（ICP）

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

ICP目的：就是求解兩堆點云之間的變換關系

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

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

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

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

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

最近在做點云匹配，需要用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軸分割……依此類推，最后直到分割的區域內只有一個點。這樣的分割過程就對應于一個二叉樹，二叉樹的分節點就對應一條分割線，而二叉樹的每個葉子節點就對應一個點。這樣點的拓撲關系就建立了。

基本原理

假定已給兩個數據集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. 求使?

平移中心點的 具體代碼為： [cpp]?view plain?copy?

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:?

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

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

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

[cpp]?view plain?copy?

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. 求使?

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

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中離他最近的點作為對應點。在某一步利用前一步得到的，求使下述函數最小的：
? 這里，? [cpp]?view plain?copy?

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);??

循環結束后能得到較好的匹配效果：

? 封裝后的效果圖：

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

2 ICP變種　　

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

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

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

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

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

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

ICP算法本質上是基于最小二乘法的最優配準方法。該算法重復進行選擇對應關系點對，?計算最優剛體變換，直到滿足正確配準的收斂精度要求。

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。