% Segundo processo de calibração apresentado no Trucco & Verri
%
% Universidade Federal de Minas Gerais -- UFMG
% Instituto de Ciências Exatas -- ICEx
% Departamento de Ciência da Computação -- DCC
%
% Disciplina: Visão Computacional
% Professor:  Mario Fernando Montenegro Campos
% Monitor:    Vilar Fiuza da Camara Neto


% Pontos de entrada, já aplicada a translação do centro de imagem
Pir = Pi - repmat(o, 1, np)

% Construção da matriz do primeiro sistema de equações (A*m = 0)
[Pc', ones(np, 1)]
[Pc', ones(np, 1), zeros(np, 4)]
-repmat(Pir(1, :)', 1, 4) .* [Pc', ones(np, 1)]
A1 = [Pc', ones(np, 1), zeros(np, 4), ...
    -repmat(Pir(1, :)', 1, 4) .* [Pc', ones(np, 1)]]
A2 = [zeros(np, 4), Pc', ones(np, 1),
    -repmat(Pir(2, :)', 1, 4) .* [Pc', ones(np, 1)]]
A = [A1; A2]

% Aplicação do SVD para resolver o sistema de equações
[Ua, Sa, Va] = svd(A)
% Checagem dos fatores retornados em s1: deve-se recuperar de v1 a coluna
% correspondente ao menor valor da diagonal principal de s1
Sa
m = Va(:, end)

% Reorganização dos elementos de m em forma de matriz
M = reshape(m, 4, 3)'

% Pontos da cena em coordenadas homogêneas: adota-se w=1 para todos os pontos
[Pc; ones(1, np)]
% Projeção dos pontos da cena para pontos de imagem, de acordo com a matriz
% M recuperada
Pih = M * [Pc; ones(1, np)]
% O resultado é em coordenadas homogêneas: transformando em coordenadas
% cartesianas...
Pih(1:2, :) ./ Pih([3, 3], :)
% ... e comparando-as com os pontos originais
ans - Pi

% A matriz M deve ser normalizada de modo que os três primeiros elementos da
% última linha formem um vetor unitário. Em geral isso não é verdade...
M(3, 1:3)
dot(M(3, 1:3), M(3, 1:3))
% ... portanto, é necessário descobrir um fator de normalização...
sqrt(ans)
% ... para ajustar M
M = M ./ ans

% q1, q2 e q3 são definidos conforme Trucco & Verri
q1 = M(1, 1:3)'
q2 = M(2, 1:3)'
q3 = M(3, 1:3)'

% Última linha da matriz de rotação
r3 = M(3, 1:3)
% Translação em z
Tz = M(3, 4)

% Coordenadas do centro de imagem
ox = q1' * q3
oy = q2' * q3
% Distância focal, medida em pixels horizontais e verticais
fx = sqrt(q1' * q1 - ox ^ 2)
fy = sqrt(q2' * q2 - oy ^ 2)
% fx e fy representam a mesma distância em mm, mas como potencialmente o
% pixel não é quadrado, então numericamente fx pode ser diferente de fy. (Se
% fx for igual a fy, então os pixels são quadrados)

% Checagem da distância focal em mm
fx * s(1)
fy * s(1)

% Demais linhas da matriz de rotação
r1 = (ox * q3 - q1)' / fx
r2 = (oy * q3 - q2)' / fy

% Por causa dos ruídos, o sistema [r1;r2;r3] pode não ser ortonormal. Para
% isso, usa-se novamente o SVD...
[Ur, Sr, Vr] = svd([r1; r2; r3])
% ... e recompõe-se a matriz substituindo Sr por uma matriz identidade
Rest = Ur * eye(1) * Vr'

% Demais ordenadas da posição do centro de projeção (Obs.: ao contrário do
% apresentado anteriormente, neste sistema usa-se a translação *após* a rotação
% da câmera)
Tx = (ox * Tz - M(1, 4)) / fx
Ty = (oy * Tz - M(2, 4)) / fy

% Translação estimada da câmera (após a rotação)
Test = [Tx; Ty; Tz]

% Translação da câmera antes da rotação, para seguir a convenção exposta
% inicialmente
Pir(1, 1) * (dot(Rest(1, :), Pc(:, 1)') + Tx)

% Se for necessário ajustar os sinais, então ajusta-se
if ans < 0
   Rest(1:2, :) = -Rest(1:2, :)
   Test(1:2) = -Test(1:2)
end

% Translação da câmera antes da rotação, para seguir a convenção exposta
% inicialmente (usa-se aqui a propriedade de que a inversa de Rest é igual à
% sua transposta)
-Rest' * Test
