1. INTRODUCCION.

Marco y motivación

La adquisición de información tridimensional fiable mediante múltiples vistas es uno de los importantes retos de investigación planteados en el campo de las Visión por Computador, con numerosas posibles aplicaciones en Robótica y Sistemas Autónomos. La reciente introducción de las técnicas de SLAM (Simultaneous Localization and Mapping) ha abierto una importante vía de investigación, pues dicho método consigue a la vez resolver dos de los principales problemas propuestos: La localización de la posición de un observador y la generación de un mapa del entorno. Aunque se han empleado diversos sensores, los de visión parece que son los que pueden suministrar una información más rica y precisa, por lo que su utilización se ha generalizado en los últimos años.

No obstante, son muchos los aspectos del tema no resueltos satisfactoriamente. Estos factores constituyen un área prioritaria en los principales centros de investigación del mundo. Entre ellos cabe destacar los inherentes a la robustez de su utilización en entornos no estructurados, la carga computacional de trabajar en entornos complejos o las limitaciones propias de modelar el proceso de captación con sistemas no lineales con posibles problemas de convergencia.

Por todos estos motivos se ha considerado necesario profundizar en las técnicas de SLAM existentes en la literatura, con el fin de poder definir nuevas líneas y aportes de investigación en un campo tan sumamente atractivo.

Objetivo

El objetivo de este algoritmo es lograr la reconstrucción tridimensional de un entorno con la ayuda de una cámara. La elección de una cámara como sensor determina muchos otros aspectos del algoritmo. Sin embargo, esta elección tiene ventajas e inconvenientes.

La elección del sensor es importante ya que las diferentes implementaciones están ligadas a una plataforma y a un sensor determinados. El uso de un sensor de distancia o un sensor de ángulo para medir las características define de forma estricta su implementación. Las implementaciones más habituales se basan en sensores de distancia como Láseres y en menor medida sónars. El alto coste de estos dispositivos así como otras limitaciones impiden su aplicación masiva.

Entre los problemas que surgen del uso de una cámara como sensor para el SLAM están:

• El coste computacional del algoritmo. La aplicación de una cámara requiere un acercamiento tridimensional al problema. Este enfoque hace que se necesiten mas parámetros en el sistema.

• Resolver el problema de la asociación de características. Este problema es una de las grandes líneas de investigación en visión artifical.

• El modelo del sistema es fuertemente no lineal. Esta no linealidad afecta a procesos fundamentales del algoritmo como el filtro de Kalman.

En este trabajo se muestra la posibilidad de realizar una reconstrucción 3D del sistema usando visión monocular, esto es, usando una sola cámara. El algoritmo se basa en el algoritmo tradicional de SLAM (EKF-SLAM) usando el filtro de Kalman. Esta implementación requiere un modelado del movimiento del sistema y del sensor. Como modelo del sistema se ha usado un modelo de sólido en movimiento con velocidad constante como modelo del sistema. Como modelo del sensor se ha aplicado el modelo de una cámara pin-hole. Se han realizado dos restricciones. La primera es que los parámetros intrínsecos del modelo pin-hole se suponen invariantes. Estos parámetros comprenden la longitud focal, el centro de la imagen y los coeficientes del modelo de distorsión. La segunda restricción es que las características observadas en las imágenes corresponden a elementos fijos en la escena. Es decir no se modela el movimiento de objetos en el entorno que no sea el de la cámara.

En este documento primero se realizará un estado del arte en el capítulo 2 donde se analizarán otros desarrollos en este área de investigación. En el capítulo 2 se realiza una descripción de los algoritmos fundamentales que componen el método de SLAM presentado. Estos algoritmos son desarrollos genéricos que se aplican en la resolución de multitud de problemas. Este capítulo cubre el modelado del proceso de captación de imágenes, el filtro de Kalman, las características visuales y filtrado de las entradas. En el capítulo 4 se analizan las particularidades el algoritmo de SLAM monocular. Dentro de estas particularidades primero se analizan los modelos usados para implementar el filtro de Kalman, luego se describe los diferentes pasos que se han dado para la inicialización de características. Por último se habla de las mejoras computacionales que se pueden realizar para limitar el número de operaciones requeridas. En el capítulo 5 se describe la implementación especifica realizada poniendo énfasis en el diseño de datos y en los test realizados. Los últimos capítulos tratan sobre las conclusiones obtenidas y futuros trabajos.

2. ESTADO DEL ARTE

SLAM

El algoritmo de SLAM (Simultaneous Localization and Mapping) se ha desarrollado rápidamente en los últimos años y en la actualidad se encuentran distintas implementaciones funcionales. El articulo de Hugh urrant-Whyte describe las principales implementaciones[1] entre las que destacan:

1. Extended Kalman Filter SLAM (EKF-SLAM)

2. Rao-Blackwellized Filter (FastSlam)

Entre los sistemas de SLAM podemos establecer diferencias según los sensores que se utilicen. De esta manera encontramos diferentes formulaciones en función de si utilizan sensores de distancia (Láser, Sonar, ...) o de ángulo (balizas, códigos de barras láser, ...).

Un obstáculo significativo en la aplicación del SLAM en grandes entornos con un elevado número de características es el elevado coste computacional que esto supone. En una implementación directa del filtro de Kalman, la actualización de la matriz de covarianza de tiene un coste computacional de donde n es el número de características. Existen gran cantidad de aproximaciones a este problema las cuales incluyen,

1. Reformulación de la ecuaciones del filtro[2]

2. Actualización Particionada[3]

3. Filtros sub-óptimos [2]

Una buena parte de la revisión de estos método se resume en la segunda parte del artículo de Tim Bailyey y Hugh Durrant-Whyte[4]

Existen pocos estudios de la consistencia del SLAM. En [5] podemos encontrar un análisis de este tema bajo las siguientes problemas:

1. Errores en la asociación de datos.

2. Errores de modelado.

3. Errores en el tratamiento de las distribuciones de probabilidad e incertidumbre.

El estudio concluye que estos problemas, en particular la linearización de los errores de orientación, producen que el falle filtro. Los fallos no únicamente significan inconsistencias numéricas sino también colapsos en el método. Existen diferentes estrategias para resolver estos problemas pero ninguna los resuelve de forma conjunta.

SLAM ESTÉREO

Un enfoque de SLAM usando visión como sensor es usar una cabeza sensora estéreo con dos o mas cámaras. Esta cámara estéreo permite en cada iteración obtener la posición tridimensional de las características por su configuración geométrica. Existen importantes resultados en este sentido en la tesis de Andrew J. Davison [6] o los mas recientemente conseguidos por Kurt Konoligue[7]. Este sistema permite una aproximación más cómoda al algoritmo de SLAM ya que evita los problemas de gestionar la inicialización de los puntos y utiliza un modelo de sensor mucho mas sencillo. Si bien en cambio el modelado de la incertidumbre del sensor y la propia construcción, diseño y operación de la cabeza estéreo son notablemente mas complicados.

Fig. 1: Robot con cabeza estéreo

SLAM MONOCULAR

Un caso particular de los sensores que miden ángulos son las implementaciones basadas en visión artificial. Esto excluye a los basados en un par estéreo que permiten conseguir una distancia directa. El punto de inicio de esta técnica fue dado con el artículo de Andrew J. Davison [8]. En este artículo demuestra la posibilidad de reconstruir un entorno tridimensionalmente usando solamente una cámara. El punto débil del enfoque seguido en este artículo es la inicialización de las características que requiere la hipótesis de que las características se van a encontrar en un entorno predefinido.

