[37] | 1 | function H = vgg_Haffine_from_x_MLE(xs1,xs2)
|
---|
| 2 | % H = vgg_Haffine_from_x_MLE(xs1,xs2)
|
---|
| 3 | %
|
---|
| 4 | % Compute MLE for affine H, i.e. find H and xhat1 such that
|
---|
| 5 | % d^2(xs1,xhat1) + d^2(xs2,xhat2) minimized where xhat2 is affine transf of xhat1.
|
---|
| 6 | %
|
---|
| 7 | % Parameters:
|
---|
| 8 | % xs1, xs2 ... double(3,N), N pairs of corresponding points (homogeneous)
|
---|
| 9 | % H ... double(3,3), affine transformation
|
---|
| 10 | %
|
---|
| 11 | % See HZ page 115 1st edition, page 130 2nd edition
|
---|
| 12 | % az 17/11/2001
|
---|
| 13 |
|
---|
| 14 | if any(size(xs1) ~= size(xs2))
|
---|
| 15 | error ('Input point sets are different sizes!')
|
---|
| 16 | end
|
---|
| 17 |
|
---|
| 18 | % condition points
|
---|
| 19 |
|
---|
| 20 | nonhomg = vgg_get_nonhomg(xs1);
|
---|
| 21 | means = mean(nonhomg');
|
---|
| 22 | maxstds = max(std(nonhomg'));
|
---|
| 23 | C1 = diag([1/maxstds 1/maxstds 1]); % only similarity
|
---|
| 24 | C1(:,3) = [-means/maxstds 1]';
|
---|
| 25 |
|
---|
| 26 | nonhomg = vgg_get_nonhomg(xs2);
|
---|
| 27 | means = mean(nonhomg');
|
---|
| 28 | C2 = C1; % nb must use same scaling for both point sets
|
---|
| 29 | C2(:,3) = [-means/maxstds 1]';
|
---|
| 30 |
|
---|
| 31 | xs1 = vgg_condition_2d(xs1,C1);
|
---|
| 32 | xs2 = vgg_condition_2d(xs2,C2);
|
---|
| 33 |
|
---|
| 34 | % NB conditioned points have mean zero, so translation
|
---|
| 35 | % part of affine transf is zero 2-vector
|
---|
| 36 |
|
---|
| 37 | xs1nh = vgg_get_nonhomg(xs1);
|
---|
| 38 | xs2nh = vgg_get_nonhomg(xs2);
|
---|
| 39 |
|
---|
| 40 | A = [xs1nh;xs2nh]';
|
---|
| 41 |
|
---|
| 42 | % Extract nullspace
|
---|
| 43 | [u,s,v] = svd(A);
|
---|
| 44 | s = diag(s);
|
---|
| 45 |
|
---|
| 46 | nullspace_dimension = sum(s < eps * s(2) * 1e3);
|
---|
| 47 | if nullspace_dimension > 2
|
---|
| 48 | fprintf('Nullspace is a bit roomy...');
|
---|
| 49 | end
|
---|
| 50 |
|
---|
| 51 | % compute affine matrix from two largest singular vecs
|
---|
| 52 |
|
---|
| 53 | B = v(1:2,1:2);
|
---|
| 54 | C = v(3:4,1:2);
|
---|
| 55 |
|
---|
| 56 | H = [ C * pinv(B) , zeros(2,1); 0 0 1];
|
---|
| 57 |
|
---|
| 58 | %decondition
|
---|
| 59 | H = inv(C2) * H * C1;
|
---|
| 60 |
|
---|
| 61 | H = H/H(3,3);
|
---|