[37] | 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 | |
---|