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

  • O3_536106
    了解作者
  • 1.9KB
    文件大小
  • zip
    文件格式
  • 0
    收藏次数
  • VIP专享
    资源类型
  • 0
    下载次数
  • 2022-06-11 03:06
    上传日期
对3D模型进行ICP点云匹配从而得到局部点云地图的示例,其中包括点云地图的绘制和ICP算法的实现(不包括用于示例的3D模型)。
ICP.zip
  • ICP.m
    881B
  • MYicp.m
    1.2KB
  • pointsearch.m
    690B
内容介绍
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);
评论
    相关推荐