高端的ICP点云配准只需要简单MATLAB代码-技术圈

点击下方卡片，关注“新机器视觉”公众号

视觉/图像重磅干货，第一时间送达

各位大家好久不见，我还是小刘。我一直从事着C++视觉方面的工作，最近被C++/Cmake/PCL弄得不要不要的，配置环境。我又想起了我的老伙计-----Matlab。他应该可以做三维点云的基础工作吧。

在我的一阵探索之下，发现matlab从2018年开始增加了计算机视觉工具箱和雷达点云工具箱，也提供了函数，这不是6666吗，于是小刘准备就用这些工具箱函数验证一下经典的ICP算法

当然，本文的风格和之前一样，不会有特别多的数学解释，我尽量用比较通俗的方式给大家讲解清楚，需要严格证明的请大家知网一下/谷歌学术一下。

点云的ICP配准算法是什么东东？

【注】只是感性的介绍，需要知道其中公式还得去看文献哦！！！！

目的：点云配准过程，就是求一个两个点云之间的旋转矩阵R和平移矩阵t

大白话来说就是：对同一个物体进行多次扫描后，会有多个点云数据，然后将两个数据中同一点的点云进行匹配，也就是将两幅点云中相同点的点云放在一起即点云配准。

既然是配准过程，肯定对应点之间会存误差，然后就要让E(R,t)这个函数最小，其中可以用到最小二乘法，随机抽样一致算法（这些常见算法都是matlab内置算法，nice）。

然后效果就如下图一样，两个条为点云向量，他们属于同一个部位的不同点云集，为了让他们配准在一起，使用算法配准。

从一个动图来演示吧：初始是两个不同角度下的笑脸（深红色和绿色），下面是红色笑脸如何通过ICP过程和绿色笑脸重合的：

三维点云配准的一个效果：

目前应用最广泛的点云精配准算法是迭代最近点算法（Iterative Closest Point, ICP）及各种变种 ICP 算法。

ICP算法流程：

首先对于一幅点云中的每个点，在另一幅点云中计算匹配点（最近点）
极小化匹配点间的匹配误差，计算位姿（旋转矩阵R和平移矩阵t）
然后将计算的位姿作用于点云
再重新计算匹配点
如此迭代，直到迭代次数达到阈值，或者极小化的能量函数变化量小于设定阈值

Matlab点云配准走一遭？

1、首先创建两个待配准的点云

 size1=size(PointCloud1);%点云1 if size1(1)==3     PointCloud1=transpose(PointCloud1); end size2=size(PointCloud2);%点云2if size2(1)==3    PointCloud2=transpose(PointCloud2); end

两个带配准的点云集

2.计算距离

for i=1:size_P        smallest=1e10;        correspondence(1,i)=0;             for j=1:size_X             dis=(P_iter(1,i)-X_ori(1,j))^2+(P_iter(2,i)-X_ori(2,j))^2+(P_iter(3,i)-X_ori(3,j))^2;             if dis                 smallest=dis;                 correspondence(1,i)=j;             end        end    end    Corre=correspondence;

3.求解旋转矩阵R和移动矩阵t

function [t]=GetTranslationVector(P_iter,Xiter_centerPos,R_iter)    %P is 3X3200 Xiter_centerPos is 3X1 R_iter is 3X3    Xsum=0;Ysum=0;Zsum=0;        size1=size(P_iter);    size1=size1(2);        for i=1:size1       Xsum=Xsum+P_iter(1,i);       Ysum=Ysum+ P_iter(2,i);        Zsum=Zsum+P_iter(3,i);    end    P_center(1,1)=Xsum/size1;    P_center(2,1)=Ysum/size1;    P_center(3,1)=Zsum/size1;        t=Xiter_centerPos-R_iter*P_center; end

4、将计算的位姿作用于点云

function [Xcenter]=GetXCenterPos(X_ori,Corre)    size_Sum=size(Corre);    size_Sum=size_Sum(2);    xsum=0;    ysum=0;    zsum=0;    for i=1:size_Sum        xsum=xsum+X_ori(1,Corre(1,i));        ysum=ysum+X_ori(2,Corre(1,i));        zsum=zsum+X_ori(3,Corre(1,i));    end        Xcenter(1,1)=xsum/size_Sum;    Xcenter(2,1)=ysum/size_Sum;    Xcenter(3,1)=zsum/size_Sum;end

5、写一个计算最小误差的函数

function [ErrorData]=ComputeErrorMap(corre,P_iter,X_ori)    size_P=size(P_iter);    size_P=size_P(2);        for i=1:size_P        x=X_ori(1,corre(i))-P_iter(1,i);        y=X_ori(2,corre(i))-P_iter(2,i);        z=X_ori(3,corre(i))-P_iter(3,i);        err=x^2+y^2+z^2;        err=sqrt(err);        P_iter(3,i)=err;    end    ErrorData=P_iter;    end