一、含義：
1.icp演算法能夠使兩個不同座標系下的點集匹配到乙個座標系中，這個過程就是配準，配準的操作就是找到從座標系1變換到座標系2的剛性變換。
2.icp的本質就是配準，但有不同的配準方案，icp演算法本質是基於最小二乘的最優配準方法。該方法重複進行選擇對應關係對，計算最優剛體變換，直到滿足正確配準的收斂精度要求。
3.icp演算法的目的就是找到待匹配點雲資料與參考點雲資料之間的旋轉引數r和平移引數t,使得兩點資料之間滿足某種度量準則下的最優匹配。
4.每次操作的都是目標點集，使目標點集不斷靠近參考點集，因此求r和t也是每次考慮目標點集中每個點在參考點集中的最近點；
二、流程圖：
假設給兩個三維點集x1和x2,icp方法的配準方案步驟如下[1]：
第一步，計算x2中每乙個點在x1點集中的最近點；
第二步，求得使上述對應點對平均距離最小的剛體變換，求得平移引數和旋轉引數；
第三步，對x2使用上一步求得的平移引數和旋轉引數，得到新點集；
第四步，如果新的變換點集和參考點集滿足兩點集的平均距離小於某一給定閾值，則停止迭代計算，否則新的變換點集作為新的x2繼續迭代，直到達到目標函式的要求。
三、注意事項：
什麼是目標點集，什麼是參考點集？
目標點集就是你每次不斷操作的點集，每次不斷移動，使其不斷靠近參考點集，每次求r和t時是在參考點集中查詢和目標點集最近的點，操作的物件目標點集。
四、程式**:
在網上參考了很多版本，都不太適合新手入門，本人寫了乙個比較簡單的**，希望與大家共同交流學習：
clc;
clear all;
clear all;
%產生點集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;%參考點雲
desti_point=pointcloud2;%目標點雲
error=1;%初始誤差
nit=0;%計數
iter=20;%最大迭代次數
%目標點雲不斷向參考點雲靠近
%可認為初始r為eyes(2);
%和初始平動t=[0;0];
%沒有任何平動或者移動
while error>0.01
index=;
nit=nit+1;
if nit>iter
break;
endfor i=1:length(desti_point(1,:))%為每乙個目標點雲中的點尋找對應點
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+dist
endsource_point_part=source_point(:,index);
%求質心
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和t
r=u*v';
t=u_sour_point-r*u_dest_point;
%變換後的點集
desti_point=r*desti_point+t*ones(1,length(desti_point(1,:)));
%畫圖觀察變換後的目標點集
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
結果圖：
紅色為參考點雲，藍色為目標點雲，黃色為最終經過不斷旋轉和移動的目標點雲
參考文獻：
[1]:
（五）迭代最近點（ICP）
icp演算法 從這裡開始，筆者便秉承恩師的教誨，用一些比較規範的寫法來介紹演算法原理。已知條件 匹配的一組三維點雲 p p dots,p q q dots,q tips 這堆點雲一般是根據兩幀影象的特徵點進行匹配，再利用匹配的畫素位置在深度圖中查詢深度值，利用相機內參以及深度值進行計算相機座標系下的...
點雲匹配介紹與ICP演算法
點雲匹配 影象配準目的在於比較或融合。針對同一物件在不同條件下獲取的影象，因為雷射掃瞄光束受物體遮擋的原因，不可能通過一次掃瞄完成對整個物體的三維點雲的獲取。因此需要從不同的位置和角度對物體進行掃瞄。三維匹配的目的就是把相鄰掃瞄的點雲資料拼接在一起。三維匹配重點關注匹配演算法，常用的演算法有最近點迭...
演算法 最近點匹配
改進 示例 1.主要思路 輸入預處理 遞迴查詢輸出 2.遞迴分析 選取中線將平面內的點劃分為兩個部分 這樣就會產生距離最短兩種情況 兩個點都在中線兩邊，這個好辦用分治遞迴處理 兩個點分別在中線兩邊，這個是演算法的難點 針對第二種情況進行分析 d min 左邊最短的距離，右邊最短的距離 p1為左邊的點...