本着尊重原创的原则:
参考文献如下: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