MATLAB【九】————ICP算法实现

1.ICP推导与求解

https://zhuanlan.zhihu.com/p/35893884

2.算法实现：

% 程序说明：输入data_source和data_target两个点云，找寻将data_source映射到data_targe的旋转和平移参数
clear;
close all;
clc;
%% 参数配置
kd = 1;
inlier_ratio = 0.999;
Tolerance = 0.05;
step_Tolerance = 0.01;
max_iteration =100;
show = 1;%% 生成数据
data_source=pcread('C:\Users\Administrator\Desktop\vescl\ply\adis800_pc_wave.ply');
data_target = pcread('C:\Users\Administrator\Desktop\vescl\result1\scene3_dis500_ref400\sparse_point_cloud.ply');
% data_source=pcread('C:\Users\Administrator\Desktop\vescl\ply\adis500_pc_egg.ply');
% data_target = pcread('C:\Users\Administrator\Desktop\vescl\result1\scene4_dis500_ref400\sparse_point_cloud.ply');
% data_source=pcread('C:\Users\Administrator\Desktop\vescl\ply\adis500_pc_step.ply');
% data_target = pcread('C:\Users\Administrator\Desktop\vescl\result1\scene2_dis500_ref400\sparse_point_cloud.ply');
% data_source=pcread('C:\Users\Administrator\Desktop\vescl\ply\adis500_pc_plane.ply');
% data_target = pcread('C:\Users\Administrator\Desktop\vescl\result1\scene1_dis500_ref400\sparse_point_cloud.ply');data_source=data_source.Location';
data_target=data_target.Location';
% 绘制原始点与旋转后的点图像
figure;
scatter3(data_source(1,:),data_source(2,:),data_source(3,:),'b.');
hold on;
scatter3(data_target(1,:),data_target(2,:),data_target(3,:),'r.');
hold off;
daspect([1 1 1]);%% 开始ICP
T_final=eye(4,4);   %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_source=Rf*data_source+Tf*ones(1,size(data_source,2));    %初次更新点集（代表粗配准结果）
err=1;
data_source_old = data_source;
%% 迭代优化
while(1)iteration=iteration+1;if kd == 1%利用Kd-tree找出对应点集kd_tree = KDTreeSearcher(data_target','BucketSize',10);[index, dist] = knnsearch(kd_tree, data_source');else%利用欧式距离找出对应点集k=size(data_source,2);for i = 1:kdata_q1(1,:) = data_target(1,:) - data_source(1,i);    % 两个点集中的点x坐标之差data_q1(2,:) = data_target(2,:) - data_source(2,i);    % 两个点集中的点y坐标之差data_q1(3,:) = data_target(3,:) - data_source(3,i);    % 两个点集中的点z坐标之差distance = sqrt(data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2);  % 欧氏距离[dist(i), index(i)] = min(distance);   % 找到距离最小的那个点endenddisp(['误差err=',num2str(mean(dist))]);disp(['迭代次数ieration=',num2str(iteration)]);err_rec(iteration) = mean(dist);% 按距离排序，只取前面占比为inlierratio内的点以应对外点[~, idx] = sort(dist);inlier_num = round(size(data_source,2)*inlier_ratio);idx = idx(1:inlier_num);data_source_temp = data_source(:,idx);dist = dist(idx);index = index(idx);data_mid = data_target(:,index);% 去中心化后SVD分解求解旋转矩阵与平移向量[R_new, t_new] = rigidTransform3D(data_source_temp', data_mid');% 计算累计的旋转矩阵与平移向量Rf = R_new * Rf;Tf = R_new * Tf + t_new;%     更新点集
%     data_source=R_new*data_source+t_new*ones(1,size(data_source,2));data_source=Rf*data_source_old+Tf*ones(1,size(data_source_old,2));% 显示中间结果if show == 1h = figure(2);scatter3(data_source(1,:),data_source(2,:),data_source(3,:),'b.');hold on;scatter3(data_target(1,:),data_target(2,:),data_target(3,:),'r.');hold off;daspect([1 1 1]);pause(0.1);drawnowendif err < Tolerancedisp('————————————————————————————');disp('情况1：优化结果已经达到目标，结束优化');breakendif iteration > 1 && err_rec(iteration-1) - err_rec(iteration) < step_Tolerancedisp('————————————————————————————');disp('情况2：迭代每一步带来的优化到极限值，结束优化');breakendif iteration>=max_iterationdisp('————————————————————————————');disp('情况3：迭代已经达到最大次数，结束优化');breakend
end%% 计算最后结果的误差
if kd == 1%利用Kd-tree找出对应点集kd_tree = KDTreeSearcher(data_target','BucketSize',10);[index, dist] = knnsearch(kd_tree, data_source');
else%利用欧式距离找出对应点集k=size(data_source,2);for i = 1:kdata_q1(1,:) = data_target(1,:) - data_source(1,i);    % 两个点集中的点x坐标之差data_q1(2,:) = data_target(2,:) - data_source(2,i);    % 两个点集中的点y坐标之差data_q1(3,:) = data_target(3,:) - data_source(3,i);    % 两个点集中的点z坐标之差distance = sqrt(data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2);  % 欧氏距离[dist(i), index(i)] = min(distance);   % 找到距离最小的那个点end
end
disp(['最终误差err=',num2str(mean(dist))]);
err_rec(iteration+1) = mean(dist);%% 迭代优化过程中误差变化曲线
figure;
plot(0:iteration,err_rec);
grid on% 最后点云匹配的结果
figure;
scatter3(data_source(1,:),data_source(2,:),data_source(3,:),'b.');
hold on;
scatter3(data_target(1,:),data_target(2,:),data_target(3,:),'r.');
hold off;
daspect([1 1 1]);% disp('旋转矩阵的真值：');
% disp(T0);  %旋转矩阵真值
disp('计算出的旋转矩阵：');
T_final = [Rf,Tf];
T_final=[T_final;0,0,0,1];
disp(T_final);%% 计算两个点集p，q的刚性变换参数，p和q的大小要一致
function [R, t] = rigidTransform3D(p, q)
n = cast(size(p, 1), 'like', p);
m = cast(size(q, 1), 'like', q);
% 去中心化
pmean = sum(p,1)/n;
p2 = bsxfun(@minus, p, pmean);
qmean = sum(q,1)/m;
q2 = bsxfun(@minus, q, qmean);
% 对协方差矩阵进行SVD分解
C = p2'*q2;
[U,~,V] = svd(C);
R = V*diag([1 1 sign(det(U*V'))])*U';
t = qmean' - R*pmean';
end%% 对点云进行旋转与平移
function [data_q,T] = rotate(data,theta_x, theta_y, theta_z, t)
theta_x = theta_x/180*pi;
rot_x = [1 0 0;0 cos(theta_x) sin(theta_x);0 -sin(theta_x) cos(theta_x)];
theta_y = theta_y/180*pi;
rot_y = [cos(theta_y) 0 -sin(theta_y);0 1 0;sin(theta_y) 0 cos(theta_y)];
theta_z = theta_z/180*pi;
rot_z = [cos(theta_z) sin(theta_z) 0;-sin(theta_z) cos(theta_z) 0;0 0 1];
% 变换矩阵
T = rot_x*rot_y*rot_z;
T = [T,t'];
T=[T;0,0,0,1];
%化为齐次坐标
rows=size(data,2);
rows_one=ones(1,rows);
data=[data;rows_one];
%返回三维坐标
data_q=T*data;
data_q=data_q(1:3,:);
end

3.点云拟合

clear ;
close all;
clc;% for dis_index=1:1:4
% if (dis_index==1)
%     dis=300;
% elseif (dis_index==2)
%     dis=500;
% elseif (dis_index==3)
%     dis=800;
% elseif (dis_index==4)
%     dis=1000;
% end
dis=300;
multip =1/4;height_low=400+64;
width_low=640+135;
% %***************************** plane  ****************************************************
ty_plane=-height_low:1:height_low-1;
tx_plane=-width_low:1:width_low-1;
[x_plane,y_plane]=meshgrid(tx_plane,ty_plane);%形成格点矩阵
z_plane=0*(10*sin(pi*x_plane/(20))+10*cos(pi*y_plane/(20)));z_plane=imresize(z_plane,multip);[z_plane_row,z_plane_col] = find(z_plane>=-20);
len_z_plane =length(z_plane_row);
depth_z_plane = zeros(len_z_plane,1);
for index_plane = 1:len_z_planez_plane_x =z_plane_row(index_plane,:);z_plane_y =z_plane_col(index_plane,:);  depth_z_plane(index_plane) = z_plane(z_plane_x,z_plane_y)+dis;
endfigure(1)
point_plane = [z_plane_row,z_plane_col,depth_z_plane];
pc_plane = pointCloud(point_plane);
pcshow(pc_plane);tic
pcwrite(pc_plane,['adis',num2str(dis),'_pc_plane.ply'],'PLYFormat','ascii');
% pcwrite(pc_plane,['adis',num2str(dis),'_pc_plane.ply'],'PLYFormat','ascii');
toc
% %***************************** step  ****************************************************z_step(2*height_low,2*width_low) = 0;
for x_step=0:(2*width_low)for y_step=0:(2*height_low)if ((y_step>190)&&(y_step<400)&&(x_step>150)&&(x_step<395))z_step(y_step,x_step) = 4;elseif ((y_step>400)&&(y_step<610)&&(x_step>150)&&(x_step<395))z_step(y_step,x_step) = 1;elseif ((y_step>190)&&(y_step<400)&&(x_step>395)&&(x_step<640))z_step(y_step,x_step) = 3;elseif ((y_step>400)&&(y_step<610)&&(x_step>395)&&(x_step<640))z_step(y_step,x_step) = 2;  elseif ((y_step>190)&&(y_step<400)&&(x_step>640)&&(x_step<885))z_step(y_step,x_step) = 2;  elseif ((y_step>400)&&(y_step<610)&&(x_step>640)&&(x_step<885))z_step(y_step,x_step) = 3; elseif ((y_step>190)&&(y_step<400)&&(x_step>885)&&(x_step<1130))z_step(y_step,x_step) = 1;elseif ((y_step>400)&&(y_step<610)&&(x_step>885)&&(x_step<1130))z_step(y_step,x_step) = 4;    elseif ((y_step<190)&&(y_step>610)&&(x_step<150)&&(x_step>1130))z_step(y_step,x_step) = 1;endend
end
z_step=imresize(z_step,multip);
% [rows_step,cols_step]=size(z_step);[z_step_row,z_step_col] = find(z_step>=-20);
len_z_step =length(z_step_row);
depth_z_step = zeros(len_z_step,1);
for index_step = 1:len_z_stepz_step_x =z_step_row(index_step,:);z_step_y =z_step_col(index_step,:);  depth_z_step(index_step) = z_step(z_step_x,z_step_y)+dis;
endfigure(2)
point_step = [z_step_row,z_step_col,depth_z_step];
pc_step = pointCloud(point_step);
pcshow(pc_step);tic
pcwrite(pc_step,['adis',num2str(dis),'_pc_step.ply'],'PLYFormat','ascii');
toc
% %***************************** wave *********************************************
ty_single=-height_low:1:height_low-1;
tx_single=-width_low:1:width_low-1;
[x_single,y_single]=meshgrid(tx_single,ty_single);%形成格点矩阵
z_single=10*sin(pi*x_single/(20));%+10*cos(pi*y_wave/(20));z_single=imresize(z_single,multip);[z_single_row,z_single_col] = find(z_single>=-20);
len_z_single =length(z_single_row);
depth_z_single = zeros(len_z_single,1);
for index_single = 1:len_z_singlez_single_x =z_single_row(index_single,:);z_single_y =z_single_col(index_single,:);  depth_z_single(index_single) = z_single(z_single_x,z_single_y)+dis;
endfigure(3)
point_wave = [z_single_row,z_single_col,depth_z_single];
pc_wave = pointCloud(point_wave);
pcshow(pc_wave);tic
pcwrite(pc_wave,['adis',num2str(dis),'_pc_wave.ply'],'PLYFormat','ascii');
toc
% %*****************************  egg  *********************************************
ty_wave=-height_low:1:height_low-1;
tx_wave=-width_low:1:width_low-1;
[x_wave,y_wave]=meshgrid(tx_wave,ty_wave);%形成格点矩阵
z_egg=5*sin(pi*x_wave/(20))+5*cos(pi*y_wave/(20));z_egg=imresize(z_egg,multip);[z_wave_row,z_wave_col] = find(z_egg>=-20);
len_z_wave =length(z_wave_row);
depth_z_wave = zeros(len_z_wave,1);
for index_wave = 1:len_z_wavez_wave_x =z_wave_row(index_wave,:);z_wave_y =z_wave_col(index_wave,:);  depth_z_wave(index_wave) = z_egg(z_wave_x,z_wave_y)+dis;
endfigure(4)
point_plate =[z_wave_row,z_wave_col,depth_z_wave];
pc_egg = pointCloud(point_plate);
pcshow(pc_egg);tic
pcwrite(pc_egg,['adis',num2str(dis),'_pc_egg.ply'],'PLYFormat','ascii');
toc
% %***************************** triangular profiler ****************************************************
% end

4.点云拟合函数

function [data_target]=scene_to_ply_downsample_function(scene,height,width,distance,multiplier)% clear;
% close all;
% clc;% dis=300;
% multip =1/2;
% height_low=400;
% width_low=640;
dis=distance;
multip =multiplier;
height_low=height;
width_low=width;
% %***************************** plane  ****************************************************
tx_plane=-height_low:1:height_low-1;
ty_plane=-width_low:1:width_low-1;
[x_plane,y_plane]=meshgrid(tx_plane,ty_plane);%形成格点矩阵
z_plane=0*(10*sin(pi*x_plane/(20))+10*cos(pi*y_plane/(20)));z_plane=imresize(z_plane,multip);[z_plane_row,z_plane_col] = find(z_plane>=-20);
len_z_plane =length(z_plane_row);
depth_z_plane = zeros(len_z_plane,1);
for index_plane = 1:len_z_planez_plane_x =z_plane_row(index_plane,:);z_plane_y =z_plane_col(index_plane,:);  depth_z_plane(index_plane) = z_plane(z_plane_x,z_plane_y)+dis;
endfigure(11)
point_plane = [z_plane_row,z_plane_col,depth_z_plane];
pc_plane = pointCloud(point_plane);
pcshow(pc_plane);% tic
% pcwrite(pc_plane,['adis',num2str(dis),'_pc_plane.ply'],'PLYFormat','ascii');
% % pcwrite(pc_plane,['adis',num2str(dis),'_pc_plane.ply'],'PLYFormat','ascii');
% toc
% %***************************** step  ****************************************************z_step(2*height_low,2*width_low) = 0;
for x_step=0:(2*width_low)for y_step=0:(2*height_low)if ((y_step>190)&&(y_step<400)&&(x_step>150)&&(x_step<395))z_step(y_step,x_step) = 4;elseif ((y_step>400)&&(y_step<610)&&(x_step>150)&&(x_step<395))z_step(y_step,x_step) = 1;elseif ((y_step>190)&&(y_step<400)&&(x_step>395)&&(x_step<640))z_step(y_step,x_step) = 3;elseif ((y_step>400)&&(y_step<610)&&(x_step>395)&&(x_step<640))z_step(y_step,x_step) = 2;  elseif ((y_step>190)&&(y_step<400)&&(x_step>640)&&(x_step<885))z_step(y_step,x_step) = 2;  elseif ((y_step>400)&&(y_step<610)&&(x_step>640)&&(x_step<885))z_step(y_step,x_step) = 3; elseif ((y_step>190)&&(y_step<400)&&(x_step>885)&&(x_step<1130))z_step(y_step,x_step) = 1;elseif ((y_step>400)&&(y_step<610)&&(x_step>885)&&(x_step<1130))z_step(y_step,x_step) = 4;    elseif ((y_step<190)&&(y_step>610)&&(x_step<150)&&(x_step>1130))z_step(y_step,x_step) = 1;endend
end
z_step=imresize(z_step,multip);
% [rows_step,cols_step]=size(z_step);[z_step_row,z_step_col] = find(z_step>=-20);
len_z_step =length(z_step_row);
depth_z_step = zeros(len_z_step,1);
for index_step = 1:len_z_stepz_step_x =z_step_row(index_step,:);z_step_y =z_step_col(index_step,:);  depth_z_step(index_step) = z_step(z_step_x,z_step_y)+dis;
endfigure(22)
point_step = [z_step_row,z_step_col,depth_z_step];
pc_step = pointCloud(point_step);
pcshow(pc_step);% tic
% pcwrite(pc_step,['adis',num2str(dis),'_pc_step.ply'],'PLYFormat','ascii');
% toc
% %***************************** wave *********************************************
ty_single=-height_low:1:height_low-1;
tx_single=-width_low:1:width_low-1;
[x_single,~]=meshgrid(tx_single,ty_single);%形成格点矩阵
z_single=10*sin(pi*x_single/(20));%+10*cos(pi*y_wave/(20));
z_single=z_single';
z_single=imresize(z_single,multip);[z_single_row,z_single_col] = find(z_single>=-20);
len_z_single =length(z_single_row);
depth_z_single = zeros(len_z_single,1);
for index_single = 1:len_z_singlez_single_x =z_single_row(index_single,:);z_single_y =z_single_col(index_single,:);  depth_z_single(index_single) = z_single(z_single_x,z_single_y)+dis;
endfigure(33)
point_wave = [z_single_row,z_single_col,depth_z_single];
pc_wave = pointCloud(point_wave);
pcshow(pc_wave);% tic
% pcwrite(pc_wave,['adis',num2str(dis),'_pc_wave.ply'],'PLYFormat','ascii');
% toc
% %*****************************  egg  *********************************************
tx_wave=-height_low:1:height_low-1;
ty_wave=-width_low:1:width_low-1;
[x_wave,y_wave]=meshgrid(tx_wave,ty_wave);%形成格点矩阵
z_egg=5*sin(pi*x_wave/(20))+5*cos(pi*y_wave/(20));z_egg=imresize(z_egg,multip);[z_wave_row,z_wave_col] = find(z_egg>=-20);
len_z_wave =length(z_wave_row);
depth_z_wave = zeros(len_z_wave,1);
for index_wave = 1:len_z_wavez_wave_x =z_wave_row(index_wave,:);z_wave_y =z_wave_col(index_wave,:);  depth_z_wave(index_wave) = z_egg(z_wave_x,z_wave_y)+dis;
endfigure(44)
point_plate =[z_wave_row,z_wave_col,depth_z_wave];
pc_egg = pointCloud(point_plate);
pcshow(pc_egg);if(strcmp(scene,'scene1'))data_target=pc_plane;
elseif(strcmp(scene,'scene2'))data_target=pc_step;
elseif(strcmp(scene,'scene3'))data_target=pc_wave;
elseif(strcmp(scene,'scene4'))data_target=pc_egg;
end
% tic
% pcwrite(pc_egg,['adis',num2str(dis),'_pc_egg.ply'],'PLYFormat','ascii');
% toc
% %***************************** triangular profiler ****************************************************
end

MATLAB【九】————ICP算法实现相关推荐

多帧点云拼接的全局ICP算法【附Matlab代码链接】
用RGBD相机采集一组多视角深度点云,假设多帧点云之间有共视邻接关系,通常会先进行Pair-wise的帧间点云匹配,具体方法见另外一个帖子: 两帧点云刚性配准的ICP算法连续的帧间ICP可以把点云变 ...
MATLAB点云处理：读取、展示、最近邻、ICP算法求取转移矩阵、旋转
MATLAB中关于点云的几个函数的简单应用.作者使用的是MATLAB R2015b,这几个函数应该是在Computer Vison包里. 全文都是作者自己结合MATLAB文档的理解,欢迎指教. 1. ...
ICP算法MATLAB仿真
2022.4.15再次附上数据satellite.txt新链接: 链接:https://pan.baidu.com/s/1mEN-FQbTlHOWfxCHyu0Dxg 提取码:02i4 (永久有效) ...
秦九昭算法——MATLAB实现
一.引入对于多项式而言,要计算时的函数值时,需要进行次乘法和n次加法,其时间复杂度为. 那我们该用一个什么用的方式来降低其时间复杂度呢? (1条消息) 一套图搞懂"时间复杂度" ...
ICP算法实现(MATLAB)
这几天因为正在学习点云数据,所以就学习了一下ICP算法. 文章目录一.算法步骤二.实现代码及效果三.总结一.算法步骤 (1)在目标点云P中取点集pi∈P: (2)找出源点云Q中的对应点集qi∈ ...
在医学图像分析中使用ICP算法进行点云配准
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文转载自「计算机视觉工坊」,该公众号重点在于介绍深度学习.智能驾驶等领域,一个小众的公众号. 论文标 ...
经典ICP算法的问题
最近可能要用三维点云实现一个三维场景重建的功能,从经典的ICP算法开始,啃了一些文档,对其原理也是一知半解. 迭代最近点算法综述大致参考了这份文档之后,照流程用MATLAB实现了一个简单的ICP算法 ...
花了1晚上diy的matlab解数独算法，很好理解！
花了1晚上diy的matlab解数独算法,很好理解! 前言一.数独的规则二.算法 1.思路 2.流程图 3.Matlab代码总结前言老婆最近迷上了数独,还给我拍了张照片,初步了解了规则之后, ...
视频教程-三十八课时零基础matlab精通优化算法-Matlab
三十八课时零基础matlab精通优化算法图像和算法等领域有多年研究和项目经验:指导发表科技核心期刊经验丰富:多次指导数学建模爱好者参赛. 宋星星 ¥100.00 立即订阅扫码下载「CSDN程序员学 ...
机器学习九大算法---支持向量机
机器学习九大算法---支持向量机出处:结构之法算法之道blog. 前言动笔写这个支持向量机(support vector machine)是费了不少劲和困难的,原因很简单,一者这个东西本身就并不好 ...

MATLAB【九】————ICP算法实现

MATLAB【九】————ICP算法实现相关推荐

最新文章

热门文章