% Activate the following line by filling in the right side. If for example the name of the root file is Main.tex, write
% "...root = Main.tex" if the chapter file is in the same directory, and "...root = ../Main.tex" if the chapter is in a subdirectory.
 
%!TEX root =  vision.tex

\chapter[Calibracion]{Calibración de Camáras}

En esta sección daremos el modelo de una cámara y la manera de calcular el m{ó}delo de la misma. El modelo esta dise{\~ n}ando principalmente para sensores como CCD pero también es aplicable a otras cámaras.

\section{El modelo de cámara pinhole}

Consideremos el punto central de proyección del espacio en un plano. Centraremos la proyección del espacio Euclidiano de coordenadas y consideraremos que el plano es \(Z=f\), el cual es llamado el plano focal de la cámara. Bajo el modelo de camara pinhole, un punto de coordenadas en el espacio \(X=[X,Y,Z]^T\) es mapeado en el punto del plano de la imagen donde las lineas se unen en el centro de proyeción. Esto puede mostrarse por triángulos similares en las figuras \ref{fig:cal1} y \ref{fig:cal2}.\\

\begin{figure}[h!]
\begin{center} 
\subfigure[Tres dimensiones \label{fig:cal1}]{\psfig{figure=figuras/calibra_camara_gr1.eps,width=5.0cm}} 
\hfill
\subfigure[dos dimensiones \label{fig:cal2}]{\psfig{figure=figuras/calibra_camara_gr2.eps,width=5.0cm}} 
\end{center}
\caption{Modelo de la cámara de pinhole}
\end{figure}

