基于ICP算法的ICP点云地图构建——MATLAB.zip

function [tform, newnext] = MYicp(last, next) point0 = last.Location; point1 = next.Location; [hangshu, ~] = size(point0); xunhuan = 256; newpoint1 = point1; %存储R和t： R = zeros(3, 3, xunhuan); T = zeros(3, 1, xunhuan); %记录误差: errors = zeros(xunhuan, 1); %初始化R和t: last_r = [1 0 0 ; 0 1 0 ; 0 0 1 ]; last_t = [0;0;0]; for i = 1:1:xunhuan %点对点式地寻找匹配对点: [newpoint0, newpoint1, dis] = pointsearch(last.Location, newpoint1); %记录距离误差： error = sum(dis); errors(i, 1) = error; %用线性代数方法求解R和t： p0 = sum(newpoint0, 1) / hangshu; p1 = sum(newpoint1, 1) / hangshu; q0 = newpoint0 - repmat(p0, hangshu, 1); q1 = newpoint1 - repmat(p1, hangshu, 1); W = q1' * q0; [U, ~, V] = svd(W); r = V * U'; t = p0' - r * p1'; %将R和t进行叠代和保存，并叠代变换点云： last_r = r * last_r; last_t = r * last_t + t; newpoint1 = (last_r * point1' + last_t)'; R(:, :, i) = r(:, :); T(:, 1, i) = t(:, 1); end %叠代完毕，输出变换点云和转换矩阵： newnext = pointCloud(newpoint1); A = [0;0;0]; B = 1; trans = [last_r A; last_t' B]; tform = affine3d(trans);