基于kd-tree搜索的ICP/NDT的MATLAB仿真

江湖路人

已于 2022-03-09 22:07:02 修改

阅读量1.1k

点赞数 2

分类专栏：笔记文章标签：算法

于 2021-06-19 15:41:45 首次发布

本文链接：https://blog.csdn.net/qq_40957243/article/details/118054405

版权

笔记专栏收录该内容

12 篇文章 13 订阅

订阅专栏

本文介绍了基于ICP算法的3D点云配准过程，包括旋转和平移的计算，以及使用kd-tree加速搜索。同时展示了仿真结果，并分享了源代码。此外，还探讨了NDT配准算法，通过实例代码解释了其工作原理和实现步骤，最终实现了点云的精确配准。

摘要由CSDN通过智能技术生成

本着尊重原创的原则：
参考文献如下：https://www.cnblogs.com/qi-zhang/p/10017670.html

前言

高楼大厦始于基石，最近在学slam相关算法，因此从最基础的icp算法入手，纯看算法推导感觉理解的不够深刻，因此在找到了该博主的仿真代码（代码），阅读后受益非浅，再次感谢该博主。在学习的过程中修改了部分代码：主要增加了三维旋转，kd-tree搜索（不得不说这算法搜索真的快），部分绘图功能（本人有强迫症，就是要把图画的是自己想想的样子，哈哈）

代码来源

有钱的捧个钱场：https://download.csdn.net/download/qq_40957243/19716942?spm=1001.2014.3001.5501

没钱的捧个人场：评论区留下你的足迹（邮箱），我给你发过去

仿真结果

没有结果就没有说服力，废话不多说直接上图

ICP配准所用的时间t=
时间已过 55.661564 秒。
ICP的迭代次数ite=
   100

旋转矩阵的计算值T_estimate=
    0.5000    0.7071   -0.5000   -3.0355
   -0.5000    0.7071    0.5000   -4.0355
    0.7071   -0.0000    0.7071   -7.7782
         0         0         0    1.0000

旋转矩阵的真值T_TruthValue=
    0.5000    0.7071   -0.5000   -3.0355
   -0.5000    0.7071    0.5000   -4.0355
    0.7071   -0.0000    0.7071   -7.7782
         0         0         0    1.0000

配准前：

在这里插入图片描述
配准后：

NDT仿真

尊重原创：https://www.cnblogs.com/tiandsp/p/14854605.html

有个小bug，此处贴出代码（详细算法请参考以上原创链接），此处仅仅本人记录学习，如有侵权请联系删除

clear all;close all;clc;

%生成原始点集
X=[];Y=[];
for i=-180:2:180
    for j=-90:2:90
        x = i * pi / 180.0;
        y = j * pi / 180.0;
        X =[X,cos(y)*cos(x)];
        Y =[Y,sin(y)*cos(x)];
    end
end
P=[X(1:3000)' Y(1:3000)'];

