% Primeiro 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*v = 0)
[Pir([1, 1, 1, 1], :)', -Pir([2, 2, 2, 2], :)']
[Pc', ones(np, 1), Pc', ones(np, 1)]
A = [Pir([1, 1, 1, 1], :)', -Pir([2, 2, 2, 2], :)'] .* ...
   [Pc', ones(np, 1), Pc', ones(np, 1)]

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

% Cálculo do módulo do fator gamma
gamma = sqrt(sum(V(1:3) .^ 2))
r2 = V(1:3)' / gamma
dot(r2, r2)

% Cálculo do fator alpha
alpha = sqrt(sum((V(5:7) / gamma) .^ 2))
r1 = V(5:7)' / gamma / alpha
dot(r1, r1)

% Terceiro vetor (ortogonal a r1 e r2)
r3 = cross(r1, r2)

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

% 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)
Ty = V(4) / gamma
Tx = V(8) / gamma / alpha

% Checagem do sinal
Pir(1, 1) * (dot(r1, Pc(:, 1)) + Tx)

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

% Recomposição de r1, r2 e r3
r1 = Rest(1, :)
r2 = Rest(2, :)
r3 = Rest(3, :)

% Novo sistema de equações: A*x = b
Pir(1, :)'
dot(repmat(r1', 1, np), Pc)' + Tx
A = [Pir(1, :)',  -(dot(repmat(r1', 1, np), Pc)' + Tx)]

dot(repmat(r3', 1, np), Pc)'
b = (-Pir(1, :)') .* dot(repmat(r3', 1, np), Pc)'

% Para solucionar, Trucco & Verri sugerem...
inv(A' * A) * A' * b
% ... mas o Matlab fornece uma maneira numericamente mais estável:
temp = A \ b

% Recuperação dos parâmetros restantes
Tz = temp(1)
fx = temp(2)

% Translação estimada da câmera (após a rotação)
[Tx, Ty, Tz]'
% Translação da câmera antes da rotação, para seguir a convenção exposta
% inicialmente
Test = -inv(Rest) * [Tx, Ty, Tz]'
% Comparação (diferença) entre a translação estimada e a original (deve ser
% próxima de zero)
T - Test
% fx é a distância focal medida em pixels horizontais. Multiplicando-se pelo
% tamanho horizontal do pixel, temos a distância focal em mm
fx * s(1)
% Comparação (diferença) com a distância focal original (deve ser próxima de
% zero)
ans - f
