一、含義：
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;
end
for 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
end
source_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,:)));
%求W
W=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]:https://www.cnblogs.com/sddai/p/6129437.html