[37] | 1 | function [H] = linEstAff3D(left, right, NUM_RESCALE) |
---|
| 2 | % [H, Sa] = linEstAff3D(left, right, NUM_RESCALE) |
---|
| 3 | % Estimate the 4x4 affine matrix H from two 4 x n matrices of |
---|
| 4 | % corresponding points left and right. |
---|
| 5 | % Here 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. |
---|
| 10 | |
---|
| 11 | if nargin < 3 |
---|
| 12 | NUM_RESCALE = 1; |
---|
| 13 | end |
---|
| 14 | |
---|
| 15 | nPts = size(left,2); |
---|
| 16 | if nPts < 5 | nPts ~= size(right,2) |
---|
| 17 | fprintf(2,'lineEstAff3D: Innappropriate number of left and right points.'); |
---|
| 18 | H = []; |
---|
| 19 | return; |
---|
| 20 | end |
---|
| 21 | |
---|
| 22 | if size(left,1) == 3 |
---|
| 23 | left = [left; ones(1, nPts)]; |
---|
| 24 | else % Normalize to pixel coords |
---|
| 25 | left = left./repmat(left(4,:), 4,1); |
---|
| 26 | end |
---|
| 27 | if size(right,1) == 3 |
---|
| 28 | right = [right; ones(1, nPts)]; |
---|
| 29 | else % Normalize to pixel coords |
---|
| 30 | right = right./repmat(right(4,:), 4,1); |
---|
| 31 | end |
---|
| 32 | |
---|
| 33 | imPts = cat(3, left, right); |
---|
| 34 | |
---|
| 35 | %% Rescale image data for numerical stability. |
---|
| 36 | if NUM_RESCALE |
---|
| 37 | Knum = repmat(eye(4), [1,1,2]); |
---|
| 38 | %%% Rescale for numerical stability |
---|
| 39 | mn = sum(imPts(1:3,:,:),2)/nPts; |
---|
| 40 | mns = reshape(mn, [3 1 2]); |
---|
| 41 | var = sum(sum((imPts(1:3,:,:)-repmat(mns, [1 nPts 1])).^2,2)/nPts, 1); |
---|
| 42 | %% Scale image points so that sum of variances of x and y = 2. |
---|
| 43 | scl = sqrt(3./var(:)); |
---|
| 44 | %% Sanity: varScl = var .* reshape(scl.^2, [1 1 2]) % Should be 3 |
---|
| 45 | |
---|
| 46 | %% Scale so x and y variance is roughly 1, translate so image mean (x,y) is zero. |
---|
| 47 | Knum(1:3,4,:) = -mn; |
---|
| 48 | Knum(1:3,:,:) = Knum(1:3,:,:).*repmat(reshape(scl, [1 1 2]), [3, 4,1]); |
---|
| 49 | for kIm = 1:2 |
---|
| 50 | imPts(:,:,kIm) = reshape(Knum(:,:,kIm),4,4) * imPts(:,:,kIm); |
---|
| 51 | end |
---|
| 52 | %% Sanity check |
---|
| 53 | % sum(imPts(1:3,:,:),2)/nPts % Should be [0 0 0]' |
---|
| 54 | % sum(sum(imPts(1:3,:,:).^2,2)/nPts,1) % Should be 3. |
---|
| 55 | end |
---|
| 56 | |
---|
| 57 | %% Make constraint matrix A. |
---|
| 58 | %% The matrix H satisfies: A h = 0, where h = (H_1,1; H_1,2; ... H_4,4). |
---|
| 59 | left = reshape(imPts(:,:,1), [4 nPts]); |
---|
| 60 | right = reshape(imPts(:,:,2), [4 nPts]); |
---|
| 61 | A = (left(1:3, :) * right') * inv(right * right'); |
---|
| 62 | |
---|
| 63 | %% Set H |
---|
| 64 | H = [A; zeros(1,3) 1]; |
---|
| 65 | |
---|
| 66 | %% Undo the renormalization |
---|
| 67 | if NUM_RESCALE |
---|
| 68 | H = inv(reshape(Knum(:,:,1),4,4)) * H * reshape(Knum(:,:,2),4,4); |
---|
| 69 | end |
---|