💻 computer science

A Numerically-Robust ROS 2 Port of iG-LIO: Diagnosing and Fixing Toolchain-Induced Failures in Incremental GICP LiDAR-Inertial Odometry

Este artigo apresenta uma porta para ROS 2 Jazzy numericamente robusta do sistema de odometria LiDAR-inercial iG-LIO, detalhando o diagnóstico e a resolução de falhas críticas induzidas pela ferramenta de desenvolvimento — especificamente incompatibilidades de QoS e acumuladores de redução paralela não inicializados — ao mesmo tempo em que adiciona suporte para sensores modernos da Ouster, Velodyne e Livox.

Autores originais: Afonso E. Carvalho, David Portugal, Paulo Peixoto

Publicado 2026-07-14

📖 5 min de leitura🧠 Leitura aprofundada

CC BY 4.0

Autores originais: Afonso E. Carvalho, David Portugal, Paulo Peixoto

Artigo original sob licença CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ Esta é uma explicação gerada por IA do artigo abaixo. Não foi escrita nem endossada pelos autores. Para precisão técnica, consulte o artigo original. Ler aviso legal completo

Imagine que você tem um robô explorador superinteligente chamado iG-LIO. Este robô é um mestre da navegação; ele combina um scanner a laser giratório (LiDAR) e um sensor de movimento (IMU) para construir um mapa 3D perfeito do mundo enquanto descobre exatamente onde está. A versão original deste robô foi construída para um sistema operacional antigo chamado ROS 1.

Recentemente, uma equipe de engenheiros tentou mover este robô para um sistema operacional novo e moderno chamado ROS 2. Eles pensaram: "É apenas um trabalho de tradução! Manteremos o cérebro do robô exatamente o mesmo, apenas mudaremos a língua que ele fala". Eles fizeram a tradução, e o robô começou a funcionar. Mas então, o desastre aconteceu: o cérebro do robô começou a gritar nonsense, enchendo sua memória com erros de "NaN" (Not a Number - Não é um Número) e travando. Foi como um carro perfeitamente saudável que, após uma pintura nova, de repente se recusou a dirigir porque a nova bomba do posto de gasolina não encaixava no bocal.

A equipe percebeu que o cérebro do robô (a matemática) estava bem. O problema era o ambiente em que ele agora vivia. Eles encontraram dois culpados sorrateiros escondidos no novo sistema operacional que quebraram o robô, e eles os consertaram.

O Primeiro Culpado: A Confusão do "Melhor Esforço"

Imagine que o sensor de movimento do robô (o IMU) é um mensageiro frenético correndo para o cérebro do robô, gritando atualizações sobre como o robô está inclinando e girando. No sistema antigo, o cérebro do robô esperava pacientemente por cada mensagem, não importava o quão lotado o corredor ficasse.

No novo sistema, o robô foi instruído a usar um serviço de entrega de "Melhor Esforço" (Best Effort). Isso é como um carteiro que diz: "Eu tentarei entregar estas cartas, mas se a bolsa ficar muito cheia, eu simplesmente descartarei as mais antigas e esperarei que você receba o resto". Como o robô estava processando os dados lentamente, o mensageiro ficou sobrecarregado. O transportador de "Melhor Esforço" começou a descartar e embaralhar a ordem das atualizações de movimento.

O cérebro do robô, que depende de uma cadeia perfeita e ininterrupta de dados de movimento para manter o equilíbrio, ficou confuso com as peças faltando. Ele tentou calcular uma trajetória baseada em uma linha do tempo quebrada e acabou com um desastre matemático (valores NaN).

A Correção: A equipe mudou o contrato de entrega. Eles disseram ao cérebro do robô: "Nada de 'Melhor Esforço'. Precisamos de entrega Confiável (Reliable)". Eles configuraram uma sala de espera enorme (uma fila de 2000 amostras) para que o mensageiro pudesse despejar todas as atualizações sem descartar uma única uma. Eles também adicionaram um guarda de segurança: se o tempo entre as atualizações for estranho (menos de 0 segundos ou mais de 0,5 segundos), o robô apenas ignora esse passo em vez de travar.

O Segundo Culpado: A Armadilha da "Caixa Vazia"

O segundo problema era ainda mais sorrateiro. O cérebro do robô usa uma ferramenta de processamento paralelo super-rápida (chamada oneTBB) para realizar o trabalho pesado. Imagine uma equipe de trabalhadores (threads) tentando contar uma pilha de pedras. Eles dividem a pilha, cada trabalhador conta sua própria pilha e, depois, eles somam seus totais.

