1. Algorithm steps
(1) Get the point set pi∈P in the target point cloud P; (2) Find the corresponding point set qi∈Q in the source point cloud Q, such that ||qi-pi||=min; (3) Calculate the rotation Matrix R and translation matrix t, so that the error function is minimized; (4) Use the rotation matrix R and translation matrix t obtained in the previous step to perform rotation and translation transformation on pi, and arrive at the new corresponding point set pi'={pi'= Rpi+t,pi∈P}; (5) Calculate the average distance between pi' and the corresponding point set qi; (6) If d is less than a given threshold or greater than the preset maximum number of iterations, stop the iterative calculation. Otherwise, return to step 2 until the convergence conditions are met.
2. Implement code and effects
clear
close all;
clc
% 读取源点云数据P
[fileName,pathName]=uigetfile("*.txt",