一、含義：
1.icp算法能夠使兩個(gè)不同坐標(biāo)系下的點(diǎn)集匹配到一個(gè)坐標(biāo)系中，這個(gè)過(guò)程就是配準(zhǔn)，配準(zhǔn)的操作就是找到從坐標(biāo)系1變換到坐標(biāo)系2的剛性變換。
2.icp的本質(zhì)就是配準(zhǔn)，但有不同的配準(zhǔn)方案，icp算法本質(zhì)是基于最小二乘的最優(yōu)配準(zhǔn)方法。該方法重復(fù)進(jìn)行選擇對(duì)應(yīng)關(guān)系對(duì)，計(jì)算最優(yōu)剛體變換，直到滿足正確配準(zhǔn)的收斂精度要求。
3.icp算法的目的就是找到待匹配點(diǎn)云數(shù)據(jù)與參考點(diǎn)云數(shù)據(jù)之間的旋轉(zhuǎn)參數(shù)R和平移參數(shù)T,使得兩點(diǎn)數(shù)據(jù)之間滿足某種度量準(zhǔn)則下的最優(yōu)匹配。
4.每次操作的都是目標(biāo)點(diǎn)集，使目標(biāo)點(diǎn)集不斷靠近參考點(diǎn)集，因此求R和T也是每次考慮目標(biāo)點(diǎn)集中每個(gè)點(diǎn)在參考點(diǎn)集中的最近點(diǎn)；
二、流程圖：

假設(shè)給兩個(gè)三維點(diǎn)集x1和x2,ICP方法的配準(zhǔn)方案步驟如下[1]：
第一步，計(jì)算x2中每一個(gè)點(diǎn)在x1點(diǎn)集中的最近點(diǎn)；
第二步，求得使上述對(duì)應(yīng)點(diǎn)對(duì)平均距離最小的剛體變換，求得平移參數(shù)和旋轉(zhuǎn)參數(shù)；
第三步，對(duì)x2使用上一步求得的平移參數(shù)和旋轉(zhuǎn)參數(shù)，得到新點(diǎn)集；
第四步，如果新的變換點(diǎn)集和參考點(diǎn)集滿足兩點(diǎn)集的平均距離小于某一給定閾值，則停止迭代計(jì)算，否則新的變換點(diǎn)集作為新的x2繼續(xù)迭代，直到達(dá)到目標(biāo)函數(shù)的要求。
三、注意事項(xiàng)：
什么是目標(biāo)點(diǎn)集，什么是參考點(diǎn)集？
目標(biāo)點(diǎn)集就是你每次不斷操作的點(diǎn)集，每次不斷移動(dòng)，使其不斷靠近參考點(diǎn)集，每次求R和T時(shí)是在參考點(diǎn)集中查找和目標(biāo)點(diǎn)集最近的點(diǎn)，操作的對(duì)象目標(biāo)點(diǎn)集。
四、程序代碼:
在網(wǎng)上參考了很多版本，都不太適合新手入門，本人寫(xiě)了一個(gè)比較簡(jiǎn)單的代碼，希望與大家共同交流學(xué)習(xí)：

clc; clear all; clear all; %產(chǎn)生點(diǎn)集1 x1=1:0.01:2; sample=randn(1,length(x1)); y1=10*x1+2*sample; pointcloud1=[x1;y1]; theta=pi/3; R=[cos(theta) -sin(theta);sin(theta) cos(theta) ]; T =[4;5] *ones(1,length(pointcloud1(1,:))) ; pointcloud2=R*pointcloud1+T; sourch_point=pointcloud1;%參考點(diǎn)云 desti_point=pointcloud2;%目標(biāo)點(diǎn)云 error=1;%初始誤差 nit=0;%計(jì)數(shù) Iter=20;%最大迭代次數(shù) %目標(biāo)點(diǎn)云不斷向參考點(diǎn)云靠近 %可認(rèn)為初始R為eyes(2); %和初始平動(dòng)t=[0;0]; %沒(méi)有任何平動(dòng)或者移動(dòng) while error>0.01index=[];nit=nit+1;if nit>iterbreak;endfor i=1:length(desti_point(1,:))%為每一個(gè)目標(biāo)點(diǎn)云中的點(diǎn)尋找對(duì)應(yīng)點(diǎn)dx=(source_point-repmat(desti_point(:,i),1,length(source_point(1,:))));dist=sqrt(dx(1,:).^2+dx(2,:).^2);[dist,ii]=min(dist);index=[index;;ii];error=error+distendsource_point_part=source_point(:,index);%求質(zhì)心u_sour_point=(sum(source_point_part,2))/length(source_point_part(1,:));u_dest_point=(sum(desti_point,2))/length(source_point_part(1,:));%去中心化source_point_center=source_point_part-u_sour_point*ones(1,length(source_point_part(1,:)));desti_point_center=desti_point-u_dest_point*ones(1,length(desti_point(1,:)));%求WW=source_point_center*desti_point_center';%SVD分解[U,S,V]=svd(W);%求R和tR=U*V';t=u_sour_point-R*u_dest_point;%變換后的點(diǎn)集desti_point=R*desti_point+t*ones(1,length(desti_point(1,:)));%畫(huà)圖觀察變換后的目標(biāo)點(diǎn)集figure;plot(pointcloud1(1,:),pointcloud1(2,:),'r.');hold on;plot(pointcloud2(1,:),pointcloud2(2,:),'b.');hold on;plot(desti_point(1,:),desti_point(2,:),'y.');error=1/length(desti_point(1,:))*sum(sum(((source_point_part-desti_point).^2),2),1); end

五、程序運(yùn)行結(jié)果：
結(jié)果圖：

紅色為參考點(diǎn)云，藍(lán)色為目標(biāo)點(diǎn)云，黃色為最終經(jīng)過(guò)不斷旋轉(zhuǎn)和移動(dòng)的目標(biāo)點(diǎn)云
參考文獻(xiàn)：
[1]:https://www.cnblogs.com/sddai/p/6129437.html

總結(jié)

以上是生活随笔為你收集整理的icp点云匹配迭代最近邻算法的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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