Práctica 3D Reconstruction - 3

Finalmente, para completar la práctica se han calculado los puntos 3D del mundo a través de los píxeles centrales de las ventanas, y para ello, se han utilizado otra vez las funciones:

point2d = graficToOptical(point2d)

point3d = backproject(point2d)

point3d = getCameraPosition(self)

Estos puntos son las lineas que se proyectan desde las cámaras para visualizar el objeto. Con ellas y el script para calcular la linea más corta entre dos lineas (Extraído de esta página y adaptado a python y la biblioteca numpy). Con esta línea se calcula la mitad entre ellas para tener el centro.

Este punto es el que ya se puede dibujar en la herramienta 3D Viewer-web y extrayendo el color del pixel central de la ventana de la imagen izquierda, ya se podría añadir con la función drawPoint(point,color). Obteniendo así el siguiente resultado (Como se puede ver en el vídeo se ha movido un poco hacia la derecha el robot Kobuki para que cogiese todos los cubos con letras, que es lo que mejor reconstruye por estar más cerca):

Como se puede ver, la reconstrucción es bastante buena, (aunque se pueden ver puntos mal calculados que aparecen por el fondo) apareciendo unas tres líneas de profundidad, con las formas de los objetos. Con los colores hay más problema, ya que la iluminación con la que consta el entorno no es muy intensa.

En la reconstrucción del vídeo anterior se ha utilizado una ventana de 40x40 píxeles para tener una mejor reconstrucción y como se puede ver al final del vídeo tarda unos 68,88 segundos en realizar toda la reconstrucción, contando que está grabando el ordenador y con una sesión remota de Teamviewer, normalmente hace toda la reconstrucción con unos 53 segundos.

Como añadidos a la práctica se ha dado la posibilidad de no dibujar los puntos predichos hasta el final de todo el procesado o mientras tanto (Como en el vídeo). Se da la posibilidad de utilizar la diferencia entre ventanas en vez de la correlación, para poder ver que es peor que la correlación. También se ha añadido en la imagen filtrada derecha (imageFiltered) la visualización de por que fila se encuentra actualmente y el número de puntos predichos. En el mismo código también se da la posibilidad de visualizar las ventanas de la imagen izquierda y de la derecha que son más parecidas. (Como en la publicación anterior)

Me parece que es interesante nombrar este problema, porque aparte de aparecer en el mismo código de Academy (JdeRobot) me pasó más de una vez. Tener en cuenta el nombre de las variables y las funciones llamadas de la cámara izquierda y de la derecha, así como de sus imágenes, porque me hizo perder mucho tiempo sin saber por que salía mal.

Práctica 3D Reconstruction - 2

Siguiendo donde terminé en la publicación anterior. Ahora quedaba recorrer toda la imagen izquierda(gradiente) con el tamaño de ventana (30x30 píxeles) y calcular la linea epipolar.

Para ello, se han utilizado las funciones internas de self.camLeftP y self.camRightP:

point2d = graficToOptical(point2d)

&

point3d = backProject(point2d)
&
point3d = getCameraPosition(self)
&
point2d = project(point3d)
&
 point2d = opticalToGraphic(point2d)

Con la función graficToOptical(point2d) se transforma el punto 2D de la imagen izquierda (el centro de la ventana creada) en el punto 2D óptico y a través de la función backProject(point2d) se obtiene la retro-proyección del punto 2D óptico en el punto 3D de la escena.

Con las función getCameraPosition(self) de la cámara izquierda se puede obtener la posición en 3 dimensiones de la cámara. Esta posición, junto con el punto 3D de la escena servirán para calcular el punto en la imagen(Graphic) de la cámara derecha, para ello se utiliza esta función:

point3d = B + ((A - B) * 4)

Siendo A y B la posición 3D de la cámara izquierda y el punto 3D retro-proyectado del centro de la la ventana izquierda.

Para ello, se utilizan las funciones project(point2d) y opticalToGraphic(point2d). Primero para obtener el primer punto, con el punto 3D retro-proyectado del centro de la la ventana izquierda, y el segundo punto3d (point3d) de la anterior fórmula.

Después con el primer punto y restando y sumando el doble de la diferencia de ambos respectivamente, se obtienen los dos puntos para calcular la línea epipolar (P1 y P2 en el código).

Para calcular los puntos de la línea se utiliza la biblioteca bresenham que dado dos puntos te da todos los puntos que unen ambos en línea recta. Pero viendo los resultados que se obtenían, aparecían puntos  negativos y decidí hacer un filtrado de estos puntos, eliminando aquellos que no se encontraban dentro de la imagen.

Con esto se conseguiría saber cuales son los puntos de la línea epipolar y ahora solo quedaría buscar en la imagen de la derecha para saber que ventana se parece más a la actual de la imagen izquierda.

Primeramente, se ha utilizado el sumatorio de la diferencia absoluta de los píxeles entre estas ventanas y encontrando el mínimo valor:

min(Σ((WindowL - WindowLMean)  - (WindowR - WindowRMean)))

