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