Práctica 2: 3D Reconstruction (Puntos homólogos y triangulación)

En la entrada anterior encontramos la recta epipolar, por lo que ya podemos buscar los puntos homólogos. Para ello emplearemos la correlación para ver cuales son los puntos más parecidos entre ellos.

En primer lugar se probó a realizar la búsqueda del punto más parecido recorriendo la epipolar y en un margen respecto a ella, ya que puede haber errores de calibración. Esta búsqueda se hacia mediante parches y comprobando la correlación que había entre ellos. Cuanto mayor correlación mayor parecido existe. Es decir, se realizaba SAD (Sum of Absolute Differences), pero se ha comprobado que se introduce bastante ruido en la reconstrucción. 

Por este motivo, se ha decidido emplear SAD, pero empleando puntuación. Es decir, se hace la diferencia absoluta y si supera un umbral se da un voto. Una vez tengamos la suma de los votos de cada parche nos quedaremos con el que mayor puntuación tenga. Hay que mencionar que la correlación se hace en el espacio de color HSV y el tamaño de parches que se ha empleado es de 23x23.

En la siguiente imagen podemos observar la epipolar en color rojo, y los márgenes hasta los cuales se busca el punto homólogo en azul.

En la siguiente imagen se puede ver un punto de interés marcado con un círculo azul y tras esta una imagen del punto homólogo de dicho punto pintado en rojo.

Una vez hemos encontrado los homólogos y tenemos emparejados los puntos, ya podemos realizar la reconstrucción 3D. La reconstrucción consistirá en calcular los rayos de retroproyección de cada punto emparejado y con ellos el punto donde intersectan. Debido a que puede haber errores en la calibración de las cámaras puede que los rayos no intersecten por lo que se busca el punto de mínima distancia.

Los rayos de retroproyección se obtienen de la siguiente forma:

1. Retroproyectamos los puntos al espacio 3D. Para ello convertimos los puntos al sistema de la cámara mediante:

 

pointInOpt = self.camRightP.graficToOptical(pointIn)

Se obtiene el punto 3D correspondiente al 2D:

p3d = self.camRightP.backproject(pointInOpt)

 

2. Una vez que tenemos los puntos en el espacio 3D y la posición de las cámaras, ya podemos calcular los rayos de retroproyección. El rayo de retroproyección será la recta que pase por el punto y la posición de la cámara.

Una vez tenemos los rayos de retroproyección, el punto 3D debería situarse donde intersectan ambos rayos de retroproyección. Esto como hemos comentado anteriormente, solamente ocurrirá en una situación ideal en la que la calibración de las cámaras sea perfecta, pero en la realidad no es así. Por lo que se busca el punto que minimiza la distancia entre ambas rectas. Para realizar esta búsqueda me he basado en la solución que se plantea en la siguiente página: http://www.homer.com.au/webdoc/geometry/lineline3d.htm. 

En la siguiente imagen se puede ver un esquema de la situación que tendríamos entre nuestros dos rayos de retroproyección.

Donde P1 es la posición de la cámara izquierda, P3 es la posición de la cámara derecha, P2 es la posición del punto 3d detectado de la cámara izquierda, y P4 es el punto 3d correspondiente al homólogo.

Tras obtener los puntos pa y pb, que se mencionan en el enlace anterior, se puede calcular nuestro punto tridimensional, ya que se encontrará entre ambos puntos. Además, cuando buscamos la intersección de los rayos para calcular el punto 3d, se ha introducido una restricción para tratar de eliminar errores. La restricción es la siguiente: si la distancia entre pa y pb  es superior a un umbral, el punto será descartado. 

El siguiente punto que se ha llevado a cabo es el pintado de los puntos. Si se pinta cada punto cada vez que vamos realizando el proceso anterior, el tiempo de cómputo es bastante elevado. Por lo que se ha decidido guardar todos los puntos en un array y pintar dichos puntos al final. Esto agiliza bastante el cómputo.

En la siguiente imagen se puede ver la reconstrucción que se ha obtenido como resultado final: 

En la reconstrucción se puede ver que existe un poco de ruido, pero es bastante complicado realizar una reconstrucción sin ruido. Además, se puede ver que los puntos tienen diferentes profundidades, esto se debe a que en la escena 3D del mundo unos puntos están más alejados que otros de la cámara.