Pero al ver los resultados obtenidos no eran muy buenos, y viendo los blogs de los ex-compañeros decidí cambiarlo por la correlación(equality en el código) entre ambas ventanas. Calculando la siguiente fórmula:

max(Σ((WindowL - WindowLMean) * (WindowR - WindowRMean)))

Con esto, ya daba buenos resultados.

Se han intentado visualizar ambas ventanas, para comprobar que eran iguales, en las imageFiltered  del GUI pero internamente como se modifica el tamaño y aparecen cortadas por arriba, abajo y a los lados, se ha optado por mostrar con OpenCV.

Como estas imágenes filtradas quedaban vacías se ha utilizado la imagen de la izquierda para mostrar el avance de la línea actual en verde sobre la imageLeft. Aunque como también aparece recortada, tarda un rato en aparecer en pantalla, en el siguiente vídeo se puede observar:

Como se puede ver en el vídeo, son muy similares ambas ventanas(Si va muy rápido se puede pausar el vídeo para comprobarlo).

Todo esto se ha realizado con los píxeles de la imagen que tienen un gradiente mayor a 0,3 para saber que hay un borde en esa zona y no realizar ninguna comprobación en el fondo o las zonas homogéneas.

Ahora tan solo quedaría con los dos píxeles centrales, encontrar el punto 3D de la escena y dibujarlo en el 3D Viewer-web. 

Práctica 3D Reconstruction - 1

En esta práctica se ha empezado de cero completamente ya que el año pasado no conseguí empezarla, por eso no hay publicaciones.

Para empezar la práctica, como me bajé el repositorio de JdeRobot(Academy) hace tiempo, que no tenía incorporado el 3D Viewer-web, y seguí las instrucciones del github (nuevas) para lanzar la práctica tardé bastante en comprender que no tenía la misma versión del repositorio.

Una vez pude arrancar el entorno, me fijé en las imágenes que capturaba el robot Kobuki y me parecían muy similares. Para comprobarlo hice una resta de ambas imágenes en valor absoluto y sume todos los valores, aparte de visualizarlo como una imagen. Como me temía, ambas imágenes eran iguales y rebuscando en el interior del código para ver de donde venían las imágenes me topé con un bug en el archivo Academy > src > 3d_reconstruction > sensors > sensor.py en la línea 39:

basecameraR = ic.stringToProxy(proxyStrCL)

e intercambiándolo por esta, se solucionaba:

basecameraR = ic.stringToProxy(proxyStrCR)

Para simplificar el problema transformé las imágenes a escala de grises y elegí como imagen inicial la izquierda (imageLeft). Posteriormente calculé la imagen gradiente de esta, normalizada de 0 a 1, calculando primero las derivadas con el eje x (dx) y con el eje y (dy) con la función gradient() de la biblioteca numpy. Para después hacer la siguiente fórmula para juntar ambas, normalizándola con su valor máximo:

√(dx² + dy²)

Obteniendo así el gradiente de la imagen píxel a píxel:

Una vez hecho esto, habría que recorrer la imagen con una ventana determinada, encontrando la epipolar de ambas imágenes y determina que ventana de la imagen derecha es más parecida a la imagen izquierda(en la línea epipolar).

Para ello primero se ha fijado tamaño de ventana (windowSize en el código) de 30x30 píxeles.

Aunque como tuve problemas para recorrer la imagen a través del tamaño de su ventana, ya que OpenCV saca el tamaño de la imagen (image.shape) con un array de número de filas, de columnas y de colores y desde un inicio pensaba que saldría con un array de x(columnas), y(filas), número de colores. (Con las filas y las columnas intercambiadas)

Con tantos problemas, ya avanzaré la práctica en otro momento.

Práctica - follow_line - 1 (Fin)

He englobado toda la práctica en una publicación ya que la práctica se ha realizado en pocos días y los primeros avances no eran significativos.

Primero se ha detectado la línea roja del centro de la carretera utilizando un filtro de color,  y para ello se  ha utilizado la herramienta ColorTuner de JdeRobot para calcular los valores HSV óptimos.

HSV mín = [0, 200, 45]

HSV máx = [10, 255, 255]

Después era interesante saber cuando empezaba la línea roja central de la carretera, y para ello, utilizando el filtro de color anterior y recorriendo todas las filas de la imagen desde el 60% de la imagen, se vio cuando empezaba la línea. Se comprobó por el centro de la imagen ya que a los laterales están las vallas que son del mismo color, o similar, a la línea.

self.row_with_line = 249

Una vez se consiguieron los datos anteriores ya se podía empezar a trabajar con la función execute() de MyAlgorithm.py. Como tan solo se utiliza la imagen de la cámara izquierda, se ha optado por mostrar en la imagen de la izquierda el filtrado que se ha realizado de la línea(rojo) y una cruz (verde) mostrando la fila que se escogió y la columna ideal de la línea en el eje horizontal, que es en el centro de la anchura de la imagen. Por otra parte, en la imagen de la derecha se han mostrado los datos relevantes:

