1 | function [H, Sa] = linEstH3D(left, right, NUM_RESCALE) |
---|
2 | % [H, Sa] = linEstH3D(left, right, NUM_RESCALE) |
---|
3 | % Estimate the 3D homography matrix H from two 4 x n matrices of |
---|
4 | % corresponding points left and right. |
---|
5 | % Here alpha(k) * left(:,k) - (H * right(:,k)) apprx= 0 |
---|
6 | % NUM_RESCALE (default TRUE) uses Hartley's rescaling. Always use |
---|
7 | % rescaling, unless you wish to show how badly the un-normalized |
---|
8 | % algorithm works. |
---|
9 | % Returns H along with the singular values Sa of the 2nPts x 9 homogeneous |
---|
10 | % linear system for H. |
---|
11 | |
---|
12 | if nargin < 3 |
---|
13 | NUM_RESCALE = 1; |
---|
14 | end |
---|
15 | |
---|
16 | nPts = size(left,2); |
---|
17 | if nPts < 5 | nPts ~= size(right,2) |
---|
18 | fprintf(2,'lineEstH: Innappropriate number of left and right points.'); |
---|
19 | H = []; |
---|
20 | return; |
---|
21 | end |
---|
22 | |
---|
23 | if size(left,1) == 3 |
---|
24 | left = [left; ones(1, nPts)]; |
---|
25 | else % Normalize to pixel coords |
---|
26 | left = left./repmat(left(4,:), 4,1); |
---|
27 | end |
---|
28 | if size(right,1) == 3 |
---|
29 | right = [right; ones(1, nPts)]; |
---|
30 | else % Normalize to pixel coords |
---|
31 | right = right./repmat(right(4,:), 4,1); |
---|
32 | end |
---|
33 | |
---|
34 | imPts = cat(3, left, right); |
---|
35 | |
---|
36 | %% Rescale image data for numerical stability. |
---|
37 | if NUM_RESCALE |
---|
38 | Knum = repmat(eye(4), [1,1,2]); |
---|
39 | %%% Rescale for numerical stability |
---|
40 | mn = sum(imPts(1:3,:,:),2)/nPts; |
---|
41 | mns = reshape(mn, [3 1 2]); |
---|
42 | var = sum(sum((imPts(1:3,:,:)-repmat(mns, [1 nPts 1])).^2,2)/nPts, 1); |
---|
43 | %% Scale image points so that sum of variances of x and y = 2. |
---|
44 | scl = sqrt(3./var(:)); |
---|
45 | %% Sanity: varScl = var .* reshape(scl.^2, [1 1 2]) % Should be 3 |
---|
46 | |
---|
47 | %% Scale so x and y variance is roughly 1, translate so image mean (x,y) is zero. |
---|
48 | Knum(1:3,4,:) = -mn; |
---|
49 | Knum(1:3,:,:) = Knum(1:3,:,:).*repmat(reshape(scl, [1 1 2]), [3, 4,1]); |
---|
50 | for kIm = 1:2 |
---|
51 | imPts(:,:,kIm) = reshape(Knum(:,:,kIm),4,4) * imPts(:,:,kIm); |
---|
52 | end |
---|
53 | %% Sanity check |
---|
54 | % sum(imPts(1:3,:,:),2)/nPts % Should be [0 0]' |
---|
55 | % sum(sum(imPts(1:3,:,:).^2,2)/nPts,1) % Should be 3. |
---|
56 | end |
---|
57 | |
---|
58 | %% Make constraint matrix A. |
---|
59 | %% The matrix H satisfies: A h = 0, where h = (H_1,1; H_1,2; ... H_4,4). |
---|
60 | left = reshape(imPts(:,:,1), [4 nPts]); |
---|
61 | right = reshape(imPts(:,:,2), [4 nPts]); |
---|
62 | A = []; |
---|
63 | Id = eye(4); |
---|
64 | for k = 1:nPts |
---|
65 | [mx ix] = max(abs(imPts(:,k,1))); |
---|
66 | eix = Id(:,ix); |
---|
67 | C = kron(-left(ix,k)* eye(4), right(:,k)'); |
---|
68 | C = C + kron(left(:,k)*eix', right(:,k)'); |
---|
69 | %% Delete row ix |
---|
70 | Q = []; |
---|
71 | if ix > 1 |
---|
72 | Q = C(1:(ix-1),:); |
---|
73 | end |
---|
74 | if ix<4 |
---|
75 | Q = [Q; C((ix+1):4,:)]; |
---|
76 | end |
---|
77 | A = [A; Q]; |
---|
78 | end |
---|
79 | |
---|
80 | %% Factor A |
---|
81 | [Ua Sa Va] = svd(A); Sa = diag(Sa); |
---|
82 | |
---|
83 | %% Set H to be the right null vector of A, reshaped to a 3x3 matrix. |
---|
84 | H = reshape(Va(:,end), 4,4)'; |
---|
85 | |
---|
86 | %% Undo the renormalization |
---|
87 | if NUM_RESCALE |
---|
88 | H = inv(reshape(Knum(:,:,1),4,4)) * H * reshape(Knum(:,:,2),4,4); |
---|
89 | end |
---|
90 | |
---|
91 | %% Modify H to make it norm 1. |
---|
92 | H = H / norm(H(:)); |
---|
93 | |
---|
94 | |
---|