CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

Autores: Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, François Goulette · ICRA 2022

Licencias

• Acceso abierto
• Artículo: CC BY 4.0

Todo lo que sigue se basa en el artículo completo bajo su licencia CC BY 4.0; su figura se muestra con crédito a los autores.

En resumen

ICP de escaneo a mapa con una pose inicial y una final por escaneo y saltos permitidos entre escaneos, más cierre de bucle con imágenes de elevación; 0,59 % de RTE en la clasificación de KITTI con 60 ms por escaneo.

Por qué importa

El método solo necesita las coordenadas de los puntos y marcas de tiempo por punto, y maneja LiDAR montados en coche, en Segway y de mano dentro de un mismo marco. Su odometría con el perfil de conducción funciona a 60-80 ms por escaneo en un solo hilo de CPU, y el SLAM completo usa solo datos LiDAR.

Problema

Un LiDAR multihaz giratorio registra cada escaneo mientras se mueve, por lo que los puntos deben corregirse según el movimiento del sensor. La mayoría de los métodos de odometría corrigen la distorsión de un escaneo una sola vez con un modelo de velocidad constante, lo que falla ante cambios bruscos de orientación o de aceleración, mientras que los métodos en tiempo continuo con trayectorias suaves no pueden seguir el movimiento de alta frecuencia y tembloroso de las plataformas reales.

Solución propuesta

CT-ICP describe cada escaneo mediante una pose inicial y una pose final, interpola una pose para cada punto a partir de su marca de tiempo (slerp para la rotación, lineal para la traslación) y resuelve un ICP punto a plano de escaneo a mapa con restricciones débiles que mantienen la pose inicial cerca de la pose final anterior y limitan la aceleración. El mapa local es una nube de puntos densa en una rejilla de vóxeles dispersa (hasta 20 puntos por vóxel), y un módulo de cierre de bucle empareja características 2D en imágenes de elevación de mapas locales agregados, refina los emparejamientos con ICP y optimiza un grafo de poses con g2o.

Ideas clave

• Permitir que la pose inicial de un escaneo difiera de la pose final del escaneo anterior absorbe el movimiento de alta frecuencia que las trayectorias suaves en tiempo continuo no pueden representar.
• La distorsión elástica dentro del escaneo importa con datos en bruto: sustituirla por una corrección de distorsión (de-skewing) de velocidad constante eleva el RTE de 0,55 % a 0,79 % en KITTI-raw y de 0,45 % a 0,60 % en KITTI-360.
• Las normales y los pesos de planaridad se calculan a partir del mapa local denso y no del escaneo actual, lo que da vecindarios más ricos para los residuos punto a plano.
• Un mapa de vóxeles disperso da acceso a vecindarios en tiempo constante en lugar del acceso logarítmico de los árboles kd, lo que mantiene utilizable en tiempo real un mapa denso.
• En los escaneos de KITTI con corrección de movimiento todos los métodos comparados están cerca; las diferencias aparecen en escaneos en bruto sin corregir.

Resultados principales

• Clasificación de odometría de KITTI (secuencias de prueba 11-21): 0,59 % de RTE, el primero entre los métodos con código público en ese momento.
• RTE medio en secuencias con ground truth: 0,53 % en KITTI con corrección de movimiento (60 ms/escaneo), 0,55 % en KITTI-raw (65 ms) y 0,45 % en KITTI-360 (70 ms); IMLS-SLAM, MULLS y pyLiDAR F2M obtienen 0,71-1,61 % en KITTI-raw y 0,68-1,55 % en KITTI-360.
• KITTI-CARLA (simulado): 0,09 % de RTE frente a 0,81 % de la mejor otra odometría; ParisLuco: 1,11 % frente a 1,26 % de IMLS-SLAM.
• Perfil de movimiento de alta frecuencia: 0,55 % de RTE en Newer College (pyLiDAR F2M 2,20 %) y 1,17 % en NCLT 2012-01-08 (pyLiDAR F2M 19,80 %), con 430 ms y 180 ms por escaneo.
• El cierre de bucle reduce el ATE de 6,22 m a 0,66 m en KITTI-raw 00, de 29,87 m a 1,07 m en KITTI-360 00 y de 29,16 m a 9,65 m en ParisLuco; en KITTI-CARLA Town01 se mantiene cerca de 0,2 m (de 0,21 m a 0,26 m).
• Cada construcción y emparejamiento de la rejilla de elevación tarda 1,1 s de media, más 1,2 s de optimización del grafo de poses cuando se encuentra un bucle.