Sobre el tema de la inicialización de las características J.M.M. Montiel en [9] propene una parametrización basada en la distancia inversa para permitir la inicialización de las características en un solo paso. Este resultado es notablemente importante ya que permite la extensión a entornos exteriores sin restricción. Este tema se tratará posteriormente en este documento.

SLAM COOPERATIVO

En los temas de mapeo siempre es muy interesante la posiblidad de resolver el problema de forma cooperativas. Esto se explica por diferentes razones:

• El problema tiene una segmentación espacial intuitiva.

• Existe mucho interés en cartografiar áreas extensas con mucha rapidez.

• Los entornos reales requieren vehículos heterogéneos.

El tema de la localización y mapeo cooperativo se estudia en los artículos de Mourikis y Roumelotis que tratan con redes de robots y formaciones de robot [10],[11]. Este trabajo se centra más en el tratamiento de la localización de robots que en el mapeo.

Otro enfoque en la investigación de SLAM cooperativo es el desarrollo de representaciones del mapa que permitan una fusión sencilla de los mapas realizados por dos robots diferentes [12].

Kurt Konolige ha realizado aportaciones en dos sentidos: la generación de trayectorias apropiadas para la exploración desde el punto de vista SLAM [13] y la fusión de mapas generados individualmente [14]

Pese a estas contribuciones, no existe ninguna solución que resuelva el problema acoplado completo de la determinación del estado del vehículo y las marcas de forma conjunta.

3. FUNDAMENTOS

MODELADO DEL PROCESO DE CAPTACIÓN DE IMÁGENES

En este documento se ha usado un modelo de cámara pin-hole con distorsiones. Este modelo es uno de los mas sencillos pero permite un buen modelado del sensor para los temas que aquí se tratan. En este modelo se define la relación entre un punto en el espacio definido en un sistema de referencia fijo y su proyección en el plano de la imagen dado en pixeles. Por lo tanto lo que queremos determinar es un modelo de la forma

Esta función se deriva para el modelo pin-hole usando 2 tipos de parámetros:

• Intrínsecos: corresponden a parámetro físicos de la cámara

• Extrínsecos: corresponden a parámetros externos a la cámara, como por ejemplo su posición y orientación.

El modelo pin-hole supone que todos los rayos proyectivos que parten de un punto en el espacio y llegan al punto proyectado pasan por un mismo punto. La distancia de este punto al plano proyectivo se denomina distancia focal. Dada esta restricción se puede comprobar que se cumplen las ecuaciones

siendo (Xc , Yc, Zc ) las coordenadas del punto en un sistema ligado a la cámara con el eje z en la dirección del eje de proyección y (u, v) las coordenadas centrales de la proyección en el plano de la cámara.

Teniendo en cuenta la ecuación anterior podemos escribir la relación de la siguiente forma

La medida obtenida por la cámara se obtiene en píxeles medidos desde la esquina superior izquierda, coordenadas laterales. Las coordenadas (u,v) representan coordenadas respecto al centro de la imagen, coordenadas centrales. La transformación de coordenadas laterales a coordenadas centrales se escribe como

sustituyendo esta expresión en las fórmulas anteriores tenemos

Teniendo en cuenta la relación de rotación y traslación que existe entre las coordenadas en el sistema de la cámara y las coordenadas en el sistema de referencia fijo la relación buscada se formula como

Además de este modelo se ha tenido en cuenta las distorsiones producidas por la lente que el modelo pin-hole no tiene en cuenta. Estas distorsiones se modelan mediante las siguientes ecuaciones

siendo u y v las coordenadas centrales del punto proyectado sobre el plano de la cámara y los parámetros intrínsecos que definen el modelo de deformación.

FILTRO DE KALMAN

El filtro de kalman es un filtro recursivo eficiente que estima el estado de un sistema dinámico a partir de una serie de medidas con ruido. El filtro de kalman es un estimador recursivo, esto significa que solo es necesario el estado estimado a partir del estado anterior y las observaciones actuales. En contraste con los procesos en serie no se necesita una relación histórica de los estados y medidas anteriores.

El estado del sistema se representa por dos variables:

-, el estado estimado en tiempo k

- , la matriz de covarianza del error

El estado del sistema en nuestro caso esta formado por el estado de la cámara o vehículo y el estado de las características

La matriz de covarianza del error representa una estimación de la precisión del estado estimado del sistema.

El filtro de Kalman tiene dos fases la fase de predicción y la fase de corrección. En la fase de predicción se calcula el estado predicho en función del modelo del sistema y su correspondiente matriz de covarianza del error.

Donde y son las matrices que definen la evolución del estado del sistema en función del estado anterior y la entrada de control al sistema, . En nuestro caso no existe entrada de control al sistema. Es la matriz de covarianza del error del modelo del sistema.

En la etapa de corrección se introducen las medidas tomadas para corregir el estado predicho del sistema. Esta etapa se realiza por medio de las siguientes ecuaciones

• Calculo de la innovación o residuo de la medida

• Cálculo de la covarianza de la innovación

• Calculo de la ganancia de Kalman

• Actualización del estado

• Actualización de la covarianza

En el filtro de extendido de Kalman (EKF) la matriz de transición del estado y el modelo de observación no necesitan ser funciones lineales sólo derivables,

Las funciones f y h se pueden usar para calcular los estados predichos usando la estimación previa. En cambio estas funciones no se pueden usar para calcular la covarianza de forma directa. Para ello se usa la matriz Jacobiana calculada a partir de las derivadas parciales de la función. En cada paso se evalúa la Jacobiana con los estados predichos. Estas matrices se pueden usar en las ecuaciones del filtro de Kalman. Esto tiene como resultado la linearización de una función no lineal alrededor del estado actual estimado.

CARACTERÍTICAS VISUALES

El algoritmo de SLAM toma como entradas un conjunto de características identificadas a lo largo de una serie de tomas. Por lo tanto es necesario implementar algoritmos que permitan identificar y seguir puntos adecuados en una secuencia de imágenes. En este trabajo se han usado las siguientes técnicas:

• Esquinas de Harris

• Características SIFT

Debemos resaltar que la estabilidad del algoritmo de SLAM esta supeditada a las entradas que recibe. Por lo tanto este algoritmo es de gran importancia. Uno de los problemas mas importantes es el hecho que pueden existir falsas correspondencias. Esta entradas erróneas afectan mucho a los resultados del filtro. Es necesario usar el algoritmo de búsqueda más adecuado en cada ocasión. Para permitir esto se implementará una arquitectura del sistema que permita intercambiar estos detectores sin modificación del resto del programa aprovechando las funcionalidades que nos permite la programación orientada a objetos.

DETECTOR DE HARRIS

El detector de esquinas de Harris es un detector muy habitual de puntos de interés debido a su fuerte invarianza frente a: rotación, escala, variación de iluminación y ruido. El detector de esquinas de Harris se basa en la función local del auto-correlación de una señal. Esta función mide los cambios locales de la señal con trozos de la imagen desplazados una cantidad pequeña en diversas direcciones.

Dado un cambio y un punto (x, y), se define la función del auto-correlación como,

Donde I(·, ·) denota la función de la imagen y son los puntos en la ventana W centrada en (x, y).

