matlab中ICP点云匹配算法

function [R1, t1] = reg(data1, data2, corr) M = data1(:,corr(:,1)); mm = mean(M,2); S = data2(:,corr(:,2)); ms = mean(S,2); Sshifted = [S(1,:)-ms(1); S(2,:)-ms(2); ]; Mshifted = [M(1,:)-mm(1); M(2,:)-mm(2); ]; b1 = Sshifted(1,:)*Mshifted(1,:)'+Sshifted(2,:)*Mshifted(2,:)'; b2 = -Sshifted(2,:)*Mshifted(1,:)'+Sshifted(1,:)*Mshifted(2,:)'; bb = (b1^2+b2^2)^0.5; c = b1/bb; s = b2/bb; R1 = [c -s s c]; t1 = mm - R1*ms;

% function icp
load comparedata.mat

lsr1 = lsrdata（1）.lsr; lsr2 = lsrdata（2）.lsr;

plot（lsr1（1:）lsr1（2:） ‘.‘）; hold on
plot（lsr2（1:）lsr2（2:）‘r.‘）;

[R t corr error data2] = icp_2d（lsr1 lsr2 0.1）;
plot（data2（1:）data2（2:）‘g.‘）;

ncorr = size（corr）;
corrline = []; xx=[];
for i = 1:ncorr（1）;
    corrline = [corrline lsr1（:corr（i1）） data2（:corr（i2））[nan;nan]];
    x = 0.5*（lsr1（:corr（i1）） + data2（:corr（i2）））;
    xx = [xxx];
end
plot（corrline（1:）corrline（2:）‘r‘）;

id1 = 1:length（lsr1）; id2 = 1:length（lsr2）;
sid1 = setdiff（id1corr（:1））; sid2 = setdiff（id2corr（:2））;
x = [lsr1（:sid1）data2（:sid2）xx];
% plot（x（1:）x（2:）‘^‘）;

hold off

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       8929  2009-07-13 10:14  ICP-matlab程序\comparedata.mat

     文件        711  2012-07-15 16:13  ICP-matlab程序\icp.m

     文件       3695  2011-08-17 22:39  ICP-matlab程序\icp_2d.m

     目录          0  2012-07-15 16:14  ICP-matlab程序

----------- ---------  ---------- -----  ----

                13335                    4