Además, se ha probado a fijar la posición de la profundidad de los puntos para ver cómo sería el resultado, ya que en la anterior imagen algunos puntos tienen diferente profundidad y es más complicado apreciar la reconstrucción. Esto lo podemos ver a continuación.

En la reconstrucción que se ve en las imágenes anteriores se hace una normalización del color entre 0 y 1. Pero está normalización se ha realizado dividiendo por el máximo valor del color del punto en la imagen para realzar ciertos tonos. Este color no es realista, por lo que se ha realizado una normalización por 255, que se puede ver en la siguiente imagen.

 

Práctica 2: 3D Reconstruction (Puntos de interés y epipolar)

En esta entrada se va a explicar cómo se calcula los puntos de interés y la epipolar de un punto.

Puntos de interés

El primer paso, como hemos visto en la entrada anterior, es buscar los puntos de interés. Los puntos de interés son los bordes, ya que son los que nos proporcionan más información acerca de la imagen. Para buscar los bordes se ha empleado el filtro de Canny.

Una vez tenemos la imagen binaria de los bordes, se emplea la función np.where que nos devuelve los puntos de la imagen donde hay borde. De esta forma, usando np.where no tenemos que recorrer la imagen, lo cual supondría más tiempo de ejecución. Una vez hayamos hecho esto ya tenemos nuestros puntos de interés.

En la siguiente imagen podemos ver en la GUI la representación de los bordes tanto de la imagen izquierda como de la derecha.

Hay que mencionar que se han tomado como referencia los puntos de la imagen izquierda y los homólogos se buscarán en la imagen derecha.

Epipolar

Para cada punto de interés que tenemos del paso anterior deberemos buscar la epipolar. Para calcular la epipolar se ha seguido el esquema de clase (el cual se describirá a continuación):

La epipolar se calcula para cada punto de interés siguiendo los siguientes pasos:

1.  Transformamos el sistema de coordenadas al de la cámara de la siguiente forma:

pointInOpt = self.camLeftP.graficToOptical(pointIn)

2. Retroproyectamos el punto 2D en el espacio 3D mediante:

point3d = self.camLeftP.backproject(pointInOpt)

3. Obtenemos el rayo de retroproyección. El rayo de retroproyección se calcula haciendo el producto cruzado (para lo que empleamos np.cross) del punto 3D (point3d) y la posición de la cámara izquierda. La posición de la cámara izquierda la obtenemos de la siguiente forma:

positionCamLeft = self.camLeftP.getCameraPosition()

4. Del rayo de retroproyección se tomarán dos puntos que se proyectarán sobre la imagen derecha. Estos puntos se calcularán de la siguiente forma:

projected1 = self.camRightP.project(point3d)

En mi caso se ha tomado el punto que habíamos retroproyectado al espacio 3D, y el punto medio entre el punto retroproyectado en el espacio 3D y la posición de la cámara.  

Estos puntos proyectados tendremos que pasarlos a las coordenadas de la imagen derecha mediante:

p1 = self.camRightP.opticalToGrafic(projected1)

5. Mediante los dos puntos proyectados en la imagen derecha se calculará la epipolar mediante las ecuaciones de la recta.

 En la siguiente imagen se puede ver un punto de interés marcado con un círculo azul y tras esta una imagen de la epipolar de dicho punto pintada en rojo.

 

 En el siguiente vídeo podemos ver la epipolar de los puntos de interés (se ve cómo se pinta una epipolar tras otra) :

Práctica 2: 3D Reconstruction (Introducción)

