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

기술 요약: iG-LIO의 수치적 견고성을 갖춘 ROS 2 이식

문제 정의
원래의 iG-LIO(Incremental GICP LiDAR-Inertial Odometry) 시스템은 일반화된 ICP(generalized-ICP)와 점-대-평면(point-to-plane) 제약을 융합하는 긴밀하게 결합된(tightly-coupled) 추정기로, ROS 1용으로 구현되었으나 현재는 지원이 종료(end-of-life)된 상태이다. 코드베이스를 catkin에서 ament_cmake로, roscpp에서 rclcpp로 변환하는 식의 기계적인 ROS 2 이식은 성공적으로 보였으나, 실제 적용 시에는 실패하였다. 수학적 추정 로직은 변경되지 않았음에도 불구하고, 시스템은 내부 값이 NaN으로 발산하며 중단되었다. 근본 원인은 알고리즘의 결함이 아니라, 현대적인 ROS 2 툴체인 및 기본 설정에 특이적인 환경 유도형 실패로 식별되었다.

방법론 및 진단
저자들은 핵심 추정 로직(복셀 맵, GICP + 점-대-평면 융합, 반복 에러 상태 칼만 필터)을 보존하면서 iG-LIO를 ROS 2 Jazzy로 "기계적으로 충실하게" 이식하였다. 조사는 왜 이식된 시스템이 실패한 반면 기존 ROS 1 버전은 정상 작동했는지 격리하는 데 집중하였다. 두 가지 뚜렷한 환경 수준의 근본 원인이 식별되었다:

1. QoS 불일치 (IMU 데이터 손실): 초기 이식 버전은 rclcpp::SensorDataQoS() (Best Effort, Keep Last 5)를 사용하여 IMU 데이터를 구독했으나, 데이터 소스(드라이버 또는 bag 파일)는 Reliable QoS로 발행하고 있었다. ROS 2에서 이 조합은 암묵적으로 Best Effort로 저하되어, 부하가 걸릴 때 IMU 샘플이 누락되거나 순서가 뒤바뀌는 현상을 초래한다. 이는 반복 필터의 전파(propagation)에 필요한 신호를 손상시켰다.
2. 초기화되지 않은 병렬 리덕션 (oneTBB + Eigen): 제약 조건 조립 과정에서 고정 크기 Eigen::Matrix를 리덕션 값 타입으로 사용하는 tbb::parallel_reduce를 활용하였다. 현대적인 ROS 2 배포판에 포함된 oneTBB 버전은 분할된 어큐뮬레이터(accumulator)를 기본 생성 시 초기화하지 않는다. 고정 크기 Eigen::Matrix는 기본 생성 시 저장 공간을 0으로 초기화하지 않으므로, 초기화되지 않은 메모리가 헤시안 행렬($H$)과 벡터($b$)에 합산되어 NaN 값을 발생시켰다. 이 문제는 기존 코드의 구형 툴체인에서는 나타나지 않았으나, 새로운 ROS 2 툴체인에서는 즉각적으로 드러났다.

주요 기여
본 논문은 이러한 실패에 대한 진단과 교정, 그리고 센서 지원 및 사용성 향상을 위한 몇 가지 엔지니어링 개선 사항을 상세히 기술한다:

• QoS 교정: IMU 구독을 깊은 큐(KeepLast(2000))를 가진 RELIABLE QoS를 사용하도록 업데이트하였으며, LiDAR 큐는 KeepLast(10)으로 설정하였다. 또한 비물리적인 예측 시간 단계($\Delta t \le 0$ 또는 $\Delta t > 0.5$ s)를 거부하는 런타임 가드를 추가하였다.
• 수치적 견고성 해결: 초기화되지 않은 리덕션 어큐뮬레이터를 작은 구조체로 감싸는 방식으로 해결하였다. 이 구조체의 멤버들은 클래스 내부에서 명시적으로 0으로 초기화되며(Eigen::Matrix::Zero()를 통해), operator+를 통해 결합된다. 이를 통해 상위의 단일 융합 tbb::parallel_reduce 구조를 유지하면서도, 모든 분할된 TBB 태스크가 0으로 초기화된 항등값(identity value)에서 시작하도록 보장하여, 병렬성을 희생하지 않고 초기화되지 않은 읽기 오류를 제거하였다.
• 센서 파싱 업데이트:
  • Ouster: Rev 7 센서의 현재 PointCloud2 필드 레이아웃에 맞게 포인트 구조체 및 파서를 업데이트하여, 기존 코드에서 깨졌던 모션 디스큐잉(motion de-skewing) 기능을 복구하였다.
  • Velodyne: Velodyne Velarray M1600 지원을 추가하였다.
  • Livox: 두 가지 인입 경로를 구현하였다. 드라이버 부재 시 우아하게 처리되는 컴파일 타임 게이트 방식의 드라이버 전용 CustomMsg 경로와, 표준 sensor_msgs/PointCloud2(예: Mid-360)를 발행하는 드라이버 독립적 경로를 구현하여 외부 드라이버 의존성 없이 사용성을 확보하였다.
• 설정 및 출력: QoS 신뢰성, 프레임 이름, 출력 디렉토리를 YAML을 통해 노출하였다. 시스템은 이제 REP-105를 준수하는 프레임에 오도메트리 공분산을 발행하며, evo를 통한 평가를 위해 TUM 형식으로 궤적을 기록한다.

결과 및 검증
교정된 이식 버전은 Ouster OS0 Rev7, Ouster OS1 Rev7, Livox MID-360 센서를 사용하여 하드웨어에서 검증되었다. 정성적 검증은 동일한 시퀀스에 대해 ROS 2 이식본의 추정된 궤적을 기존 ROS 1 구현과 중첩하는 방식으로 진행되었다. 결과는 "질적으로 동일"하였으며, 이는 이식 및 수치적 수정이 추정기의 동작을 보존했음을 확인시켜 주었다. 시스템은 초기 이식 버전에서 관찰되었던 NaN 발산 및 SO3::exp(nan) 중단 현상을 성공적으로 제거하였다.

의의 및 주장
저자들은 본 보고서가 알고리즘 자체의 성능이나 독창성에 대한 주장이 아니라, 이식 작업 자체에 대한 인용 가능한 참조임을 명시한다(알고리즘의 독창성은 원본 iG-LIO 저자들에게 있음 [1]). 본 연구의 의의는 로보틱스 소프트웨어 엔지니어링에 대한 두 가지 일반화 가능한 교훈에 있다:

1. QoS는 정확성의 계약이다: ROS 2에서 QoS 설정은 단순한 처리량 문제가 아니라, 긴밀하게 결합된 추정기들을 위한 핵심적인 정확성 계약이다. ROS 1의 암묵적인 전송 보장은 더 이상 적용되지 않는다.
2. 툴체인 민감도: 현대적인 oneTBB 배포판을 사용할 때, 고정 크기 Eigen 행렬에 대한 병렬 리덕션 연산은 명시적인 초기화가 반드시 필요하다. 기본 생성자가 0 초기화를 보장하지 않기 때문이다.

본 연구는 기계적으로 올바른 이식만으로는 부족하며, 수치적 안정성을 확보하기 위해서는 대상 환경의 툴체인과 통신 계약에 대한 세심한 주의가 필요함을 보여준다.