Figura 1: Diagrama de estructura general de S-PTAM (Pire et al., 2015).
IV. S-PTAM distribuido
IV-A. Arquitectura
Previo a este trabajo, el diseño de S-PTAM hacía uso de memoria compartida en el que existe un único mapa sobre el cual trabaja todo el sistema. En base a este planteo identificamos las funcionalidades predominantes y rediseñamos la arquitectura para que sean ejecutadas en módulos independientes. Cada uno de estos módulos será implementado como un nodo ROS individual, que podrá interactuar con otros mediante el sistema de mensaje que proporciona este framework. La comparación entre dichas arquitecturas puede apreciarse en la Figura 2.
(a) Arquitectura original.
(b) Arquitectura propuesta.
Figura 2: Comparación entre la arquitectura de S-PTAM original y la arquitectura distribuida aquí propuesta. En la arquitectura distribuida tanto el Tracker como el Mapper tienen una instancia del Mapa, y la comunicación entre ambos nodos se realiza a través de mensajes de ROS.
Al proponer un funcionamiento distribuido será necesario contar con una copia del mapa (lo más actualizada posible) en cada uno de los nodos y será fundamental que ambos mapas sean consistentes entre sí. Para lograrlo, será necesario que cada nodo informe sobre los cambios que han sido realizados sobre su copia del mapa para que el otro nodo aplique esos cambios en el propio. En el diseño actual de S-PTAM los elementos del mapa tales como Key Frames, Map Points y Measurements pueden ser accedidos mediante el uso de punteros debido a que se encuentran en espacios de memoria compartida. En cambio, en el diseño propuesto en este trabajo fue necesario desarrollar un mecanismo de identificación de objetos que se encuentran en distintos espacios de memoria.
Es así que se decide incorporar identificadores enteros en los objetos que forman parte del mapa y que son creados y modificados de forma dinámica durante el proceso. Estos identificadores se corresponderán, en cada nodo, al mismo objeto del mapa (cada uno con direcciones distinta de memoria).
Para abstraer este comportamiento de correspondencia se recurre a la creación de la clase Translations, que internamente consta de un vector de punteros a objetos y métodos que permiten asignar sus valores, obtener el objeto apuntado a partir de un identificador dado y otras funciones de control. Dicho vector contendrá en la posición i-ésima el puntero correspondiente al elemento con id igual a i.
Figura 3: Modificaciones de los objetos translations durante el inicio de la ejecución.
Entonces, teniendo una instancia de esta clase para Key Frames y otra para Map Points, tanto Tracker como Mapper pueden hacer referencia a un objeto determinado sin importar que cada nodo lo tenga almacenado en un espacio de memoria distinta y de esta forma se provee un mecanismo para comunicarle al otro nodo sobre cambios en sus atributos de una manera clara y consistente. La Figura 3 muestra la evolución de los objetos translations de cada nodo durante el inicio dela ejecución.Por otro lado, se propone una estructura de mensajes para la comunicación entre los nodos definida en base a los estándares de ROS, los cuales incluyen tipos de dato como enteros, booleanos, flotante, caracteres, cadenas, etc. Además, las estructuras pueden conformarse con vectores anidados de estos tipos primitivos mencionados.
IV-B. Tracker
Con el objetivo de lograr un procesamiento distribuido, y considerando que S-PTAM dispone de un único mapa, se decide incorporar como primera medida una instancia del mapa al nodo Tracker. A su vez, también se incorporan las funciones y métodos que le permiten agregar, modificar y eliminar Key Frames, Map Points y Measurements al mapa.
Entonces, considerando que este nodo posee la funcionalidad de extracción de features podemos afirmar que el nodo Tracker está capacitado para recibir las imágenes estéreo provenientes de la cámara del robot móvil y procesarlas para luego crear y actualizar su instancia del mapa del entorno que lo rodea. De esta forma, se logra autonomía e independencia del nodo Tracker respecto del nodo Mapper, pudiendo ser ejecutado sin necesidad de que Mapper esté ejecutándose simultáneamente. Esta característica se constituye como uno de los principales aportes de este trabajo, ya que hasta el momento no era posible llevarla a cabo con el diseño monolítico actual de S-PTAM. Aunque como veremos luego, los resultados serán mejor al ejecutar ambos nodos concurrentemente, poder utilizar el nodo Tracker de forma autónoma nos permite, por ejemplo, ejecutar una versión simplificada de S-PTAM en dispositivos que no tengan gran capacidad de cálculo.
IV-C. Mapper
El otro módulo principal del sistema que fue implementado como un nodo ROS es el correspondiente a Mapper. Este nodo se acopla al nodo Tracker trabajando de forma complementaria. El nodo Mapper también fue dotado con una instancia del mapa el cual será ajustado continuamente mediante el método de optimización Bundle Adjustment y cada modificación será comunicada a Tracker para que actualice su mapa a fin de lograr consistencia entre ellos.
IV-D. Comunicación entre los nodos
La comunicación entre los nodos se lleva a cabo mediante la utilización del sistema de comunicación y gestión de mensajes provistos por ROS el cual facilita esta tarea a partir de canales de información entre nodos llamados topics. Para enviar mensajes a través de un topic un nodo ROS deberá publicar en él, mientras que para recibir un mensaje publicado en un topic deberá suscribirse al mismo. Cabe destacar que el nodo emisor no conoce quienes son los nodos receptores de su mensaje, es decir que estos son unidireccionales y asíncronos (difusión).
Entonces Tracker y Mapper se comunican utilizando la estructura de mensajes diseñada para tal fin en la que se incluye la información relacionada con los cambios en el mapa, es decir que cuando Tracker agrega elementos como Key Frames, Map Points y Measurements genera un mensaje, respetando la estructura propuesta en la que se incluye toda la información necesaria para que el nodo Mapper pueda agregar esos objetos en su propio mapa. Como contrapartida, cuando el nodo Mapper modifica su mapa al realizar el refinamiento del mismo, le comunica a Tracker dichos cambios utilizando el mencionado sistema de mensajes, incluyendo en ellos sólo información relevante para que Tracker pueda reflejar en su mapa dichas modificaciones y así evitar un mensaje que contenga información redundante e impacte negativamente en la performance del sistema. Por una cuestión de simplicidad en el diseño, se decidió utilizar la misma estructura de mensaje tanto para los que fueran enviados por Tracker como por Mapper.
Para facilitar el manejo de los mensajes se implementó la librería messages handler, la cual contiene funciones que convierten elementos de S-PTAM en mensajes ROS y viceversa. Es decir, que para crear un mensaje la función tomará como argumento objetos de C++ (que representan loselementos que S-PTAM utiliza internamente) y devolverá un mensaje correspondiente siguiendo la estructura definida para tal fin. En contrapartida, la función inversa tomará como argumento un mensaje y retornará un objeto utilizando para ello los constructores de la clase adecuada.
V. EXPERIMENTOS
En este trabajo utilizamos el dataset KITTI (Geiger et al., 2013) de dominio público, ampliamente utilizado en el campo de la robótica móvil para evaluar sistemas de SLAM basados en visión. La secuencia utilizada del dataset es la 00, la misma consiste en el conjunto de imágenes capturadas por una cámara montada sobre un vehículo que transita un ambiente urbano. La trayectoria realizada por el vehículo es de 4 km aproximadamente. Las imágenes cuentan con una resolución de 1344391 píxeles y son obtenidas a un frame rate de 10 Hz.
Para realizar los experimentos se utilizaron dos equipos: una PC con procesador Intel Core i3-4130 de 3.40 GHz con 4 núcleos, 8GB de RAM con Ubuntu 14.04 de 64 bits y una notebook con procesador Intel Core i3-3217U de 1.80 GHz con 4 núcleos, 8 GB de RAM con Ubuntu MATE 16.04 de 64 bits. Ambos equipos fueron conectados mediante una red cableada para simular la distribución de cómputo entre un robot móvil y una estación base. Esta configuración distribuida intenta replicar la existencia de un robot que se comunica durante todo el proceso con otra PC con mayor poder de cómputo, aunque en este caso ambos equipos cuentan con el mismo poder computacional.
La Figura 4 muestra la comparación entre las trayectorias estimadas por el sistema propuesto (para distintas configuraciones) contra la trayectoria real realizada por el vehículo (ground-truth). El color celeste representa la trayectoria para la ejecución del nodo Tracker en la PC, mientras que en rojo se muestra la trayectoria para la ejecución de ambos nodos (Tracker y Mapper) en la misma PC. Finalmente, en color verde, la ejecución distribuida de los nodos en la que Tracker fue ejecutada en la PC mientras que Mapper se ejecutó en la notebook.
Figura 4: Comparación de las trayectorias estimadas por cada una de las configuraciones (Tracker, Tracker + Mapper y S- PTAM distribuido) para la secuencia 00 del dataset KITTI.
En la Tabla 1 se muestran los tiempos de procesamiento en promedio que demora el nodo Tracker en ejecutar sobre un par de imágenes estéreo para cada configuración del sistema (Tracker, Tracker + Mapper y S-PTAM distribuido). Se toma como punto de comparación dicho nodo dado quela tarea de localización debe correr en tiempo real en un sistema SLAM. La configuración con el Tracker individuales el que menos tiempo consume como es de esperarse. La configuración Tracker + Mapper lleva un mayor costo computacional demorando más tiempo que la configuración individual de Tracker. Esto se debe al alto costo computacional agregado por la ejecución del nodo de mapping en la misma PC. S-PTAM distribuido obtiene una performance similar a la de ejecutar el Tracker individualmente con el beneficio de que obtiene una mejor precisión de la localización del robot como se muestra en la Figura 4 y la Tabla 2. La Tabla 2 muestra la comparación de los errores donde t indica error de traslación, mientras que r hace mención al error de rotación, los subíndices abs, rel y RMSE hacen referencia a los errores absolutos, relativos y a la raíz cuadrada del error cuadrático medio (Root Mean Square Error), respectivamente. Max hace referencia a los valores máximos obtenidos para cada tipo de error. Por ejemplo, tabs es el máximo valor de los errores absolutos de traslación obtenidos en toda la secuencia. Se puede observar que los errores relativos y RMSE son similares entre las tres configuraciones planteadas, los máximos errores absolutos de traslación y de rotación para la configuración del nodo Tracker ejecutado de manera individual son muy superiores a los valores obtenidos por las configuraciones a las que se integra el nodo de refinamiento de mapa, Mapper. En el caso de la configuración Tracker + Mapper se observa un marcado descenso en dichos errores, mientras que para el caso de la configuración de S-PTAM distribuido se obtienen errores aún menores, llevando a una mejora en la estimación de la trayectoria del robot al verse disminuida la tasa de error. Observar que la mejora del error entre las configuraciones Tracker+ Mapper y S-PTAM distribuido se debe a la menor carga que esta última configuración requiere sobre la computadora que se encuentra ejecutando el nodo Tracker.
|
Tiempos de ejecución |
Tracker |
Tracker + Mapper |
S-PTAM distr. |
|
Tracking (s) |
0.043 |
0.051 |
0.044 |
Tabla 1: Comparación de tiempos para cada una de las configuraciones (Tracker, Tracker + Mapper y S-PTAM distribuido).
|
Errores |
Tracker |
Tracker + Mapper |
S-PTAM distr. |
|
Max tabs(m) |
47.95 |
17.32 |
16.63 |
|
Max trel(m) |
0.30 |
0.30 |
0.30 |
|
t𝑅𝑀𝑆𝐸 (m) |
0.030 |
0.029 |
0.029 |
|
Max rabs (deg) |
11.70 |
8.99 |
8.84 |
|
Max rrel(deg) |
2.20 |
2.20 |
2.17 |
|
r𝑅𝑀𝑆𝐸 (deg) |
0.12 |
0.12 |
0.12 |
Tabla 2: Comparación de los errores absolutos y relativos máximos, y RMSE obtenidos para cada una de las configuraciones.
VI. CONCLUSIONES Y TRABAJO FUTURO
En este trabajo presentamos un sistema de SLAM distribuido. Inicialmente partimos del desarrollo de S-PTAM (Stereo Parallel Tracking and Mapping) (Pire et al., 2015), el cual es un método que aborda el problema de SLAM basándose en visión estéreo siguiendo el enfoque propuesto por PTAM.
Teniendo en cuenta que S-PTAM es un sistema monolítico que divide las tareas de localización y mapeo en distintos hilos de ejecución, propusimos un nuevo diseño que permite la ejecución de manera distribuida de sus componentes. De esta manera se podría lograr mayor versatilidad a la hora de contar con distintos tipos de hardware con capacidades distintas de procesamiento. Entonces, un robot móvil podría ejecutar parte del procesamiento correspondiente al módulo de localización (tracking) de manera onboard y por otro lado ejecutar el módulo de mantenimiento del mapa (mapping) en un dispositivo externo que de disponga mayor poder de procesamiento, ya que esta tarea es la de mayor costo computacional. Deesta manera, cada módulo dispondrá exclusivamente de toda la capacidad de procesamiento del dispositivo en el que se ejecute.
La arquitectura distribuida permite también numerosas con- figuraciones, como ejecutar sólo el Tracker, el Tracker junto con el Mapper o incluso configuraciones más avanzadas, como por ejemplo tener más de un Mapper analizando cada uno una sección distinta del mapa.
Como trabajo futuro se propone adaptar el módulo de Loop Closing recientemente planteado por Pire et al. (2017) a la versión de S-PTAM distribuida aquí propuesta.
REFERENCIAS
Bailey, T. & Durrant-Whyte, H. (2006). Simultaneous localization and mapping (SLAM): part II. IEEE Robotics Automation Magazine, 13(3):108–117.
Bischof, H. (2012). Dense reconstruction on-the-fly. In Proceedings of the 2012 IEEE Conference on Computer Vision and Pattern Recognition (CVPR), CVPR ’12, pages 1450–1457, Washington, DC, USA. IEEE Computer Society.
Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scara- muzza, D., Neira, J., Reid, I., & Leonard, J. J. (2016). Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust- Perception Age. IEEE Transactions on Robotics, 32(6):1309–1332.
Durrant-Whyte, H. & Bailey, T. (2006). Simultaneous localization and mapping: part i. IEEE Robotics Automation Magazine, 13(2):99–110.
Engel, J., Schöps, T., & Cremers, D. (2014). LSD- SLAM: Large-Scale Direct Monocular SLAM. In Fleet, D., Pajdla, T., Schiele, B., and Tuytelaars, T., editors, Proceedings of the European Conference on Computer Vision (ECCV), pages 834–849, Cham. Springer International Publishing.
Engel, J., Stückler, J., & Cremers, D. (2015). Large- scale direct slam with stereo cameras. In Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS), pages 1935–1942. IEEE.
Forster, C., Lynen, S., Kneip, L., & Scaramuzza, D. (2013). Collaborative monocular slam with multiple micro aerial vehicles. In IROS, pages 3962–3970. IEEE.
Forster, C., Zhang, Z., Gassner, M., Werlberger, M., & Scaramuzza, D. (2017). SVO: Semidirect visual odometry for monocular and multicamera systems. IEEE Trans. Robotics, 33(2):249–265.
Gamage, R. & Tuceryan, M. (2016). An experimental distributed framework for distributed simultaneous localiza- tion and mapping. In Electro Information Technology (EIT), 2016 IEEE International Conference on, pages 0665–0667. IEEE.
Geiger, A., Lenz, P., Stiller, C., & Urtasun, R. (2013). Vision Meets Robotics: The KITTI Dataset. The International Journal of Robotics Research, IJRR, 32(11):1231–1237.
Jazwinski, A. H. (1970). Stochastic processes and filtering theory. Mathematics in science and engineering. Academic press, New York. UKM.
Klein, G. & Murray, D. (2007). Parallel Tracking and Mapping for Small AR Workspaces. In Proceedings of the IEEE International Symposium on Mixed and Augmented Reality (ISMAR), pages 1–10, Washington, DC, USA. IEEE Computer Society.
Mahdoui, N., Natalizio, E., & Frémont, V. (2016). Multi-UAVs network communication study for distributed visual simultaneous localization and mapping. In ICNC, pages 1–5. IEEE Computer Society.
Metropolis, N. & Ulam, S. (1949). The Monte Carlo Method. Journal of the American Statistical Association, 44(247):335–341. PMID: 18139350.
Mur-Artal, R. & Tardós, J. D. (2016). ORB- SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB- D Cameras. CoRR, abs/1610.06475.
Pire, T., Fischer, T., Castro, G., De Cristóforis, P.,Civera, J., & Jacobo Berlles, J. (2017). S-PTAM: Stereo Parallel Tracking and Mapping. Robotics and Autonomous Systems (RAS), 93:27 – 42.
Pire, T., Fischer, T., Civera, J., De Cristóforis, P., & Berlles, J. J. (2015). Stereo parallel tracking and mapping for robot localization. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 1373–1378.
Riazuelo, L., Civera, J., & Montiel, J. M. M. (2014). C2TAM: A cloud framework for cooperative tracking and mapping. 62:401– 413.
Strasdat, H., Davison, A. J., Montiel, J. M. M., & Konolige, K. (2011). Double window optimisation for constant time visual slam. In ICCV, pages 2352–2359. IEEE Computer Society.
Triggs, B., McLauchlan, P. F., Hartley, R. I., & Fitzgibbon, A. W. (2000). Bundle Adjustment — A Modern Synthesis, chapter 21, pages 298–372. Springer Berlin Heidelberg, Berlin, Heidelberg.
Williams, R., Konev, B., & Coenen, F. (2014). Multi-agent environment exploration with ar.drones. In TAROS, volume 8717 of Lecture Notes in Computer Science, pages 60–71. Springer.
Williams, R., Konev, B., & Coenen, F. (2015). Scalable distributed collaborative tracking and mapping with micro aerial vehicles. In IROS, pages 3092–3097. IEEE.