%生成变换后点集
theta=0.5;
R=[cos(theta) -sin(theta);sin(theta) cos(theta)];
X=(R*P')' + [2.4,3.5];

plot(P(:,1),P(:,2),'b.');
hold on;
plot(X(:,1),X(:,2),'r.');

meanP = mean(P);
meanX = mean(X);

P = P - meanP;          %统一起始点，否则两组点云间可能没有交集，算法会失效
X = X - meanX; 

minx = min(X(:,1));
miny = min(X(:,2));
maxx = max(X(:,1));
maxy = max(X(:,2));

cellsize = 0.3;         %网格大小

M = floor((maxx - minx)/cellsize+1);
N = floor((maxy - miny)/cellsize+1);

gridPoint = cell(M,N);
meangridPoint = zeros(2,M,N);
convgridPoint = zeros(2,2,M,N);

for i = 1:length(X)             %划分网格并填入数据
    m = floor((X(i,1) - minx)/cellsize + 1);
    n = floor((X(i,2) - miny)/cellsize + 1);
    gridPoint{m,n} = [gridPoint{m,n};X(i,:)];
end

%计算每个网格中的均值和方差
for i=1:M
    for j=1:N
        if(size(gridPoint{i,j},1)>=2)
            meangridPoint(:,i,j) = mean(gridPoint{i,j});
            convgridPoint(:,:,i,j) = cov(gridPoint{i,j});
        end
    end
end

%%
 %% Start add waitbar
 hWaitbar = waitbar(0, 'Computing，wait...', 'CreateCancelBtn', 'delete(gcbf)');
 set(hWaitbar, 'Color', [0.9, 0.9, 0.9]);
 %
 hBtn = findall(hWaitbar, 'type', 'Uicontrol');
 set(hBtn, 'String', 'cancle', 'FontSize', 10);
%%
iter=5000;
pre =zeros(3,1);
for i=1:iter          %迭代40次
    compT=i/iter;
    waitbar(compT, hWaitbar, ['Computing...', num2str(round(compT, 2) * 100), '%']);
    
    g = zeros(3,1);
    H = zeros(3,3);
    
    tx = pre(1);
    ty = pre(2);
    theta = pre(3);
    for j=1:length(P)
        x = P(j,1);
        y = P(j,2);
        
        p_trans = [x*cos(theta)-y*sin(theta)+tx;x*sin(theta)+y*cos(theta)+ty];
        
        m = floor((p_trans(1) - minx)/cellsize + 1);
        n = floor((p_trans(2) - miny)/cellsize + 1);
        
        if m>=1 && n>=1 && m<=M && n<=N         %只计算投影到网格中的点云数据
            if (size(gridPoint{m,n},1)>=2)
                
                q = meangridPoint(:,m,n);
                sigma = convgridPoint(:,:,m,n);
                
                if(cond(sigma)>50)              %根据矩阵条件数判断是否是病态矩阵
                    continue;
                end
                invsigma = inv(sigma);
                
                xk = p_trans - q;
                
                dx = [1;0];
                dy = [0;1];
                dt = [-x*sin(theta)-y*cos(theta);x*cos(theta)-y*sin(theta)];
                ddt = [-x*cos(theta)+y*sin(theta);-x*sin(theta)-y*cos(theta)];
                
                g(1) = g(1) + (xk'*invsigma* dx *exp(-0.5*xk'*invsigma*xk));    %计算损失函数对x的偏导
                g(2) = g(2) + (xk'*invsigma* dy *exp(-0.5*xk'*invsigma*xk));    %计算损失函数对y的偏导
                g(3) = g(3) + (xk'*invsigma* dt *exp(-0.5*xk'*invsigma*xk));    %计算损失函数对theta的偏导
                
                %计算黑塞矩阵，分别计算损失函数对x,y,theta的二阶偏导
                H(1,1) = H(1,1) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dx)*(xk'*invsigma*dx)+(dx'*invsigma*dx));
                H(1,2) = H(1,2) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dx)*(xk'*invsigma*dy)+(dx'*invsigma*dy));
                H(1,3) = H(1,3) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dx)*(xk'*invsigma*dt)+(dx'*invsigma*dt));
                
                H(2,1) = H(2,1) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dy)*(xk'*invsigma*dx)+(dy'*invsigma*dx));
                H(2,2) = H(2,2) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dy)*(xk'*invsigma*dy)+(dy'*invsigma*dy));
                H(2,3) = H(2,3) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dy)*(xk'*invsigma*dt)+(dy'*invsigma*dt));
                
                H(3,1) = H(3,1) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dt)*(xk'*invsigma*dx)+(dt'*invsigma*dx));
                H(3,2) = H(3,2) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dt)*(xk'*invsigma*dy)+(dt'*invsigma*dy));
                H(3,3) = H(3,3) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dt)*(xk'*invsigma*dt)+(dt'*invsigma*dt) + xk'*invsigma*ddt);
                
            end
        end
    end
    
    %牛顿迭代求解
    delta = -H\g;
    pre = pre + delta;
end

pre
theta=pre(3);
R=[cos(theta) -sin(theta);sin(theta) cos(theta)];       %画出变换后的点云

XX=(R*P')';
plot(XX(:,1),XX(:,2),'g.');
XX=P+[pre(1),pre(2)] + meanX;
plot(XX(:,1),XX(:,2),'k.');
XX=(R*P')' + [pre(1),pre(2)] + meanX;
plot(XX(:,1),XX(:,2),'y.');
axis equal;
legend('原始点云','变换后点云','旋转点云','平移点云','配准点云');
grid on;

clear all;close all;clc;

%生成原始点集
X=[];Y=[];
for i=-180:2:180
    for j=-90:2:90
        x = i * pi / 180.0;
        y = j * pi / 180.0;
        X =[X,cos(y)*cos(x)];
        Y =[Y,sin(y)*cos(x)];
    end
end
P=[X(1:3000)' Y(1:3000)'];

%生成变换后点集
theta=-0.5;
R=[cos(theta) -sin(theta);sin(theta) cos(theta)];
X=P*R + [2.4,3.5];

plot(P(:,1),P(:,2),'b.');
hold on;
plot(X(:,1),X(:,2),'r.');

P = [P zeros(length(P),1)];
X = [X zeros(length(X),1)];

moving = pointCloud(P);
fixed = pointCloud(X);

gridStep = 0.3;
tform = pcregisterndt(moving,fixed,gridStep);

R = tform.T(1:3,1:3);
T = tform.T(4,1:3);

XX=P*R + T;
plot(XX(:,1),XX(:,2),'g.');
axis equal
legend('原始点云','变换后点云','配准点云')

代码链接

链接：https://pan.baidu.com/s/149kRj0OZCeqFtYB6jSLE6g
提取码：s0nr

江湖路人

关注

2
点赞
踩
13

收藏

觉得还不错? 一键收藏
12
评论
基于kd-tree搜索的ICP/NDT的MATLAB仿真

本着尊重原创的原则：参考文献如下：https://www.cnblogs.com/qi-zhang/p/10017670.html前言高楼大厦始于基石，最近在学slam相关算法，因此从最基础的icp算法入手，纯看算法推导感觉理解的不够深刻，因此在找到了该博主的仿真代码（代码），阅读后受益非浅，再次感谢该博主。在学习的过程中修改了部分代码：主要增加了三维旋转，kd-tree搜索（不得不说这算法搜索真的快），部分绘图功能（本人有强迫症，就是要把图画的是自己想想的样子，哈哈）代码来源有钱的捧个钱场：ht
复制链接

扫一扫

专栏目录