La imagen desplazada se aproxima por una en serie de Taylor truncada a los términos de primer orden con lo que obtenemos que

Donde es el gradiente de la función c y A es la matriz hessiana. Por como esta definida la función c los dosprimerso términos de la expresión se anulan. Desarrollando la matriz a obtenemos

Donde la matriz C(x, y) captura la estructura de la intensidad de la vecindad local.

Sean L1 y L2 los valores propios de la matriz C(x, y). Los valores propios forman una descripción rotacionalmente invariante. Se consideran tres casos:

• L1 y L2 bajos: El punto presenta pocas variaciones en la función de correlación, por lo tanto no es un descriptor preciso.

• Uno alto y otro bajo: El punto presenta gran variación de la función de correlación en una dirección pero en otra dirección es mal descriptor.

• L1 y L2 altos: El punto presenta gran variación en la función de correlación, por lo tanto es una esquina y una buena característica descriptiva.

El resultado de esta etapa es un conjunto de puntos de interés, esquinas, que pueden son característicos de la imagen por su contenido espectral. Estos puntos se pueden busca en otras imágenes usando técnicas de correlación. Esta técnica parte de un patrón en la imagen original al rededor de la esquina obtenida por el método de Harris. Este pequeño recorte se desplaza sobre la imagen desconocida y se calcula su correlación cruzada entre el patrón y el área que tapa. En el proceso de señal, la correlación cruzada (“cross-correlation") es una medida de semejanza de dos señales, usada comúnmente para encontrar características en una señal desconocida comparándola con la señal conocida. La función de correlación normalizada se expresa como

Donde x’ y’ es el dominio de un patrón dado por la función de intensidad T e I(·,·) es la función de intensidad desconocida. Esta función presenta un indice de similitud entre un patrón y los píxeles de una imagen desconocida.

El reconocimiento se obtiene asociando el patrón al píxel con una correlación más alta. Existe gran cantidad de procesos para robustecer este reconocimiento. En esta aplicación se han usado dos:

• Alcance de un umbral C(x,y)>U

• Coincidencia correlación en la imagen y en la primera derivada (aproximada por la imagen de bordes)

SIFT

Para cualquier objeto hay muchas características, puntos interesantes en el objeto, que se puede extraer para proporcionar una descripción de la "característica" del objeto. Esta descripción puede ser utilizada para localizar el objeto en una imagen que contiene muchos otros objetos. Hay muchas consideraciones sobre extraer estas características y cómo registrarlas. Las características SIFT de una imagen proporcionan un sistema de las características de un objeto que no se ven afectadas por muchas de las complicaciones experimentadas en otros métodos, tales como escala o rotación. Las características SIFT son también muy resistente a los efectos del "ruido" en la imagen.

El algoritmo SIFT genera un conjunto de vectores cada uno de ellos asociado a un punto en la imagen. Estos vectores son característicos de ese punto de la imagen y se pueden usar para reconocer ese mismo punto en otra imagen. Los vectores suelen estar compuestos por 128 valores enteros. Cada uno de estos vectores de características es invariante a cualquier escalamiento, rotación o traslación de la imagen.

Para realizar la extracción de estas características el algoritmo SIFT aplica un filtro de 4 etapas:

1. Detección de los extremos del espacio escalar. Esta etapa del filtro intenta identificar esas posiciones y escalas que sean identificables en diversas vistas del mismo objeto. El espacio de la escala se define por la función:

   

   Donde * es el operador de convolución, es una gausiana de escala variable e I(x, y) es la imagen de la entrada. Se pueden utilizar varias técnicas para detectar posiciones estables de puntos clave. La diferencia de gausianas es una de ellas. Esta técnica localiza los extremos del espacio, que representa la diferencia entre dos imágenes, una con la escala k veces la otra. se define por:

   

   Para detectar los máximos y los mínimos locales de cada punto se compara con sus 8 vecinos en la misma escala, y sus 9 vecinos arriba y abajo una escala. Si este valor es el mínimo o el máximo de todos estos puntos entonces este punto es un extremo.

   
   

2. Localización de puntos clave. En esta etapa se eliminan puntos de la lista de características encontrando los que tengan bajo contraste o se localicen mal en un borde. Esto se consigue calculando la Laplaciana para cada punto encontrado en la etapa 1. La localización del extremo, z, se define por

   

   Si el valor de la función z está por debajo de un valor de umbral entonces se excluye este punto. Esto quita extremos con contraste bajo. Para eliminar los extremos basados en una localización pobre se observa la derivada direccional del punto. En estos casos en la función de diferencia de gausianas hay una curvatura grande a través del borde pero una curvatura pequeña en la dirección perpendicular. Es decir hay mucha variación al moverse en una dirección pero poca al moverse en otra. Si esta diferencia está por debajo debajo del cociente entre el vector propio más grande y el más pequeño de la matriz del hessiana 2x2 se rechaza el punto de interés

3. Asignación de la Orientación. En este paso se asigna una orientación a los puntos de interés basada en características locales de la imagen. El descriptor del punto, descrito más abajo, se puede entonces representar respecto a esta orientación, alcanzando la invarianza a la rotación. El criterio usado para encontrar una orientación es:

   1. Utilizar la escala de los puntos para seleccionar la imagen gausiana suavizada L

   2. Calcular la magnitud del gradiente, m

      

   3. Calcular la orientación como

      

   4. Formar un histograma de la orientación del gradiente de los puntos de la muestra

   5. Localizar el pico más alto en el histograma. Utilice este pico y cualquier otro pico local dentro del 80% de la altura de este pico para crear un keypoint con esa orientación

   6. Algunos puntos serán asignados orientaciones múltiples

   7. Ajustar una parábola a los 3 valores del histograma más cercanos a cada pico para interpolar la posición de los picos

4. Descriptor del punto de interés Los datos locales del gradiente, usados arriba, también se utilizan para crear descriptores del punto de interés. La información del gradiente se rota para alinearlo con la orientación del punto de interés y después es ponderada por una gausiana con la varianza de 1.5 por la escala del punto de interés. Estos datos se utilizan para crear un sistema de histogramas sobre una ventana centrada en el punto de interés. Los descriptores del punto de interés utilizan normalmente un sistema de 16 histogramas, alineado en una rejilla 4x4, de cada uno con 8 compartimientos de la orientación, uno para cada uno de las direcciones principales del compás y uno para cada uno de los puntos medianos de estas direcciones. Esto da lugar a un vector de la característica que contiene 128 elementos.

Estos vectores que resultan son las características SIFT. El algoritmo de características SIFT en una imagen de 500x500 pixel generará típicamente en la región 2000 características.

Las características SIFT se utilizan en una estrategia de vecino más próximo para identificar objetos posibles en una imagen. Esta estrategia se basa en la distancia cartesiana entre dos vectores. A un vector de clase desconocida se asigna la clase del vector más cercano. Esta estrategia requiere gran cantidad de cálculos pues requiere comparar cada característica con todas las almacenadas en el mapa. No existen técnicas sencillas para reducir la carga computacional que esto supone.

FILTRADO DE LAS ENTRADAS

FILTRO DE MAHALANOBIS

El filtro de Mahalanobis es un filtro estadístico basado en la función de distribución Chi. La distribución fue obtenida por K. Earson a principio del siglo XX y es una de las herramientas de análisis más utilizada en la ciencia actual. Supongamos que generamos, mediante el método de Montecarlo, n variables aleatorias independientes normales con media cero y varianza unidad y definimos la operación:

Es decir, elevamos los n valores generados al cuadrado y lo sumamos. Si aplicamos este procedimiento muchas veces, obtendremos la distribución de una variable que depende del número de sumandos. Esta distribución se denomina con n grados de libertad. Podemos ver su representación en la siguiente figura.

La distancia estandarizada no tiene en cuenta la posible dependencia entre las variables. Intuitivamente, si dos variables están muy relacionadas y en dos individuos toman valores muy distintos estos individuos deben considerarse más separados que si esa distancia se hubiese observado entre variables independientes. El cuadrado de la distancia estandarizada puede escribirse como

donde D es una matriz diagonal cuyos términos son las varianzas de las variable y dos vectores. Si en lugar de D utilizamos M, la matriz de varianzas y covarianzas entre las variables, obtenemos la distancia de Mahalanobis, definida por

Se puede representar la distancia de Mahalanobis como curvas de nivel

En caso particular de la aplicación al SLAM este filtro consiste en evaluar la expresión

con y las demás variables siguiendo la misma notación que en el filtro de Kalman. Si esta expresión es mayor que el valor dado para la variable chi para esos grados de libertad entonces el punto se rechaza.

FILTRO RANSAC

RANSAC es una abreviatura para "consenso de la muestra escogida al azar". Es un algoritmo para estimar parámetros de un modelo matemático de un sistema a partir de los datos observados que contienen datos atípicos.

El algoritmo fue publicado por Fischler y Bolles en 1981. Una hipótesis básica es que los datos consisten en puntos correctos, es decir, los puntos de referencias que se pueden explicar por un cierto sistema de los parámetros modelo, y en datos atípicos que son los puntos de referencias que no caben el modelo. Además de esto, los puntos de referencias pueden estar influidos por el ruido. Los datos atípicos pueden venir, por ejemplo, de los valores extremos del ruido o de medidas erróneas o de hipótesis incorrectas sobre la interpretación de datos. RANSAC también asume que, dada una cantidad (generalmente pequeña) fija de datos correctos, existe un procedimiento que pueda estimar los parámetros de un modelo que explique o se ajuste óptimamente a estos datos.

RANSAC alcanza su meta seleccionando iterativamente al azar un subconjunto de los puntos de referencias originales. Estos puntos son puntos correctos hipotéticamente y esta hipótesis se prueba como sigue. Un modelo se ajusta a los puntos correctos hipotéticos, es decir, todos los parámetros libres del modelo se reconstruyen a partir del conjunto de puntos. El resto de los puntos de referencias se prueban contra el modelo ajustado, es decir, para cada punto del sistema restante, el algoritmo determina si se ajusta bien el punto al modelo estimado. Si se ajusta bien, ese punto también se considera como un punto bueno hipotético. Si muchos puntos se han clasificado como puntos buenos hipotéticos sobre el modelo estimado, entonces tenemos un modelo razonablemente bueno. Sin embargo, se ha estimado solamente el sistema con los puntos buenos hipotéticos iniciales, así que volvemos a estimar el modelo del sistema entero con los puntos buenos hipotéticos. Al mismo tiempo, estimamos el error de los puntos buenos concerniente al modelo. Este procedimiento se repite un número fijo de veces. En cada iteración o el modelo se rechaza que pocos puntos se clasifican como puntos buenos o se genera un modelo refinado junto con una medida correspondiente del error. En el último paso, mantenemos el modelo refinado si su error es más bajo que el modelo ajustado pasado.

4. SLAM MONOCULAR

El algoritmo tiene una etapa de inicialización y una etapa de funcionamiento normal.

En la etapa de inicialización se debe suministrar la siguiente información:

• Parámetros intrínsecos de la cámara.

• Parámetros extrínsecos (iniciales) de la cámara.

• Posición tridimensional de un primer juego de características.

• Posición bidimensional en el espacio de la imagen de las características sobre dos frames consecutivos.

Este juego de parámetros compone el estado del sistema. Hay que notar que el estado del sistema comprende variables que corresponden a una medida en el instante anterior (t-1). Por lo tanto es necesario tener los valores de dos instantes de la mayor parte de los elementos del estado, por ejemplo posiciones de la cámara y posición de las características en el espacio de la imagen. Otra observación es que la inicialización de los parámetros determina la escala de la escena reconstruida.

La etapa de funcionamiento se centra en dos partes principales:

• El filtro de Kalman.

• El mantenimiento del mapa.

El filtro de Kalman inicializado en la fase anterior evoluciona con los modelos y las medidas del sistema. El estado del sistema esta compuesto por el estado de la cámara y el estado de las características que forman el mapa. El modelo de la cámara es el de un sólido con movimiento a velocidad constante y seis grados de libertad y las características son objetos puntuales fijos en la escena.

El mantenimiento del mapa comprende varias tareas

• Identificar las características que salen del campo de visión para retirarlas del modelo del sensor que usa el filtro de Kalman

• Añadir características al sistema de tal manera que se mantengan dentro del campo de visión un número mínimo de características.

• Recuperar las características que vuelven a entrar en el campo de visión después de estar fuera.

FILTRO DE KALMAN

Modelo del sistema.

El sistema esta compuesto por la cámara y las características del mapa. Suponemos que la cámara se mueve con seis grados de libertad con movimiento uniforme. El estado de al cámara estará compuesto por el vector de posicióny el vector de rotación El modelo de la cámara es

donde w es el ruido del modelo. Aproximando las velocidades por diferencias tenemos

Despejando para el vector de posción

Con esto podemos expresar el estado de una de las variables como

Se puede realizar el mismo análisis para el vector de rotación.

Suponemos que las características son estáticas por lo tanto su modelo será la matriz identidad, por otra parte al ser puntuales no consideramos su orientación. De manera que el modelo completo tendrá 12+3n variables de estado.

Modelo del sensor

El modelo del sensor es un modelo pin hole. Este modelo es no lineal, por lo que tenemos que usar un filtro extendido de Kalman con este modelo. Esto añade la dificultad del cálculo de la jacobiana.

El modelo es una función de la forma Esta función se define en coordenadas homogéneas como

Como es una función no lineal necesitamos la Jacobiana del modelo para poder implementar el filtro de Kalman. El cálculo de la jacobiana se realiza de forma análitica en varias etapas. Primero considermos la derivada de las coordenadas centrales (u,v) respecto del punto en el espacio.

Debido a que estamos considerando el modelo con distorsiones debemos introducir la derivada de las funciones de distorsión.

Finalmente se obtiene la derivada de las coordenadas laterales del sensor usando las derivadas anteriores.

MANTENIMIENTO DEL MAPA

Las tareas de mantenimiento del mapa comprenden 3 partes:

• Puntos que salen del campo de visión

• Adquisición de puntos nuevos

• Readquisición de puntos vistos previamente.

La gestión de estos eventos determine en gran medida el coste computacional, el gasto de memoria y la buena estabilidad del algoritmo. Esto hace que sea una tarea determinante para este procedimiento. A continuación se describirán estos 3 puntos más en detalle

Puntos que salen del campo de visión.

Cuando los puntos se salen del campo de visión hay que modificar la matriz de medida del filtro de kalman. Este caso es el más sencillo ya que solo es necesario eliminar la fila y columna correspondiente en la matriz de medida.

La forma mas sencilla de detectar un punto que se sale del campo de visión es retirarlo cuando está cerca del borde. Esta estrategia no es óptima por las siguientes razones.

• Un punto puede volver a entrar en el centro del campo de visión antes de salir

• Un punto que se sale del campo de visión sin pasar por el borde puede producir una identificación equivocada.

Para estos problemas no se pueden solucionar con facilidad. Para minimizar su influencia nos basamos en las predicciones realizadas por el filtro de Kalman y en tests robustos que permitan evitar las asociaciones incorrectas.

Adquisición de puntos nuevos

Cuando el número de puntos visibles baja de un mínimo hay que tomar un punto nuevo. La complejidad de esta etapa está en iniciarlo correctamente. Una mala inicialización puede resultar en la divergencia del filtro de kalman o en un error irrecuperable en el mapeo.

La inicialización de un punto es un proceso que se debe realizar a lo largo de varios ciclos y es necesario un movimiento suficientemente amplio como para poder triangular con precisión el punto

El algoritmo de inicialización tiene tres fases:

• Almacenamiento de variables en la primera toma y generación de posibilidades.

• Seguimiento de los puntos a lo largo de los frames

• Evaluación del criterio de inicialización.

Esto se realiza usando un modelo de datos que considera una serie de estasdos asociados al punto. Estos estados permiten que cada módulo interactúe con los puntos para generar proyecciones evaluar su inicialización o insertarlos en el procedimiento general.

Puntos que entran en el campo de visión

La realización efectiva de esta etapa es fundamental en el algoritmo de SLAM. Existen dos problemas fundamentales en esta tarea. El primero es que el error acumulado durante la trayectoria del robot produce grandes divergencias cuando se revisita un entorno entre el punto observado y el punto esperado y predicho por el modelo. El segundo es el conseguir característica suficientemente unívocas que permitan reconocer un punto entre un conjunto números de características.

El primer problema dificulta el filtrado y debe ser resuelto mediante ajustes conjuntos de un número de puntos. Estos ajustes son computacionalmente costosos ya que a lo largo del recorrido del sensor se van adquiriendo gran cantidad de puntos. El ajuste conjunto de todas estas posibilidades complica significativamente las necesidades del algoritmo.

El segundo problema es mucho más complicado que el primero. Actualmente las características descriptivas como esquinas de harris o características sift o surf tienen limitaciones importantes y no son perfectas en absoluto. Esto hace que produzcan una mayor o menor proporción de falsos positivos en cualquier caso. Este problema solo se puede resolver mejorando los algoritmos que generan estas identificación.

En nuestro caso el primer problema se ha resuelto mediante ajustes robustos estadísticos como RANSAC entre las características previstas y las obtenidas. El segundo problemas no se ha resuelto completamente ya que no es objeto de este trabajo desarrollar un nuevo método de seguimiento de características. A efectos de este problema se ha implementado el detector SIFT que es uno de los detectores mas efectivos de la literatura actual. Esta elección no es absoluta si no que se ha desarrollado todo el algoritmo sobre la base de que este componente sea intercambiable por otros según se vayan desarrollando.

INICIALIZACIÓN DE PUNTOS NUEVOS.

La inicialización de puntos nuevos según se van viendo en el sensor ha sido uno de los mayores problemas de este algoritmo. La inicialización directa de los puntos no es posible dadas las características de un sensor visual. En principio estos sensores no aportan información sobre la profundidad de los puntos que se observan en una imagen. Sin embargo podemos realizar la inicialización usando varios frames por ejemplo. En este trabajos se han probado los siguientes métodos:

• Optimización no lineal usando varios frames.

• Trigangulación usando dos frames no consecutivos.

• Reformulación de la parametrización.

El primer método tenía dos problemas, primero la complejidad computacional y segundo la inestabilidad. De los dos problemas el segundo era el más grave ya que el algoritmo iterativo utilizado no siempre convergía si existían errores en los puntos de entrada.

El segundo método es el más sencillo. Se realizó un gran número de pruebas con este sistema pero tenía un problema de implementación practica. La elección de los 2 frames que se usan para incializar el punto deben tener cierta cantidad de ángulo entre ellos para que la ecuación esté bien condicionada y los errores de determinación de los pixeles no provoque un gran error en la reconstrucción. El criterio que se siguió fue un desplazamiento mínimo entre dos posiciones de la cámara. Este criterio solo es válido si los puntos se encuentran dentro de un rango conocido. Por el contrario puede que para puntos muy profundos el desplazamiento no sea suficiente.

El tercer método es el que mejores resultados prácticos ha obtenido siendo el elegido para la implementación final. Este método sin embargo a incrementado la complejidad del tratamiento de los datos y el cálculo de las derivadas. Ademas el incremento del número de parámetros para cada punto produce gran complejidad computacional.

Optimización por Levenberg-Marquadt

Los puntos considerados para añadir hay que seguirlos a lo largo de varios frames. El objetivo es obtener una estimación tridimensional. Si se quiere obtener una estimación usando el modelo con distorsiones hay que recurrir a un algoritmo de optimización. El problema consiste en minimizar la función

Siendo los puntos proyectados según el modelo de la cámara.

El método de optimización es el método de Levenberg-Marquadt. Este método usa el siguiente algoritmo:

1. Cálculo de las matrices jacobianas de las ecuaciones de proyección de las dos imágenes J y J’

2. Estimación inicial del parámetro

3. Calculo del paso de movimiento h

4. Calculo de coeficiente y actualización de

5. Comprobación de parada. Si no se cumple se itera desde el punto 3

Para la estimación incial del parámetro se usa la siguiente fórmula de aproximación.

Donde es un parámetro definido por el usuario y cado uno de los elementos de la matriz A

El siguiente paso es calcular el incremento que se va a realizar en la dirección de minimización. Este parámetro tiene el nombre de h

El algoritmo tiene en cuenta la mejora realizada por el movimiento h. Esta mejora bien medida por el parámetro según la siguiente fórmula.

donde F(x) es el valor de la función de coste en el punto x.

Teniendo en cuenta el parámetro anterior se actualiza de forma iterativa para corregir el movimiento en la dirección de minimización por un descenso más adecuado. Existen dos estrategias de actualización, la primera es el siguiente procedimiento:

1. Si entonces

2. Si entonces

3. Si entonces

La segunda estrategia es considerar :

Si entonces

En caso contrario

El algoritmo se desarrolla iterativamente hasta que se cumple un criterio de terminación. Los criterios de terminación pueden ser cualquiera de los siguientes

Generación de puntos posibles e inicialización final.

Alrededor de la solución obtenida por el algoritmo de optimización se genera un conjunto de posibles soluciones. El objetivo es refinar la solución obtenida anteriormente y robustecer el algoritmo.

Estos puntos se generan sobre el vector que une el centro de la cámara con el resultado del algoritmo de optimización.

Estos puntos se proyectan sobre el espacio de la imagen y se comparan con el punto obtenido por el seguimiento de la imagen real. Si estos puntos difieren se van descartando. El punto se considera inicializado cuando existe un solo punto posible sin descartar.

Triangulación de un punto 3d a partir de dos vistas

A partir de las ecuaciones de proyección en dos vistas se trata de resolver el sistema por mínimos cuadrados. Par tiendo de la ecuación para una vista

podemos simplificarla obteniendo:

Despejando:

Simplificando y aplicándolo a dos vistas tenemos

Con esto ya podemos obtener la posición por mínimos cuadrados

Inverse Depth Parametrization

Esta técnica se basa en parametrizar las características de forma que puedan ser inicializadas en un solo frame. En este algoritmo se describe una característica por 6 parámetros.

Como se ve en la figura anterior los tres primeros parámetros representan un punto tomado sobre la recta sobre la que se vió por primera vez el punto. Los dos siguientes parámetros representan las coordenadas polares de la dirección de la recta. El tercer parámetro es la inversa de la distancia desde el punto representado por los tres primeros parámetros hasta el punto de interés. Éstos parámetros modelan el punto 3D según la siguiente fórmula

donde m es un vector director que se obtiene según la fórmula

La inicialización se lleva a cabo obteniendo un punto sobre la recta de visión que une el centro de la cámara con el punto mediante la fórmula,

y después usando este punto en coordenadas respecto al centro de la cámara podemos obtener las coordenadas polares de un rayo de vista desde el centro de la cámara al punto.

Los tres primeros parámetros se asignan como la posición que ocupa la cámara en ese momento.

La nueva parametrización modifica la ecuación de medida teniendo que añadir un paso que transforma la parametrización inversa en un punto 3D de tres coordenadas sobre el que se aplica la ecuación habitual descrita en el apartado anterior. La modificación más importante que hay que introducir respecto del algoritmo convencional es en el cálculo del Jacobiano de la equación de proyección. Este jacobiano se modifica siguiendo las normas de derivación por la regla de la cadena multiplicando por el jacobiano de la transformación de 3D a 6D que viene dado por la siguiente matriz:

Las ventajas esta parametrización son enormes ya que aumentan la estabilidad del algoritmos y permiten una mayor facilidad de programación. La principal desventaja es el aumento del coste computacional.

MEJORAS COMPUTACIONALES

Debido a que la solución del problema de SLAM mediante el filtro de Kalman implementado en su forma más básica tiene una complejidad del orden de siendo N el número de caracterísiticas el aumento del tamaño del mapa aumenta el tiempo de cálculo rápidamente. Esto hace que existan muchos acercamientos para conseguir un menor tiempo computacional. A continuación se explican dos de los mas importantes del conjunto de optimizaciones aplicables a nuestro problema.

CONSTRAINED LOCAL SUBMAP FILTER (CLSF-SLAM)

El algoritmo CLSF presenta una aproximación a la complejidad computacional del SLAM permitiendo que la matriz completa de covarianzas se actualice intervalos apropiados. Este método usa un sub-mapa independiente que contiene sólo las características en la vecindad del vehículo. Las observaciones se introducen este sub-mapa cuyos ejes de referencia son conocidos en el mapa absoluto. A intervalos adecuados la información del mapa local se transfiere al mapa global. Esto permite evitar la actualización completa de la matriz de covarianzas en cada paso lo cual reduce el excesivo gasto computacional que esto produce.

El vector de estado del filtro CLSF comprende el estado local del vehículo y las marcas conjuntamente con los estados globales de las marcas y la posición relativa entre marcos de referencia.

donde el superíndice G indica el marco de referencia global y el superíndice L indica el marco local.

En la etapa de predicción sólo se actualiza la posición del vehículo en el marco de referencia local. La propagación de la matriz de covarianza se realiza también exclusivamente en los términos relacionados con los estados locales según la ecuación siguiente,

La etapa de observación y actualización es exactamente igual en el filtro clásico.

A intervalos regulares las características del marco local se transforman al marco global. Para esto se suma la información de la posición del marco local sobre el marco global. A partir de la cual se obtiene una matriz de transformación T(k) que transforma los vectores locales en globales. De esta forma se obtiene unos estados globales de la forma

Los estados de una misma característica que se ha inicializado en el marco global y en el marco local se relacionan a través de ecuaciones de restricción de la forma

Con esta restricción el estado estimado por las dos observaciones se determina por las fórmulas

con

En el caso de características descritas por sus coordenadas (x,y,z) se puede aplicar la ecuación de restricción

En el caso de la parametrización por distancia inversa esto no se realiza con facilidad ya que esta parametrización es distinta dependiendo del punto donde esté la cámara cuando se inicialize por lo tanto se debería convertir a coordenadas comunes antes de fusionarlo y después recuperar las matrices de covarianzas . Otra opción es realizar una ecuación de restricción más compleja.

En el caso de no realizar la parametrización por distancia inversa se obliga a inicializar todas las marcas en cada sub mapa. Esto no es posible hacerlo en un solo frame con lo que el algoritmo no podría realizar la transición de forma automática

COMPACT KALMAN FILTER

El según [2] este algoritmo reduce la complejidad de aproximadamentea . Estos resultados no hacen ninguna aproximación y los resultados son iguales a los de una implementación SLAM completa.

En la etapa de predicción se tiene en cuenta los ceros del jacobiano del modelo por lo que se puede escribir

Se puede probar que la evaluación de esta matriz requiere aproximadamente 9M multiplicaciones.

En la etapa de corrección la matriz jacobiana de la matriz de medida tendrá un gran número de ceros. Por lo tanto tendremos una matriz con la forma de

donde H1 depende es la derivada de la medida respecto de la posición del vehículo y H2 es la derivada respecto del estado de la característica. Los ceros representan matrices nulas. En el instante k el calculo de la matriz de kalman requiere el cálculo de P por H transpuesta. Esta descomposición permite reducir esta evaluación a 10M multiplicaciones.

La mayor aportación del filtro comprimido es que no es necesario realizar una acutalización completa cuando se trabaja en un área local. Si dividimos el estado en dos partes Xa, que contiene el estado del vehículo y el estado de las características en el área actual, y Xb que contiene el estado del resto de las características tenemos que la ecuación de medida solo es función de Xa y no del estado completo X. Entonces el jacobiano, H, de la ecuación de medida solo tiene términos no nulos en esto vectores. Considerando los ceros de la matriz H tenemos:

Esta formulación demuestra que no hay dependencia de los términos relacionados con los estados contenido en Xb y por lo tanto solo depende de las características dadas por una región A

La evaluación de la matriz de covarianza se realiza de la siguiente forma:

La matriz de covarianza despues de un paso es

Las matrices se evalúan de forma iterativa con la fórmula

Este método reduce la complejidad del algoritmo a .

5. IMPLEMENTACIÓN

La implementación del algoritmo se ha realizacio en C++ usando las librerías OpenCV. El objetivo de la implementación es que sea lo sufcientemente flexible para poder extender el algoritmo con nuevas técnicas y realizar variaciónes de forma sencilla. Por ello se ha realizado un diseño orientado a objetos que permite reemplazar bloques de proceso y extender los algoritmos de forma sencilla.

Secuencia del algoritmo

El bucle principal del algoritmo sigue el siguiente proceso:

1. Inicialización

2. Toma de Frame

3. Selección de primeros puntos

4. Asociación de los puntos con posiciones 3D conocidas.

5. Toma de Frame

6. Seguimiento de puntos

7. Calculo de proyecciones y jacobianos según el modelo de la cámara

8. Ejecución del kalman

   • Predicción

   • Test

   • Corrección

9. Actualización del mapa.

   • Puntos que salen del campo de visión

   • Inserción de nuevos puntos

   • Puntos que entran en el campo de visión

10. Se itera desde el punto 5

La inicialización del algoritmo es un proceso complejo que crea todos los objetos necesarios para la ejecución y realiza los vínculos entre ellos además se establecen los valores de varios parámetros configurables.

Es preciso adquirir un primer Frame sobre el que se inicializan los primeros puntos del mapa. Esta primera selección se hace fuera del bucle principal para que luego se pueda asociar una posción 3D conocida. Esta asociación es necesaria para introducir una escala de trabajo en el sistema. Si no se realiza el sistema no puede distinguir un sensor que se mueve muy rápido sobre objetos grandes de un sensor que se mueve más despacio sobre objetos más pequeños.

El proceso de seguimiento de los puntos se basa en encontrar una asociación entre dos puntos en dos frames consecutivos. Esta etapa tiene como entrada un frame y la lista de puntos vistos en la etapa anterior y obtiene como salida una nueva lista de puntos vistos y no vistos en ese frame.

Los puntos que tienen una reconstrucción 3D disponible se proyectan usando el modelo de la cámara. Esto es una entrada necesaria de la siguiente etapa. En este proceso tamien se obtiene las derivadas del modelo de la cámara que luego se usaran en la etapa de corrección y test del siguiente paso.

El filtro de Kalman se divide en dos partes predicción y corrección. El resultado de esta etapa es una posición corregida de los puntos y de la cámara. La entrada de esta etapa son los resultados del ciclo anterior y las proyecciones y jacobianos de los puntos. En esta etapa se ha introducción una fase de tests estadísticos que aprovecha resultados intermedios del filtro de Kalman.

El último paso consiste en acutalizar un parámetro de estado de los puntos que contiene información sobre su visibilidad, inicialización, etc.. En este paso se realizan comprobaciones de todos los puntos usando los resultados de las etapas anteriores y se realizan tests robustos que mantienen la integridad del sistema.

Diseño de Datos

Es muy importante la distribución de los datos a lo largo del algoritmo. Dado que existe una gran cantidad de datos el intercambio de información debe realizarse de la forma mas eficiente posible. Existen dos conjuntos de datos principales:

• Datos asociados a la cámara

• Datos asociados al mapa

Los datos asociados a la cámara son:

• Parámetros intrínsecos

  • Focal (fx, fy)

  • Centro de la imagen (cx,cy)

  • Distorsiones

• Parámetros extrínsecos

  • Posición (x,y,z)

  • Orientación (roll, pitch, yaw)

Los datos asociados al mapa son para cada punto característico:

• Ultima posición en espacio de la imagen.

• Antigua posición en el espacio de la imagen:

  • Píxel x, y.

  • Parámetros extrínsecos de la cámara en ese momento.

• Posición 3D del píxel inicializado (en el caso de parametrización inversa 6 parámetros).

• Información de covarianza .

• Información característica de ese punto que la distingue de otro puntos.

6. Diseño de tests

Módulo de proyección.

Módulo	Proyección
Entradas	Puntos 3D y posiciones de la cámara
Salidas	Puntos proyectados sobre el sensor de la cámara
Test	Se suministra una secuencia de posiciones que observan puntos con una posición conocida y una estructura regular. Se almacenan los puntos proyectados (2 coordenadas) y se representan.
Comprobación	Se debe realizar una inspección visual del gráfico de salida que permita reconocer los punto vistos.
Dependencias	Este test no tiene dependencias.

Módulo de Kalman

Módulo	Kalman
Entradas	Puntos 3D y posiciones de la cámara del ciclo anterior y proyecciones de los puntos y medidas del ciclo actual.
Salidas	Puntos 3D y posiciones de la cámara filtradas para el ciclo actual.
Test	En este test se usará una escena de puntos tridimensionales. Estos puntos se proyectarán para varias posiciones de la cámara a lo largo de una trayectoria conocida. En en una fase siguiente estos puntos serán la entrada del filtro de Kalman. Las entradas del módulo podrán tener sumado una cantidad controlada de ruido.
Comprobación	La reconstrucción obtenida por el filtro de kalman de la trayectoria de la cámara y de las posiciones de los puntos tiene que coincidir con la que se ha usado para generar los puntos.
Dependencias	Este test depende del módulo de proyección.

Módulo de seguimiento de caractrísticas

Módulo	Seguimiento de características.
Entradas	Secuencia vídeo
Salidas	Asociación de puntos a lo largo de diferentes frames.
Test	Este modulo solo se puede probar usando secuencias de vídeo reales. Se alimentará al algoritmo con secuencias de vídeo real. Dado que estas entradas contienen ruido y no pueden ser completamente controladas el análisis sera manual. Se obtendrán imágenes con representaciones superpuestas de los puntos obtenidos numerados para su identificación.
Comprobación	Se debe observar el seguimiento de un número de características significativo a lo largo de un número de frames. Se valorará el número de veces que se pierden puntos o se producen asociaciones incorrectas
Dependencias	Este test no tiene dependencias.

Módulo de gestión del Mapa

Módulo	Gestión del mapa.
Entradas	Información 2D y 3D de los puntos reconstruidos y estado
Salidas	Actualización del estado de los puntos
Test	Se genera un escenario de puntos 3D de gran tamaño. Se escoge un trayectoria simple que recorra los puntos de tal manera que no visualize todos al mismo tiempo sino que vayan entrando y saliendo del campo de visión. Estos puntos se proyectan usando el modelo de la cámara y se alimentan al algoritmo completo. Los puntos se representarán numerados en los frames con diferentes colores según el estado en el que se encuentren.
Comprobación	Los puntos deben cambiar de estado según las reglas establecidas. El algoritmo debe mantenerse estable. Se compararán las posiciones reconstruidas con las posiciones reales.
Dependencias	Este test depende de los módulos de Kalman, inserción de puntos y proyección.

Integración de Módulos

Módulo	Integración
Entradas	Vídeo real
Salidas	Reconstrucción 3D de los puntos y de la trayectoria
Test	Se prueba el algoritmo con diferentes entradas de vídeo tomadas en posiciones conocidas. Se almacenan la reconstrucción de los puntos. En este caso todos los módulos están funcionando. Por lo tanto esto es una prueba integral del sistema.
Comprobación	La reconstrucción debe mantener coherencia con el escenario y la trayectoria debe coincidir con la realizada por la cámara.
Dependencias	Este test depende de los módulos de Kalman, inserción de puntos y proyección.

7. Pruebas realizadas

Test 1: Módulo de proyección

Se colocan puntos en (0,0,10);(10,0,0);(0,10,0); y se realiza con la cámara un giro alrededor de eje Y

según el siguiente dibujo:

En esta prueba se ha podido comprobar que las referencias están correctamente escogidas y que el sistema de proyección obtiene resultados coherentes con lo esperado.

Test 2: Módulo Kalman

En esta prueba se ha construido un entorno como se puede ver en la Fig. 6. Sobre este entorno se ha realizado un movimiento circular mirando al centro de la referencia.

En la Fig. 7 podemos ver en la parte superior los puntos reconstruidos que se han mantenido estables durante todo el trayecto sin mostrar divergencias. En la parte inferior se ve las distintas posiciones que ha tomado la cámara a lo largo de su trayectoria. Esta trayectoria como se puede ver corresponde con fidelidad la realizada para obtener los puntos proyectados.

Test 3: Triangulación

El objetivo de este test es realizar la triangulación de un punto mientras que se realiza la trayectoria descrita en el Test 3. En la siguiente figura podemos observar marcado con un circulo el punto que se ha añadido por triangulación. Este punto se ha reconstruido en el punto correcto desde donde se había proyectado previamente.

Fig. 8: Resultado del test de triangulación

Test 5 Detector de Harris

En este test se ha alimentado al detector con imágenes tomadas en un entorno controlado donde se pueden seguir los puntos con facilidad. Se representan en verde los puntos encontrados y en azul los puntos por debajo de un umbral de calidad. Las dos imágenes muestran adquisiciones de la misma escena tomados desde puntos de vista distintos:

Se puede comprobar que al detección es buena en la mayoría de los casos. Esto sería suficente en otras tareas pero para este algoritmo se ha considerado poco fiable.

Test 5 SIFT

Este tes es equivalente al test anterior pero se ha usado un detector de características SIFT. Este detector es mucho más robusto que el de Harris en la mayoría de los casos. La desventaja de este algoritmo es el gran número de cálculos numéricos que realiza. Esto hace que el tiempo de proceso sea muy elevado.

El algoritmo presenta otra ventaja respecto al algoritmo de Harris y es que busca el punto en toda la imagen y no solo en un entorno del último punto visto. Esto es muy importante cuando se intenta cerrar un bucle de recorrido y se quiere ver una característica vista previamente pero que se dejó de ver.

En las siguientes figuras podemos ver la identificación de los puntos en dos frames no consecutivos

Test 6 Inicialización de características y mantenimiento del mapa

Este test intenta probar que los algoritmos de mantenimiento del mapa funcionan correctamente. Para ello se ha simulado un esquema como el mostrado en la figura siguiente. En este esquema la cámara se desplaza a lo largo de una trayectoria recta. El campo de visión no permite observar todos los puntos simultáneamente.

En la imagen de la derecha podemos observar la reconstrucción realizada por el algoritmo. Se puede ver en rojo la trayectoria de la cámara y en negro los puntos reconstruidos. En la figura podemos ver como la reconstrucción es coherente con la simulación hecha.

El resultado más importantes de esta prueba ha sido poder corregir los errores que introducidos en la gestión de los puntos. También se puede considerar como una prueba previa de integración sin tener en cuenta las complejidades del sistema de captación real.

En la siguiente figura podemos observar como varía la incertidumbre de la reconstrucción a lo largo de las diferentes vistas.

Fig. 15: Evolución de la incertidumbre

Test 7 Integración con vídeo real

En este tes se han realizado dos pruebas la primera con un vídeo grabado en un entorno controlado y la segunda con un vídeo tomado en exteriores por un vehículo aéreo no tripulado.

En la primera prueba podemos ver en la siguiente figura la reconstrucción realizada del vídeo mostrado en la figura de la izquierda. Vemos como la trayectoria rectilínea de la cámara se reconstruye fielmente y se muestra en rojo en el gráfico de la derecha.

Las imágenes tomadas con el UAV son imágenes mas ruidosa con mas variabilidad. El movimiento de la cámara es aproximadamente rectilíneo y se observan características en una zona con bajo relieve. Los resultados de esta reconstrucción se pueden ver a la derecha. Si bien esta prueba ha requerido gran cantidad de filtros para evitar la asociación incorrecta de datos.

8. CONCLUSIONES

En este trabajo se ha realizado una implementación de SLAM visual monocular completa. Esta implementación se distingue de otras implementaciones en que se han incluido aportaciones importantes de diferentes algoritmos como la parametrización inversa o las mejoras computacionales.

Otra de las aportaciones de este trabajo es el desarrollo de una aplicación del algoritmo sobre imágenes aéreas para la obtención de modelos digitales de terreno. El proyecto se ha realizado en conjunto con la implementación de un UAV permitiendo la obtención de imágenes aéreas junto con posiciones de la cámara obtenidas por GPS.

Se ha construido una arquitectura genérica de SLAM suficientemente flexible como para realizar cambios y probar nuevos desarrollos. Esta flexibilidad permite reemplazar componentes sin cambios en el resto del algoritmo.

El algoritmo de SLAM tal cual se ha desarrollado aquí presenta todavía carencias que no estan resueltas en la literatura. Entre ellas se encuentran las mejoras computación, la robustez de las características visuales y la estabilidad del sistema.

El algoritmo implementado se encuentra al nivel del estado del arte. Esto hace que sea un buen punto para el desarrollo del algoritmo para su aplicación práctica y para realizar nuevas aportaciones en este sentido.

9. FUTUROS TRABAJOS

Este trabajo es una primera parte de un desarrollo más amplio. Existen gran cantidad de caminos que se pueden tomar desde este punto. Entre esos campos están la aplicación a vehículos autónomos, la reconstrucción usando vehículos cooperativos y la fusión con otros sensores. También existen necesidades de mejora en campos como nuevas características visuales más robustas o reducción de las necesidades de computación.

Es particularmente interesante las aplicaciones a vehículos aereos. Existe un amplio campo de investigación en reconstrucción 3D de modelos digitales de terreno y en producción de mosaicos fotográficos ortorectificados. Las características de movimiento y capacidades de observación de estos vehículos hacen que las técnicas de SLAM visual sean muy aplicables. Además otras técnicas de SLAM como por ejemplo las basadas en sensores láser no son aplicables con un coste razonable.

En lo que se refiere a técnicas de exploración de entornos existe una paralelización natural del problema usando diferentes equipos coordinados. Esto conduce a la exploración cooperativa de escenarios usando diferentes robots o vehículos autonomos. Este problema tiene grades posibilidades de éxito en el caso del SLAM visual ya que el tratamiento de las coordenadas y la existencia de carcterísticas visuales puntuales permiten una sencilla formulación del problema conjunto. Esto hace que sea una extensión natural del algoritmos.

En lo que se refiere al algoritmo de SLAM monocular todavía existen campos donde se puede mejorar. Por ejemplo en la mejora computacional de algoritmos que usen parametrización de distancia inversa. Esta parametrización por ejemplo evita que se puedan realizar divisiones del problema en submapas de forma simple. Ademas del incremento computacional que requiere este método por el mayor número de parámetros por característica.

El algoritmo es todavía muy sensible ante entradas erróneas por lo que cualquier avance en la robustez de los métodos que generan las entradas es muy importante. La búsqueda de características también es un método que tiene grandes necesidades computacionales lo cual impide muchas veces la aplicación en tiempo real. Por lo tanto mejoras en este sentido todavía son posible..

El desarrollo de filtros que impidan que una mala asociación o una medida contaminada afecte a la reconstrucción es una necesidad del algoritmo para su implementación en problemas reales. La inclusión de mecanismos que aumenten la precisión y la robustez son lagunas que deben ser cubiertas para dar lugar al uso generalizado del algoritmo en otras aplicaciones.

Aunque en los últimos años ha existido una gran actividad investigadora en el tema de SLAM existen todavía huecos que deben ser rellenados. Las contribuciones en las áreas mencionadas son de gran importancia y no están fuera del alcance de la técnica. Por último la aplicación de la técnica SLAM a otros problemas es un elemento muy innovador que puede dar lugar a muchos desarrollos no centrados en el SLAM pero basados en él.