function [R, t, corr, error, data2] = icp_2d(data1, data2, res, tri)
%2D情况下的icp算法，主要流程来源于icp2.m
%改写了reg函数，其他的未变
%输入data1,data2的维数为2D数据
% [R, t, corr, error, data2] = icp2(data1, data2, res, tri)
%
% This is an implementation of the Iterative Closest Point (ICP) algorithm.
% The function takes two data sets and registers data2 with data1. It is
% assumed that data1 and data2 are in approximation registration. The code
% iterates till no more correspondences can be found.
%
% This is a modified version (12 April, 2005). It is more accurate and has
% less chances of getting stuck in a local minimum as opposed to my earlier
% version icp.m
%
% Arguments: data1 - 3 x n matrix of the x, y and z coordinates of data set 1
% data2 - 3 x m matrix of the x, y and z coordinates of data set 2
% res - the tolerance distance for establishing closest point
% correspondences. Normally res equal to the resolution
% of data1
% tri - optional argument. obtained by tri = delaunayn(data1');
%
% Returns: R - 3 x 3 accumulative rotation matrix used to register data2
% t - 3 x 1 accumulative translation vector used to register data2
% corr - p x 3 matrix of the index no.s of the corresponding points of
% data1 and data2 and their corresponding Euclidean distance
% error - the mean error between the corresponding points of data1
% and data2 (normalized with res)
% data2 - 3 x m matrix of the registered data2
%
% figure(7); clf;
% plot(data1(1,:),data1(2,:),'b.'); hold on; plot(data2(1,:),data2(2,:),'r.');
% hold off; axis equal
maxIter = 60;
c1 = 0;
c2 = 1;
R = eye(2);
t = zeros(2,1);
if nargin < 4
tri = delaunayn(data1');
end
n = 0;
while c2 ~= c1
c1 = c2;
[corr, D] = dsearchn(data1', tri, data2');
corr(:,2:3) = [[1 : length(corr)]' D];
corr(D>2*res,:) = [];
corr = -sortrows(-corr,3);
corr = sortrows(corr,1);
[B, Bi, Bj] = unique(corr(:,1));
corr = corr(Bi,:);
[R1, t1] = reg(data1, data2, corr);
data2 = R1*data2;
data2 = [data2(1,:)+t1(1); data2(2,:)+t1(2)];
R = R1*R;
t = R1*t + t1;
c2 = length(corr);
n = n + 1;
if n > maxIter
break;
end
end
e1 = 1000001;
e2 = 1000000;
n = 0;
noChangeCount = 0;
while noChangeCount < 10
e1 = e2;
[corr, D] = dsearchn(data1', tri, data2');
corr(:,2:3) = [[1:length(corr)]' D];
corr(D>2*res,:) = [];
corr = -sortrows(-corr,3);
corr = sortrows(corr,1);
[B, Bi, Bj] = unique(corr(:,1));
corr = corr(Bi,:);
[R1 t1] = reg(data1, data2, corr);
data2 = R1*data2;
data2 = [data2(1,:)+t1(1); data2(2,:)+t1(2)];
R = R1*R;
t = R1*t + t1;
e2 = sum(corr(:,3))/(length(corr)*res);
n = n + 1;
if n > maxIter
break;
end
if abs(e2-e1)<res/10000
noChangeCount = noChangeCount + 1;
end
end
error = min(e1,e2);
%-----------------------------------------------------------------
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;
- 1
- 2
前往页