Navegación visual: mejoras I

Tras trabajar la cabeza y el código, he incluido las siguientes mejoras:

1. He acotado la visión del robot, dado que a partir de una cierta distancia, las medidas son menos exactas. Con ello generamos menos segmentos imprecisos.
2. He añadido una navegación sencilla, donde la aceleración (positiva / negativa) es progresiva, con lo que evitamos los movimientos bruscos del robot (y consecuentemente de la cámara) que nos daba coordenas incorrectas de los puntos.
3. He afinado aún más las constantes de fusión, con lo que se fusionan más segmentos y por tanto se almacenan menos.

Aún cuento con dos problemas que explico en el foro de la asignatura y con la negativa de recordmydesktop para grabar los vídeos…

Una prueba de los resultados:

En la imagen se ve al robot después de haber girado hacia el pasillo de la derecha. Se ve claramente el mapa en memoria generado desde su salida hasta que ha alcanzado la posición en la que se encuentra.

Otra imagen más, esta vez le he dado la vuelta al robot..

Y en breve os colgaré un par de vídeos.

Navegación Visual: «viendo» obstáculos

Una de las aplicaciones interesantes de la navegación visual es la detección de obstáculos que no son detectados por el láser, dado que están en el suelo por debajo de este.

Veamos cómo se traduce esto en nuestro esquema.

El obstáculo utilizado es una puerta que he tirado (bueno, más bien ha sido el robot guiado por mi malicia) como terapia anti-stresss, tras pasar una hermosa tarde de verano frente al ordenador sin solucionar los problemas del movimiento (os lo recomiendo, te deja relajado y feliz!).

Bueno, vamos al tema. Os dejo una imagen donde podéis comprobar cómo el láser no detecta ningún obstáculo, mientras que el mapa me muestra el segmento que me impide pasar:

Navegación Visual: problemas con el movimiento

Es hora de darle movimiento a nuestro robot y afinar el diseño para que pueda navegar de forma autónoma, basándose en los datos extraidos de las imágenes capturadas. Y con esto llegaron más problemas…

Resulta que al iniciar la marcha, parar, acelerar, frenar o bien girar (aunque un poco menos en este último caso), la cámara sufre un movimiento que hasta ahora no había considerado importante y que está produciendo segmentos basura en el mapa construido por el robot.

Al cambiar la posición de la cámara, las coordenadas de los puntos detectados no son exactas, lo que genera errores.

Esto podemos intentar solucionarlo acelarando/frenando de forma progresiva… algo así como el controlador PID programado para la práctica del siguelíneas (en este punto me encuentro trabajando).

Otro problema lo encontramos cuando detectamos segmentos pero a una gran distancia del robot. Al acercarse, la posición de estos no coincide, quedando generalmente por detrás del nuevo detectado:

Por último, otro efecto negativo del movimiento es la imprecisión de los puntos detectados al aumentar la velocidad, lo cual nos lleva a definir segmentos incorrectos… ¿solución? tendremos que pensarlo…

Navegación Visual: cambio de estrategia

Como dice Juanes en una de sus canciones, «It’s time to change»… ¿y por qué?. Porque después de complicar cada vez más el código intentando mantener la detección de la «frontera» descrita en los posts anteriores, me he dado cuenta de que para comprobar si existen obstáculos en el suelo se complica todo tanto que el código generado se ha vuelto demasiado ineficiente (da demasiadas vueltas).

Por ello he dado marcha atrás y he puesto a funcionar mi cabecita en busca de una solución mejor y…. ¡eureka! la encontré.

El nuevo método para obtener tanto la frontera como los obstáculos es el siguiente:

La idea es recorrer la imagen por columnas, de izquierda a derecha y de abajo hacia arriba. De esta forma buscamos el primer punto que no coincida con el color del suelo.

También comprobamos si detrás de ese primer punto encontrado, en la siguiente fila, hay suelo.  En ese caso el punto detectado no se corresponde con un límite, si no más bien con una esquina que no me deja ver lo que hay detrás…

