% Projeção de pontos, usando coordenadas cartesianas
%
% 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


% Características da câmera:
% CCD de 640 × 480 pixels (essa informação não é usada no processo de
% calibração)

% Centro de imagem em (320, 240)
o = [320; 240]

% CCD de 1/3" com pixels quadrados:
% - Diagonal do CCD, medida em pixels
sqrt(sum(o.^2))
% - Isso é o número de pixels em 1/3 de polegada.  Em 1 polegada, temos:
ans * 3      % [pixels/pol]
% - Convertendo em mm:
ans / 25.4   % [pixels/mm]
% - Obtendo o tamanho dos pixels (na verdade, a distância entre os centros dos
%   pixels):
1 / ans      % [mm/pixel]
% - Armazenando o tamanho horizontal e vertical dos pixels
s = [ans; ans]

% Distância focal f = 30mm
f = 30

% Conjunto de pontos da cena que será usado no exemplo. Os pontos
% correspondem aos vértices de uma cunha e aos extremos de um "T" impresso
% em uma das faces da cunha
Pc = [0,0,0;
    0,50,0;
    50,0,0;
    50,50,0;
    50,0,40;
    50,50,40;
    10,25,8;
    40,25,32;
    40,10,32;
    40,40,32]'

% Para o processo de calibração, apenas as coordenadas de Pc são relevantes,
% não a forma geométrica em si. Mas, para visualizarmos o modelo de entrada
% e os resultados, é interessante termos os índices dos pontos que são
% ligados para formar os segmentos da geometria da cena:
v = [1,2;
    1,3;
    2,4;
    3,4;
    1,5;
    2,6;
    3,5;
    4,6;
    5,6;
    7,8;
    9,10]

% Plotando a figura geométrica em 3D
figure
hold on
for i = 1:size(v, 1)
    p1 = v(i, 1);
    p2 = v(i, 2);
    plot3([Pc(1,p1), Pc(1,p2)], ...
        [Pc(2,p1), Pc(2,p2)], ...
        [Pc(3,p1), Pc(3,p2)], ...
        'b-*');
end
hold off
axis equal
% Após a plotagem, você terá uma vista superior do objeto. Use a ferramenta
% de rotação do Matlab (menu Tools > Rotate 3D) sobre a figura para
% visualizar a cena de vários ângulos.

% Número de pontos em Pc
np = size(Pc, 2)

% Câmera posicionada em (-700, -100, 200),
% rotacionada para a esquerda em 15° e para baixo em 10°:
% - Translação da origem do sistema de coordenadas para o centro de projeção
%   da câmera
T = [-700, -100, 200]'
% - Primeira rotação (em torno do eixo y global)
R1 = [cosd(90), 0, sind(90); 0, 1, 0; -sind(90), 0, cosd(90)]
% - Segunda rotação (em torno do eixo x global)
R2 = [1, 0, 0; 0, cosd(-90), -sind(-90); 0, sind(-90), cosd(-90)]
% - Terceira rotação (em torno do eixo y global)
R3 = [cosd(15), 0, sind(15); 0, 1, 0; -sind(15), 0, cosd(15)]
% - Quarta rotação (em torno do eixo z global)
R4 = [cosd(10), -sind(10), 0; sind(10), cosd(10), 0; 0, 0, 1]
% - Combinação das rotações em uma única matriz de rotação
%   (note que esta matriz é ortonormal)
R = R4 * R3 * R2 * R1

% Seqüência de transformações para projetar pontos 3D em pontos 2D:
% - translação 3D (posição da câmera)
P = Pc + repmat(-T, 1, np)
% - rotação 3D (orientação da câmera)
P = inv(R) * P
% - projeção (3D -> 2D)
P = P(1:2, :) .* repmat(f ./ P(3, :), 2, 1)
% - escala (tamanho dos pixels)
P = P ./ repmat(s, 1, np)
% - translação 2D (coordenadas do centro da imagem)
Pi = P + repmat(o, 1, np)

% Pi contém as coordenadas 2D, sem ruídos, das projeções dos pontos de cena Pc

% Plotando a imagem 2D gerada pela projeção da figura geométrica
figure
hold on
for i = 1:size(v, 1)
  p1 = v(i, 1);
  p2 = v(i, 2);
  plot([Pi(1, p1), Pi(1, p2)], [Pi(2, p1), Pi(2, p2)], 'b-*');
end
hold off

% Ajustando orientação dos eixos, proporção das escalas e limites da imagem
axis ij
axis equal
axis([0, 639, 0, 479])
