Práctica 2. Reconstrucción 3D: Obtención del Punto 3D

Teniendo ya ubicado cada punto de interés en ambas imágenes, podemos ya calcular las rectas de retroproyección de los dos puntos y obtener la posición 3D del punto, que será aquella donde ambas rectas se corten. No obstante, como suelen existir errores de calibración, no siempre se cruzarán las dos rectas, por se debe obtener el punto más cercano a ambas rectas y no la interseción como tal.

Para este cálculo se ha utilizado el algoritmo que referencia la compañera Jessica Fernández en su blog de la asignatura.

También se ha añadido la claúsula señalada por Julián Lamoso de no devolver aquellas distancias mayores de 1, consiguiendo con ello descartar puntos con correspondencias erróneas.

El resultado es bastante robusto (aunque como se puede ver hay algún error en alguno de los cálculos), pero existe un problema ìmportante de tiempos de ejecución del algoritmo. El cálculo de correspondencias de cada punto tarda en torno a un segundo de media, y para la ejecución del algoritmo con todos los puntos de interés obtenidos con Canny, la ejecución supera ampliamente la hora.

Todos los puntos

Para intentar bajar estos tiempos se ha añadido una condición para que sólo se traten la mitad de puntos de interés. El resultado es menos denso y detallado que tratando todos los puntos, pero se reducen de forma sensible los tiempos de ejecución totales de la reconstrucción 3D, aunque siguen siendo muy extensos: en torno a los 45 minutos.

Reconstrucción con la mitad de puntos

Como mejora se decide calcular primero los puntos, almacenarnos en una lista, y dibujarlos posteriormente todos de golpe en el 3DViewer. Según se aprecia, pasamos de tiempos de 45 minutos a en 15, por lo que se mejora altamente el algoritmo con este cambio.

