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;