这里主要是记录一下算法实现的步骤。

公式推导：我将会放上课程的推导截图，以备查阅。

一.公式的推导部分

---

图中的第三个等号的备注：

 

---

 

 

 

 

 

二.代码实现思路

1.寻找点集

%利用欧式距离找出对应点集 搜索每个点（这里是查找的对应点是直接查找的最近点）
    k=size(data_target,2);
    for i = 1:k
        data_q1(1,:) = data_source(1,:) - data_target(1,i);    % 两个点集中的点x坐标之差，（将data_source的第一行整个数组与data_target(1,i)这个数作差）
        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;  %存的欧氏距离（data_source数组里面的点与data_target数组里面的点之间的距离）
        [min_dis, min_index] = min(distance);   % 找到距离最小的那个点
        data_mid(:,i) = data_source(:,min_index);   % 将那个点保存为对应点——这个就是找的data_source对应点了（因为是3*n的矩阵，列表示xyz值，这里是等于直接把xyz赋值给了data_mid）
        error(i) = min_dis;     % 保存距离差值
    end

2.去中心化

%去中心化
    % 质心
    data_target_mean=mean(data_target,2);%求data_target每一行的平均数
    data_mid_mean=mean(data_mid,2);%求data_mid每一行的平均数
    
    data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));%3*1 * 1*39856 = 3*39856 这是把之质心坐标复制成了39856份
    data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));

3.求解H矩阵

x y 就是从点云里面找到互相对应的点集。

    %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);%这个W为什么要这么求？

4.求解旋转平移

4.1 旋转

对第三步的H矩阵进行svd分解。

那么R就是svd分解其中的两个相乘的结果。

Rf=U*V';%两点云之间的旋转矩阵

4.2 平移

 即可得到平移量。

 Tf=data_mid_mean-Rf*data_target_mean;%计算质心平移矢量（公式推导就是拿的质心相减的）

所以以上总结下来的步骤得话，就如下：

 

三.全部代码

1.主函数

% ICP 算法
clear;
close all;
clc;

data_source=load('a.txt');
data_source=data_source';
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
data_source = [x1',y1',z1'];
%旋转角度
alpha = 0;
theta = 0;
grama = 0;

t=[1,1,30];   %平移向量

[data_target,T0] =rotate(data_source,alpha ,theta,grama,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(1);
title('初始位置');
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;
data_source = data_source';
while(err>0.0001)
    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_source的第一行整个数组与data_target(1,i)这个数作差）
        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;  %存的欧氏距离（data_source数组里面的点与data_target数组里面的点之间的距离）
        [min_dis, min_index] = min(distance);   % 找到距离最小的那个点
        data_mid(:,i) = data_source(:,min_index);   % 将那个点保存为对应点——这个就是找的data_source对应点了（因为是3*n的矩阵，列表示xyz值，这里是等于直接把xyz赋值给了data_mid）
        error(i) = min_dis;     % 保存距离差值
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
    
    
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %去中心化
    % 质心
    data_target_mean=mean(data_target,2);%求data_target每一行的平均数
    data_mid_mean=mean(data_mid,2);%求data_mid每一行的平均数
    
    data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));%3*1 * 1*39856 = 3*39856 这是把之质心坐标复制成了39856份
    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);%这个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(inv(T_final));
    
    data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %更新点集
    if iteration >= 300
        break
    end
end

disp('真值');
disp(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(2);
title('配准后');
scatter3(x1,y1,z1,'r*');
hold on;
scatter3(x2,y2,z2,'b*');
hold off;

2.函数

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,:);    %返回三维坐标

参考文章：(102条消息) ICP配准MATLAB实现_空的空sky的博客-CSDN博客_icp matlab

点云配准之SVD解法的由来 - 知乎 (zhihu.com)

侵权请联系~