MAD-BA: 3D LiDAR Bundle Adjustment -- from Uncertainty Modelling to Structure Optimization

Resumen Técnico: MAD-BA: Ajuste de Bunde 3D con LiDAR – desde la Modelización de la Incertidumbre hasta la Optimización de la Estructura

Enunciado del Problema
La modelización 3D precisa y globalmente consistente es crítica para la navegación autónoma, la robótica y la inspección de infraestructuras. Aunque los sensores LiDAR modernos capturan millones de puntos por segundo, lograr consistencia global sigue siendo un desafío debido a distribuciones de datos dispersas e irregulares y a la falta de pistas visuales. El SLAM (Localización y Mapeo Simultáneos) con LiDAR tradicional a menudo depende de la Optimización de Gráficos de Poses (PGO), que refina las trayectorias manteniendo fija la estructura del mapa. Este desacoplamiento conduce a ruido no corregido e inconsistencias en las nubes de puntos. Además, los métodos existentes de refinamiento global para LiDAR o bien tratan la optimización de poses y estructura por separado (lo que genera inconsistencias) o dependen de técnicas de registro local (como ICP) que no optimizan conjuntamente la estructura global. Asimismo, las mediciones LiDAR son inherentemente ruidosas debido a las condiciones ambientales y a la física del sensor, sin embargo, muchos sistemas carecen de un modelo de incertidumbre generalizado para ponderar eficazmente las mediciones durante la optimización.

Metodología
El artículo introduce MAD-BA, un marco unificado de Ajuste de Bundles (BA) global diseñado para optimizar simultáneamente las poses de los sensores y la estructura del mapa 3D. La metodología se basa en cuatro componentes principales:

1. Representación basada en Surfels: El mapa 3D se representa utilizando surfels (discos orientados), definidos por un punto central, una normal de superficie y un radio. Esta representación se elige por su eficiencia en la fusión y actualizaciones, su capacidad para capturar superficies delgadas mejor que los enfoques basados en voxels, y su evitación de actualizaciones topológicas computacionalmente costosas requeridas por los métodos basados en mallas.
2. Asociación de Datos Eficiente: Para coincidir las mediciones LiDAR con los surfels, el sistema emplea árboles kd construidos mediante Análisis de Componentes Principales (PCA). Esta estructura codifica la segmentación de planos y permite consultas eficientes de vecinos más cercanos. La asociación de datos se realiza coincidiendo las hojas del árbol con los surfels existentes basándose en la distancia euclidiana, la distancia normal y umbrales angulares.
3. Modelo de Incertidumbre LiDAR Generalizado: Para abordar el ruido de medición, los autores proponen un modelo de incertidumbre basado en muestras. En lugar de depender de datos de forma de onda propietarios (a menudo no disponibles), el sistema simula el haz LiDAR como un cono basado en la divergencia del haz. Mediante el lanzamiento de múltiples sub-haces dentro de este cono hacia la media de una hoja, el sistema muestrea un conjunto de rangos. La desviación estándar de estos rangos muestreados se calcula para estimar la incertidumbre de medición ($\sigma$). Este $\sigma$ se utiliza para ponderar los residuos en la función de coste, priorizando las mediciones fiables.
4. Optimización Conjunta: El sistema minimiza una función de coste utilizando el algoritmo de Levenberg-Marquardt (LM). La función de coste agrega las distancias punto-a-plano ponderadas por la función de pérdida robusta de Huber y la incertidumbre estimada. Crucialmente, la optimización actualiza conjuntamente tanto las poses del sensor (parametrizadas en el álgebra de Lie $se(3)$) como las posiciones de los surfels. Los surfels están restringidos a moverse solo a lo largo de su dirección normal para preservar la geometría local y reducir los grados de libertad, mientras que las poses se actualizan en los seis grados de libertad.

Contribuciones Clave
El artículo describe tres contribuciones principales:

1. Un marco de Ajuste de Bundles escalable y robusto, adaptado específicamente para datos LiDAR que integra la optimización de poses y estructura.
2. Un modelo de incertidumbre LiDAR generalizado que estima la fiabilidad de la medición basándose en la divergencia del haz y el rango, permitiendo una optimización ponderada por la confianza sin requerir datos de forma de onda crudos.
3. Una implementación de código abierto del sistema para apoyar la reproducibilidad y la investigación futura.

Resultados Experimentales
El sistema se evaluó en tres conjuntos de datos públicos: Newer College Dataset (NC), A Vision Benchmark in Rome (VBR) y KITTI. La evaluación comparó MAD-BA con métodos del estado del arte, específicamente BALM2 (un método de refinamiento global que optimiza solo las poses) y HBA (un método de BA jerárquico).

• Precisión de la Trayectoria: Los resultados cuantitativos utilizando el Error de Trayectoria Absoluta (ATE) muestran que MAD-BA supera consistentemente a BALM2 y HBA en la mayoría de las secuencias. Por ejemplo, en la secuencia "math-easy" de NC, MAD-BA con incertidumbre logró un error RMS de 0.043 m, en comparación con 0.087 m para la trayectoria inicial y 0.135 m para HBA.
• Calidad del Mapa: La evaluación en el conjunto de datos NC (que proporciona mapas de verdad fundamental de alta resolución) demostró que MAD-BA mejora significativamente la precisión, la completitud y la puntuación F del mapa en comparación con los mapas iniciales y otros métodos. Los mapas basados en surfels mostraron discrepancias geométricas reducidas y una mejor alineación con la verdad fundamental.
• Robustez y Ablación:
  • Modelado de Incertidumbre: Incorporar el modelo de incertidumbre redujo el número de iteraciones requeridas para la convergencia y mejoró la precisión final en comparación con una versión donde todas las mediciones se ponderaron por igual.
  • Inicialización: El sistema demostró una fuerte tolerancia a los errores iniciales de la trayectoria, convergiendo de manera fiable con ruido gaussiano de hasta 1 m en traslación y 0.2 rad en rotación.
  • Densidad del Sensor: Cuando se submuestreó para simular un sensor VLP-16, el sistema mantuvo su efectividad en la mayoría de los escenarios, aunque el rendimiento se degradó en casos de movimiento vertical (por ejemplo, escaleras) debido a un campo de visión vertical insuficiente.
  • Filtrado Dinámico: Los resultados cualitativos indicaron que la representación de surfels filtra implícitamente los objetos dinámicos (por ejemplo, coches en movimiento, peatones), resultando en mapas estáticos más limpios.

Significado y Afirmaciones
El artículo afirma que MAD-BA aborda la brecha entre la optimización solo de poses y el refinamiento de estructura separado, proporcionando un marco unificado que logra un mapeo globalmente consistente y robusto. Al optimizar conjuntamente poses y surfels e incorporar un modelo de incertidumbre motivado por la física, el sistema mitiga el ruido y la dispersión inherentes a los datos LiDAR. Los autores declaran que su enfoque supera a los métodos existentes basados en LiDAR en robustez y precisión, produciendo mapas 3D limpios y de alta fidelidad mientras compensa los artefactos dinámicos y la distorsión del LiDAR. El trabajo se presenta como un paso hacia una estimación de estado más fiable en robótica, siendo la naturaleza de código abierto del código destinada a facilitar la adopción por parte de la comunidad y el desarrollo futuro.