一、算法步骤

（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",