카메라 3대에서 다음과 같은 특징점(frame_data.txt)을 추출하였다면
cam1 cam2 cam3
191 426 243 442 263 453
422 457 467 419 430 394
183 363 233 376 263 388
422 388 469 355 434 338
173 292 228 305 259 317
422 311 473 284 440 275
161 207 220 213 261 234
422 215 477 198 447 205
148 117 214 127 263 150
422 117 479 113 451 136
136 27 206 33 263 63
422 13 484 19 457 63
Matlab 코드는 아래와 같다.
-----------------------------------------------------------------------
load frame_data.txt
fcam1 = [frame_data(:,1) frame_data(:,2) ones(length(frame_data),1)]';
fcam2 = [frame_data(:,3) frame_data(:,4) ones(length(frame_data),1)]';
fcam3 = [frame_data(:,5) frame_data(:,6) ones(length(frame_data),1)]';
Homo12 = HomographyDLT(fcam2, fcam1);
Homo13 = HomographyDLT(fcam3, fcam1);
위과 같이 호모그래피를 계산하는 함수를 이용하면 된다.
호모그래피 계산 함수는 아래와 같다. Marco Zuliani 라는 분이 만든 코드로 좌표에 대한 정규화까지 수행하여 호모그래피를 계산한다. 조금더 정확한 결과를 얻고자 한다면 최적화까지 수행하여야 한다.
function [H, A] = HomographyDLT(X1, X2, mode, normalization)
% [H A] = HomographyDLT(X1, X2, mode)
%
% DESC:
% computes the homography between the point pairs X1, X2
%
% AUTHOR
% Marco Zuliani - zuliani@ece.ucsb.edu
%
% VERSION:
% 1.0.0
%
% INPUT:
% X1, X2 = point matches (cartesian coordinates)
% mode = 0 -> Hartley Zisserman formulation
% 1 -> Zuliani formulation (default)
% normalization = true (default) or false to enable/disable point
% normalzation
%
% OUTPUT:
% H = homography
% A = homogenous linear system matrix
if (nargin < 3)
mode = 'MZ';
end;
if (nargin < 4)
normalization = true;
end;
N = size(X1, 2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% checks
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (size(X2, 2) ~= N)
error('The set of input points should have the same cardinality')
end;
if N < 4
error('At least 4 point correspondences are needed')
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% normalize the input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if normalization
% fprintf('\nNormalizing...')
[X1, T1] = normalize_points(X1);
[X2, T2] = normalize_points(X2);
end;
% compute h
switch mode
case 'HZ'
A = get_A_HZ(X1, X2);
case 'MZ'
A = get_A_MZ(X1, X2);
end;
[U S V] = svd(A);
h = V(:, 9);
% reshape the output
switch mode
case 'HZ'
H = [h(1:3)'; h(4:6)'; h(7:9)'];
case 'MZ'
H = reshape(h, 3, 3);
end;
% and denormalize
if normalization
H = inv(T2)*H*T1;
end;
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matrix construction routine
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Hartley Zisserman formulation
function A = get_A_HZ(X1, X2)
X1 = cart2homo(X1);
X2 = cart2homo(X2);
N = size(X1, 2);
A = zeros(2*N, 9);
zero = [0; 0; 0];
row = 1;
for h = 1:N
a = X2(3,h)*X1(:,h)';
b = X2(2,h)*X1(:,h)';
c = X2(1,h)*X1(:,h)';
A(row, :) = [zero' -a b];
A(row+1, :) = [a zero' -c];
row = row + 2;
end;
% Zuliani's formulation
function A = get_A_MZ(X1, X2)
N = size(X1, 2);
A = zeros(2*N, 9);
row = 1;
for h = 1:N
A(row, :) = [X1(1,h) 0 -X1(1,h)*X2(1,h) X1(2,h) 0 -X1(2,h)*X2(1,h) 1 0 -X2(1,h)];
A(row+1, :) = [0 X1(1,h) -X1(1,h)*X2(2,h) 0 X1(2,h) -X1(2,h)*X2(2,h) 0 1 -X2(2,h)];
row = row + 2;
end;
return
데이터 정규화를 위한 코드(normalize_points.m)는 아래와 같다.
function [xn, T] = normalize_points(x)
% [xn, T] = normalize_points(x)
%
% DESC:
% normalize a set of points using the procedure described in the book by
% Hartley and Zisserman
%
% AUTHOR
% Marco Zuliani - zuliani@ece.ucsb.edu
%
% VERSION:
% 1.0.0
%
% INPUT:
% x = points to be normalized
%
% OUTPUT:
% Xn = normalized set of points
% T = transformation matrix such that Xn = T * X
% HISTORY
% 1.0.0 - ??/??/05 - Initial version
% compute the translation
x_bar = mean(x, 2);
% center the points
% faster than xc = x - repmat(x_bar, 1, size(x, 2));
xc(1, :) = x(1, :) - x_bar(1);
xc(2, :) = x(2, :) - x_bar(2);
% compute the average point distance
rho = sqrt(sum(xc.^2, 1));
rho_bar = mean(rho);
% compute the scale factor
s = sqrt(2)/rho_bar;
% scale the points
xn = s*xc;
% compute the transformation matrix
T = [s 0 -s*x_bar(1); 0 s -s*x_bar(2); 0 0 1];
return
