高端的ICP點(diǎn)云配準(zhǔn)只需要簡(jiǎn)單MATLAB代碼
點(diǎn)擊下方卡片，關(guān)注“新機(jī)器視覺”公眾號(hào)
視覺/圖像重磅干貨，第一時(shí)間送達(dá)
各位大家好久不見，我還是小劉。我一直從事著C++視覺方面的工作，最近被C++/Cmake/PCL弄得不要不要的，配置環(huán)境。我又想起了我的老伙計(jì)-----Matlab。他應(yīng)該可以做三維點(diǎn)云的基礎(chǔ)工作吧。
大白話來(lái)說(shuō)就是：對(duì)同一個(gè)物體進(jìn)行多次掃描后，會(huì)有多個(gè)點(diǎn)云數(shù)據(jù)，然后將兩個(gè)數(shù)據(jù)中同一點(diǎn)的點(diǎn)云進(jìn)行匹配，也就是將兩幅點(diǎn)云中相同點(diǎn)的點(diǎn)云放在一起即點(diǎn)云配準(zhǔn)。
然后效果就如下圖一樣，兩個(gè)條為點(diǎn)云向量，他們屬于同一個(gè)部位的不同點(diǎn)云集，為了讓他們配準(zhǔn)在一起，使用算法配準(zhǔn)。
從一個(gè)動(dòng)圖來(lái)演示吧：初始是兩個(gè)不同角度下的笑臉（深紅色和綠色），下面是紅色笑臉如何通過(guò)ICP過(guò)程和綠色笑臉重合的：
目前應(yīng)用最廣泛的點(diǎn)云精配準(zhǔn)算法是迭代最近點(diǎn)算法（Iterative Closest Point, ICP）及各種變種 ICP 算法。
ICP算法流程：
首先對(duì)于一幅點(diǎn)云中的每個(gè)點(diǎn)，在另一幅點(diǎn)云中計(jì)算匹配點(diǎn)（最近點(diǎn)）
極小化匹配點(diǎn)間的匹配誤差，計(jì)算位姿（旋轉(zhuǎn)矩陣R和平移矩陣t）
然后將計(jì)算的位姿作用于點(diǎn)云
再重新計(jì)算匹配點(diǎn)
如此迭代，直到迭代次數(shù)達(dá)到閾值，或者極小化的能量函數(shù)變化量小于設(shè)定閾值
1、首先創(chuàng)建兩個(gè)待配準(zhǔn)的點(diǎn)云?size1=size(PointCloud1);%點(diǎn)云1?if?size1(1)==3?????PointCloud1=transpose(PointCloud1);?end?size2=size(PointCloud2);%點(diǎn)云2if size2(1)==3????PointCloud2=transpose(PointCloud2);?end??
兩個(gè)帶配準(zhǔn)的點(diǎn)云集
?2.計(jì)算距離
for?i=1:size_Psmallest=1e10;correspondence(1,i)=0;?????????????for?j=1:size_Xdis=(P_iter(1,i)-X_ori(1,j))^2+(P_iter(2,i)-X_ori(2,j))^2+(P_iter(3,i)-X_ori(3,j))^2;if dissmallest=dis;correspondence(1,i)=j;end????????endendCorre=correspondence;
?3.求解旋轉(zhuǎn)矩陣R和移動(dòng)矩陣tfunction [t]=GetTranslationVector(P_iter,Xiter_centerPos,R_iter)is 3X3200 Xiter_centerPos is 3X1 R_iter is 3X3Xsum=0;Ysum=0;Zsum=0;size1=size(P_iter);size1=size1(2);for i=1:size1Xsum=Xsum+P_iter(1,i);Ysum=Ysum+ P_iter(2,i);Zsum=Zsum+P_iter(3,i);end=Xsum/size1;=Ysum/size1;=Zsum/size1;t=Xiter_centerPos-R_iter*P_center;?end
4、將計(jì)算的位姿作用于點(diǎn)云function [Xcenter]=GetXCenterPos(X_ori,Corre)size_Sum=size(Corre);size_Sum=size_Sum(2);xsum=0;ysum=0;zsum=0;for i=1:size_Sumxsum=xsum+X_ori(1,Corre(1,i));ysum=ysum+X_ori(2,Corre(1,i));zsum=zsum+X_ori(3,Corre(1,i));end=xsum/size_Sum;=ysum/size_Sum;=zsum/size_Sum;end
?5、寫一個(gè)計(jì)算最小誤差的函數(shù)function [ErrorData]=ComputeErrorMap(corre,P_iter,X_ori)size_P=size(P_iter);size_P=size_P(2);for i=1:size_Px=X_ori(1,corre(i))-P_iter(1,i);y=X_ori(2,corre(i))-P_iter(2,i);z=X_ori(3,corre(i))-P_iter(3,i);err=x^2+y^2+z^2;err=sqrt(err);=err;endErrorData=P_iter;end
迭代第十次：
迭代第二十次：
迭代第五十次：我們可以看出兩個(gè)點(diǎn)云集合幾乎重合在一起。
我猜大家一時(shí)半會(huì)看不出迭代的差別，那我放一下第十次迭代和最后一次的迭代的誤差放出來(lái)大家體會(huì)一下效果放在一起，看z軸是不是誤差越來(lái)越小~~
結(jié)束語(yǔ)：看來(lái)matlab做點(diǎn)云真的不錯(cuò)，不過(guò)小劉在寫算法的時(shí)候，其實(shí)覺得matlab用來(lái)做算法驗(yàn)證真的不錯(cuò)，不過(guò)有一點(diǎn)就是，迭代次數(shù)越多，稠密點(diǎn)云的時(shí)候會(huì)十分的慢，可能這就是腳本語(yǔ)言的通病吧，不過(guò)我用了并行加速之后，提升了不少，萬(wàn)物的資本家~~關(guān)注我，帶你關(guān)注更多matlab用法，我們下次再見~
—版權(quán)聲明—
僅用于學(xué)術(shù)分享，版權(quán)屬于原作者。
若有侵權(quán)，請(qǐng)聯(lián)系微信號(hào):yiyang-sy 刪除或修改！