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

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

Licenze

• Open access
• Articolo: CC BY 4.0

Tutto ciò che segue si basa sull'articolo completo secondo la sua licenza CC BY 4.0; la figura è riportata citando gli autori.

In breve

ICP scan-to-map con una posa iniziale e una finale per scansione e salti consentiti tra scansioni, più chiusura del loop con immagini di elevazione; 0,59 % di RTE nella classifica KITTI con 60 ms per scansione.

Perché è importante

Il metodo richiede solo le coordinate dei punti e i timestamp per punto, e gestisce LiDAR montati su auto, su Segway e portatili a mano in un unico framework. La sua odometria con il profilo di guida gira a 60-80 ms per scansione su un singolo thread CPU, e lo SLAM completo usa solo dati LiDAR.

Problema

Un LiDAR multi-fascio rotante registra ogni scansione mentre si muove, quindi i punti devono essere corretti per il moto del sensore. La maggior parte dei metodi di odometria corregge la distorsione di una scansione una sola volta con un modello a velocità costante, che fallisce in caso di cambi improvvisi di orientamento o di accelerazione, mentre i metodi a tempo continuo con traiettorie lisce non riescono a seguire il moto ad alta frequenza e traballante delle piattaforme reali.

Soluzione proposta

CT-ICP descrive ogni scansione con una posa iniziale e una posa finale, interpola una posa per ogni punto a partire dal suo timestamp (slerp per la rotazione, lineare per la traslazione) e risolve un ICP punto-piano scan-to-map con vincoli deboli che mantengono la posa iniziale vicina alla posa finale precedente e limitano l'accelerazione. La mappa locale è una nuvola di punti densa in una griglia di voxel sparsa (fino a 20 punti per voxel), e un modulo di chiusura del loop allinea feature 2D su immagini di elevazione di mappe locali aggregate, raffina le corrispondenze con ICP e ottimizza un grafo delle pose con g2o.

Idee chiave

• Consentire che la posa iniziale di una scansione differisca dalla posa finale della scansione precedente assorbe il movimento ad alta frequenza che le traiettorie lisce a tempo continuo non possono rappresentare.
• La distorsione elastica all'interno della scansione conta sui dati grezzi: sostituirla con una correzione della distorsione (de-skewing) a velocità costante porta l'RTE da 0,55 % a 0,79 % su KITTI-raw e da 0,45 % a 0,60 % su KITTI-360.
• Le normali e i pesi di planarità sono calcolati dalla mappa locale densa anziché dalla scansione corrente, il che dà intorni più ricchi per i residui punto-piano.
• Una mappa a voxel sparsa offre accesso agli intorni in tempo costante invece dell'accesso logaritmico dei kd-tree, il che mantiene utilizzabile in tempo reale una mappa densa.
• Sulle scansioni KITTI con correzione del moto tutti i metodi confrontati sono vicini; le differenze compaiono sulle scansioni grezze non corrette.

Risultati principali

• Classifica di odometria KITTI (sequenze di test 11-21): 0,59 % di RTE, primo tra i metodi con codice pubblico all'epoca.
• RTE medio sulle sequenze con ground truth: 0,53 % su KITTI con correzione del moto (60 ms/scansione), 0,55 % su KITTI-raw (65 ms) e 0,45 % su KITTI-360 (70 ms); IMLS-SLAM, MULLS e pyLiDAR F2M ottengono 0,71-1,61 % su KITTI-raw e 0,68-1,55 % su KITTI-360.
• KITTI-CARLA (simulato): 0,09 % di RTE contro lo 0,81 % della migliore altra odometria; ParisLuco: 1,11 % contro l'1,26 % di IMLS-SLAM.
• Profilo di moto ad alta frequenza: 0,55 % di RTE su Newer College (pyLiDAR F2M 2,20 %) e 1,17 % su NCLT 2012-01-08 (pyLiDAR F2M 19,80 %), con 430 ms e 180 ms per scansione.
• La chiusura del loop riduce l'ATE da 6,22 m a 0,66 m su KITTI-raw 00, da 29,87 m a 1,07 m su KITTI-360 00 e da 29,16 m a 9,65 m su ParisLuco; su KITTI-CARLA Town01 resta vicino a 0,2 m (da 0,21 m a 0,26 m).
• Ogni costruzione e allineamento della griglia di elevazione richiede in media 1,1 s, più 1,2 s per l'ottimizzazione del grafo delle pose quando viene trovato un loop.

Limiti

• La chiusura del loop richiede che il moto del sensore sia per lo più planare e che la calibrazione estrinseca allinei l'asse z con la normale al suolo, il che limita le configurazioni di sensori e i movimenti supportati.
• La chiusura del loop gira sul thread principale (1,1 s per griglia di elevazione più 1,2 s per ottimizzazione del grafo delle pose), quindi lo SLAM completo non è ancora in tempo reale; gli autori affermano che thread separati lo renderebbero tale.
• La mappa locale non dimentica i punti in una finestra scorrevole, quindi una registrazione errata la contamina; il profilo robusto che protegge da questo aumenta il tempo di esecuzione (430 ms per scansione su Newer College, 180 ms su NCLT).
• Il solver di Gauss-Newton usato in tutti gli esperimenti ha jacobiani calcolati a mano, quindi i suoi vincoli non possono essere modificati; l'opzione più flessibile con Ceres richiede più thread per l'uso in tempo reale.
• Il numero di candidati per la chiusura del loop (10) va adattato alla deriva attesa, e su KITTI-CARLA l'ATE non è migliorato dopo la chiusura del loop.

Impatto sul settore

Finalista dell'ICRA 2022 Outstanding Paper, con 301 citazioni. L'articolo rilascia l'odometria CT-ICP in C++ (github.com/jedeschaud/ct_icp), che descrive come codice open source pubblico e permissivo, inserisce il rilevamento dei loop e il back-end del grafo delle pose in pyLiDAR-SLAM (github.com/Kitware/pyLiDAR-SLAM) e rende disponibili online i dataset usati nella forma che descrive.

Abstract

I sensori LiDAR multi-fascio sono sempre più usati in robotica, in particolare nelle auto autonome per compiti di localizzazione e percezione, entrambi basati sulla capacità di costruire una mappa precisa dell'ambiente. A tal fine proponiamo un nuovo metodo di odometria in tempo reale basato solo su LiDAR chiamato CT-ICP (per Continuous-Time ICP), completato in uno SLAM completo con una nuova procedura di rilevamento dei loop. Il nucleo di questo metodo è l'introduzione combinata di continuità nell'allineamento delle scansioni e discontinuità tra le scansioni. Ciò consente sia la distorsione elastica della scansione durante la registrazione, per una maggiore precisione, sia una maggiore robustezza ai movimenti ad alta frequenza grazie alla discontinuità.

Costruiamo uno SLAM completo sopra questa odometria, usando un rilevamento dei loop rapido e puramente LiDAR basato sull'allineamento 2D di immagini di elevazione, che fornisce un grafo delle pose con vincoli di loop. Per mostrare la robustezza del metodo, lo abbiamo testato su sette dataset: KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College e NCLT, in scenari di guida e di movimento ad alta frequenza. Sia l'odometria CT-ICP sia il rilevamento dei loop sono disponibili online. CT-ICP è attualmente primo, tra i metodi che danno accesso a codice pubblico, nella classifica di odometria KITTI, con un errore di traslazione relativo (Relative Translation Error, RTE) medio dello 0,59 % e un tempo medio per scansione di 60 ms su una CPU con un solo thread.

Link

• 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
• Codice: https://github.com/jedeschaud/ct_icp