En la figura, por triangulos similares podemos ver que el punto \([X, Y, Z]^T\) es mapeado a un punto en el plano de la cámara como \([fX/Z, f Y/Z, f]^T\). Esto lo podems escribir de forma matricial como\\
\\
\(\left(
\begin{array}{c}
 f X \\
 f Y \\
 Z
\end{array}
\right)=\left(
\begin{array}{cccc}
 f &    &   & 0 \\
   & f  &   & 0 \\
   &    & 1 & 0
\end{array}
\right)\)\(\left(
\begin{array}{c}
 X \\
 Y \\
 Z \\
 1
\end{array}
\right)\)\\
\\
esta matriz puede ser escrita como \(\text{diag}(f,f,1)[I|0]\) as{\' \i} la matriz de proyecci{ó}n para una cámara con modelo pinhole es \(P=\text{diag}(f,f,1)[I|0].\) La expresi{ó}n anterior asume que el origen de coordenadas en el plano de la imagen es el punto principal. En la práctica no es as{í} por lo cual debemos aplicar un mapeo\\
\\
\((X,Y,Z)^T|\rightarrow \left(f X/Z+ p_x, f X/Z + p_y\right){}^T\) \\
\\
esta ecuaci{ó}n puede expresarse como\\
\\
\(\left(
\begin{array}{c}
 f X+ Z p_x \\
 f Y+ Z p_y \\
 Z
\end{array}
\right)=\left(
\begin{array}{cccc}
 f &    & p_x & 0 \\
   &  f & p_y & 0 \\
   &    & 1 & 0
\end{array}
\right)\)\(\left(
\begin{array}{c}
 X \\
 Y \\
 Z \\
 1
\end{array}
\right)\)\\
donde \\
\\
\(K=\left(
\begin{array}{ccc}
 f &    & p_x \\
   & f & p_y \\
   &   & 1
\end{array}
\right)\)\\
\\
y \(x=K[I|0]X_{\text{cam}} \) donde la matriz \(K\) es llamada la matriz de calibración de la cámara.\\
\\
En general puntos en el espacio son expresados en términos de coordenadas euclidianas diferentes, conocidas como marco de coordenadas del mundo. Estos dos marco son relacionados por una rotaci{ó}n y una translaci{ó}n. Si \(\tilde{X }\)son las coordenadas homogeneas del mundo estas pueden ser representadas en el marco de la camara como como \(\tilde{X}_{\text{cam}}=R\left(\tilde{X}-\tilde{C}\right)\), donde \(\tilde{C}\) representa las coordenadas de la cámara centradas en las coordenadas del mundo. El modelo puede ser escrito en coordenadas homogenas como\\
\\
\(x_{\text{cam}}=\left(
\begin{array}{cc}
 R & -R \tilde{C} \\
 0 & 1
\end{array}
\right)\left(
\begin{array}{c}
 X \\
 Y \\
 Z \\
 1
\end{array}
\right)\) \\
\\
Poniendo todo junto tenemos { }\(x=K R\left[I\left|-\tilde{C}\right.\right]X\) donde la matriz de la cámara es \(P=K[R|t]\) donde \(t=-R \tilde{C}\)

\section {Cámaras CCD}

El modelo de cámara pinhole asume que las coordenadas de la imagen tienen igual escala en ambos ejes. En el caso de la cámaras CCD existe
una posibilidad adicional de no tener pixeles cuadrados. Por lo tanto manejaremos en lugar de usar un factor de escala \(f\) en la diagonal consideraremos
que existe un n{\' u}mero de pixeles en la direción \(x\) llamado \(m_x\) y uno en la direción \(y\) llamado \(m_y\) por lo tanto nuestra
matriz de parámetros queda como \\
\\
\(K=\left(
\begin{array}{ccc}
 f m_x &         & x_0 \\
          & f m_y & y_0 \\
          &          & 1
\end{array}
\right)=\left(
\begin{array}{ccc}
 \alpha _x &   & x_0 \\
   & \alpha _y & y_0 \\
   &   & 1
\end{array}
\right)\)\\
\\
Podemos consideran un modelo mas general si agregamos un parametro  \(s\) al que denominaremos sesgo. En la mayor{í}a de la cámaras normales este valor es cero, sin embargo lo consideraremos en el sentido de tener un modelo más general.\\
\\
\(K=\left(
\begin{array}{ccc}
 \alpha _x & s & x_0 \\
    & \alpha _y & y_0 \\
   &    & 1
\end{array}
\right)\)\\
\\
El modelo de la cámara proyectiva es una matriz no cuadrada con tres renglones y cuatro columnas, por lo tanto el modelo tendrá 11 grados
de libertad. Este modelo lo podemos escribir como \(P=M\left[I\left|M^{-1}p_4\right.\right]=K R\left[I\left|-\tilde{C}\right.\right] \)donde \(p_4\)
{ }es la {\' u}ltima columna de la matriz P. El modelo completo es conocido como el módelo de la cámara proyetiva.

\section{Propiedades de la cámara Proyectiva}

Algunas propiedades de la cámara proyectiva son

\begin{itemize}
\item Centro de la camara. El centro de la cámara es el vector C el cual puede ser calculado para una cámara finita como el vector en el espacio
nulo es decir PC=0. Esto se resuelve utilizando descomposición en Valores singulares y \\
\item Puntos columna. Para i =1,2,3 los vectores columna \(p_i\)son los puntos de fuga en la imagen correpondientes a los ejes X, Y y Z. Columna \(p_4\)
es la imagen de las coordenadas del origen.\\
\item Plano principal. El plano principal de la cámara is \(p^3\), el {\' u}ltimo renglón de P.\\
\item  Ejes del plano. Los planos \(p^1\)y \(p^2\) representan los planos a través del centro de la cámara.\\
\item Punto principal. Punto de imagen \(x_0=M m^3\)es el punto principal de la cámar, donde \(m^{3T}\) es el tercer renglón de la matriz
M.\\
\item Rayo principal. El rayo principal de la camara es el rayo que pasa a través de la cámara con centro C con dirección { }\(m^{3T}\).
El eje principal es el vector \(v=\det (M)m^3 \)es la dirección hacia el frente de la cámara.
\end{itemize}

\section {Calibración de Cámaras a partir de puntos correspondientes}

La calibración de cámaras la definiremos como el proceso de calcular los parámetros internos a partir de la observación de una imagen y sus puntos correspondiente en el mundo real. Comenzaremos por dada la matriz de proyección, calcular la matriz de parametros internos K, el centro de la cámara C y la matriz de rotación de la cámara \(P=K R[I|C]\). 
\subsection{Centro de la Cámara}
Podemos ver de lo expuesto en la sección anterior que el centro de la cámara dado la matriz de proyección se puede calcular como \(P C = 0\) cuya solución es el vector con el menor eigenvalor una vez calculado la descomposici{ó}n en valores singulares (espacio nulo). 
\subsection{Descomposición RQ}
Definamos la matriz \(M=K R\) la cual es una matrix de 3 renglones por 3 columnas. Para ello tendremos que hacer una factorización matricial.
{ }Recordemos que { }la matriz R puede calcularse como la multiplicación de tres matrices de rotación en cada eje como \(R = R_z^TR_y^T
R_x^T\text{  }\)dado que todas estas matrices son ortogonales, entonces podemos escibir\\
\\
\(M=K R = K R_z^TR_y^T R_x^T \)\\
\( M R_xR_yR_z= K \)\\
\\
Las matrices de rotación estan definidas como\\
\\
\(R_x=\left(
\begin{array}{ccc}
 1 & 0 & 0 \\
 0 & c_x & -s_x \\
 0 & s_x & c_x
\end{array}
\right), R_y=\left(
\begin{array}{ccc}
 c_y & 0 & s_y \\
 0 & 1 & 0 \\
 -s_y & 0 & c_y
\end{array}
\right)\text{  }y\text{  }R_x=\left(
\begin{array}{ccc}
 c_y & -s_z & 0 \\
 s_z & 1 & 0 \\
 0 & 0 & c_y
\end{array}
\right) \) { }\\
\\
donde \(c_i= \cos \left(\theta _i\right)\) y \(s_i= \text{seno}\left(\theta _i\right)\) \\
\\
Dada la matriz M Los pasos son\\
\\
1. Multiplicar \(M' = M R_x\) y calcular \(c_x\) y \(s_{x }\)para que el elemento \(M'_{3,2} =0\) y \(c_x^2+s_x^2 = 1\). La solución es\\
\\
\(c_x=\frac{M_{3,3}}{\sqrt{M_{3,2}^2+M_{3,3}^2}}y s_x=-\frac{M_{3,2}}{\sqrt{M_{3,2}^2+M_{3,3}^2}} \)\\
2. Multiplicar \(M\text{''} = M' R_y\) y calcular \(c_y\) y \(s_{y }\)para que el elemento \(M\text{''}_{3,1} =0\) y \(c_y^2+s_y^2 = 1\). La soluci{\'
o}n es\\
\\
\(c_y=\frac{M_{3,3}}{\sqrt{M_{3,1}^2+M_{3,3}^2}}y s_y=-\frac{M_{3,1}}{\sqrt{M_{3,1}^2+M_{3,3}^2}} \)\\
3. Finalmente multiplicar \(K = M"R_z\) y calcular \(c_z\) y \(s_{z }\)para que el elemento \(M\text{''}_{2,1} =0\) y \(c_z^2+s_z^2 = 1\). La soluci{\'
o}n es\\
\\
\(c_z=-\frac{M_{2,2}}{\sqrt{M_{2,1}^2+M_{2,2}^2}}y s_z=\frac{M_{2,1}}{\sqrt{M_{2,1}^2+M_{2,2}^2}} \)\\
\\
ver calibra$\_$camaras.java

\subsection{Ejemplo}

Dada la matriz de proyección de la cámara

\[P=\left(
\begin{array}{cccc}
 353.553 & 339.645 & 277.744 & -1.44946\times 10^6 \\
 -103.528 & 23.3212 & 459.607 & -632525. \\
 0.707107 & -0.353553 & 0.612372 & -918.559
\end{array}
\right)\]
determinar el centro de la cámara y la matriz de parámetros internos.

La matriz $M$ de acuerdo con la definición es:

\(M=\left(
\begin{array}{ccc}
 353.553 & -103.528 & 0.707107 \\
 339.645 & 23.3212 & -0.353553 \\
 277.744 & 459.607 & 0.612372
\end{array}
\right)\)

Para encontrar el centro de la cámara hacemos la descomposición en Valores singulares de la matriz $[u, s, v] = svd(P)$ donde 

\(u=\left(
\begin{array}{ccc}
 -0.916531 & -0.399962 & -0.000424686 \\
 -0.399962 & 0.916532 & -0.000479022 \\
 -0.000580829 & 0.00026918 & 1.
\end{array}
\right)\)

\(s=\left(
\begin{array}{cccc}
 1.58146\times 10^6 & 0. & 0. & 0. \\
 0. & 406.369 & 0. & 0. \\
 0. & 0. & 0.837954 & 0.
\end{array}
\right)\)

\noindent\(v=\left(
\begin{array}{cccc}
 -0.000178718 & -0.581477 & 0.723847 & 0.371391 \\
 -0.000202738 & -0.281691 & -0.607393 & 0.742782 \\
 -0.000277203 & 0.76324 & 0.327293 & 0.557086 \\
 1. & 0.0000505429 & 0.0000969494 & 0.00037139
\end{array}
\right)\)

El vector $v_4$ es el que tiene el menor eigenvalue así que dividiendo este entre $v_{4,4}$ tenemos que el centro de la camára es \(C=\{1000.,2000.,1500.,1.\}\)

De la descomposición QR tenemos que la matriz $M$ puede representarse como:
\[
K \times R = 
\left(
\begin{array}{ccc}
468.1648 & 91.2251 & 300.0001 \\
0.0 & 427.201 & 199.9999 \\
0.0 & 0.0 & 1.0
\end{array}
\right) 
\times
\left(
\begin{array}{ccc}
0.4138 & 0.9091 & 0.0471 \\
-0.5733 & 0.2201 & 0.7892 \\
0.7071& -0.3535 & 0.6124 
\end{array}
\right)
\]


\subsection{Calculo del modelo utilizando puntos 3D - 2D}
As{\' \i} dado un conjunto de puntos en 3D \(Q=\left\{Q_1, Q_2, \text{...}, Q_N\right\}\) con \(Q_i=\left(X_i, Y_i, Z_i\right){}^T \) y otro conjunto de puntos correspondientes en el plano de la imagen \(q=\left\{q_1, q_2, \text{...}, q_N\right\}\) con \(q_i=\left(x_i, y_i, w_i\right){}^T\) podemos determinar el modelo de la cámara con al menos 6 puntos correspondientes \(X_i\longleftrightarrow x_i \)resolviendo\\
\\
\(\left(
\begin{array}{c}
 x_i \\
 y_i \\
 w_i
\end{array}
\right)=\left(
\begin{array}{ccc}
 P_{1,1} & P_{1,2} & P_{1,3} \\
 P_{2,1} & P_{2,2} & P_{2,3} \\
 P_{3,1} & P_{3,2} & P_{3,3}
\end{array}
\right)\left(
\begin{array}{c}
 X_i \\
 Y_i \\
 Z_i
\end{array}
\right)=\left(
\begin{array}{c}
 Q_i^TP_1 \\
 Q_i^TP_2 \\
 Q_i^TP_3
\end{array}
\right)\)\\
\\
Para resolver calculamos un vector paralelo, el cual tiene un producto vectorial igual a cero \(q_i\times P Q_i=0\). El sistema resultante lo podemos
calcular encontrando el vector en el espacio nulo con descomposición en valores singulares. El sistema resultante a resolver es\\
\\
\(q_i\times P Q_i=\left(
\begin{array}{ccc}
 \hat{i} & \hat{j} & \hat{k} \\
 x_i & y_i & w_i \\
 Q_i^TP_1 & Q_i^TP_2 & Q_i^TP_3
\end{array}
\right)=\left(
\begin{array}{c}
 y_iQ_i^TP_3-w_iQ_i^TP_2 \\
 x_iQ_i^TP_3-w_iQ_i^TP_1 \\
 x_iQ_i^TP_3-w_iQ_i^TP_1
\end{array}
\right)=0\)\\
\\
de lo cual podemos desprender\\
\\
\(\left(
\begin{array}{ccc}
 0 & -w_iQ_i^T & y_iQ_i^T \\
 -w_iQ_i^T & 0 & x_iQ_i^T
\end{array}
\right)\left(
\begin{array}{c}
 P_1 \\
 P_2 \\
 P_3
\end{array}
\right)= A P =0\) 

\subsection{Ejemplo}

Dados los los puntos correpondientes Q = $\{$ [0.0, { }0.0, { }0.0], [ 100.0, 0.0, 0.0], { }[100.0, { }100.0, 0.0], { }[0.0, { }100.0 , 0.0], [0.0,
0.0, 100.0], [100.0, 0.0 100.0], [100.0, { }100.0, { }100.0], { }[0.0, { }100.0, { }100.0 ]$\}$ y q = $\{$[1577.9715837523775, { }688.6057400776651],
[1667.8746657863205, 758.2462570249891], [1562.6523714350803, 725.2525691697816], [1483.8812040033367, 660.6388854847863], { }[1658.2870049496003,
684.1821822330893], [1762.4087684498734, 758.8465253032915], [1645.2811209801278, 723.3689600937458], [1554.5611061379307, 654.4720145727945]$\}$,
calcular\\
\\
a) La matriz de proyección de la cámara,\\
b) La posición del centro de la cámara,\\
c) La matriz de Rotación y\\
d) la matriz K de parámetros internos.\\

\tiny
\noindent\(A_{1..6}\left(
\begin{array}{cccccc}
 0. & 0. & 0. & 0. & 0. & 0. \\
 0. & 0. & 0. & 1. & 0. & 0. \\
 0. & 0. & 0. & 0. & -99.9999 & 0. \\
 100. & 0. & 0. & 1. & 0. & 0. \\
 0. & 0. & 0. & 0. & -99.9999 & -99.9999 \\
 100. & 100. & 0. & 1. & 0. & 0.  \\
 0. & 0. & 0. & 0. & 0. & -99.9999 .\\
 0. & 100. & 0. & 1. & 0. & 0.  \\
 0. & 0. & 0. & 0. & 0. & 0.  \\
 0. & 0. & 100. & 1. & 0. & 0.  \\
 0. & 0. & 0. & 0. & -99.9999 & 0.  \\
 0. & 0. & 0. & 0. & 0. & 0.  \\
 100. & 100. & 100. & 1. & 0. & 0.  \\
 0. & 0. & 0. & 0. & 0. & 0. 
\end{array}
\right)\)

\noindent\(A_{7..12}\left(
\begin{array}{cccccccccccc}
 0. & -0.9999 & 0. & 0. & 0. & 688.606 \\
 0. & 0. & 0. & 0. & 0. & -1577.97 \\
 0. & -0.9999 & 75824.6 & 0. & 0. & 758.246 \\
 0. & 0. & -166787. & 0. & 0. & -1667.87 \\
 0. & -0.9999 & 72525.3 & 72525.3 & 0. & 725.253 \\
 0. & 0. & -156265. & -156265. & 0. & -1562.65 \\
 0. & -0.9999 & 0. & 66063.9 & 0. & 660.639 \\
 0. & 0. & 0. & -148388. & 0. & -1483.88 \\
 -99.9999 & -0.9999 & 0. & 0. & 68418.2 & 684.182 \\
 0. & 0. & 0. & 0. & -165829. & -1658.29 \\
 -99.9999 & -0.9999 & 75884.7 & 0. & 75884.7 & 758.846 \\
 0. & 0. & -1.27487\times 10^{10} & 0. & -1.27487\times 10^{10} & -1.27487\times 10^6 \\
 0. & 0. & -164528. & -164528. & -164528. & -1645.28 \\
 0. & 0. & -1.01742\times 10^{10} & -1.01742\times 10^{10} & -1.01742\times 10^6
\end{array}
\right)\)
\normalsize
\\
a) Comenzamos por hacer la SVD. El \'ultimo vector de la matriz v después de la descomposición es
\\
$
V_{12} = [ 2.2356E-4,  2.1476E-4, 1.7562E-4, -0.9165, -6.5463E-5,  1.4746E-5,  2.9062E-4,  -0.3999, 4.4712E-7,  -2.2356E-7,  3.8721E-7,  -5.8082E-4 ]
$
\\
La matriz de proyección
\\
$
P = \left[ 
\begin{array}{cccc}
 2.2356E-4 & 2.1476E-4 & 1.7562E-4 & -0.9165 \\
-6.5463E-5 & 1.4746E-5 &  2.9062E-4 & -0.3999 \\
 4.4712E-7 &  -2.2356E-7 &  3.8721E-7 &  -5.8082E-4 
\end{array}
\right]
$
\\
b) El centro de la cámara es
\\ 
Para calcular el centro de la cámara calculamos svd de P. La matriz v es

\begin{equation*}
P = \left[ 
\begin{array}{cccc}
-1.0E-4 & 0.5815 & -0.7237 & 0.3714 \\
-1.0E-4 & 0.2817 & 0.6074 & 0.7428 \\
-2.0E-4 & -0.7631 & -0.3272 & 0.5571\\
1.0 & 0.0 & 0.0 & 4.0E-4
\end{array}
\right]
\end{equation*}

El vector con el eigenvalor menor es $v_4 = [0.3714, 0.7128, 0.5571, 4.0 E-4]$  por lo tanto el centro de la cámara es $C= [1000.0007,2000.002,1500.0003,1.0]$


De la descomposición QR tenemos que la matriz $M$ puede representarse como:
\[
K \times R = 
\left(
\begin{array}{ccc}
468.1648 & 91.2251 & 300.0001 \\
0.0 & 427.201 & 199.9999 \\
0.0 & 0.0 & 1.0
\end{array}
\right) 
\times
\left(
\begin{array}{ccc}
0.4138 & 0.9091 & 0.0471 \\
-0.5733 & 0.2201 & 0.7892 \\
0.7071& -0.3535 & 0.6124 
\end{array}
\right)
\]


