ICP算法实现(MATLAB)

这几天因为正在学习点云数据，所以就学习了一下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