ICP算法实现(MATLAB)

向右看齐 2023-07-11 15:19 27阅读 0赞

这几天因为正在学习点云数据，所以就学习了一下ICP算法。

### 文章目录 ###

*  一、算法步骤
 *  二、实现代码及效果
 *  三、总结

# 一、算法步骤 #

（1）在目标点云P中取点集pi∈P；  
（2）找出源点云Q中的对应点集qi∈Q，使得||qi-pi||=min；  
（3）计算旋转矩阵R和平移矩阵t，使得误差函数最小；  
（4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的 对应点集pi’=\{pi’=Rpi+t,pi∈P\}；  
（5）计算pi’与对应点集qi的平均距离；  
（6）如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。否则返回第2步，直到满足收敛条件为止。

# 二、实现代码及效果 #

clear
    close all;
    clc
    % 读取源点云数据P
    [fileName,pathName]=uigetfile("*.txt","文本文件");
    P=importdata([pathName,fileName]);
    P=P(:,1:3);
    %读取目标点云数据Q
    [fileName,pathName]=uigetfile("*.txt","文本文件");
    Q=importdata([pathName,fileName]);
    Q=Q(:,1:3);
    ShowData(P,Q);
    difference=zeros(size(Q,1),3);   %创建记录坐标差值矩阵
    mapPoint=zeros(size(P,1),3);    %创建与源点集对应的映射矩阵
    distance=zeros(size(Q,1),1);    %创建记录所有距离的矩阵
    n=size(P,1);
    
    tic
    j=0; 
    d=100;
    while d>0.0002
        j=j+1;
        fprintf("迭代次数：%d\n",j);
        %%
        %寻找点云数据P中点Pi的对应点Qi
        %使用欧式距离最近的点作为对应点，遍历筛选
        for i=1:n
            difference=Q-P(i,:);       %坐标差值
            difference=difference.^2;
            distance=sum(difference,2);
            [~,minIndex]=min(distance);
            mapPoint(i,:)=Q(minIndex,:);    %保存对应点
        end
        
        %%
        %计算旋转矩阵R和平移矩阵t的最优解，使用svd方法
        centerP=mean(P);    %P点集的质心点
        centerMap=mean(mapPoint);      %对应点集的质心点
        tempP=P-centerP;        %进行去中心化
        tempMapPoint=mapPoint-centerMap;
        
        H=tempP'*tempMapPoint;  %得到H矩阵
        [U,~,V]=svd(H);
        R=V*U';
        %     T=(centerP-centerMap)';
        T=-R*centerP'+centerMap';   %利用质心点求解T参数
        %%
        %使用R和T来得到新的点集
        P=(R*P'+T)';       %使用转换参数得到新的点集P
        d=sum(sum((P-mapPoint).^2,2))/n;	%计算新的点集P到对应点的平均距离
    end
    toc
    ShowData(P,Q);

ShowData.m

function ShowData(data1,data2)
    %SHOWDATA 显示点云数据
        x1 = data1(:, 1);
        y1 = data1(:, 2);
        z1 = data1(:, 3);
    
        x2 = data2(:, 1);
        y2 = data2(:, 2);
        z2 = data2(:, 3);
        
        figure();
        plot3(x1, y1, z1, '.');
        grid on;
        hold on;
        plot3(x2, y2, z2, '.');
        hold off;
    end

实现效果：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2RheXVoYWl0YW5nMQ_size_16_color_FFFFFF_t_70]

# 三、总结 #

因为我采取的是作为欧式距离最近的点作为对应点，并且使用枚举法遍历筛选对应点，这就导致了匹配对应点的过程花费了很长的时间(3105.396793 秒),如果想要提高效率网上说可以使用k-d tree结构来实现，但是现阶段的我还不会哈哈，等日后学会了在补上吧。

**参考文章：**  
[【1】https://zhuanlan.zhihu.com/p/35893884][1_https_zhuanlan.zhihu.com_p_35893884]  
[【2】https://blog.csdn.net/sinat\_29886521/article/details/77506426?utm\_source=blogxgwz0][2_https_blog.csdn.net_sinat_29886521_article_details_77506426_utm_source_blogxgwz0]  
[【3】https://blog.csdn.net/kksc1099054857/article/details/80280964][3_https_blog.csdn.net_kksc1099054857_article_details_80280964]  
[【4】https://blog.csdn.net/u013517182/article/details/53995993?depth\_1-utm\_source=distribute.pc\_relevant.none-task&utm\_source=distribute.pc\_relevant.none-task][4_https_blog.csdn.net_u013517182_article_details_53995993_depth_1-utm_source_distribute.pc_relevant.none-task_utm_source_distribute.pc_relevant.none-task]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2RheXVoYWl0YW5nMQ_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200306135315452.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2RheXVoYWl0YW5nMQ==,size_16,color_FFFFFF,t_70
[1_https_zhuanlan.zhihu.com_p_35893884]: https://zhuanlan.zhihu.com/p/35893884
[2_https_blog.csdn.net_sinat_29886521_article_details_77506426_utm_source_blogxgwz0]: https://blog.csdn.net/sinat_29886521/article/details/77506426?utm_source=blogxgwz0
[3_https_blog.csdn.net_kksc1099054857_article_details_80280964]: https://blog.csdn.net/kksc1099054857/article/details/80280964
[4_https_blog.csdn.net_u013517182_article_details_53995993_depth_1-utm_source_distribute.pc_relevant.none-task_utm_source_distribute.pc_relevant.none-task]: https://blog.csdn.net/u013517182/article/details/53995993?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task