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

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

Licenças

Acesso aberto
Artigo: CC BY 4.0

Tudo o que segue se baseia no artigo completo sob a licença CC BY 4.0; sua figura é mostrada com crédito aos autores.

Em resumo

ICP de varredura para mapa com uma pose inicial e uma final por varredura e saltos permitidos entre varreduras, mais fechamento de loop com imagens de elevação; 0,59 % de RTE no ranking do KITTI com 60 ms por varredura.

Por que importa

O método precisa apenas das coordenadas dos pontos e de marcas de tempo por ponto, e lida com LiDAR montado em carro, em Segway e de mão dentro de um único framework. Sua odometria com o perfil de direção roda a 60-80 ms por varredura em uma única thread de CPU, e o SLAM completo usa apenas dados LiDAR.

Problema

Um LiDAR multifeixe giratório registra cada varredura enquanto se move, então os pontos precisam ser corrigidos pelo movimento do sensor. A maioria dos métodos de odometria corrige a distorção de uma varredura uma única vez com um modelo de velocidade constante, o que falha diante de mudanças bruscas de orientação ou de aceleração, enquanto métodos em tempo contínuo com trajetórias suaves não conseguem acompanhar o movimento de alta frequência e trepidante de plataformas reais.

Solução proposta

O CT-ICP descreve cada varredura por uma pose inicial e uma pose final, interpola uma pose para cada ponto a partir de sua marca de tempo (slerp para rotação, linear para translação) e resolve um ICP ponto a plano de varredura para mapa com restrições fracas que mantêm a pose inicial próxima da pose final anterior e limitam a aceleração. O mapa local é uma nuvem de pontos densa em uma grade de voxels esparsa (até 20 pontos por voxel), e um módulo de fechamento de loop casa características 2D em imagens de elevação de mapas locais agregados, refina os casamentos com ICP e otimiza um grafo de poses com g2o.

Principais ideias

Permitir que a pose inicial de uma varredura difira da pose final da varredura anterior absorve o movimento de alta frequência que trajetórias suaves em tempo contínuo não conseguem representar.
A distorção elástica dentro da varredura importa em dados brutos: substituí-la por uma correção de distorção (de-skewing) de velocidade constante eleva o RTE de 0,55 % para 0,79 % no KITTI-raw e de 0,45 % para 0,60 % no KITTI-360.
As normais e os pesos de planaridade são calculados a partir do mapa local denso, e não da varredura atual, o que dá vizinhanças mais ricas para os resíduos ponto a plano.
Um mapa de voxels esparso dá acesso a vizinhanças em tempo constante em vez do acesso logarítmico das árvores kd, o que mantém um mapa denso utilizável em tempo real.
Nas varreduras do KITTI com correção de movimento todos os métodos comparados ficam próximos; as diferenças aparecem em varreduras brutas, sem correção.

Principais resultados

Ranking de odometria do KITTI (sequências de teste 11-21): 0,59 % de RTE, o primeiro entre os métodos com código público na época.
RTE médio em sequências com ground truth: 0,53 % no KITTI com correção de movimento (60 ms/varredura), 0,55 % no KITTI-raw (65 ms) e 0,45 % no KITTI-360 (70 ms); IMLS-SLAM, MULLS e pyLiDAR F2M atingem 0,71-1,61 % no KITTI-raw e 0,68-1,55 % no KITTI-360.
KITTI-CARLA (simulado): 0,09 % de RTE contra 0,81 % da melhor outra odometria; ParisLuco: 1,11 % contra 1,26 % do IMLS-SLAM.
Perfil de movimento de alta frequência: 0,55 % de RTE no Newer College (pyLiDAR F2M 2,20 %) e 1,17 % no NCLT 2012-01-08 (pyLiDAR F2M 19,80 %), com 430 ms e 180 ms por varredura.
O fechamento de loop reduz o ATE de 6,22 m para 0,66 m no KITTI-raw 00, de 29,87 m para 1,07 m no KITTI-360 00 e de 29,16 m para 9,65 m no ParisLuco; no KITTI-CARLA Town01 ele fica perto de 0,2 m (de 0,21 m a 0,26 m).
Cada construção e casamento da grade de elevação leva 1,1 s em média, mais 1,2 s de otimização do grafo de poses quando um loop é encontrado.

Limitações

O fechamento de loop exige que o movimento do sensor seja majoritariamente planar e que a calibração extrínseca alinhe o eixo z com a normal do solo, o que limita as configurações de sensores e os movimentos suportados.
O fechamento de loop roda na thread principal (1,1 s por grade de elevação mais 1,2 s por otimização do grafo de poses), então o SLAM completo ainda não é em tempo real; os autores afirmam que threads separadas o tornariam.
O mapa local não esquece pontos em uma janela deslizante, então um registro ruim o contamina; o perfil robusto que protege contra isso aumenta o tempo de execução (430 ms por varredura no Newer College, 180 ms no NCLT).
O solver de Gauss-Newton usado em todos os experimentos tem jacobianos calculados à mão, então suas restrições não podem ser alteradas; a opção mais flexível com Ceres precisa de várias threads para uso em tempo real.
O número de candidatos a fechamento de loop (10) precisa ser adaptado à deriva esperada, e no KITTI-CARLA o ATE não melhorou após o fechamento de loop.

Impacto na área

Finalista do ICRA 2022 Outstanding Paper, com 301 citações. O artigo disponibiliza a odometria CT-ICP em C++ (github.com/jedeschaud/ct_icp), que descreve como código aberto público e permissivo, coloca a detecção de loops e o back-end do grafo de poses no pyLiDAR-SLAM (github.com/Kitware/pyLiDAR-SLAM) e disponibiliza online os conjuntos de dados usados na forma que descreve.

Resumo

Sensores LiDAR multifeixe são cada vez mais usados em robótica, em particular em carros autônomos para tarefas de localização e percepção, ambas dependentes da capacidade de construir um mapa preciso do ambiente. Para isso, propomos um novo método de odometria em tempo real somente com LiDAR chamado CT-ICP (de Continuous-Time ICP), completado em um SLAM completo com um novo procedimento de detecção de loops. O núcleo deste método é a introdução da combinação de continuidade no casamento de varreduras e descontinuidade entre varreduras. Isso permite tanto a distorção elástica da varredura durante o registro, para maior precisão, quanto maior robustez a movimentos de alta frequência, graças à descontinuidade.

Construímos um SLAM completo sobre essa odometria, usando uma detecção de loops rápida puramente LiDAR baseada no casamento 2D de imagens de elevação, que fornece um grafo de poses com restrições de loop. Para mostrar a robustez do método, nós o testamos em sete conjuntos de dados: KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College e NCLT, em cenários de direção e de movimento de alta frequência. Tanto a odometria CT-ICP quanto a detecção de loops estão disponíveis online. O CT-ICP é atualmente o primeiro, entre os que dão acesso a código público, no ranking de odometria do KITTI, com um erro de translação relativo (Relative Translation Error, RTE) médio de 0,59 % e um tempo médio por varredura de 60 ms em uma CPU com uma única thread.

Open Source Robotics