ICP算法主要用於點雲精配準，精度很高，但是相應的缺點就是叠代過程中容易陷入局部極值。具體的ICP算法推導過程很多書上都有，就不再詳述了，此次仿真用的是SVD分解的方法。

直接貼代碼：

clear;
close all;
clc;

data_source=load(‘satellite.txt‘);
data_source=data_source‘;
theta=4;  %旋轉角度（此處只有繞z軸旋轉）
t=[2,1.6,7];   %平移向量
[data_target,T0]=rotate(data_source,theta,t);   


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%繪制兩幅原始圖像
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,‘b‘);
hold on;
scatter3(x2,y2,z2,‘r‘);
hold off;


T_final=eye(4,4);   %旋轉矩陣初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %初次更新點集（代表粗配準結果）
err=1;
while(err>0.001)
    iteration=iteration+1;    %叠代次數
    disp([‘叠代次數ieration=‘,num2str(iteration)]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %利用歐式距離找出對應點集
    k=size(data_target,2);
    for i = 1:k
        data_q1(1,:) = data_source(1,:) - data_target(1,i);    % 兩個點集中的點x坐標之差
        data_q1(2,:) = data_source(2,:) - data_target(2,i);    % 兩個點集中的點y坐標之差
        data_q1(3,:) = data_source(3,:) - data_target(3,i);    % 兩個點集中的點z坐標之差
        distance = data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2;  % 歐氏距離
        [min_dis, min_index] = min(distance);   % 找到距離最小的那個點
        data_mid(:,i) = data_source(:,min_index);   % 將那個點保存為對應點
        error(i) = min_dis;     % 保存距離差值
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %去中心化
    data_target_mean=mean(data_target,2);
    data_mid_mean=mean(data_mid,2);
    data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));
    data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %SVD分解
    W=zeros(3,3);
    for j=1:size(data_target_c,2)
        W=W+data_mid_c(:,j)*data_target_c(:,j)‘;
    end
    [U,S,V]=svd(W);
    Rf=U*V‘;
    Tf=data_mid_mean-Rf*data_target_mean;
    err=mean(error);
    T_t=[Rf,Tf];
    T_t=[T_t;0,0,0,1];
    T_final=T_t*T_final;   %更新旋轉矩陣
    disp([‘誤差err=‘,num2str(err)]);
    disp(‘旋轉矩陣T=‘);
    disp(T_final);
    
    data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %更新點集
    if iteration>=200
        break
    end
end

disp(inv(T0));  %旋轉矩陣真值

x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,‘b‘);
hold on;
scatter3(x2,y2,z2,‘r‘);
hold off;

rotate.m

function [data_q,T] = rotate(data,theta,t)

theta=-theta/180*pi;   
T=[cos(theta),sin(theta),0,t(1);
    -sin(theta),cos(theta),0,t(2);
    0,0,1,t(3);
    0,0,0,1];          %旋轉矩陣

rows=size(data,2);
rows_one=ones(1,rows);
data=[data;rows_one];    %化為齊次坐標

data_q=T*data;
data_q=data_q(1:3,:);    %返回三維坐標

仿真結果（配準前和配準後）：

旋轉矩陣（真值和配準結果）

誤差：err=0.00019434

註：關於誤差的定義每個人可以不同，此處不用太過於計較

上面是比較好的配準結果，下面來一組局部極值的情況。

仿真結果（配準前和配準後）：

旋轉矩陣（真值和配準結果）

結論： 從上面兩組仿真結果可以明顯看到，ICP算法在一定情況下精度很高，很適合用來精配準，但是缺點在於需要很好的叠代初值，這個直接關系到配準結果的準確性，因此ICP最好不要單獨使用，應該在該算法之前進行粗配準（該方法很多，比如利用特征點等）。另外，由於不知道兩堆點雲的點對對應關系，在此使用的是尋找最近點的方法，該方法最大的不足時運算量很大，因此如果在C++中使用，可以考慮采用KD-tree的存儲方法提高搜索效率或者想辦法進行高效率點雲配對。

ICP算法MATLAB仿真