No sistema antigo, os trabalhadores começavam com baldes vazios que eram magicamente zerados. No novo sistema, os trabalhadores receberam baldes que pareciam vazios, mas na verdade tinham sujeira aleatória e empoeirada dentro deles porque a nova fábrica não os limpou primeiro. Quando os trabalhadores somavam seus totais, eles acidentalmente somavam essa sujeira aleatória à contagem final. Essa "sujeira" era tão ruim que transformou a matemática do robô em lixo (NaNs).

A Correção: A equipe não parou de usar os trabalhadores paralelos rápidos. Em vez disso, eles envolveram os baldes em uma proteção especial "Zero-Primeiro" (Zero-First). Agora, antes de qualquer trabalhador começar a contar, eles são forçados a limpar seu balde e começar exatamente com zero. Isso manteve a velocidade do processamento paralelo, mas garantiu que a matemática estivesse limpa.

Novos Gadgets e Melhores Mapas

Além de consertar os travamentos, a equipe atualizou o kit de ferramentas do robô:

Novos Scanners: Eles atualizaram o robô para entender os scanners a laser mais novos (como o Ouster OS0 e OS1 Rev 7) para que ele não fique confuso com seus novos formatos de dados. Eles também adicionaram suporte para um Velodyne Velarray M1600 específico.
Flexibilidade Livox: Para sensores Livox, o robô agora pode trabalhar de duas maneiras. Ele pode falar com o driver especial se você o tiver, ou pode apenas ouvir o fluxo de dados padrão (como um sensor Mid-360) sem precisar de nenhum software extra. Isso significa que os usuários não precisam mais caçar drivers específicos.
Configurações Fáceis: Tudo agora é controlado por um arquivo de texto simples (YAML). Você pode dizer ao robô o quão confiável ele precisa ser, o que nomear seus mapas e onde salvar seus registros de viagem.

Funcionou?

A equipe testou o robô em hardware real, incluindo o Ouster OS0 Rev7, Ouster OS1 Rev 7 e Livox MID-360. Eles executaram a mesma sequência de teste na nova versão ROS 2 e na antiga versão ROS 1. O resultado? Os caminhos que o robô desenhou foram qualitativamente idênticos. O robô navegou tão bem quanto antes, provando que as correções não mudaram a forma como o robô pensa, elas apenas impediram que o novo sistema operacional o quebrasse.

Em resumo, mover um robô complexo para um novo sistema não é apenas uma tradução; é sobre entender as novas regras da estrada. Ao corrigir os contratos de entrega e limpar os baldes, a equipe salvou o robô de um travamento silencioso e o colocou de volta para explorar o mundo.

Resumo Técnico: Uma Portabilidade de iG-LIO para ROS 2 com Robustez Numérica

Definição do Problema
O sistema iG-LIO original (Estimativa de Odometria LiDAR-Inercial com GICP Incremental), um estimador fortemente acoplado que funde restrições de GICP generalizado e de ponto-a-plano, foi implementado para o ROS 1, que agora está em fim de vida útil. Embora uma migração mecânica da base de código para o ROS 2 (convertendo de catkin para ament_cmake, de roscpp para rclcpp, etc.) tenha parecido bem-sucedida, a portabilidade falhou silenciosamente na prática. Apesar de manter a matemática de estimativa inalterada, o sistema divergiu com valores internos NaN e abortou. As causas raízes foram identificadas não como falhas algorítmicas, mas como falhas induzidas pelo ambiente, específicas à moderna cadeia de ferramentas (toolchain) e às suas configurações padrão do ROS 2.

Metodologia e Diagnóstico
Os autores realizaram uma portabilidade "mecanicamente fiel" do iG-LIO para o ROS 2 Jazzy, preservando a lógica central de estimativa (mapa de voxel, fusão GICP + ponto-a-plano, filtro de Kalman de estado de erro iterado). A investigação focou em isolar por que o sistema portado falhou enquanto a versão original em ROS 1 funcionava corretamente. Duas causas raízes distintas, ao nível do ambiente, foram identificadas:

