CALIBRACIÓN DE UNA CÁMARA,
CÁLCULO DE HOMOGRAFIAS Y
GEOMETRÍA EPIPOLAR
GONZALO LUZARDO MOROCHO
ABRIL 2008
1. Calibración de la cámara
Extrayendo las esquinas del grid
El primer paso de la calibración de la cámara es la extracción de las esquinas del grid1 presente en las
imágenes de calibración que nos fueron proporcionadas. Utilizando el toolbox2 se extraen las
esquinas de la cámara introduciendo los parámetros del número de cuadros por fila (7) y por
columna (9) y la dimensión de los mismos 28mm x 28mm.
Figura 1. Esquinas predichas
A cada una de las imágenes se aplica el proceso de extraer las esquinas del grid, en la Figura 1
podemos ver como cada una de las esquinas de los cuadros se han predicho de manera correcta. En
todas las imágenes fue necesario agregar un coeficiente de distorsión de lentes con un valor entre -
0.16 y -0.17 ya que las esquinas predichas no coincidían con las de la imagen.
Proceso de calibración
Después de haber hecho la extracción de las esquinas, podemos comenzar con la calibración de la
cámara utilizando los datos extraídos en el proceso anterior. El proceso iterativo lo podemos ver a
continuación.
WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).
Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).
Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set
center_optim=0
1 Patrón cuadriculado virtual, resultado de la proyección del patrón de cuadros blancos y negros sobre el plano
de proyección 2 Camera Calibration Toolbox for Matlab,
Skew not optimized (est_alpha=0) - (DEFAULT)
Distortion not fully estimated (defined by the variable est_dist):
Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) . Initialization of the principal point at the center of the image.
Initialization of the intrinsic parameters using the vanishing points of planar patterns.
Initialization of the intrinsic parameters - Number of images: 4
Calibration parameters after initialization:
Focal Length: fc = [ 745.76141 745.76141 ]
Principal point: cc = [ 319.50000 241.00000 ]
Skew: alpha_c = [ 0.00000 ] => angle of pixel = 90.00000 degrees
Distortion: kc = [ 0.00000 0.00000 0.00000 0.00000 0.00000 ]
Main calibration optimization procedure - Number of images: 4
Gradient descent iterations: 1...2...3...4...5...6...7...8...9...10...done Estimation of uncertainties...done
Calibration results after optimization (with uncertainties):
Focal Length: fc = [ 738.02801 736.57715 ] ± [ 6.93390 6.13745 ]
Principal point: cc = [ 321.01988 235.61779 ] ± [ 6.46386 5.87339 ]
Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes = 90.00000
± 0.00000 degrees Distortion: kc = [ -0.21237 0.20008 -0.00538 -0.00246 0.00000 ] ± [
0.03290 0.17241 0.00162 0.00158 0.00000 ]
Pixel error: err = [ 0.37555 0.36149 ]
Note: The numerical errors are approximately three times the standard deviations (for reference).
Como vemos, el proceso concluyó después de 10 iteraciones utilizando el algoritmo de descenso del
gradiente (10 evaluaciones de la función de la reproyección más el cómputo Jacobiano y luego una
inversión), esta rapidez se debió a la proximidad de los valores calculados en la primera parte del
proceso.
Luego de esto, utilizando los valores intrínsecos y extrínsecos se reproyectan los grids sobre todas las
imágenes de calibración para poder visualizar el error del modelo que hemos calculado. En la Figura
2 podemos ver el resultado de esta reproyección.
Podemos observar que la reproyección de los puntos es relativamente buena, esto es, los puntos
reproyectados sobre las imágenes están ubicados de manera casi correcta sobre la misma. En la
Figura 3 podemos observar un gráfico que representa los errores obtenidos en la reproyección.
Adicionalmente podemos visualizar en la Figura 4 los parámetros extrínsecos obtenidos, esto es la
posición relativa de cada uno de los grids con respecto a la cámara, mostrados en un plano
tridimensional, (a) teniendo como referencia la cámara y (b) teniendo como referencia el mundo.
Figura 2. Grid proyectado en cada una de las imágenes de calibración
Figura 3. Gráfico del error de reproyección
-2 -1.5 -1 -0.5 0 0.5 1 1.5
-1
-0.5
0
0.5
1
1.5Reprojection error (in pixel)
x
y
(a)
(b)
Figura 4. Parámetros extrínsecos obtenidos. (a) Centrado en la cámara. (b) Centrado en el mundo
Al observar la gráfica del error de reproyección podemos notar que existen valores un poco altos
(aunque en la reproyección visual no sean apreciados), aunque se podrían asumir como aceptables.
Esto se puede deber al error introducido en la extracción de las esquinas de los grids, en la calidad
-100
-50
0
50
100
050100150200250300350400450500
-100
-50
0
50
100
Oc
Yc
Xc
Zc
Extrinsic parameters (camera-centered)
213
4
0
100
200
300
0
100
200
300
0
50
100
150
200
250
300
350
400
1
Xw orld
2
3
Extrinsic parameters (world-centered)
Yworld
4
Zworld
de las imágenes de calibración, o en este caso en particular la leve ondulación de la hoja que tiene
impreso el patrón de cuadros. Este error puede ser mejorado, aunque como veremos después muy
poco, haciendo un recálculo automático de las esquinas en todas las imágenes.
Re-extraction of the grid corners on the images (after first calibration)
Window size for corner finder (wintx and winty):
wintx ([] = 5) =
winty ([] = 5) =
Window size = 11x11
Number(s) of image(s) to process ([] = all images) =
Use the projection of 3D grid or manual click ([]=auto, other=manual):
Processing image 1...2...3...4...
Done
Luego de esto ejecutamos nuevamente la calibración de la cámara. Esta vez los parámetros fueron
obtenidos a partir de la extracción automática de las esquinas ejecutada anteriormente.
WARNING: The principal point estimation may be unreliable (using less than 5
images for calibration).
Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are
estimated (DEFAULT).
Principal point optimized (center_optim=1) - (DEFAULT). To reject principal
point, set center_optim=0
Skew not optimized (est_alpha=0) - (DEFAULT)
Distortion not fully estimated (defined by the variable est_dist):
Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .
Main calibration optimization procedure - Number of images: 4
Gradient descent iterations: 1...done
Estimation of uncertainties...done
Calibration results after optimization (with uncertainties):
Focal Length: fc = [ 738.02809 736.57688 ] ± [ 6.93278 6.13647 ]
Principal point: cc = [ 321.01456 235.61678 ] ± [ 6.46287 5.87245 ]
Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes =
90.00000 ± 0.00000 degrees
Distortion: kc = [ -0.21235 0.20000 -0.00538 -0.00246 0.00000 ]
± [ 0.03290 0.17239 0.00162 0.00158 0.00000 ]
Pixel error: err = [ 0.37548 0.36144 ]
Note: The numerical errors are approximately three times the standard deviations
(for reference).
Como podemos observar, la mejora fue muy poca que el error resultó ser casi el mismo para ambos
calibrados. El siguiente paso dentro de la calibración es estimar los valores de alpha_c que como
vemos en este momento tienen un valor inicial de 0. Los resultados que obtuvimos fueron:
Calibration results after optimization (with uncertainties):
Focal Length: fc = [ 739.47532 737.86732 ] ± [ 7.18063 6.35579 ]
Principal point: cc = [ 320.47727 235.20940 ] ± [ 6.50009 5.89507 ]
Skew: alpha_c = [ -0.00122 ] ± [ 0.00166 ] => angle of pixel axes
= 90.07007 ± 0.09513 degrees
Distortion: kc = [ -0.21481 0.20670 -0.00520 -0.00201 0.00000 ]
± [ 0.03310 0.17327 0.00163 0.00168 0.00000 ]
Pixel error: err = [ 0.37430 0.35980 ]
En este momento ya hemos calculado los parámetros intrínsecos de la cámara, y extrínsecos de cada
una de las imágenes de calibración. Los parámetros intrínsecos de la cámara obtenidos al final del
proceso son los siguientes:
Focal Length: fc = [ 739.47532 737.86732 ] ± [ 7.18063 6.35579 ]
Principal point: cc = [ 320.47727 235.20940 ] ± [ 6.50009 5.89507 ]
Skew: alpha_c = [ -0.00122 ] ± [ 0.00166 ]
=> angle of pixel axes = 90.07007 ± 0.09513 degrees
Distortion: kc = [ -0.21481 0.20670 -0.00520 -0.00201 0.00000 ] ±
[ 0.03310 0.17327 0.00163 0.00168 0.00000 ]
Con estos datos podemos obtener la matriz de parámetros extrínsecos, sabiendo que:
De esta forma la matriz de parámetros intrínsecos KK o simplemente K es: � = �739.475320706477 −0.90437273724306 320.477273079830240 737.8673248542212 235.209401351722330 0 1 �
2. Cálculo de posición y orientación relativas de un objeto
En esta parte vamos a calcular la posición y orientación relativas (matriz de rotación R y vector de
traslación T) entre la puerta y la cámara en las imágenes 2753 y 2754, sabiendo que las dimensiones
del marco interior de la puerta son de 80cm de ancho por 150cm de alto.
En primer lugar vamos a calcular la matriz H de transformación 2D→2D, que transforma un punto en
el plano del mundo Xx,y al plano de la cámara Xi,j, de tal forma que: ��,� = � ��,�
Donde la matriz H3x3 la podemos llamar matriz de transformación mundo-cámara, así tenemos: ���1� = �ℎ�� ℎ�� ℎ��ℎ�� ℎ�� ℎ��ℎ�� ℎ�� ℎ��� �1�
Para una correspondencia de puntos, la ecuación anterior la podemos reescribirla de la siguiente
forma:
� 10 0 0 0 0 0 1 −� −� −�−� −� −� ����������ℎ��ℎ��ℎ��ℎ��ℎ��ℎ��ℎ��ℎ��ℎ��������
���� = 0
De esta forma el problema se resume en resolver un sistema matricial AX = 0, que puede ser visto
como un problema de autovalores para B = ATA. Donde debemos buscar el autovector relacionado
con el menor autovalor obtenido para B, para calcular los valores de los elementos de H.
Luego de esto, para cada una de las esquinas de la puerta en las dos imágenes se obtiene las
coordenadas (i,j) correspondientes a las coordenadas en el mundo (X,Y). La siguiente tabla
resume los datos obtenidos para ambas imágenes:
Imagen 1 Posición (i,j) (pixeles) Posición (X,Y) (milímetros)
[60,234] [0,0]
[61,383] [0,8000]
[424,375] [15000,80]
[418,233] [15000,0]
Imagen 2 Posición (i,j) (pixeles) Posición (X,Y) (milímetros)
[166,194] [0,0]
[152,340] [0,8000]
[585,340] [15000,8000]
[553,203] [15000,0]
Utilizando el código MATLAB descrito en 6.1, el valor de H para la imagen 1 obtenido es: � = �0.000104389227527 0.000000057393603 0.2483732219268910.000002841034708 0.000074210674081 0.9686555581350170.000000013377649 −0.000000007541912 0.004139554491026� Utilizando el código MATLAB descrito en 6.2, el valor de H para la imagen 2 obtenido es: � = �0.000110531628414 −0.000015120511802 0.6501411019483550.000005831968284 0.000052985252554 0.7598033911363490.000000017152848 −0.000000054385883 0.003916514746551�
Una vez obtenida las matrices de transformación H para cada una de las imágenes, el paso siguiente
es obtener la matriz de rotación y vector de traslación (valores extrínsecos) para cada una de ellas.
Sabiendo que: ���� = � �� ���� = � [ �� | �� | � ] Y que �� y �� son ortonormales, esto es: � �� � = ‖ �� ‖ = 1
De esta forma: ��� � ℎ� | ℎ� | ℎ� � = [ ��� | ��� | �� ] Utilizando las propiedades de la norma de vectores, podemos decir que: � ��� ℎ� � = ‖ ��� ℎ� ‖ = �
Una solución para resolver esto sería: � = ‖ ��� ℎ� ‖+ ‖ ��� ℎ� ‖2
De esta forma podríamos obtener la matriz �� de la siguiente manera: �� = �����
Utilizando el código MATLAB descrito en 6.3, el valor de la matriz �� para la imagen 1 es: �� = �1.147489636738092 0.026104979848348 −6611.7153729870980.019679385254219 0.720160286687176 −1767.05596977101410.13856944750137 −0.439356882878124 31639.602228701657 � Utilizando el código MATLAB descrito en 6.3, el valor de la matriz �� para la imagen 2 es: �� = � 1.131174807628756 0.029013838608088 −12184.691743752728−0.003459913164431 0.860517324807972 −56.698921265332650.111787202562851 −0.063022231636684 34591.22192970978 �
3. Comprobación de la transformación lineal H asociada la puerta
para cada una de las imágenes
Para comprobar la transformación lineal H asociada a la puerta, debemos utilizar esta matriz para
proyectar sobre la imagen los puntos de las cuatro esquinas de la puerta y comprobar visualmente
que coinciden en la imagen con las esquinas asociadas.
Utilizando el código MATLAB descrito en 6.4 para cada una de las imágenes obtenemos el siguiente
resultado:
Proyección de los puntos asociados a las cuatro esquinas
en la imagen 1
Proyección de los puntos asociados a las cuatro
esquinas en la imagen 2
Como vemos en las dos imágenes anteriores la matriz de transformación H es correcta ya que vemos
claramente que los puntos correspondientes a las cuatro esquinas de la puerta se han proyectado
correctamente sobre la imagen.
4. Comprobación de la matriz de rotación y el vector de traslación
Primero debemos construir la matriz de proyección de la cámara P, conocidas las matrices K, R y el
vector t. De tal forma que: � = � [ �� | �� | �� | � ] Donde �� es perpendicular al plano formado por los vectores ��y �� , de esta forma: �� = �� ∧ �� Utilizando el código MATLAB descrito en 6.5, el valor de la matriz P para la imagen 1 es: � = � 872.304240540066223.7404441227256270.111787202562851 0.479596262830466620.1241950716553−0.063022231636684 240.94345267243617283.97051236270250.973495904713021 2075472.91857012738094344.32075543734591.22192970978 �
Utilizando el código MATLAB descrito en 6.5, el valor de la matriz P para la imagen 2 es: � = �892.930828302167747.113612144742720.13856944750137 −122.15120071796726428.0418748025935−0.439356882878124 184.02343529568304568.92112964389030.825862735808379 5252171.1749694926138079.03793691731639.602228701657�
Luego debemos pintar sobre cada una de las imágenes un triedro de aristas ortogonales, de 50 cm
de longitud cada una, ubicado en la esquina superior derecha de la puerta, con dos de sus ejes a lo
largo del borde de la puerta y el tercero apuntando hacia la cámara.
En vez de dibujar un triedro, vamos a dibujar un cubo, para esto debemos saber las coordenadas en
el mundo de los ocho puntos que forman el cubo. Así tenemos los siguientes puntos 3D a proyectar
sobre la imagen:
Punto 1 [0 0 0 1]
Punto 2 [0 5000 0 1]
Punto 3 [5000 5000 0 1]
Punto 4 [5000 0 0 1]
Punto 5 [0 0 -50000 1]
Punto 6 [0 5000 -5000 1]
Punto 7 [5000 5000 -5000 1]
Punto 8 [5000 0 -5000 1]
Utilizando el código MATLAB descrito en 6.6 para cada una de las imágenes obtenemos el siguiente
resultado:
Trazado de un cubo de arista 50cm en la esquina
superior derecha de la puerta en la imagen 1
Trazado de un cubo de arista 50cm en la esquina
superior derecha de la puerta en la imagen 2
Como vemos en las dos imágenes anteriores la matriz de proyección de la cámara P es correcta ya
que vemos claramente como el cubo sigue la proyección que debería tener un cubo al ser colocado
en dicho sitio.
5. Visión sin calibrar
En esta parte debemos en primer lugar calcular la homografía que relaciona
plano en las dos imágenes, que en este caso es la ventana de la pared
puntos presentes sobre dicho plano, estos cuatro puntos serán aquellos
esquinas de la ventana presente en ambas imágenes que nos fueron proporcionadas
son denominados coplanares al estar colocados sobre el mismo plano (ventana)
debemos calcular la matriz H que relacione
se encuentra la ventana) de la imagen 1 con los de la imagen 2
Utilizando el código MATLAB descrito en 6.7 podemos calcular
calcular H, proyecta los cuatro puntos sobre las esquinas de la ventana en cada una de las imágenes.� � � �
Imagen 1
mos en primer lugar calcular la homografía que relaciona los puntos de nuestro
que en este caso es la ventana de la pared. Para esto necesitamos cuatro
estos cuatro puntos serán aquellos correspondientes a las
esquinas de la ventana presente en ambas imágenes que nos fueron proporcionadas, estos puntos
son denominados coplanares al estar colocados sobre el mismo plano (ventana). En otras palabras
debemos calcular la matriz H que relacione los puntos presentes en nuestro plano (la pared donde
de la imagen 1 con los de la imagen 2 (puntos del plano), de tal forma que:�� � ��
Utilizando el código MATLAB descrito en 6.7 podemos calcular esta matriz H. Este código además de
calcular H, proyecta los cuatro puntos sobre las esquinas de la ventana en cada una de las imágenes. 0.0319 �0.00030 �0.0033 0.02580 0.92230.38240.0387
Imagen 2
los puntos de nuestro
ara esto necesitamos cuatro
pondientes a las
, estos puntos
. En otras palabras
ntos presentes en nuestro plano (la pared donde
, de tal forma que:
. Este código además de
calcular H, proyecta los cuatro puntos sobre las esquinas de la ventana en cada una de las imágenes.
Como vemos en las imágenes anteriores la matriz H proyecta correctamente
encuentran sobre el plano.
El siguiente paso consiste en determinar si un punto de la escena vis
detrás o delante de la ventana. Para esto es necesario calcular
Una vez obtenido el epipolo e’ podríamos decir
punto x’ ,que es la proyección de dicho punto en la segunda imagen
epipolar l1‘, se encuentra ubicado entre el epipolo H), caso contrario el punto estará ubicado delante del plano. La siguiente gráfica mu
hemos descrito anteriormente:
Para calcular el epipolo e’, además de tener los cuatro puntos
ubicados sobre el plano (la coordenada Z tiene valor 1
necesitamos tener dos puntos en la imagen 1 con su correspondencia en la imagen 2 ubicados
fuera del plano.
Cada uno de estos puntos servirán para calcular la recta epipolar
de ellos, el epipolo estará localizado en la intersecció
Los dos puntos fuera del plano que vamos a tomar de las dos imágenes de muestra son dos puntos
ubicados sobre la pequeña mesa donde aparecen algunas revistas. Estos dos puntos los utilizaremos
para poder calcular la ubicación del epipolo de la segunda imagen.
Utilizando el código MATLAB descrito en 6.8 podemos calcular la ubicación del epipolo, y
adicionalmente nos muestra la línea epipol
podemos ver el resultado obtenido:
Como vemos en las imágenes anteriores la matriz H proyecta correctamente los puntos que se
El siguiente paso consiste en determinar si un punto de la escena visto por ambas cámaras está
detrás o delante de la ventana. Para esto es necesario calcular el epipolo e’ en la imagen 2.
podríamos decir que un punto X está delante del plano P, cuando el
icho punto en la segunda imagen y situado además sobre la recta
tre el epipolo e’ y el punto ��� (punto inducido por la homografía
, caso contrario el punto estará ubicado delante del plano. La siguiente gráfica muestra lo que
, además de tener los cuatro puntos de las esquinas de ventana que están
la coordenada Z tiene valor 1) que nos permiten calcular la homografía H
en la imagen 1 con su correspondencia en la imagen 2 ubicados
Cada uno de estos puntos servirán para calcular la recta epipolar l1’ y l2’ correspondiente a cada uno
de ellos, el epipolo estará localizado en la intersección de estas dos rectas.
Los dos puntos fuera del plano que vamos a tomar de las dos imágenes de muestra son dos puntos
ubicados sobre la pequeña mesa donde aparecen algunas revistas. Estos dos puntos los utilizaremos
ipolo de la segunda imagen.
Utilizando el código MATLAB descrito en 6.8 podemos calcular la ubicación del epipolo, y
adicionalmente nos muestra la línea epipolar correspondiente a cada punto. A continuación
los puntos que se
to por ambas cámaras está
, cuando el
sobre la recta
nducido por la homografía
lo que
de las esquinas de ventana que están
que nos permiten calcular la homografía H,
en la imagen 1 con su correspondencia en la imagen 2 ubicados
correspondiente a cada uno
Los dos puntos fuera del plano que vamos a tomar de las dos imágenes de muestra son dos puntos
ubicados sobre la pequeña mesa donde aparecen algunas revistas. Estos dos puntos los utilizaremos
Utilizando el código MATLAB descrito en 6.8 podemos calcular la ubicación del epipolo, y
ar correspondiente a cada punto. A continuación
Epipolo ubicado en: [304.9210, -751.6473]
Como podemos observar visualmente las líneas epipolares están correctamente graficadas, además
por intuición podemos decir que la ubicación del epipolo es la correcta. Este punto no puede ser
dibujado sobre la gráfica al encontrarse fuera de ella.
Una vez obtenido el epipolo, podemos probar nuestro algoritmo para establecer si un objeto esta
delante o detrás del plano. Para esto vamos a elegir dos correspondencias de puntos en particular a
ser analizada, una delante y otra detrás del plano. En nuestro caso vamos a establecer dos puntos,
un punto será la esquina de una de las patas de la mesa (delante del plano) y el otro será la esquina
superior derecha del poster donde se puede ver un oso (detrás del plano).
En primer lugar, para ambos puntos obtenemos en la primera imagen el punto en cuestión y en la
segunda imagen su correspondencia. Con estos puntos vamos a utilizar el código MATLAB descrito
en 6.9, este código nos indica si dada una correspondencia de puntos en dos imágenes, establecer si
dicho punto está delante o detrás del plano, adicionalmente nos muestra la línea epipolar de dicho
punto.
Al ejecutar este código utilizando la primera correspondencia de puntos correspondiente a la pata
de la mesa (sobre el plano), los resultados fueron los siguientes:
El punto se encuentra delante del plano
En la gráfica anterior el punto blanco representa la proyección del punto que queremos analizar, y el
punto rojo representa el punto generado por la matriz de homografía H. Como vemos el punto
blanco está ubicado entre el epipolo y el punto rojo, lo que quiere decir que el punto analizado
(blanco) está ubicado delante del plano.
Ahora ejecutaremos el mismo código utilizando la segunda correspondencia de puntos
correspondiente al poster del oso (detrás del plano), los resultados fueron los siguientes:
El punto se encuentra detrás del plano
6. Codificación en MATLAB
6.1. Cálculo de la matriz de H para la imagen 1
%Matriz de parametros intrínsecos que obtuvimos K = [739.475320706477 -0.90437273724306 320.47727307983024 ; 0 737.8673248542212 235.20940135172233 ; 0 0 1]; %4 Puntos X,Y de las 4 esquinas de la puerta Pm1 = [0 0 1]; Pm2 = [0 8000 1]; Pm3 = [15000 8000 1]; Pm4 = [15000 0 1]; image_name = 'imagen1.jpg'; %nombre de la imagen %4 Puntos i,j de la camara que corresponden a los puntos del mundo Pc1 = [60,234]; Pc2 = [61,383]; Pc3 = [424,375]; Pc4 = [418,233]; %Construimos el sistema lineal para calcular H, tal que Pmi H = Pci %H es una matriz espacio 2D del mundo a otro 2D de la camara %El sistema unido en total va a tener un total de 9 incognitas %El sistema Xm h = Xc puede ser expresado como una sola matriz con los %elementos de la matriz H como incognitas %Resolucion de la forma Ax=0 %Matriz de incognitas A 9x9 A = zeros(8,9); %Primer punto
A(1,[1:3]) = Pm1; A(2,[4:6]) = Pm1; A(1,7) = -Pc1(1)*Pm1(1); A(1,8) = -Pc1(1)*Pm1(2); A(2,7) = -Pc1(2)*Pm1(1); A(2,8) = -Pc1(2)*Pm1(2); A([1:2],9) = -Pc1'; %Segundo punto A(3,[1:3]) = Pm2; A(4,[4:6]) = Pm2; A(3,7) = -Pc2(1)*Pm2(1); A(3,8) = -Pc2(1)*Pm2(2); A(4,7) = -Pc2(2)*Pm2(1); A(4,8) = -Pc2(2)*Pm2(2); A([3:4],9) = -Pc2'; %Tercer punto A(5,[1:3]) = Pm3; A(6,[4:6]) = Pm3; A(5,7) = -Pc3(1)*Pm3(1); A(5,8) = -Pc3(1)*Pm3(2); A(6,7) = -Pc3(2)*Pm3(1); A(6,8) = -Pc3(2)*Pm3(2); A([5:6],9) = -Pc3'; %Cuarto punto A(7,[1:3]) = Pm4; A(8,[4:6]) = Pm4; A(7,7) = -Pc4(1)*Pm4(1); A(7,8) = -Pc4(1)*Pm4(2); A(8,7) = -Pc4(2)*Pm4(1); A(8,8) = -Pc4(2)*Pm4(2); A([7:8],9) = -Pc4'; %Resolvemos el sistema con eigenvalores B = A'*A; [V,D] = eig(B); %Creamos la matris H H = zeros(3); %Llenamos la matriz con el eigenvector que esta relacionado con el eigenvalue %mas pequeno, que en este caso es la primera columna H(1,:) = V([1:3],1); H(2,:) = V([4:6],1); H(3,:) = V([7:9],1); fprintf('Matriz de transformacion H '); H
6.2. Cálculo de la matriz de H para la imagen 2
%Matriz de parametros intrinsecos K = [739.475320706477 -0.90437273724306 320.47727307983024 ; 0 737.8673248542212 235.20940135172233 ; 0 0 1]; %4 Puntos X,Y del mundo, extendido Pm1 = [0 0 1]; Pm2 = [0 8000 1]; Pm3 = [15000 8000 1]; Pm4 = [15000 0 1]; image_name = 'imagen2.jpg'; %nombre de la imagen %4 Puntos i,j de la camara que corresponden a los puntos del mundo Pc1 = [166,194]; Pc2 = [152,340]; Pc3 = [585,340];
Pc4 = [553,203]; %Construimos el sistema lineal para calcular H, tal que Pmi H = Pci %H es una matriz espacio 2D del mundo a otro 2D de la camara %El sistema unido en total va a tener un total de 9 incognitas %El sistema Xm h = Xc puede ser expresado como una sola matriz con los %elementos de la matriz H como incognitas %Resolucion de la forma Ax=0 %Matriz de incognitas A 9x9 A = zeros(8,9); %Primer punto A(1,[1:3]) = Pm1; A(2,[4:6]) = Pm1; A(1,7) = -Pc1(1)*Pm1(1); A(1,8) = -Pc1(1)*Pm1(2); A(2,7) = -Pc1(2)*Pm1(1); A(2,8) = -Pc1(2)*Pm1(2); A([1:2],9) = -Pc1'; %Segundo punto A(3,[1:3]) = Pm2; A(4,[4:6]) = Pm2; A(3,7) = -Pc2(1)*Pm2(1); A(3,8) = -Pc2(1)*Pm2(2); A(4,7) = -Pc2(2)*Pm2(1); A(4,8) = -Pc2(2)*Pm2(2); A([3:4],9) = -Pc2'; %Tercer punto A(5,[1:3]) = Pm3; A(6,[4:6]) = Pm3; A(5,7) = -Pc3(1)*Pm3(1); A(5,8) = -Pc3(1)*Pm3(2); A(6,7) = -Pc3(2)*Pm3(1); A(6,8) = -Pc3(2)*Pm3(2); A([5:6],9) = -Pc3'; %Primer punto A(7,[1:3]) = Pm4; A(8,[4:6]) = Pm4; A(7,7) = -Pc4(1)*Pm4(1); A(7,8) = -Pc4(1)*Pm4(2); A(8,7) = -Pc4(2)*Pm4(1); A(8,8) = -Pc4(2)*Pm4(2); A([7:8],9) = -Pc4'; %Resolvemos el sistema con eigenvalores B = A'*A; [V,D] = eig(B); %Creamos la matris H H = zeros(3); %Llenamos la matriz con el eigenvector que esta relacionado con el eigenvalue %mas pequeno, que en este caso es la primera columna H(1,:) = V([1:3],1); H(2,:) = V([4:6],1); H(3,:) = V([7:9],1); fprintf('Matriz de transformacion H '); H
6.3. Obtención de la matriz R y vector t a partir de la matriz H y K
%En primer lugar vamos a calcular el valor de Lambda h1 = H(:,1); h2 = H(:,2); h3 = H(:,3);
lambda = (norm(inv(K)*h1) + norm(inv(K)*h2))/2; RT = inv(K)*H ./ lambda; KRT = K*RT; %Matriz de proyeccion 2D fprintf('Matriz RT '); RT
6.4. Reproyección sobre la imagen de los puntos asociados a las esquinas
de la puerta utilizando la matriz H
%RETROPROYECCION DE LOS PUNTOS EN LA IMAGEN imagen1 = imread(image_name); %cargamos la imagen %Reproyectamos los cuatro puntos del marco interior de la puerta %Punto 1 Pcr1 = H * Pm1'; %Normalizamos Pcr1(1,1) = Pcr1(1,1)/Pcr1(3,1); Pcr1(2,1) = Pcr1(2,1)/Pcr1(3,1); Pcr1(3,1) = Pcr1(3,1)/Pcr1(3,1); %Punto 2 Pcr2 = H * Pm2'; %Normalizamos Pcr2(1,1) = Pcr2(1,1)/Pcr2(3,1); Pcr2(2,1) = Pcr2(2,1)/Pcr2(3,1); Pcr2(3,1) = Pcr2(3,1)/Pcr2(3,1); %Punto 3 Pcr3 = H * Pm3'; %Normalizamos Pcr3(1,1) = Pcr3(1,1)/Pcr3(3,1); Pcr3(2,1) = Pcr3(2,1)/Pcr3(3,1); Pcr3(3,1) = Pcr3(3,1)/Pcr3(3,1); %Punto 4 Pcr4 = H * Pm4'; %Normalizamos Pcr4(1,1) = Pcr4(1,1)/Pcr4(3,1); Pcr4(2,1) = Pcr4(2,1)/Pcr4(3,1); Pcr4(3,1) = Pcr4(3,1)/Pcr4(3,1); %Dibujamos la imagen imshow(imagen1) hold on %Dibujamos las lineas line([Pcr1(2,1) Pcr2(2,1)],[ Pcr1(1,1) Pcr2(1,1)]); line([Pcr2(2,1) Pcr3(2,1)],[ Pcr2(1,1) Pcr3(1,1)]); line([Pcr3(2,1) Pcr4(2,1)],[ Pcr3(1,1) Pcr4(1,1)]); line([Pcr4(2,1) Pcr1(2,1)],[ Pcr4(1,1) Pcr1(1,1)]); %Dibujamos los puntos rectangle('Position',[Pcr1(2,1)-2 Pcr1(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pcr2(2,1)-2 Pcr2(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pcr3(2,1)-2 Pcr3(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pcr4(2,1)-2 Pcr4(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g');
6.5. Obtención de la matriz P a partir de la matriz R y vector t
%Para esto necesito calcular el r3 que es el resultado del producto cruz %entre dos vectores r1 = RT(:,1); r2 = RT(:,2); r3 = cross(r1,r2);
t = RT(:,3); P = K * [r1 r2 r3 t]; fprintf('Matriz de Proyeccion 3D P '); P
6.6. Trazado de un cubo de arista 50cm ubicado en la esquina superior
derecha de la puerta
%Vamos a proyectar los 8 puntos que forman el cubo que queremos %modelar sobre la puerta, %Puntos sobre el plano p1 = [0,0,0,1]; p2 = [0,5000,0,1]; p3 = [5000,5000,0,1]; p4 = [5000,0,0,1]; %Puntos fuera del plano p5 = [0,0,-5000,1]; p6 = [0,5000,-5000,1]; p7 = [5000,5000,-5000,1]; p8 = [5000,0,-5000,1]; %Proyectamos primero los puntos en el plano, utilizando la matriz P Pp1 = P*p1'; %Proyectamos Pp1 = Pp1 ./ Pp1(3,1); %Normalizamos Pp2 = P*p2'; %Proyectamos Pp2 = Pp2 ./ Pp2(3,1); %Normalizamos Pp2 = P*p2'; %Proyectamos Pp2 = Pp2 ./ Pp2(3,1); %Normalizamos Pp3 = P*p3'; %Proyectamos Pp3 = Pp3 ./ Pp3(3,1); %Normalizamos Pp4 = P*p4'; %Proyectamos Pp4 = Pp4 ./ Pp4(3,1); %Normalizamos Pp5 = P*p5'; %Proyectamos Pp5 = Pp5 ./ Pp5(3,1); %Normalizamos Pp6 = P*p6'; %Proyectamos Pp6 = Pp6 ./ Pp6(3,1); %Normalizamos Pp7 = P*p7'; %Proyectamos Pp7 = Pp7 ./ Pp7(3,1); %Normalizamos Pp8 = P*p8'; %Proyectamos Pp8 = Pp8 ./ Pp8(3,1); %Normalizamos %Dibujamos las aristas line([Pp1(2,1) Pp2(2,1)],[ Pp1(1,1) Pp2(1,1)]); line([Pp2(2,1) Pp3(2,1)],[ Pp2(1,1) Pp3(1,1)]); line([Pp3(2,1) Pp4(2,1)],[ Pp3(1,1) Pp4(1,1)]); line([Pp4(2,1) Pp1(2,1)],[ Pp4(1,1) Pp1(1,1)]); line([Pp5(2,1) Pp6(2,1)],[ Pp5(1,1) Pp6(1,1)]); line([Pp6(2,1) Pp7(2,1)],[ Pp6(1,1) Pp7(1,1)]); line([Pp7(2,1) Pp8(2,1)],[ Pp7(1,1) Pp8(1,1)]); line([Pp8(2,1) Pp5(2,1)],[ Pp8(1,1) Pp5(1,1)]); line([Pp1(2,1) Pp5(2,1)],[ Pp1(1,1) Pp5(1,1)]); line([Pp2(2,1) Pp6(2,1)],[ Pp2(1,1) Pp6(1,1)]); line([Pp3(2,1) Pp7(2,1)],[ Pp3(1,1) Pp7(1,1)]); line([Pp4(2,1) Pp8(2,1)],[ Pp4(1,1) Pp8(1,1)]); %Dibujamos los puntos rectangle('Position',[Pp1(2,1)-2 Pp1(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos rectangle('Position',[Pp2(2,1)-2 Pp2(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos rectangle('Position',[Pp3(2,1)-2 Pp3(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos
rectangle('Position',[Pp4(2,1)-2 Pp4(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos rectangle('Position',[Pp5(2,1)-2 Pp5(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos rectangle('Position',[Pp6(2,1)-2 Pp6(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos rectangle('Position',[Pp7(2,1)-2 Pp7(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos rectangle('Position',[Pp8(2,1)-2 Pp8(1,1)-2 5 5], 'FaceColor','r', 'EdgeColor' , 'r'); %Dibujamos
6.7. Obtención de la homografía que relaciona las cuatro esquinas de la
ventana en dos imágenes
image_name1 = 'ventana1.jpg'; %nombre de la imagen %4 Puntos i,j de la ventana en la foto 1 Pv11 = [61,102,1]; Pv12 = [71,549,1]; Pv13 = [216,550,1]; Pv14 = [249,110,1]; image_name2 = 'ventana2.jpg'; %nombre de la imagen %4 Puntos i,j de la ventana en la foto 2 Pv21 = [70,83]; Pv22 = [56,600]; Pv23 = [245,595]; Pv24 = [235,87]; %Construimos el sistema lineal para calcular H, tal que Pv1 H = Pv2 %H es una matriz espacio 2D del mundo a otro 2D de la camara %Resolucion de la forma Ax=0 %Matriz de incognitas A 9x9 A = zeros(8,9); %Primer punto A(1,[1:3]) = Pv11; A(2,[4:6]) = Pv11; A(1,7) = -Pv21(1)*Pv11(1); A(1,8) = -Pv21(1)*Pv11(2); A(2,7) = -Pv21(2)*Pv11(1); A(2,8) = -Pv21(2)*Pv11(2); A([1:2],9) = -Pv21'; %Segundo punto A(3,[1:3]) = Pv12; A(4,[4:6]) = Pv12; A(3,7) = -Pv22(1)*Pv12(1); A(3,8) = -Pv22(1)*Pv12(2); A(4,7) = -Pv22(2)*Pv12(1); A(4,8) = -Pv22(2)*Pv12(2); A([3:4],9) = -Pv22'; %Tercer punto A(5,[1:3]) = Pv13; A(6,[4:6]) = Pv13; A(5,7) = -Pv23(1)*Pv13(1); A(5,8) = -Pv23(1)*Pv13(2); A(6,7) = -Pv23(2)*Pv13(1); A(6,8) = -Pv23(2)*Pv13(2); A([5:6],9) = -Pv23'; %Cuarto punto A(7,[1:3]) = Pv14; A(8,[4:6]) = Pv14; A(7,7) = -Pv24(1)*Pv14(1); A(7,8) = -Pv24(1)*Pv14(2);
A(8,7) = -Pv24(2)*Pv14(1); A(8,8) = -Pv24(2)*Pv14(2); A([7:8],9) = -Pv24'; %Resolvemos el sistema con eigenvalores B = A'*A; [V,D] = eig(B); %Creamos la matris H H = zeros(3); %Llenamos la matriz con el eigenvector que esta relacionado con el eigenvalue %mas pequeno, que en este caso es la primera columna H(1,:) = V([1:3],1); H(2,:) = V([4:6],1); H(3,:) = V([7:9],1); fprintf('Matriz de transformacion H '); H %DIBUJO SOBRE LA IMAGEN 1 figure(1); imshow(image_name1) hold on %Dibujamos las lineas line([Pv11(1,2) Pv12(1,2)],[ Pv11(1,1) Pv12(1,1)]); line([Pv12(1,2) Pv13(1,2)],[ Pv12(1,1) Pv13(1,1)]); line([Pv13(1,2) Pv14(1,2)],[ Pv13(1,1) Pv14(1,1)]); line([Pv14(1,2) Pv11(1,2)],[ Pv14(1,1) Pv11(1,1)]); %Dibujamos los puntos rectangle('Position',[Pv11(1,2)-2 Pv11(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pv12(1,2)-2 Pv12(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pv13(1,2)-2 Pv13(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pv14(1,2)-2 Pv14(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); hold off %DIBUJO SOBRE LA IMAGEN 2 figure(2); imshow(image_name2) hold on %Dibujamos las lineas line([Pv21(1,2) Pv22(1,2)],[ Pv21(1,1) Pv22(1,1)]); line([Pv22(1,2) Pv23(1,2)],[ Pv22(1,1) Pv23(1,1)]); line([Pv23(1,2) Pv24(1,2)],[ Pv23(1,1) Pv24(1,1)]); line([Pv24(1,2) Pv21(1,2)],[ Pv24(1,1) Pv21(1,1)]); %Dibujamos los puntos rectangle('Position',[Pv21(1,2)-2 Pv21(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pv22(1,2)-2 Pv22(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pv23(1,2)-2 Pv23(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g'); rectangle('Position',[Pv24(1,2)-2 Pv24(1,1)-2 5 5], 'FaceColor','g', 'EdgeColor' , 'g');
6.8. Obtención del epipolo dado dos puntos fuera del plano
%VAMOS A CALCULAR el epipolo ep Pv15 = [367,320,1]; %esquina de la mesa Pv16 = [335,373,1]; %esquina del libro naranja Pv25 = [370,188,1]; %esquina de la mesa
Pv26 = [352,294,1]; %esquina del libro naranja Pv15 = Pv15'; Pv16 = Pv16'; Pv25 = Pv25'; Pv26 = Pv26'; %Punto x5 proyectado sobre la imagen 2 hp5 = H * Pv15; hp5 = hp5 ./hp5(3,1); % Punto x6 proyectado sobre la imagen 2 hp6 = H * Pv16; hp6 = hp6 ./hp6(3,1); %Calculamos la recta l1 %Ecuacion de la recta Y = m(X-Xo) + Yo m = (Pv25(1,1)-hp5(1,1))/(Pv25(2,1)-hp5(2,1)); % Pendiente YoXo = hp5; %Punto que pasa por la recta X = [0:0.1:ancho_img2]; Y = m*(X-YoXo(2,1)) + YoXo(1,1); %Dibujamos la recta que pasa por los dos puntos plot(X,Y); line([Pv25(2,1) hp5(2,1)],[ Pv25(1,1) hp5(1,1)]); %Calculamos la recta l2 %Ecuacion de la recta Y = m(X-Xo) + Yo m2 = (Pv26(1,1)-hp6(1,1))/(Pv26(2,1)-hp6(2,1)); % Pendiente YoXo2 = hp6; %Punto que pasa por la recta Y2 = m2*(X-YoXo2(2,1)) + YoXo2(1,1); %Dibujamos la recta que pasa por los dos puntos plot(X,Y2); line([Pv26(2,1) hp6(2,1)],[ Pv26(1,1) hp6(1,1)]); %Buscamos la recta l1 l1 = cross(Pv25',hp5'); %Buscamos la recta l2 l2 = cross(Pv26',hp6'); %Encontrar la interseccion entre estas dos rectas AXB = [l1;l2]; s=rref(AXB); ep= [-s(:,3)]; ep
6.9. Establecer si a través de una correspondencia de puntos, el punto en
cuestión está delante o detrás del plano
%ANALIZAR SI UN PUNTO ESTA DELANTE O DETRAS DEL PLANO %Punto en la imagen 1 %Pa = [412,432,1]; %delante del plano Pa = [113,248,1]; Pa = Pa'; %Punto en la imagen 2 %Pb = [449,318,1]; %delante del plano Pb = [112,452,1]; Pb = Pb'; %Proyectamos Pa en la segunda imagen Pap = H*Pa; Pap = Pap ./Pap(3,1); %Recta entre el epipolo y el punto en la imagen 2 %Calculamos la recta que une el punto Pb con el epipolo %Ecuacion de la recta Y = m(X-Xo) + Yo m3 = (ep(1,1)-Pap(1,1))/(ep(2,1)-Pap(2,1)); % Pendiente YoXo3 = ep; %Punto que pasa por la recta Y3 = m3*(X-YoXo3(2,1)) + YoXo3(1,1); %Dibujamos la recta que pasa por los dos puntos plot(X,Y3); %Dibujamos el punto para el cual vamos a hacer el analisis rectangle('Position',[Pap(2,1)-2 Pap(1,1)-2 5 5], 'FaceColor','r',
'EdgeColor' , 'r'); rectangle('Position',[Pb(2,1)-2 Pb(1,1)-2 5 5], 'FaceColor','w', 'EdgeColor' , 'w'); %Si el epipolo se encuentra en la parte izquierda del punto real if(ep(2,1) < Pap(2,1)) if(Pb(2,1) > ep(2,1) && Pb(2,1) < Pap(2,1)) fprintf('El punto se encuentra delante del plano\n'); else fprintf('El punto se encuentra detras del plano\n'); end; else % el epipolo se encuentra a la derecha del punto real if( ep(2,1) > Pb(2,1) && Pap(2,1) < Pb(2,1)) fprintf('El punto se encuentra delante del plano\n'); else fprintf('El punto se encuentra detras del plano\n'); end; end;