Limitaciones

• El cierre de bucle requiere que el movimiento del sensor sea mayormente plano y que la calibración extrínseca alinee el eje z con la normal del suelo, lo que limita las configuraciones de sensores y los movimientos admitidos.
• El cierre de bucle se ejecuta en el hilo principal (1,1 s por rejilla de elevación más 1,2 s por optimización del grafo de poses), por lo que el SLAM completo aún no es en tiempo real; los autores afirman que hilos separados lo harían posible.
• El mapa local no olvida puntos en una ventana deslizante, por lo que un registro erróneo lo contamina; el perfil robusto que protege contra esto aumenta el tiempo de ejecución (430 ms por escaneo en Newer College, 180 ms en NCLT).
• El solver de Gauss-Newton usado en todos los experimentos tiene jacobianos calculados a mano, por lo que sus restricciones no se pueden cambiar; la opción más flexible con Ceres necesita varios hilos para uso en tiempo real.
• El número de candidatos de cierre de bucle (10) debe adaptarse a la deriva esperada, y en KITTI-CARLA el ATE no mejoró tras el cierre de bucle.

Impacto en el campo

Finalista del ICRA 2022 Outstanding Paper, con 301 citas. El artículo publica la odometría CT-ICP en C++ (github.com/jedeschaud/ct_icp), que describe como código abierto público y permisivo, incorpora la detección de bucles y el back-end del grafo de poses en pyLiDAR-SLAM (github.com/Kitware/pyLiDAR-SLAM) y pone en línea los conjuntos de datos usados en la forma que describe.

Resumen

Los sensores LiDAR multihaz se usan cada vez más en robótica, en particular en coches autónomos para tareas de localización y percepción, ambas basadas en la capacidad de construir un mapa preciso del entorno. Para ello, proponemos un nuevo método de odometría en tiempo real solo con LiDAR llamado CT-ICP (por Continuous-Time ICP), completado hasta un SLAM completo con un nuevo procedimiento de detección de bucles. El núcleo de este método es la introducción de la combinación de continuidad en el emparejamiento de escaneos y discontinuidad entre escaneos. Permite tanto la distorsión elástica del escaneo durante el registro, para una mayor precisión, como una mayor robustez frente a movimientos de alta frecuencia gracias a la discontinuidad.

Construimos un SLAM completo sobre esta odometría, usando una detección de bucles rápida puramente LiDAR basada en el emparejamiento 2D de imágenes de elevación, que proporciona un grafo de poses con restricciones de bucle. Para mostrar la robustez del método, lo probamos en siete conjuntos de datos: KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College y NCLT, en escenarios de conducción y de movimiento de alta frecuencia. Tanto la odometría CT-ICP como la detección de bucles están disponibles en línea. CT-ICP es actualmente el primero, entre los que dan acceso a código público, en la clasificación de odometría de KITTI, con un error de traslación relativo (Relative Translation Error, RTE) medio de 0,59 % y un tiempo medio por escaneo de 60 ms en una CPU con un solo hilo.

Enlaces

• arXiv 2109.12979: https://arxiv.org/abs/2109.12979
• DOI 10.48550/arXiv.2109.12979: https://doi.org/10.48550/arXiv.2109.12979
• PDF: https://arxiv.org/pdf/2109.12979v1
• Código: https://github.com/jedeschaud/ct_icp