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.variable_odometria

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.tecnica

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…

Anuncios

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.
camara
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:

departamental

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!

Construcción de mapas (parte I)

Ya estoy de vuelta,

después de una larga temporada sin escribir en este foro vuelvo para compartir mis nuevos logros en el campo de la robótica.

En este caso os muestro la práctica en la que me encuentro trabajando: contrucción de mapas a través de la interpretación de la información recibida del laser.

Lo que en un principio parece sencillo se complica cuando se quiere afinar el diseño.

En nuestro caso, lo que vamos a intentar es interpretar las 180 distancias recibidas del laser (una por cada grado), de forma que si es esta es menor de 7500 (realmente el laser llega hasta los 8000)  sabemos que existe un obstáculo.

La primera mejora es ir interpretando los puntos para agruparlos en segmentos, de manera que evitamos almacenar mucha información. Para cada segmento tan solo necesitaremos un punto inicial y otro final.

Tras conseguir realizar un escaneado de todos los puntos y agruparlos por segmentos, almacenándolos en la memoria, tendremos que volver a realizar otra pasada, pero en este caso, cada vez que se detecte un segmento comprobaremos si existe ya en memoria. Puede ocurrir que se detecte el mismo segmento (el robot no se ha movido), o bien que se pueda fusionar con alguno los que están en memoria.

En la siguiente imagen podéis ver cómo se interpretan los 180 puntos, construyendo 8 segmentos.

fusion2

Ahora tendremos que implementar un procedimiento que elimine los segmentos de memoria no válidos. Estos se detectan en el caso de que el segmento formado entre la posició del robot y el punto interpretado intersecte con alguno de los almacenados en memoria.

Un saludo!