1 | function [s,R,T] = estsimt(X1,X2) |
---|
2 | % ESTimate SIMilarity Transformation |
---|
3 | % |
---|
4 | % [s,R,T] = estsimt(X1,X2) |
---|
5 | % |
---|
6 | % X1,X2 ... 3xN matrices with corresponding 3D points |
---|
7 | % |
---|
8 | % X2 = s*R*X1 + T |
---|
9 | % s ... scalar scale |
---|
10 | % R ... 3x3 rotation matrix |
---|
11 | % T ... 3x1 translation vector |
---|
12 | % |
---|
13 | % This is done according to the paper: |
---|
14 | % "Least-Squares Fitting of Two 3-D Point Sets" |
---|
15 | % by K.S. Arun, T. S. Huang and S. D. Blostein |
---|
16 | % |
---|
17 | % $Id: estsimt.m,v 2.1 2005/05/23 16:24:59 svoboda Exp $ |
---|
18 | |
---|
19 | % number of points |
---|
20 | N = size(X1,2); |
---|
21 | |
---|
22 | if N ~= size(X2,2) |
---|
23 | error('estsimt: both sets must contain the same number of points') |
---|
24 | end |
---|
25 | |
---|
26 | X1cent = mean(X1,2); |
---|
27 | X2cent = mean(X2,2); |
---|
28 | % normalize coordinate systems for both set of points |
---|
29 | x1 = X1 - repmat(X1cent,1,N); |
---|
30 | x2 = X2 - repmat(X2cent,1,N); |
---|
31 | |
---|
32 | % first estimate the scale s |
---|
33 | % dists1 = sum(sqrt(x1.^2)); |
---|
34 | % dists2 = sum(sqrt(x2.^2)); |
---|
35 | |
---|
36 | % mutual distances; |
---|
37 | d1 = x1(:,2:end)-x1(:,1:(end-1)); |
---|
38 | d2 = x2(:,2:end)-x2(:,1:(end-1)); |
---|
39 | ds1 = sum(d1.^2).^(1/2); |
---|
40 | ds2 = sum(d2.^2).^(1/2); |
---|
41 | |
---|
42 | scales = ds2./ds1; |
---|
43 | |
---|
44 | % the scales should be the same for all points |
---|
45 | % because of noise they are not |
---|
46 | % scales = dists2./dists1 |
---|
47 | s = median(sort(scales)); |
---|
48 | |
---|
49 | % undo scale |
---|
50 | x1s = s*x1; |
---|
51 | |
---|
52 | % finding rotation |
---|
53 | H = zeros(3,3); |
---|
54 | for i=1:N |
---|
55 | H = H+ x1s(:,i)*x2(:,i)'; |
---|
56 | end |
---|
57 | |
---|
58 | [U,S,V] = svd(H,0); |
---|
59 | X = V*U'; |
---|
60 | if det(X) > 0 |
---|
61 | R = X; % estimated rotation matrix |
---|
62 | else |
---|
63 | % V = [V(:,1:2),-V(:,3)]; |
---|
64 | % R = V*U'; |
---|
65 | % s = -s; |
---|
66 | R = X; |
---|
67 | end |
---|
68 | |
---|
69 | T = X2cent - s*R*X1cent; |
---|
70 | |
---|
71 | |
---|