3D点云配准 ICP算法源码（matlab亲测 可用，）

ICP算法的原理 ，这里略

直接上源码

function [data_q,T] = rotate(data,x,y,z,t)
 
%欧拉角转旋转矩阵
 
x = x/180*pi;
y = y/180*pi;
z = z/180*pi;
 
Rx = [1      0      0;
    0 cos(x) -sin(x);
    0 sin(x) cos(x)];
Ry = [cos(y)  0 sin(y);
    0       1      0;
    -sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
    sin(z) cos(z)  0;
    0      0       1];
T = Rz*Ry*Rx;        %旋转矩阵
 
T = [T(1,1),T(1,2),T(1,3),t(1);
    T(2,1),T(2,2),T(2,3),t(2);
    T(3,1),T(3,2),T(3,3),t(3);
    0 0 0 1];  % 复合
 
 
rows=size(data,1);
rows_one=ones(rows,1);%创建一个rows行一列全是1的数组（ones的具体使用请查看文档 help ones）
%size(rows_one,1)%这是为了验证看看rows_one这个数组是不是自己想要的行和列
%size(rows_one,2)
data=[data,rows_one];    %化为齐次坐标
%size(data',1)
%size(data',2)
data_q=T*data';%把data专置，这样就变成了一列就是一个坐标点（即xyz的值），在加上一个1。这个1主要是去处理平移的，xyz是去处理旋转的。
data_q=data_q(1:3,:);    %返回三维坐标

function [data_q,T] = rotate(data,x,y,z,t)
 
%欧拉角转旋转矩阵
 
x = x/180*pi;
y = y/180*pi;
z = z/180*pi;
 
Rx = [1      0      0;
    0 cos(x) -sin(x);
    0 sin(x) cos(x)];
Ry = [cos(y)  0 sin(y);
    0       1      0;
    -sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
    sin(z) cos(z)  0;
    0      0       1];
T = Rz*Ry*Rx;        %旋转矩阵
 
T = [T(1,1),T(1,2),T(1,3),t(1);
    T(2,1),T(2,2),T(2,3),t(2);
    T(3,1),T(3,2),T(3,3),t(3);
    0 0 0 1];  % 复合
 
 
rows=size(data,1);
rows_one=ones(rows,1);%创建一个rows行一列全是1的数组（ones的具体使用请查看文档 help ones）
%size(rows_one,1)%这是为了验证看看rows_one这个数组是不是自己想要的行和列
%size(rows_one,2)
data=[data,rows_one];    %化为齐次坐标
%size(data',1)
%size(data',2)
data_q=T*data';%把data专置，这样就变成了一列就是一个坐标点（即xyz的值），在加上一个1。这个1主要是去处理平移的，xyz是去处理旋转的。
data_q=data_q(1:3,:);    %返回三维坐标