Con ello estamos simulando, mejor que con el método anterior, el funcionamiento del laser, obteniendo un total de 340 puntos.

Realmente hemos reducido a una cuarta parte el código tanto del escaneado de la pantalla como de la construcción de segmentos.

Resultados:

Y un detalle de una esquina:

Navegación Visual III: Segmentación

Esta es quizás una de las tareas más sencillas, dado que vamos a reutilizar el código de la práctica anterior (construcción de mapas), modificándolo para que trabaje con los puntos obtenidos de la «frontera» en vez de con el laser.

Dado que estamos escaneando la imagen de abajo a arriba, y con el fin de simular un comportamiento similar (que no igual) al laser, realizaremos dos bucles:

1. Recorrido por todos los límites izquierdos del suelo (de abajo a arriba).
2. Lo mismo pero con el lado derecho.

De esta forma, cada punto frontera será traducido a su correspondiente punto en el mundo (coordenadas absolutas – explicado en el post anterior) y pasado al constructor de mapas.

Resultado:

Navegación Visual II: Conversión de puntos 2D a 3D

Bueno, realmente el concepto es más amplio de lo que se expresa en el título. La tarea sería mas bien convertir los puntos que delimitan el suelo, expresados mediante coordenadas 2D (y en un sistema de referencia de la imagen tomada por la cámara), a puntos 3D en el sistema de referencia absoluto (en el que se define la posición del robot, ..).

Aquí es donde entra en juego la biblioteca Progeo.

La idea es que, a través de los parámetros de la cámara, dado un pixel, se pueda calcular su correspondiente punto en 3D (con respecto a la cámara).

Aquí tenéis la receta que explica cómo funciona y qué hay que hacer para utilizar esta biblioteca. A lo que añado modificar el Makefile para que al compilar enlace con el «Progeo.h».

El primer paso previo al uso de progeo es calibrar la cámara, esto es, pasarle todos los parámetros correspondientes de la cámara (extrínsecos e intrinsecos) necesarios para realizar las conversiones. Estos (en la versión 4.2.1 de jde) son:

• HPoint3D position; /* camera 3d position in mm */
• HPoint3D foa; /* camera 3d focus of attention in mm */
• float roll; /* camera roll position angle in rads */
• float fdist; /* focus distance in mm*/
• float u0,v0; /* pixels */

El primero, position, coincide con la ubicación del robot en sus componentes X e Y, y la Z indicando la altura a la que se encuentra la cámara sobre le robot. Hay que tener en cuenta que estas coordenadas serán relativas al robot, y por tanto nuestras X e Y serán 0.0 y 0.0.

El foa va a indicar la orientación de la cámara, es decir, hacia donde está mirando. Para ello hay que indicarle un punto 3D. En nuestro caso, la cámara mira hacia delante del robot y por ello un punto válido podría ser (100.0 0.0 200.0), o cualquier punto que se encuentre delante de él. roll será 0, dado que la cámara no estará inclinada con respecto a la vertical.

Por último, para calcular fdist, u0 y v0, seguiremos lo expuesto en la receta anterior de progeo.

Para aplicar todos estos parámetros tendremos que utilizar la función update_camera_matrix, y con esto ya estamos preparados para utilizar progeo.

Como podéis comprobar, esta biblioteca se compone principalmente de dos funciones: project y backproject. En nuestro caso nos interesa la segunda, backproject, con la que, proporcionándole un punto de la imagen (un pixel en 2D) obtendremos la proyección en 3D de este.

Previamente, el punto 2D perteneciente al pixel de la imagen tendremos que convertirlo de gráfico a óptico (Xóptica=239-Ygráfica e  Yóptica=Xgráfica).

Project trabaja en el sistema de referencia solidario al robot (relativo). Esto es importante a tener en cuenta, dado que el punto que obtengamos tendrá como origen de coordenadas la posición del robot.

