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

Resumen Técnico: Un puerto de iG-LIO a ROS 2 numéricamente robusto

Planteamiento del problema
El sistema original iG-LIO (Incremental GICP LiDAR-Inertial Odometry), un estimador fuertemente acoplado que fusiona restricciones de GICP generalizado y de punto a plano, fue implementado para ROS 1, el cual ya ha alcanzado el fin de su vida útil. Aunque una migración mecánica del código base a ROS 2 (convirtiendo de catkin a ament_cmake, de roscpp a rclcpp, etc.) pareció exitosa, el puerto falló silenciosamente en la práctica. A pesar de que la matemática de la estimación permaneció sin cambios, el sistema divergió con valores internos NaN y abortó. Las causas raíz se identificaron no como fallos algorítmicos, sino como fallos inducidos por el entorno, específicos de la moderna cadena de herramientas y las configuraciones por defecto de ROS 2.

Metodología y Diagnóstico
Los autores realizaron un puerto "mecánicamente fiel" de iG-LIO a ROS 2 Jazzy, preservando la lógica central de estimación (mapa de vóxeles, fusión de GICP + punto a plano, filtro de error de estado iterado). La investigación se centró en aislar por qué el sistema portado fallaba mientras que la versión original de ROS 1 funcionaba correctamente. Se identificaron dos causas raíz distintas a nivel de entorno:

1. Desajuste de QoS (Pérdida de datos de IMU): El puerto inicial se suscribía a los datos de la IMU utilizando rclcpp::SensorDataQoS() (Best Effort, Keep Last 5), mientras que las fuentes de datos (drivers o bags) publicaban con un QoS Reliable. En ROS 2, este emparejamiento degrada silenciosamente a Best Effort, lo que provoca que las muestras de la IMU se pierdan y se reordenen bajo carga. Esto corrompió la señal requerida para la propagación del filtro iterado.
2. Reducción Paralela No Inicializada (oneTBB + Eigen): El ensamblaje de restricciones utilizaba tbb::parallel_reduce con un tipo de valor de reducción Eigen::Matrix de tamaño fijo. La versión de oneTBB incluida en las distribuciones modernas de ROS 2 construye por defecto acumuladores divididos. Dado que una Eigen::Matrix de tamaño fijo no inicializa su almacenamiento con ceros al construirse por defecto, se sumó memoria no inicializada a la matriz Hessiana ($H$) y al vector ($b$), resultando en valores NaN. Este problema era invisible en la cadena de herramientas antigua utilizada para el código original, pero surgió inmediatamente en la nueva cadena de herramientas de ROS 2.

Contribuciones Clave
El artículo detalla el diagnóstico y la corrección de estos fallos, junto con varias mejoras de ingeniería en el soporte de sensores y la usabilidad:

• Corrección de QoS: La suscripción a la IMU se actualizó para usar un QoS RELIABLE con una cola profunda (KeepLast(2000)), mientras que las colas de LiDAR se configuraron en KeepLast(10). Se añadió una guarda en tiempo de ejecución para rechazar pasos de tiempo de predicción no físicos ($\Delta t \le 0$ o $\Delta t > 0.5$ s).
• Corrección de Robustez Numérica: El acumulador de reducción no inicializado se resolvió envolviendo la Eigen::Matrix en una pequeña estructura. Los miembros de esta estructura se inicializan explícitamente con ceros en la clase (mediante Eigen::Matrix::Zero()) y se unen mediante operator+. Esto preserva la estructura de un único tbb::parallel_reduce ascendente mientras asegura que cada tarea dividida de TBB comience con un valor de identidad en cero, eliminando la falla de lectura no inicializada sin sacrificar el paralelismo.
• Actualizaciones de Procesamiento de Sensores:
  • Ouster: Se actualizó la estructura/el analizador de puntos para coincidir con el diseño actual de campos de PointCloud2 para los sensores Rev 7, restaurando las capacidades de des-skewing de movimiento que se habían roto en el código original.
  • Velodyne: Se añadió soporte para el Velodyne Velarray M1600.
  • Livox: Se implementaron dos rutas de ingestión: una ruta controlada por compilación para el CustomMsg específico del driver (que falla de forma controlada si el driver falta) y una ruta libre de drivers para sensores que publican sensor_msgs/PointCloud2 estándar (por ejemplo, Mid-360), asegurando la usabilidad sin dependencias de drivers externos.
• Configuración y Salida: Se expusieron la fiabilidad de QoS, los nombres de los frames y los directorios de salida mediante YAML. El sistema ahora publica la covarianza de la odometría en un frame conforme a REP-105 y escribe las trayectorias en formato TUM para su evaluación con evo.

Resultos y Validación
El puerto corregido fue validado en hardware utilizando sensores Ouster OS0 Rev7, Ouster OS1 Rev7 y Livox MID-360. La validación cualitativa consistió en superponer la trayectoria estimada del puerto de ROS 2 contra la implementación original de ROS 1 para la misma secuencia. Los resultados fueron "cualitativamente idénticos", confirmando que la migración y las correcciones numéricas preservaron el comportamiento del estimador. El sistema eliminó con éxito la divergencia NaN y los abortos SO3::exp(nan) observados en el puerto inicial.

Significancia y Reivindicaciones
Los autores declaran explícitamente que este informe sirve como una referencia citable para el puerto en sí mismo, no como una reivindicación sobre el rendimiento o la novedad del algoritmo subyacente, que pertenece a los autores originales de iG-LIO [1]. La significancia de este trabajo reside en dos lecciones generalizables para la ingeniería de software robótico:

1. QoS como Contrato de Corrección: En ROS 2, las configuraciones de Calidad de Servicio (QoS) no son meramente cuestiones de rendimiento, sino contratos críticos de corrección para estimadores fuertemente acoplados; las garantías implícitas de transporte de ROS 1 ya no aplican.
2. Sensibilidad de la Cadena de Herramientas: Las operaciones de reducción paralela sobre matrices Eigen de tamaño fijo deben manejarse con una inicialización explícita cuando se utilizan distribuciones modernas de oneTBB, ya que los constructores por defecto ya no garantizan la inicialización a cero.

El trabajo demuestra que un puerto mecánicamente correcto es insuficiente para la estabilidad numérica; se requiere una atención específica al entorno de la cadena de herramientas y a los contratos de comunicación para evitar fallos silenciosos.