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);
|
---|