基于kdtree与svd的迭代最近点ICP算法的matlab实现

概述

##最近在研究点云配准算法，发现用matlab来对点云进行处理比PCL更加方便，于是自己在matlab中实现了对点云配准算法（ICP）的实现，该方法首先通过kdtree来搜索创建对应点集，然后利用奇异值分解的方法求解旋转矩阵和平移矢量，最终将求得的齐次变换矩阵应用于待配准的点云，完成配准操作

代码实现

废话不多说直接上代码分享给大家：

// An highlighted block
function [P_registered,e1,e2]=icp(P,Q,max_iterations)
tic
NS = createns(Q,'NSMethod','kdtree');
j=0;
d=100;
n=size(P,1);
while d>0.0002
    j=j+1;
    fprintf("迭代次数：%d\n",j);
    if j>max_iterations
        break
    end
    %寻找Q的对应点集
    [idx, ~] = knnsearch(NS,P,'k',1);
    Qn= Q(idx,:);
    
    %计算旋转矩阵R和平移矩阵t的最优解，使用svd方法
    centerP=mean(P);    %P点集的质心点
    centerQn=mean(Qn);      %对应点集的质心点
    tempP=P-centerP;        %进行去中心化
    tempQn=Qn-centerQn;
    
     H=tempP'*tempQn;  %得到H矩阵
     [U,~,V]=svd(H);
     R=V*U';
        
    %     T=(centerP-centerMap)';
    T=-R*centerP'+centerQn';   %利用质心点求解T参数
    
    %使用R和T来得到新的点集

    P=(R*P'+T)';       %使用转换参数得到新的点集P
    d=sum(sum((P-Qn).^2,2))/n;	%计算新的点集P到对应点的平均距离
    e1=std(sum((P-Qn).^2,2));
    e2=d;
end
P_registered = P;
toc

配准结果

---------------------------1.配准前-------------------------

---------------------------2.配准后-------------------------