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