• Velocidad (V)
• Velocidad angular (W)
• Error, con respecto a la posición ideal de la línea en el eje horizontal (E)
• Estado actual (S)
• Tiempo, en segundos, de la vuelta (T)
• Número de vueltas dado (LAPS)

Antes de explicar el funcionamiento del algoritmo es interesante nombrar los estados posibles así como su significado:

• STRAIGHT, significa que el coche se encuentra en el centro de la línea con un error:

-20 < Error(e) < 20

• +/- CURVE, son dos estados diferentes y el positivo significa que el error es superior a 20 y el negativo inferior a -20.

• NOLINE, significa que en la fila 249 no se ha encontrado línea y se inicia el contador para pasar al siguiente estado.

• SEARCH, este estado aparece cuando pasan más de 10 repeticiones seguidas sin encontrar la línea, entonces se establece una velocidad y una velocidad angular (si la anterior es negativa pasa a positiva y a la inversa) por defecto con tal de que el coche de vueltas poco a poco hasta encontrar de nuevo la línea, aunque esto no asegura que el coche siga la carretera hacia el sentido adecuado.

Para explicar el cambio de estados se ha creado un modelo de estados para que sea mucho más visual que redactado:

Para calcular el error se ha creado una función straight() que utiliza la posición ideal de la línea (mitad de imagen(eje x), self.initial_position_value en el código) y la posición actual de la línea(eje x) para calcular la norma lineal entre estas dos, con la función lineal.norm() de la biblioteca numpy. Para determinar si este error es negativo o positivo se utiliza también la posición ideal y la actual, ya que si la actual es menor que la ideal esta se pasa a negativo y en caso contrario sigue siendo positivo. En caso del estado NOLINE se ha optado por aumentar el error anterior en un 20% para el error actual.

Finalmente, utilizando este error y el error anterior (self.last_error en el código) se han creado los controladores Proporcional y Derivativo,  tanto para la velocidad como para la velocidad angular, siendo estas las fórmulas:

w_1, w_2 = - self.Kp_w * self.error, - self.Kd_w * (self.error - self.last_error)

self.w = w_1 + w_2

v_1, v_2 = - self.Kp_v * self.error, - self.Kd_v * (self.error - self.last_error)

        self.v = 13 - abs(v_1 + v_2) + self.time_v * 0.3

Siendo self.last_error  el error anterior, self.time_v es el número de iteraciones en el estado STRAIGHT, self.error el error actual y 13 una constante para la velocidad, ya que en caso estar en los estados +/-CURVE la velocidad sería menor a 13 y en caso del estado STRAIGHT la velocidad iría aumentando con las iteraciones. Para las constantes self.Kp_w/v y  self.Kd_w/v se ha ido adaptando el valor para obtener el mejor tiempo por vuelta posible, quedando finalmente en los valores:

self.Kp_w = 0.02
self.Kd_w = 0.015
self.Kp_v = 0.010
self.Kd_v = 0.015

Obteniendo así el siguiente resultado:

Dos vueltas en 1:40 minutos

Como añadido para saber exactamente cuanto tarda en cada vuelta, se ha realizado un filtrado de las líneas de inicio de carrera (blancas) cuando el self.time_straight(== self.time_v) sea mayor que 30 ya que es la recta más larga de la carrera y se ha guardado una imagen de la posición y el tiempo actual, como también se ha reiniciado el tiempo de inicio (self.start_time en el código):

Lap 1  Tiempo = 50.25s

Lap 2 Tiempo = 50.24s

Como se puede ver es un poco antes del letrero de START pero más o menos se puede estimar que como máximo tardaría 51 segundos cada vuelta.

Otra funcionalidad añadida es el poder pausar el tiempo cuando se para el algoritmo, para seguir con el mismo tiempo que se ha parado, aunque éste tiene un retraso de ~5 mili-segundos.

Visión Robótica - 2018

Este blog se creó en el 2017 para documentar todos los avances con las prácticas de la asignatura Visión Robótica del MOVA. Este año repito la asignatura y por lo tanto he de volver a postear los avances con las prácticas.

Todas las publicaciones anteriores se encuentran obsoletas y por lo tanto se empezarán de cero todas las prácticas, menos la práctica 0 que debido a su simplicidad (y que no es necesario entregarla) no se repetirá.

Todas estas prácticas van a realizarse en el sistema operativo Ubuntu 16.04 con los siguientes componentes:

• Procesador: Intel® Core™ i7-6800K CPU @ 3.40GHz × 12
• GPU: Nvidia GTX 1080 Ti
• RAM: 16GB
• SSD

Es interesante saberlo ya que los tiempos de cómputo serán diferentes según los componentes y en la presentación de las prácticas, estas se lanzaran con un ordenador portátil con peores componentes.

En esta publicación tan solo se ha instalado el entorno utilizando la guía de JdeRobot. Es interesante nombrar el problema que tuve con el error GPG. Al tener otra "llave pública" (diferente de D8A3751519274DEF) como no se nombra en la publicación que se ha de cambiar el valor, perdí un rato intentando entenderlo. Por si a otro alumno le pasó lo dejo escrito aquí.