Incompatibilidade de QoS (Perda de Dados de IMU): A portabilidade inicial subscreveu dados de IMU usando rclcpp::SensorDataQoS() (Best Effort, Keep Last 5), enquanto as fontes de dados (drivers ou bags) publicavam com QoS Reliable. No ROS 2, este emparelhamento degrada silenciosamente para Best Effort, causando a perda e o reordenamento de amostras de IMU sob carga. Isso corrompeu o sinal necessário para a propagação do filtro iterado.
Redução Paralela Não Inicializada (oneTBB + Eigen): A montagem das restrições utilizava tbb::parallel_reduce com um tipo Eigen::Matrix de tamanho fixo como o tipo de valor de redução. A versão do oneTBB incluída nas distribuições modernas do ROS 2 constrói por padrão acumuladores divididos (split accumulators). Como uma Eigen::Matrix de tamanho fixo não inicializa seu armazenamento ao ser construída por padrão, memória não inicializada foi somada à matriz Hessiana ( $H$ ) e ao vetor ( $b$ ), resultando em valores NaN. Este problema era invisível na ferramenta antiga usada no código original, mas surgiu imediatamente na nova toolchain do ROS 2.

Principais Contribuições
O artigo detalha o diagnóstico e a correção dessas falhas, juntamente com várias melhorias de engenharia para suporte a sensores e usabilidade:

Correção de QoS: A subscrição de IMU foi atualizada para usar QoS RELIABLE com uma fila profunda (KeepLast(2000)), enquanto as filas de LiDAR foram definidas como KeepLast(10). Um guarda de tempo de execução foi adicionado para rejeitar passos de tempo de predição não físicos ( $\Delta t \le 0$ ou $\Delta t > 0,5$ s).
Correção de Robustez Numérica: O acumulador de redução não inicializado foi resolvido envolvendo a Eigen::Matrix em uma pequena struct. Os membros desta struct são explicitamente inicializados com zero em classe (via Eigen::Matrix::Zero()) e unidos via operator+. Isso preserva a estrutura de tbb::parallel_reduce única do upstream, garantindo que cada tarefa dividida do TBB comece com um valor de identidade zerado, eliminando a falha de leitura de memória não inicializada sem sacrificar o paralelismo.
Atualizações de Parsing de Sensores:
- Ouster: O struct/parser de pontos foi atualizado para corresponder ao layout atual de campos de PointCloud2 para sensores Rev 7, restaurando as capacidades de desinclinação de movimento (motion de-skewing) quebradas no código original.
- Velodyne: Adicionado suporte para o Velodyne Velarray M1600.
- Livox: Implementados dois caminhos de ingestão. Um caminho controlado por compilação para o CustomMsg específico do driver (que falha graciosamente se o driver estiver ausente) e um caminho livre de driver para sensores que publicam sensor_msgs/PointCloud2 padrão (ex: Mid-360), garantindo a usabilidade sem dependências externas de drivers.
Configuração e Saída: Expôs confiabilidade de QoS, nomes de frames e diretórios de saída via YAML. O sistema agora publica covariância de odometria em um frame compatível com o REP-105 e escreve trajetórias no formato TUM para avaliação com evo.

Resultos e Validação
A portabilidade corrigida foi validada em hardware utilizando sensores Ouster OS0 Rev7, Ouster OS1 Rev7 e Livox MID-360. A validação qualitativa envolveu a sobreposição da trajetória estimada do port para ROS 2 contra a implementação original em ROS 1 para a mesma sequência. Os resultados foram "qualitativamente idênticos", confirmando que a migração e as correções numéricas preservaram o comportamento do estimador. O sistema eliminou com sucesso a divergência NaN e os abortos SO3::exp(nan) observados na portabilidade inicial.

Significância e Alegações
Os autores declaram explicitamente que este relatório serve como uma referência citável para a própria portabilidade, não uma alegação sobre o desempenho ou novidade do algoritmo subjacente, que pertence aos autores originais do iG-LIO [1]. A significância deste trabalho reside em duas lições generalizáveis para a engenharia de software robótico:

QoS como um Contrato de Correção: No ROS 2, as configurações de Qualidade de Serviço (QoS) não são meramente preocupações de vazão (throughput), mas contratos críticos de correção para estimadores fortemente acoplados; as garantias implícitas de transporte do ROS 1 já não se aplicam.
Sensibilidade da Toolchain: Operações de redução paralela sobre matrizes Eigen de tamanho fixo devem ser tratadas com inicialização explícita ao usar distribuições modernas do oneTBB, pois os construtores padrão não garantem a inicialização com zero.

O trabalho demonstra que uma portabilidade mecanicamente correta é insuficiente para a estabilidade numérica; atenção específica ao ambiente da toolchain alvo e aos contratos de comunicação é necessária para evitar falhas silenciosas.

O Primeiro Culpado: A Confusão do "Melhor Esforço"

O Segundo Culpado: A Armadilha da "Caixa Vazia"

Novos Gadgets e Melhores Mapas

Funcionou?

Mais como este