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