La práctica 3D Reconstruction (https://github.com/JdeRobot/Academy/tree/master/src/3d_reconstruction) consiste en realizar una reconstrucción 3D a partir de un par estéreo. Para realizar la práctica se empleará el simulador Gazebo, la plataforma JdeRobot y la librería OpenCV para procesamiento de imágenes. En Gazebo se encuentra el entorno o mundo que observaremos con las cámaras y el cuál debemos reconstruir. La reconstrucción 3D que realicemos se proyectará en el espacio 3DViewer (una herramienta para la visualización 3D).

Para poder ver la práctica en funcionamiento primero deberemos preparar el entorno de la práctica. Para ello hay que ejecutar los siguientes comandos en un terminal:

Primero copiar la nueva interfaz:

sudo cp interface/visualization_ice.py /opt/jderobot/lib/python2.7

Luego preparar el 3dViewer:

sudo apt-get install nodejs

sudo apt-get install npm

cd 3DViewer-web

npm install electron --save-dev --save-exact

npm install jquery

npm install js-yaml 

Es posible que durante la instalación de electrón salte  un error. Para solucionarlo ejecutar:

sudo apt-get install nodejs-legacy

source ~/.bashrc 

Para poder ejecutar nuestra solución de la práctica habrá que ejecutar en un terminal:

* Ejecutar el mundo en Gazebo: gazebo reconstruccion3D.world

* Ejecutar el componente 3d_reconstruction: python2 3d_reconstruction.py 3d_reconstruction_conf.yml

* Ejecutar el visor 3D:

cd 3DViewer-web
npm start

Si ejecutamos el mundo de Gazebo podemos ver el robot Kobuki, que mediante las dos cámaras, tendrá que ser capaz de reconstruir justo los objetos que tiene en frente. El escenario de Gazebo lo vemos en la siguiente imagen:

Antes de reconstruir la escena se ha probado a pintar algún punto en el visor 3D. Un punto cualquiera se puede pintar en el visor si ponemos en el método execute de MyAlgorithm.py el siguiente código:

point = np.array([1,1,1])

color = (0, 255, 0) 

 self.sensor.drawPoint(point, color)

Si pintamos 4 puntos de ejemplo en color verde en el visor, obtenemos el siguiente resultado:

Para poder realizar el código que nos permita reconstruir la escena 3D debemos seguir los siguientes pasos:

1. Obtener los puntos de interés.

2. Encontrar los puntos de la imagen derecha que se corresponden con los puntos de la izquierda, es decir, los puntos homólogos. Para ello deberemos hacer uso de la epipolar.

3. Realizar una triangulación para hacer la reconstrucción 3D de la escena mediante los pares de puntos.

4. Representación.

Práctica 1: Follow line (Prueba 2)

En la segunda prueba que se ha realizado se ha empleado un control PD en vez de un control P, ya que como vimos en la prueba 1 el fórmula 1 oscilaba mucho si empleamos únicamente un control P.

Como vimos en la anterior entrada, solamente se analizaba una única fila de la imagen, lo cual no nos proporciona suficiente información para saber si nos encontramos en recta o en curva. En esta prueba se ha modificado este procesamiento de la imagen. En esta prueba se analizan tres filas de la imagen izquierda para saber si el coche se sitúa encima de una recta o de una curva, o si por otro caso se ha salido el coche de la línea. En este caso se analizan las filas situadas en las posiciones y1 = 260, y2 = 310, e y3 = 350. En estas líneas se calcula el centro de la línea roja, para lo cual se comprueban los valores de la imagen filtrada. Con estos centros podremos saber si nos encontramos en recta o curva.

El siguiente paso es procesar esta información. El centro de la línea roja situado en la fila y1 (260) será el único que no perderemos siempre que nos encontremos cerca de la línea roja, ya que es la posición que está situada más arriba. Si no encontramos la línea roja en la posición y1 es debido a que nos hemos salido de la línea roja. En esta situación el coche deberá ir hacia atrás y girar hacia el circuito para continuar su recorrido. Si la última vez que se ha visto la línea estaba a la izquierda de la imagen, quiere decir que el coche se ha salido hacia la derecha, y por tanto tendrá que girar a la izquierda y viceversa. Este caso se ha añadido en la práctica.

Los centros situados de la línea roja situados en las líneas y2 e y3 es posible que se pierdan al llegar a una curva grande. Esto introducirá errores.

La desviación que se tiene se calcula respecto a la posición ideal, que hemos visto en diferentes pruebas que es x=326. Para esto se calculará la diferencia entre la posición x de y1 (260) y la posición ideal. Esta es la desviación que tendremos en cuenta a la hora de hacer los diferentes controles PD que se van a hacer. Como hemos visto anteriormente, se tiene un caso por si el coche se ha salido del circuito.

Otro de los casos que se han tenido en cuenta es si el centro situado en la fila situada más abajo (y3 = 350) se ha perdido. Esto puede ocurrir si estamos en una curva grande. Si se da este caso se realizará un control PD adaptado a esta excepción. En función de la desviación que hay entre el centro situado en la fila de arriba (y1) y el centro situado en la fila de en medio (y2), se darán diferentes situaciones en las que se ajustará un PD diferente.

Hay que mencionar que un control PD está definido por un control proporcional y un control derivativo. El control PD sigue la siguiente fórmula:

Corrección = kp * error + kd * (error - error_anterior)

Otro caso que evaluaremos es si nos situamos en recta o curva. Si estamos en recta se aplicarán unas situaciones de control PD y si estamos en curva otras. Para diferenciar si nos encontramos en recta o en curva se empleará la diferencia entre la posición x de y3 y de y1. Lo que se hace exactamente es calcular la recta que pasa por el centro de y1 e y3, y después se mira la posición de x que se encuentra en esta recta para la fila y2 (310). De este modo conociendo el centro de la fila y2 y el punto x que se encuentra en la recta calculada, podemos saber si estamos en curva o recta. Esto se explica mejor a continuación. Para saber si es curva o recta se siguen los siguientes pasos:

1. Conociendo la ecuación de una recta (y = m *  (x - x1) + y1) podemos calcular la pendiente de la recta del siguiente modo:

y = m *  (x - x1) + y1

260 - 350 = m * (x_arriba - x_abajo)

m = -90 / (x_arriba - x_abajo)

Donde x_arriba es el centro de la fila y1 y x_abajo es el centro de la fila de abajo.

2. Una vez conocida la ecuación de la recta que pasa por estos dos puntos, vamos a calcular el punto x de la fila de en medio (y2) que pasaría por esta recta. Este valor de x lo calculamos del siguiente modo:

 y2 - y3 = m * (x - x_abajo)

(310 - 350) * (x_arriba - x_abajo) / (-90) = (x - x_abajo) 

x = (310 - 350) * (x_arriba - x_abajo) / (-90) + x_abajo

3. Una vez tenemos el punto x que pasa por la recta calculada a partir de y1 e y3, podemos calcular la desviación de x respecto al centro de y2. Si esta desviación es mayor a un umbral es que estamos en curva y si no es que estamos en recta. Y como hemos dicho aplicamos unas situaciones de PD para recta y otras para curva.

Hay que mencionar que se ha tratado de ajustar los valores de los controles PD para que no oscilara mucho el coche, aunque sigue oscilando un poco, pero no tanto como en la prueba 1. 

Se ha hecho una prueba en la que se ha modificado la posición del Fórmula 1 en la salida para que no esté situado el coche justamente encima de la línea roja. En este caso se pone el coche en la zona izquierda de la línea. En el vídeo siguiente, se puede observar que el vehículo es capaz de volver otra vez encima de la línea roja.

Se ha hecho una prueba en la que durante la ejecución del código se mueve el vehículo con el teleoperador. En el vídeo siguiente, se puede observar que el vehículo es capaz de volver otra vez encima de la línea roja a pesar de intentar girar el coche con el teleoperador.

A continuación, se puede ver un vídeo de los resultados obtenidos en esta prueba. En esta prueba el tiempo de simulación en completar una vuelta es de 1 minuto y 34 segundos, mientras que el tiempo real es aproximadamente 2 minutos y 30 segundos.

 

A continuación se explica a que se debe esta diferencia entre el tiempo simulado y el tiempo real. En la parte inferior de Gazebo se puede ver el Real Time, el Sim. Time (tiempo simulado) y el Real Time Factor, los cuales tienen mucho que ver en el tiempo de ejecución de Gazebo. El parámetro Real Time expresa el tiempo real en ejecución. El factor Sim. Time expresa el tiempo simulado. Si utilizáramos un ordenador potente entonces el Sim. Time deberı́a estar próximo al Real Time. Mientras que si usamos un ordenador sin tantas capacidades veremos que el Sim. Time es mucho menor que el Real Time. Por su parte, el factor Real Time Factor es un producto de la tasa de actualización y el tamaño del paso. Si queremos obtener un tiempo de simulación bajo deberá estar alrededor de 1. Si este parámetro es menor que 1 veremos que la ejecución es más lenta, y cuando se aproxima a 0.2 o menos es demasiado lenta. 

En el caso del ordenador empleado para la práctica, el Real Time Factor es muy

bajo en algunas ocasiones durante el pilotaje, lo que hace que el Real Time sea mucho mayor que el Sim. Time. En este ordenador el Real Time Factor normalmente oscila entre 0.4 y 0.7, siendo en grandes ocasiones cercano a 0.55. Esto implicará que haya una gran diferencia entre el tiempo simulado y el tiempo real.