Una vez que obtenemos el punto con el backproject tendremos que construir el rayo proyectado (recta formada por la posición de la cámara y el punto obtenido) y calcular su intersección con el suelo (el plano con la componente Z=0).

Por último, traduciremos el punto obtenido (sistema de referencia relativo al robot) al sistema de referencia absoluto del mundo, para lo cual utilizaremos la función «relativas2absolutas».

Con todo esto habremos calculado, para cada pixel «frontera» de la imagen (aquellos que nos representan el suelo) su correspondiente punto en el mundo y estaremos preparados para comenzar a segmentar…

Navegación Visual I: Extracción del suelo

Para identificar el suelo utilizaremos un filtro de color. En nuestro mundo el suelo está pintado de amarillo (R=255 G=255 B=164), con lo que en principio vamos a extraerlo de la siguiente forma:

La imagen que nos proporciona la cámara es de 320×240 píxeles, con lo que tendremos 240 líneas (rows), y en cada una 320 posiciones (columns).

Recorreremos la imagen de abajo a arriba (dado que suponemos que el suelo estará abajo) y de izquierda a derecha para buscar el primer pixel (situado más a la izquieda), que coincida con el color del suelo, y realizaremos la misma tarea de derecha a izquierda para buscar el último píxel(situado más a la derecha). Realizaremos esto para cada línea hasta que no obtengamos suelo.

De esta forma, obtendremos los límites del pasillo.

Las posiciones anteriores se identificarán mediante las coordenadas correspondientes a la fila (comenzando de abajo a arriba) y la columna (de izquierda a derecha), situando por tanto el origen de coordenadas (0,0) en el extremo inferior-izquierdo de la imagen.

Hay que notar que estas coordenadas están expresadas en 2D (dimensiones) y en un sistema de referencia perteneciente a la imagen… nada que ver el sistema de referencia absoluto (del mundo) ni con el relativo al robot.

Navegación visual

Ya estamos de vuelta para ponernos manos a la obra con la última práctica (por fin!).

En esta ocasión el objetivo está en poder navegar por el mundo utilizando el pioneer (desde el simulador Gazebo) usando para ello sólo la información que nos da la cámara (nada de laser).

Las tareas a realizar serán las siguientes:

1. Identificar el suelo en la imagen captada por la cámara (2D).
2. Realizar una conversión de los puntos 2D a 3D (utilizando la biblioteca Progeo).
3. Representarlo mediante segmentos.
4. Utilizar esta información para navegar.

El mundo en el que se desarrolla esta práctica es el departamental de la URJC de Móstoles:

Construcción de mapas (final)

… o eso espero!

Por indicación de José María (el profe 😉 ), os muestro un último vídeo para que se pueda comprobar con detalle la eliminación de segmentos no válidos.

Para saber si un segmento es o no válido, por cada punto del laser detectado, comprobaremos si el segmento formado por la posición del robot y este punto tiene intersección con cualquiera de los segmentos que tenemos almacenados. En ese caso, si el segmento es «pequeño» lo eliminanos, mientras que si es grande lo acortamos por el punto de intersección.

Pero lo mejor es verlo:

Como podéis comprobar, al avanzar el robot azul, el rojo va detectando lo que había detrás de este, fusionando con los segmentos que ya teníamos e, igualmente, eliminando los segmentos «no válidos» (donde estaba el robot azul).

La calidad del video es peor que la de los anteriores, pero es que mi recordmydesktop se niega a trabajar (no se si es porque es fin de semana, porque no aguanta el player con los dos jde de cada robot, …) y he tenido que hacer uso de la cámara del móvil.

Hasta otra!

Menos segmentos, captura mejorada

He mejorado tanto la fusión de segmentos como la eliminación, así como restringido los segmentos que se almacenan en función de su tamaño.

Os muestro otra imagen con los resultados:

y el vídeo prometido: