Reconstrucción 3D desde un par estéreo de cámaras

Mi segundo trabajo en el ámbito de la robótica. Objetivo: programar un robot que, usando la información de sus dos cámaras calibradas, reconstruya en 3D y tiempo real lo que tiene delante. Esta reconstrucción puede usarse posteriormente para navegar, por ejemplo, puesto que incluye la distancia a los objetos. 

Figura 1: Robot Pioneer 2DX simulado en Gazebo.

El robot a programar es un Pioneer 2DX con sistema de dos cámaras y sensor láser, aunque únicamente se utilizarán las cámaras.

.

Figura 2: Escena/mundo simulado en Gazebo.

Las pruebas se realizarán en el simulador Gazebo, sobre la escena/mundo que puede verse en la Figura 2.








Se programará sobre el código fuente del componente Introrob, que recibe los datos de los sensores simulados del robot y permite controlar sus actuadores, 8 veces por segundo.

                  PRIMER PASO: CÁLCULO DE PUNTOS                   

Ya que se busca un algoritmo que funcione en tiempo real, no se realiza una reconstrucción de nube de puntos densa, sino que se analizan únicamente puntos de interés, que en este caso se han definido como los bordes en las imágenes, obtenidos mediante un filtro de Canny.

Figura 3: Imágenes de Bordes obtenidas mediante un filtro de Canny, para ambas cámaras.

Se recorre la imagen de bordes izquierda y, para cada punto de interés hallado (es decir, para cada píxel blanco), se calculan sus coordenadas en el mundo 3D, siguiendo la metodología que se explica a continuación:

Obtención del rayo de proyección izquierdo
Primero, y con las funciones graficas2opticas y backproject implementadas en la API de Introrob, se retroproyecta dicho punto al mundo 3D. La profundidad z de este punto no es la del punto real (salvo coincidencias), sino que es una de sus infinitas posiciones posibles dadas por el rayo de proyección, que une las coordenadas 3D del foco de la cámara y la del punto 2D en la imagen de la cámara. Con  las coordenadas del punto 3D y de la cámara, se calcula dicho rayo de proyección y se define en él un segundo punto 3D.

Figura 4:  Obtención del rayo de proyección del punto de interés izquierdo y de dos puntos 3D en él.

Obtención de la línea epipolar y del punto homólogo en la imagen derecha

Ahora, con la ayuda de las funciones project y opticas2graficas, se proyectan ambos puntos sobre la segunda cámara, es decir, se obtienen sus posiciones 2D en la imagen derecha. Calculando la recta que atraviesa ambos puntos, se obtiene la línea epipolar, es decir, la proyección del rayo izquierdo o, dicho de otra manera, todas las posibles ubicaciones del punto homólogo al punto de interés que se está analizando en la imagen izquierda. Así, se reduce el área de búsqueda del punto homólogo de una imagen completa a una línea de la misma (o una franja, para mayor seguridad). 

Dicha búsqueda se realiza mediante template matching sobre las imágenes a color originales, comparando una vecindad o "parche" del punto de interés izquierdo con los parches de los puntos de interés de la imagen derecha que estén situados en la línea epipolar. Aquél con el parche de mayor similitud será considerado el punto homólogo.

Figura 5:  Proyección de los dos puntos 3D en la imagen derecha y obtención de la línea epipolar (en rojo).

Obtención del rayo de proyección derecho y de las coordenadas 3D del punto

Por último, y de manera análoga al cálculo del rayo de proyección izquierdo, se calcula el rayo derecho. Idealmente, estos dos rayos intersectarán en la posición 3D del punto. Sin embargo, rara vez intersectan, así que se calcula el punto de mínima distancia entre ambos, y se toman sus coordenadas como las coordenadas 3D del punto en el mundo real.

Figura 6:  Obtención de las coordenadas 3D a partir de los rayos de proyección de los puntos homólogos 2D.

Este algoritmo se ha implementado en la rutina de navegación del robot (RunNavigationAlgorithm) para obtener una carga computacional menor, almacenando en un vector las coordenadas 3D de todos los puntos analizados.

                   SEGUNDO PASO: REPRESENTACIÓN                    

Para visualizar el resultado, se ha implementado en la rutina gráfica del robot (RunGraphicsAlgorithm) una función que recorre el vector de coordenadas previamente creado y, gracias a la función drawSphere de la API de Introrob, los representa en la ventana gráfica tridimensional de dicho componente.

Figura 7:  Representación de los puntos obtenidos.

                                        RESULTADOS                                          

En la Figura 7 se puede ver el resultado del algoritmo. Puede apreciarse que la representación está ligeramente desplazada hacia abajo y a la derecha. No parece que haya ningún error en el código, así que quizá se deba a algún dato erróneo en los archivos de los parámetros de las cámaras, o quizá en la función de proyección de la API. La coherencia local de los puntos calculados, sin embargo, es correcta.

En el siguiente vídeo puede verse el algoritmo en funcionamiento, se aprecia la tasa de refresco de la representación, y además la cámara se mueve para mostrar que los puntos son en efecto puntos 3D, con diferentes profundidades:

En este segundo vídeo se muestra la reconstrucción creada mientras el robot navega por la escena:

Nuevamente, cabe decir que el comportamiento del simulador Gazebo y por tanto el rendimiento del algoritmo depende mucho del equipo con que se trabaje, y de que se estén ejecutando o no otros procesos al mismo tiempo. El vídeo se ha grabado a partir de un ordenador portátil core i5 con 8gb de RAM y tarjeta gráfica Nvidia Geforce 720M.