Construcción de mapas 3D a partir de la extracción de primitivas geométricas obtenidas de datos de un LIDAR. González Barbosa José Joel CICATA Qro Resumen A inicios del año el CICATA Querétaro cuenta con un LIDAR (un acrónimo del inglés Light Detection and Ranging o Laser Imaging Detection and Ranging) de la marca Velodyne. Sin embargo el software perteneciente al LIDAR no permitía el manejo de datos en tiempo real, por tal razón se desarrollaron los drivers de adquisición de datos tanto para el sistema operativo LINUX como WINDOWS. Igualmente se desarrollo un Exhibidor de datos basado en OpenGL. El LIDAR es capaz de reconstruir 360 grados alrededor de él, y reconstruye objetos a distancia de 2 metros hasta 60 metros. La información dada por el LIDAR no puede ser interpretada, por lo que hay que procesarla para identificar estructuras geométricas a partir de esta información. Por lo que hemos implementado métodos automático de extracción de primitivas geométricas que nos permitan la construcción de mapas a partir de esta información. Las primitivas geométricas extraídas automáticamente corresponden a planos, cilindros y esquina. Los planos son indispensables en el modelo de la construcción de un mapa 3D. Como hemos mencionado, la máxima distancia que el LIDAR puede reconstruir son 60 metros, si deseamos reconstruir zonas más extensa debemos pegar varias adquisiciones hechas en posiciones diferentes del LIDAR. De ahí surge las preguntas: Donde colocamos el LIDAR? y Cuántas posiciones son necesarias?. Estas respuestas fueron dadas automáticamente por un software que hemos desarrollado de optimización, el cual ha sido aceptado en IEEE International Conference on Robotics and Automation después de una estricta revisión. 1. Introducción Las ciudades están creciendo y expandiéndose rápidamente generando la necesidad de modelos 3D que sean reproducidos en un corto período de tiempo y de manera muy precisa. Los sensores actuales como el LIDAR Velodyne HDL 64E presentan una gran ventaja al proporcionar una nube de puntos que representan las distancias a la que se encuentran objetos 360 grados alrededor de él, desde un par hasta 60 metros, con los que se pueden generar escenarios virtuales de ambientes reales. Los modelos 3D son creados a partir de la unión de los puntos por medio de líneas formando conjuntos de polígonos que a su vez forman parte de unas mallas las cuales representan el modelo de alambre de los objetos. El resultado final debe ser un modelo realista añadiendo texturas y renderizando los modelos 3D de manera que la experiencia final del usuario sea como si estuviera en ese momento en el lugar
que se está representando. Los modelos 3D son portables de manera que no necesariamente se tiene que hacer una visita al lugar de interés para tomar decisiones. Se puede utilizar como herramienta de apoyo en proyectos arquitectónicos para remodelación y restauración, para promover sitios turísticos en los cuales se puede acceder virtualmente desde cualquier parte del mundo por medio de internet, para hacer un levantamiento de sitios arqueológicos, y para la toma decisiones en planeación urbana al recopilar información sobre avenidas, edificios, manzanas, transporte, etc. Con una utilización eficaz de la generación de modelos de ciudades 3D, es posible proporcionar rápidamente los datos de altura y medidas preventivas de ingeniería para la protección civil en la región afectada. 1.1. Hipótesis Se pueden crear mapas 2D y 3D de manera fácil, rápida y precisa con el LIDAR Velodyne HDL 64E ya que es una de las mejores herramientas para trabajos de navegación autónoma así como de reconstrucción urbana. 1.2. Objetivo general Desarrollar algoritmos computacionales, que a partir de los puntos x,y,z calculados por el LIDAR, determinen las primitivas geométricas tales como planos, cilindros, esferas y esquinas, con las cuales se construyan mapas 3D. 1.3. Objetivos específicos Desarrollar una aplicación para capturar y visualizar la información entregada por el LIDAR, así como los resultados experimentales. Extraer información geométrica de las nubes de puntos. Construcción de mapas 2D de navegación. Construcción de mapas 3D. 1.4. Justificación El LIDAR Velodyne HDL 64E entrega nubes de puntos de diferente resolución cambiando su velocidad de rotación, lo cual permite flexibilidad para trabajar tanto en navegación autónoma como en reconstrucción urbana. En la primera se requiere de nubes de puntos
de baja resolución para que el procesamiento de los datos y la sincronización con otros dispositivos sean mucho más rápidos. En la segunda las nubes de puntos deben tener alta resolución para poder identificar a detalle características de las construcciones. Así el HDL 64E es una muy buena herramienta para realizar este tipo de actividades. 2. Características del LIDAR HDL 64E Con el desarrollo de la nueva tecnología LIDAR de alta definición (HDL, High Definition LIDAR), se ha encontrado nuevos campos de aplicación como navegación autónoma de de vehículos, vigilancia fija y móvil, cartografía, modelado industrial y otras aplicaciones. 2.1. Funcionamiento El LIDAR HDL 64E es un sensor laser de segunda generación y su principio de operación del es bastante simple, obtiene las distancias de los objetos situados a su alrededor utilizando el principio del medidor laser de distancia. Un emisor laser hace incidir un rayo el cual rebota en la superficie de un objeto y un receptor lo captura calculando la distancia de acuerdo al tiempo que tardo en regresar, si tarda más tiempo del esperado o no regresa entonces no se toma en cuenta. Con los ángulos en que se proyecto el laser y la distancia se construye un vector en forma esférica, y así se obtiene un punto en el espacio (la figura 2.1 muestra una nube e puntos adquirida por el LIDAR), y acumulando cada uno de estos se forma la nube de puntos. Cuando esta colección de puntos es renderizada se crea una imagen tridimensional del escenario. Entre más densa sea la nube de puntos mayor resolución tendrá la imagen. Figura 2.1. Ejemplo de una nube de puntos generada por LIDAR
El HDL 64E se muestra en la Figura 2.2 y tiene montado un arreglo de 64 láseres, divididos en un bloque superior y otro inferior de 32 cada uno, y toda la unidad gira en el eje vertical ofreciendo un campo de visión horizontal de 360 grados, al girar en un ángulo θ, y uno vertical de26.8 grados, va desde 2 hasta -24.8 por el acomodo de los láseres en un ángulo φ. Cada laser es disparado miles de veces por segundo, proveyendo más puntos de datos por segundo y una nube de puntos mucho más robusta que los diseños convencionales, con un alcance de hasta 60 metros y una precisión de 5 cm. El laser es de clase 1M, seguro para la vista, con una longitud de onda de 905 nanómetros y con pulsos de 10 nanosegundos. Figura 2.2. Diseño del LIDAR HDL 64E. Este dispositivo opera con 12 Volts, cualquier voltaje mayor a 16 puede dañar la unidad, y de 4 a 6 amperes. Cuenta con interfaces Ethernet y serial, con conectores RJ 45 y RS 232 DB9 respectivamente, y el cable de alimentación de poder. Una vez que el l LIDAR alcanza cierta velocidad de rotación comienza una transmisión simple de paquetes UDP que son capturados por la computadora por medio de su interfaz Ethernet, es necesario un software que funcione como escucha de manera que cuando se reciban inmediatamente sean convertidos en la información requerida y se pueda continuar recibiendo los paquetes restantes.
La cabeza rota horizontalmente en un ángulo y dispara los láseres por grupos ya sea superior o inferior. Para obtener una mayor resolución a grandes distancias se emiten y capturan tres veces el grupo de laser superior por solo uno del inferior. Figura 2.3. Ejemplo de la secuencia de capturaa de datos. 2.2. Adquisición de datos El LIDAR HDL 64E entrega como salida paquetes UDP Ethernet a 100 Mbps. Los paquetes están divididos en 12 bloques de 100 bytes, seguidos de seis bytes al final de cada paquete con información de estado que contienenn un contador de giro y versión del firmware o temperatura. Cada paquete puede ser tanto para el conjunto de laser superior o el inferior, y cada conjunto contiene información de 32 láseres (ver Figura 2.3 y 2.4). Distancia Distancia Encabezado Ángulo Laser 0 Laser 1 2 bytes 2 bytes 3 bytes 3 bytes...... Distancia Laser 30 3 bytes Distancia Laser 31 3 bytes Figura 2.4. Estructura de un bloque de 100 bytes del paquete UDP entregado por el LIDAR. Encabezado. Contiene 2 bytes que indican cuando un paquete pertenece al bloque superior o inferior. El bloque superior se representa con 0xEEFF y el inferior con 0xDDFF. Ángulo. Son 2 bytes que representan un entero entree 0 y 35999, es necesario dividir entre 100 para obtener al ángulo real. Distancia. Conjuntos de 3 bytes con dos de información de la distancia, en incrementos de.2 centímetros, y uno con información de intensidad, de 0 a 255. Y si la distanciaa es cero o menor de tres pies es un dato no valido.
Estado. Muestra o la temperatura interna de la unidad o el número del firmware. La temperatura se muestra con 2 bytes seguidos por la cadena " DegC" en ASCII. El firmware con el formato de cadena " Vn.n" en ASCII. Estos datos son leídos con un código en C++ que por medio se sockets o librerías como WinPcap realiza la conexión y transferencia de los paquetes desde el LIDAR a la computadora. También se puede hacer una modificación en la velocidad de rotación, esto generará que aumente o disminuya la resolución de la imagen ya que la tasa de muestreo de datos no cambia. La velocidad rotación default es de 600 rpm (10 Hz), y puede girar desde 300 rpm (5 Hz) hasta 900 rpm (15 hz). Estas modificaciones se realizan con una conexión serial RS 232 COM. (http://www.velodyne.com/lidar/). Bloque inferior RPM RPS Puntos por revolución Puntos por revolución por laser Resolución angular (grados) 300 600 900 5 10 15 50000 25000 16667 1562.5 781.25 521 0.2304 0.4608 0.6912 Bloque superior RPM 300 600 900 RPS 5 10 15 Puntos por revolución 200,000 100,000 66,667 Puntos por revolución por laser 6250 3125 2083 Resolución angular (grados) 0.2304 0.4608 0.6912 Resolución angular Porst-Bloque inferior 0.1152 0.2304 0.3456 Tabla 2.1 Resolución angular del HDL 64E. 2.3. Conversión de los datos para su análisis y visualización Una etapa importante después de capturar datos del LIDAR es la conversión de los mismos en información útil. Los métodos de análisis propuestos en el presente trabajo utilizan los datos tanto en coordenadas esféricas como en cartesianas. Inicialmente solo tenemos como datos el ángulo θ, la distancia e intensidad, y aplicando los factores de corrección se obtienen coordenadas esféricas, que después son convertidas en coordenadas cartesianas. Antes de hacer la conversión es necesario realizar un filtrado de los datos validos, seleccionar solo aquellos que su distancia está entre 2 y 120 metros. Los factores de corrección se leen de un archivo, con los cuales se hace la corrección de rotación horizontal (rotcorrection), corrección vertical (vertcorrection), en distancia
(distcorrection), compensación vertical (vertoffsetcorrection), y compensación horizontal (horizoffsetcorrection). Factor rotcorrection vertcorrection distcorrection vertoffsetcorrection horizoffsetcorrection Descripción Ángulo de corrección rotacional para cada laser visto desde atrás de la unidad. Los valores positivos lo rotan hacia la izquierda y negativos a la derecha. Ángulo de corrección vertical para cada laser. Valores positivos apuntan hacia arriba y negativos hacia abajo. Distancia de corrección, en centímetros, para cada laser debido al diseño. Debe sumarse directamente a la distancia leída de los paquetes. Distancia que representa la altura de cada láser medida desde la parte inferior de la base. Es fija para el bloque superior y diferente valor para el bloque inferior. Distancia de compensación horizontal para cada laser. Es una constante positiva o negativa para el valor de todos los láseres. Tabla 2.2 Descripción de los factores de corrección. Cada HDL 64E tiene su propio archivo de calibración y una aplicación para visualizar y capturar los datos, que vienen originalmente en el CD de instalación. Este archivo tiene formato.xml y es usado por el visualizador para realizar los cálculos correspondientes para la visualización. Si un punto es representado con el vector V r,,, para realizar la conversión de coordenadas esféricas a cartesianas se aplica la siguiente relación: cos sen cos
Figura 2.5. Conversión de coordenadas esféricas a cartesianas, a) representación del vector V en forma polar y b) forma cartesiana. 3. Extracción de primitivas: planos La primera actividad como parte de la segmentación de regiones que representen objetos en el conjunto de datos tridimensionales es la identificación de planos. No es posible determinar qué puntos caen exactamente en un plano, es por esto que se deben utilizar métodos que aproximen un conjunto de puntos al modelo matemático que representa un plano en el espacio tridimensional. La ecuación general de un plano es: Y su respectiva normal 3.1. Extracción del plano principal Uno de los problemas críticos al analizar las nubes de puntos es encontrar el plano principal, en este caso la horizontal o el piso, que en teoría debe ser ortogonal a los planos verticales o paredes. Partiendo de la idea en que idealmente en una nube de puntos capturada en un piso sin inclinación o paralelo a la horizontal el plano principal es paralelo a los ejes X Z y los verticales están proyectados sobre el eje Y, ésta orientación de los ejes es tomada de acuerdo al motor de gráficos de OpenGL. En la figura 3.1
Figura 3.1. Plano del piso de manera ideal colocado sobre los ejes X Z. El vector normal del plano X Z es, de esta manera al no tener un desplazamiento, la ecuación normal al plano X Z es: Para determinar qué puntos de del conjunto caen dentro del plano se evalúa cada uno calculando su distancia con respecto a este y tomando como validos los que su distanciaa se aproxime a cero. Teniendo un punto y un plano con ecuación, la distancia del punto al plano está determinada por: (1) Por lo tanto para evaluar los puntos del plano principal de maneraa ideal: No se puede considerar al plano de manera ideal ya que los datos que nos entrega el LIDAR pueden tener un pequeño error. Además de que en el mundo real se tienen puntos muy cercanos que aunque idealmente no caen dentro del plano se deben considerar parte de este. Al método se le adapto un umbral, que es la distancia mínima permitida entre el punto y el
plano, para decretar si el punto es inlier u outlier, los primeros son todos los que se ajustaron al modelo. De manera que la ecuación (1) queda como: Los inliers existen solo dentro de un volumen imaginario delimitado por los planos paralelos a definidos por un desplazamiento positivo y negativo. Figura 3.2. Inliers y outliers dentro de un plano. Pero como el algoritmo parte de un plano ideal paralelo al eje X Z y se necesita el plano real se vuelve a calcular su ecuación solo con los inliers. Considerando la ecuación general del plano y los inliers se puede formar un sistema de la siguiente manera: Donde tenemos como incógnitas a, b, c y d, así: Comoo A es una matriz de m x n donde m>n, la solución de este sistema se obtiene con métodos comoo Gauss Newton o descomposición en valores singulares (SVD).
Resolviendo el sistema se tiene una nueva ecuación del plano con los inliers. Y se vuelve a realizar el proceso de calcular las distancias hacia este nuevo plano y resolver el nuevo sistema 0 hasta que el número de inliers ya no se incremente. Para calcular el plano principal en la siguiente captura o segunda nube de puntos, ya no se parte del plano ideal X Z sino que se toma el plano principal resultante de la nube de puntos anterior y de ahí se comienza la evaluación, de esta manera se reduce el tiempo de cálculo ya que es muy probable que solo haya tenido un pequeña variación. El algoritmo para encontrar el plano principal en una sola nube de puntos procede de la siguiente manera: Entrada: Datos (Nubes de puntos), Π (Ec. Plano anterior), (umbral o threshold). Salida: El mejor plano (Ecuación del plano). Inicializar: Hacer: o plano numantinliers=0; Calcular, para los puntos Seleccionar cuales puntos son inliers, ; Evaluar si (numinliers == numantinliers) Salir del ciclo Sino numantinliers= numinliers o o Formar el sistema y resolverlo. Establecer la nueva ecuación del plano principal con los nuevos parámetros. Hasta que numinliers = numantinliers Al terminar el mejor plano es el anterior mejor plano. Quitar los inliers de la nube de puntos y tomar el resto de puntos como la nueva entrada para encontrar otros planos. 3.2. Extracción aleatoria de planos
Un método que permite detectar los puntos que se acercan a un plano teniendo una distancia que consideramos un error mínimo es el RANSAC (RANdom SAmple Consensus), el cual ajusta una cierta cantidad de parámetros a un modelo matemático, obteniendo cuales son inliers. Este método es comúnmente utilizado para detección de figuras geométricas en 2D como líneas, elipses, etc., y en ésta aproximación se adapto el algoritmo para ajustar al modelo del plano. Se utiliza un punto (A) escogido de manera aleatoria y suss dos vecinos más cercanos (B y C), con los cuales se calcularan los vectores directores (.), para formar un plano inicial. Figura 3.3. Plano formado con tres puntos de los cuales se deriva la normal y sus vectores directores. Se calcula un vector normal N al plano con el producto vectorial de los dos vectores directores: Donde Así se determina la ecuación del plano como: De manera cíclica en cada iteración se calcula la distancia de ese plano con otro punto, también escogido de manera aleatoria, evaluando si puede caer dentro del plano con un umbral de distancia máxima con respecto a éste. El ciclo termina cuando se tiene una cierta cantidad de inliers o se completa el número de iteraciones. El algoritmo procede de la siguiente manera: Entrada: Datos (Nube de puntos), N (iteraciones), t (umbral o threshold).
Salida: El mejor plano (Ecuación del plano). Repetir N veces: o Escoger aleatoriamente un punto. o Determinar los parámetros de la ecuación del plano que forma el punto con dos de sus vecinos más cercanos. o Encontrar los datos que son compatibles con el modelo estimado (inliers), con un margen de error t. o Si el número de inliers es más grande que el anterior mejor plano este se convierte en el mejor plano. Al terminar el mejor plano es el anterior mejor plano. Las desventajas del método aleatorio son: Tener una gran cantidad de ruido o conjuntos de puntos que representan objetos no estructurados que pueden tomarse como datos validos y no se obtenga un resultado ideal El tiempo que pueda tomar en procesar los datos se incrementa si el conjunto de puntos es muy grande y/o se realizan muchas iteraciones. 4. Calibración El LIDAR cuenta con 64 láseres, sin embargo el software del fabricante solo usa 8 factores de corrección, ya que considera que los el LIDAR contiene 4 grupos de 8 láseres. Los ocho factores consisten en tener 4 correcciones en theta (un valor paraa cada grupo) y 4 correcciones en altitud (1 valor para cadaa grupo). Esto equivale a decir que tenemos 8 láseres en un solo punto, y repetido esto 4 veces. Lo anterior físicamente no es posible, por tal motivo hemos desarrollado un modelo propio para el LIDAR. Este modeloo considera tres factores de corrección por láser, esto corresponde a un total de 192 factores de calibración. La ecuación del modelo es presentada en la ecuación 4.1 Ec (4.1)
La calibración del LIDAR consiste en calcula,, para cada laser a partir de comparar los datos adquiridos por el LIDAR con unos datos de dimensiones exactas. Usamos las dimensiones del patrón de calibración para crear valores exactos. La figura siguiente muestra la creación de las mediciones exactas del LIDAR usando la forma real del patrón, en esta figura el LIDAR es colocado virtualmente en dos posiciones. Figura 4.2 Datos teóricos adquiridos por el LIDAR. Estos son calculados computacionalmente a partir de la dimensiones del LIDAR y considera la orientación y posición del LIDAR con respecto a la referencia del patrón La orientación y translación entre el sistema de referencia del LIDAR y el patrón de calibración también son calculados en el proceso de calibración. Por lo que el número de variables a encontrar aumenta 6 mas (3 de rotación y 3 de traslación). Es decir el número total de variables es de 198.
5. Resultados: Construcción de mapas En esta sección se presentan técnicas de creación de mapas en 2D y 3D que servirán como base para trabajos futuros de navegación autónoma y reconstrucción urbana. El proceso consiste en unir información que se obtiene entre capturas continuas. 5.1. Mapas 2D Para la creación de un mapa 2D se capturan los datos generados por el LIDAR en un giro de 360 y se forma la nube de puntos solo con aquellos que caen dentro del área de interés. Como solo interesa conocer las zonas que son navegables, es decir que no tienen obstáculos con los cuales el auto pueda colisionar, se aplica el método para detectar el piso o plano principal expuesto en la sección 3.1 y así solo se analizarían puntos que representan objetos. Figura 5. 1. Nubes de puntos, a) Total de datos, b) Resto de puntos después de que el plano del suelo fue segmentado. Una vez que el plano del suelo ha sido segmentado de la nube de puntos se puede generar el mapa de navegación al tomar el resto de los puntos y proyectarlos en el plano X Z. Esta proyección es dividida por una cuadricula en pequeñas zonas las cuales dirán si es posible navegar por ahí. El mapa 2D es representado por una matriz cuadrada con
La cuadricula se forma tomando en cuenta el área cuadrangular a evaluar de a cuerdo a una distancia D en dirección positiva y negativa en los ejes X y Z. La relación del tamaño de las áreas y el de la cuadricula es la precisión con la que se quiere evaluar cada zona. (a) (b) (c) (d) Figura 5.2. Vista superior de los puntos 3D a) Proyección de los puntos en los ejes X Z, b) Mapa de navegación en la misma perspectiva que a), c) Mapa de navegación en dos dimensiones, d) Mapa de navegación ampliado donde las zonas de sombreadas no son navegables. 5.2 Reconstrucción de Mapas 3D. A partir de la adquisición del LIDAR y la extracción de características (ver figura 5.3 (a) y (b)) el objetivo es empatar características visible en ambas vistas con el propósito de crear un mas más extenso formada por ambas vistas, estos resultados son mostrado en la figura 5.4. Obsérvese como en las figuras 5.3 (a) y (b) existe una zona en el centro que no es reconstruida y debido a la superposición de esta información zona ciega del centro desaparece en la figura 5.4.
(a) (b) Figura 5.3. Vista superior de los mapas calculados por el software tomados en dos posiciones diferentes. NOTA: las imágenes son presentadas en MATLAB pero todo el sistema se encuentra codificado en C++ Figura 5.4 Vista superior de la reconstrucción 3D a partir de varios mapas. Obsérvese como desaparece la zona ciega del centro de las imágenes presentes en la figura 5.3 5.2 Calibración. Los datos adquiridos por el LIDAR son corregidos por factores proporcionados por el fabricante. Sin embargo creemos que estos valores no son del todo correctos. La figura 5.5 muestra un patrón de calibración con dimensiones conocidas, y que corresponde a un cubo. Sin embargo la figura 5.6 muestra los resultados adquiridos por el LIDAR vistos desde la parte superior. Los factores de corrección utilizados en esta figura corresponden a los proporcionados por el fabricante. En ello podemos ver que la adquisición no se parece al
cubo. La figura 5.7 muestra la misma adquisición pero corregida con nuestros factores de calibración. En esta figura podemos observar que la exactitud de la medición es mucho mayor a la proporcionada usando los factores del fabricante. Figura 5.5 Patrón de calibración utilizado. Figura 5.6 Reconstrucción del patrón de calibración usando los factores de corrección del fabricante
Figura 5.7 Reconstrucción del patrón de calibración usando nuestros factores de corrección 5.3 Resultados adicionales Resultados adicionales como la calibración de la cámara no son reportados en este documento debido a que no fueron considerados en la propuesta de proyecto. Pero en este año 2009 nos van a servir para colocarle textura a los mapas creados por la información del LIDAR. Resultados adicionales como el posicionamiento óptimo del lidar y de las cámaras panorámicas no son reportados en este documento debido a que no fueron considerados en la propuesta de proyecto. Pero en este año 2009 nos van a servir para construir de manera óptima mapas 3D de zonas muy grandes. 6. Impacto Se puede utilizar como herramienta de apoyo en proyectos arquitectónicos para remodelación y restauración, para promover sitios turísticos en los cuales se puede acceder virtualmente desde cualquier parte del mundo por medio de internet, para hacer un levantamiento de sitios arqueológicos, y para la toma decisiones en planeación urbana al recopilar información sobre avenidas, edificios, manzanas, transporte, etc. Con una utilización eficaz de la generación de modelos de ciudades 3D, es posible proporcionar rápidamente los datos de altura y medidas preventivas de ingeniería para la protección civil en la región afectada.