icp算法的实现_【数据处理】ICP算法实现三维点云数据的自动配准

     ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数，然后通过最小乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小，ICP处理流程分为四个主要的步骤：

     1. 对原始点云数据进行采样

     2.确定初始对应点集

     3.去除错误对应点对

     4.坐标变换求解

%% demo.m%% Shows a couple of sample registrations using % ICP - Iterative Closest Point%% Jakob Wilm and Martin Kjer, Technical University of Denmark, 2012m = 80; % width of gridn = m^2; % number of points[X,Y] = meshgrid(linspace(-2,2,m), linspace(-2,2,m));X = reshape(X,1,[]);Y = reshape(Y,1,[]);Z = sin(X).*cos(Y);% Create the data point-matrixD = [X; Y; Z];% Translation values (a.u.):Tx = 0.5;Ty = -0.3;Tz = 0.2;% Translation vectorT = [Tx; Ty; Tz];% Rotation values (rad.):rx = 0.3;ry = -0.2;rz = 0.05;Rx = [1 0 0;      0 cos(rx) -sin(rx);      0 sin(rx) cos(rx)];  Ry = [cos(ry) 0 sin(ry);      0 1 0;      -sin(ry) 0 cos(ry)];  Rz = [cos(rz) -sin(rz) 0;      sin(rz) cos(rz) 0;      0 0 1];% Rotation matrixR = Rx*Ry*Rz;% Transform data-matrix plus noise into model-matrix M = R * D + repmat(T, 1, n);% Add noise to model and datarng(2912673);M = M + 0.01*randn(3,n);D = D + 0.01*randn(3,n);%% Run ICP (standard settings)[Ricp Ticp ER t] = icp(M, D, 15);% Transform data-matrix using ICP resultDicp = Ricp * D + repmat(Ticp, 1, n);% Plot model points blue and transformed points redfigure;subplot(2,2,1);plot3(M(1,:),M(2,:),M(3,:),'bo',D(1,:),D(2,:),D(3,:),'r.');axis equal;xlabel('x'); ylabel('y'); zlabel('z');title('Red: z=sin(x)*cos(y), blue: transformed point cloud');% Plot the resultssubplot(2,2,2);plot3(M(1,:),M(2,:),M(3,:),'bo',Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.');axis equal;xlabel('x'); ylabel('y'); zlabel('z');title('ICP result');% Plot RMS curvesubplot(2,2,[3 4]);plot(0:15,ER,'--x');xlabel('iteration#');ylabel('d_{RMS}');legend('bruteForce matching');title(['Total elapsed time: ' num2str(t(end),2) ' s']);%% Run ICP (fast kDtree matching and extrapolation)[Ricp Ticp ER t] = icp(M, D, 15, 'Matching', 'kDtree', 'Extrapolation', true);% Transform data-matrix using ICP resultDicp = Ricp * D + repmat(Ticp, 1, n);% Plot model points blue and transformed points redfigure;subplot(2,2,1);plot3(M(1,:),M(2,:),M(3,:),'bo',D(1,:),D(2,:),D(3,:),'r.');axis equal;xlabel('x'); ylabel('y'); zlabel('z');title('Red: z=sin(x)*cos(y), blue: transformed point cloud');% Plot the resultssubplot(2,2,2);plot3(M(1,:),M(2,:),M(3,:),'bo',Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.');axis equal;xlabel('x'); ylabel('y'); zlabel('z');title('ICP result');% Plot RMS curvesubplot(2,2,[3 4]);plot(0:15,ER,'--x');xlabel('iteration#');ylabel('d_{RMS}');legend('kDtree matching and extrapolation');title(['Total elapsed time: ' num2str(t(end),2) ' s']);%% Run ICP (partial data)% Partial model point cloudMp = M(:,Y>=0);% Boundary of partial model point cloudb = (abs(X(Y>=0)) == 2) | (Y(Y>=0) == min(Y(Y>=0))) | (Y(Y>=0) == max(Y(Y>=0)));bound = find(b);% Partial data point cloudDp = D(:,X>=0);[Ricp Ticp ER t] = icp(Mp, Dp, 50, 'EdgeRejection', true, 'Boundary', bound, 'Matching', 'kDtree');% Transform data-matrix using ICP resultDicp = Ricp * Dp + repmat(Ticp, 1, size(Dp,2));% Plot model points blue and transformed points redfigure;subplot(2,2,1);plot3(Mp(1,not(b)),Mp(2,not(b)),Mp(3,not(b)),'bo',...      Mp(1,b),Mp(2,b),Mp(3,b),'go',...      Dp(1,:),Dp(2,:),Dp(3,:),'r.')axis equal;xlabel('x'); ylabel('y'); zlabel('z');title('Red: z=sin(x)*cos(y), blue: transformed point cloud');% Plot the resultssubplot(2,2,2);plot3(Mp(1,not(b)),Mp(2,not(b)),Mp(3,not(b)),'bo',...      Mp(1,b),Mp(2,b),Mp(3,b),'go',...      Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.');axis equal;xlabel('x'); ylabel('y'); zlabel('z');title('ICP result');% Plot RMS curvesubplot(2,2,[3 4]);plot(0:50,ER,'--x');xlabel('iteration#');ylabel('d_{RMS}');legend('partial overlap');title(['Total elapsed time: ' num2str(t(end),2) ' s']);

往期回顾>>>>>>

【模式识别】Matlab指纹识别【优化求解】A*算法解决三维路径规划问题【优化求解】模拟退火遗传实现带时间窗的车辆路径规划问题【数学建模】Matlab实现SEIR模型【优化求解】基于NSGA-2的求解多目标柔性车间调度算法 【优化求解】蚁群算法求最优值  【图像处理】粒子群算法结合模糊聚类分割算法实现图像的分割 【优化算法】基于粒子群算法的充电站最优布局 【优化求解】粒子群算法之电线布局优化 【优化求解】遗传算法解决背包问题

【优化求解】基于粒子群之机器人栅格路径规划