No obstante, los tiempos siguen siendo muy altos, por lo que se deberían considerar otras mejoras como:

  • Hacer menos denso el resultado del cálculo de puntos de interes con Canny, perdiendo no obstante detalle en la reconstrucción.
  • Quitar iluminación, dejando una imagen a contraluz con la que sólo obtendríamos un contorno<(li>
  • Mejorar la búsqueda de parches actual. Aunque a la hora de recorrer la imagen derecha se tiene en cuenta la franja epipolar para optimizar la zona del cálculo de parches, el bucle en busca de esa franca recorre toda la imagen, una operación computacionalmente muy cara. Una mejora a tener en cuenta sería mejorar este recorrido de la franja epipolar.

Práctica 2. Reconstrucción 3D: Línea epipolar

El objetivo de la segunda práctica es dotar a un robot de la capacidad de reconstruir su entorno en 3D a través de dos cámaras estéreo.

Para conseguir este objetivo, seguiremos los pasos indicados en la teoría de Reconstrucción 3D de los apuntes de Visión Robótica:

  1. Obtención de Puntos de interés
  2. Búsqueda del punto homólogo
  3. Triangulación
  4. Representación

Así mismo, nos guiaremos a partir a la imagen de representación de funcionamiento de la Reconstrucción 3D mostrada en los apuntes y en los foros de ayuda:

Para el primer paso, aplicamos un filtro de Canny a la imagen izquierda de nuestra cámara, lo cual nos dará los bordes de la imagen. A partir de ahí recorremos la imagen binaria resultante, y los puntos que valgan 1 – sean parte de los bordes – serán nuestros puntos de interés. Lo siguiente, será buscar la línea epipolar del punto en la otra imagen.

Para cada punto de interés realizaremos los pasos indicados en los comentarios del código de la práctica:

  • opticalToGrafic: Convierte el punto de la imagen gráfico, en óptico. Toma como Origen de coordenadas la posición de la cámara.
  • backproject: Convierte el punto óptico en su representación 3D, sabiendo que esta conversión será parte de la recta de retroproyección.
  • project: Proyecta el punto 3D en el plano de la cámara derecha, en coordenadas ópticas.
  • graficToOptical: Convierte el punto óptico a coordenadas gráficas.

Así mismo, para poder obtener la recta epipolar necesitaremos dos puntos, por lo que deberemos obtener la recta de retroproyección de la cámara izquierda y proyectar otro de sus puntos sobre la cámara derecha. Sólo así podremos calcular la epipolar correctamente. Esto lo haremos de la siguiente forma:

p1 = Punto de interés
posCam = self.camLeftP.getCameraPosition
v = p1 – posCam (vector de proyección)
p2 = p1 + v * k (K será un desplazamiento cualquiera)

En mi caso tuve ciertos problemas con la coordenada homóloga, que vale 1 y sirve para el cálculo de la proyección a través de la matriz KRT, y que al tenerse en cuenta en las operaciones no mantenía ese valor de unidad, dando resultados incorrectos. Una vez se hicieron las operaciones sólo con las 3 coordenadas de posición se pudo obtener la recta epipolar correctamente, tal como se puede ver en la imagen (con varios ejemplos de K):

Con esto ya obtendremos un segundo punto que podremos proyectar sobre la cámara derecha y con el que podremos calcular la recta epipolar, siguiendo de nuevo la fórmula de la recta:

El siguiente paso será buscar a partir de esa recta una franja – con varios pixeles arriba y abajo para evitar errores de calibración de la cámara – y recorrerla buscando el parche más parecido al punto de interés de nuestra cámara izquierda. Así encontraremos el punto de interés en la segunda cámara y ya sí podremos obtener la posición 3D de dicho punto

Práctica 1. Follow Line: Giro con Control PD

El siguiente paso para el control reactivo del giro es añadir la componente D, derivativa, la cual introduce una derivada del error que nos da memoria del error y nos hace girar más o menos según su tendencia. Esto nos dará mayor suavidad en los giros.

Control PD = kP * errorP + kD * errorD

Siendo el errorD la derivada de error, la cual calcularemos de la siguiente manera:

errorD = (error actual de P – error anterior de P)

kD de nuevo será una constante de proporcionalidad que traducirá nuestras unidades de error en unidades de giro. Esta se ha obtenido a partir de ensayo y error y, curiosamente, tiene el mismo valor que nuestra kP, 0.01.

Con este añadido, como se puede observar, nuestro F1 toma las curvas con mucha mayor suavidad y se ha confirmado después de tener durante más de media hora ejecutándose nuestro algoritmo, que no se sale del trazado en ningún caso.

No obstante, añadir la componente Derivativa no nos ha supuesto una mejora en nuestros tiempos, por lo que será necesario definir un nuevo Control para la Velocidad, que aumente a más de 10 en rectas.

Práctica 1. Follow Line: Giro con Control P

Una vez conseguido el paso del filtro de color, se comienza a implementar el control reactivo con el que, a través de control visual sobre el resultado de nuestro filtro, se calcularán los parámetros de giro y velocidad.

Para comenzar mantendremos la Velocidad constante a 10 tal y como estaba indicada en los comentarios de nuestro método execute, e implementaremos un Control P para el giro.

Siguiendo la formula de los controladores PID, tenemos que:

Control P = Kp * errorP

¿Y cuál es nuestro error?

Una posible solución es la indicada en el enunciado práctica: La diferencia entre el centro del circuito cuando el robot está perfectamente alineado y el centro del circuito actualmente observado. Para calcularlo nos situaremos en una línea horizontal de la imagen y la recorreremos, almacenando el primer y ultimo pixel blanco encontrados en la imagen. Conocidos estos dos, podremos obtener el punto medio a partir de la fórmula (x1 + x2) / 2.

La primera vez que ejecutemos nuestro algoritmo almacenaremos este punto como referencia de alineamiento, ya que sabemos que en ese momento nuestro robot está perfectamente alineado, y a partir de ahí calcularemos errorP = (initCenter – actualMedPoint).

La Kp traducirá el valor de nuestro error (num pixeles) a la unidad de giro, y se obtiene a partir de prueba y error.

En el enunciado de la práctica se propone realizar el cálculo del error a un cuarto de altura. Se probó esta alternativa, pero se vió que estaba demasiado abajo y no daba tiempo para rectificar el giro a tiempo y si ampliábamos mucho la constante para intentar que el giro fuera grande el control se hacia ingobernable.

Es por ello que se probó a realizar la medición a mayor altura, analizando con mayor tiempo de respuesta el circuito. En concreto se realiza practicamente en la línea del horizonte,  height / 2 + 10. Con este punto de referencia, sí se consigue encontrar una constante con la cual podamos terminar el circuito (Kp = 0.01), aunque con grandes oscilaciones, lo cual causa que en algunas ocasiones, que no se precedibles, las oscilaciones hagan que nuestro robot se salga del circuito y sea incapaz de volver de nuevo al trazado.

En esta primera aproximación se completa el circuito en 1:46 min, como se puede observar en el siguiente vídeo:

Práctica 1. Follow Line: Filtro de Color

El objetivo de la primera practica es desarrollar un algoritmo con el que un robot sea capaz de completar de forma autónoma un circuito de F1. Para ello se realizará un control visual a través de un filtro de color con el cual identificar el circuito, que será de color rojo, y un control reactivo PID para el giro y la velocidad.

En primer lugar se ha comenzado con el desarrollo del filtro de color. Para que nuestro algoritmo no se vea afectado por la iluminación (como puede ser la ocasionada por sombras), se desarrollará éste siguiendo el modelo de color HSV, por lo que necesitaremos previamente pasar las imagenes ofrecidas por nuestra cámara de RGB a HSV.

Al no poder ejecutar correctamente los componentes de desarrollo, como es el caso del componente colorTuner, se ha tenido que buscar los limites inferior y superior a través de software externo de edición de imágenes, concretamente GIMP, e ir ajustando los límites inferior y superior ejecutando el código de la práctica, follow_line.py.  Finalmente, se consigue un buen resultado sin necesidad tampoco de partir la imagen a la mitad para evitar que el filtro capte las paredes de nuestro circuito, de color rojo oscuro.

No obstante, existe un problema importante con las oclusiones intermitentes de la línea de salida, como se puede ver en la siguiente imagen:

Para intentar evitar esta problemática se ha probado la alternativa comentada por otros compañeros en sus blogs de realizar un operación morfológica de cierre, pero en mi caso, pese a realizar multitud de pruebas, no se resuelve el problema en absoluto, por lo que finalmente se decide desechar este procesado.

 

Instalación del entorno

El entorno de prácticas es completamente libre. Como Sistema Operativo se utilizará Ubuntu 16.04 Xenial LTS y el entorno de desarrollo será propio de la universidad, JdeRobot.

Para la instalación del entorno se han seguido los pasos descritos en el Manual-5 de la web oficial de JdeRobot.

En un primer momento hubo ciertos problemas con las shared libraries necesarias para ejecutar las prácticas de la asignatura, pero realizando todos los pasos, inclusive el paso que se especifica como prescindible, Installation for running JdeRobot , se consiguió ejecutar el entorno con la práctica 1.

El código de las prácticas se encuentra alojado en el proyecto Academy del github de la asignatura.