FAST-LIO2: Feature 없이 100Hz — LiDAR Odometry의 새 기준
Direct Point Registration + ikd-Tree로 LIO-SAM 대비 8배 빠르고 더 정확한 LiDAR-IMU Odometry
TL;DR
FAST-LIO2는 HKU MARS Lab에서 개발한 고속·고정밀 LiDAR-IMU Odometry 시스템입니다. 두 가지 핵심 혁신: (1) Feature 추출 없이 Raw point cloud를 직접 map에 등록, (2) Incremental k-d Tree(ikd-Tree)로 map을 O(log n) 시간에 실시간 업데이트. 결과: Intel i7에서 100Hz, ARM 임베디드 보드에서도 실시간 동작. 19개 시퀀스 벤치마크에서 LIO-SAM, LOAM, LILI-OM 등 동시대 최강 방법들을 정확도·속도 모두에서 앞섰으며, IEEE Transactions on Robotics 2022에 게재됐습니다.
Background: LiDAR Odometry의 오랜 트레이드오프
LiDAR odometry의 역사는 사실 트레이드오프와의 싸움이었습니다.
LOAM 계열 (2014~): edge + planar feature를 추출해서 scan-to-map 매칭. 정확하지만 feature extraction 비용이 크고, feature-less 환경(터널, 복도, 열린 광장)에서 취약.
LIO-SAM (2020): IMU pre-integration + factor graph + LiDAR feature matching. 루프 클로저까지 지원하지만 무겁고, LiDAR 종류마다 파라미터 재튜닝 필요.
FAST-LIO (2021, 전작): 여전히 feature 기반이지만 iterated Kalman filter로 연산량을 줄임. FAST-LIO2의 직접적인 선행 연구.
이 흐름에서 자연스럽게 나오는 질문:
"Feature를 아예 뽑지 않고, map도 실시간으로 업데이트하는 구조를 만들 수 없을까?"
FAST-LIO2가 그 답입니다.
Core Architecture: 두 가지 혁신
Fig.1. FAST-LIO2 시스템 전체 파이프라인 — LiDAR scan + IMU 입력, iterated Kalman filter로 상태 추정, ikd-Tree map에 직접 등록
혁신 1: Direct Point Registration (Feature 추출 제거)
LOAM/LIO-SAM은 raw scan에서 edge·planar feature를 뽑아서 매칭합니다. FAST-LIO2는 이 단계를 완전히 제거합니다.
대신 raw point 하나하나를 map의 k nearest neighbor와 매칭해서 planar residual을 계산합니다:
residual = uⱼᵀ · (T̂ᴳᴵ · T̂ᴵᴸ · pʲₗ - qʲ)
uⱼ: map의 local plane normal vectorpʲₗ: LiDAR 좌표계의 pointqʲ: map의 nearest neighbor centroid
장점:
- 환경별 feature extraction 파라미터 튜닝 불필요
- 실내, 실외, 복도, outdoor 모두 동일한 파이프라인
- LiDAR 종류를 바꿔도 코드 수정 불필요 (Livox solid-state ↔ Velodyne spinning 그대로 사용 가능)
Fig.2. Measurement model — map plane normal과 LiDAR point 사이의 residual 계산 구조
혁신 2: ikd-Tree (Incremental k-d Tree)
Map을 어떻게 관리할까? 기존 방법들의 문제:
- Static k-d tree: 점이 계속 추가되면 주기적으로 full rebuild 필요 → 레이턴시 스파이크
- Voxel grid: 해상도 고정, 빠르지만 정밀도 제한
ikd-Tree는 이 두 문제를 동시에 해결합니다:
| 연산 | 복잡도 |
| Point insertion (with downsampling) | O(log n) |
| Box-wise deletion | O(H(n)) |
| k-NN search | O(log n) |
| Re-balancing | O(n log n), 별도 스레드에서 병렬 실행 |
핵심 메커니즘:
- α-balanced: 트리 높이 불균형 시 자동 rebalance
- α-deleted: 삭제된 노드 비율 초과 시 garbage collection
- Lazy deletion: 실제 삭제 없이 flag만 설정, 나중에 정리
- Parallel re-building: 메인 스레드를 블로킹하지 않고 별도 스레드에서 rebuild
Fig.3. Map region management — LiDAR FOV 밖의 오래된 포인트는 box-wise delete로 제거, 새 포인트는 O(log n)으로 삽입
Fig.4. ikd-Tree subtree re-building — 불균형 서브트리를 병렬 스레드에서 재구성, 메인 스레드 차단 없음
Iterated Extended Kalman Filter (iEKF)
State vector (24차원, manifold SO(3)×ℝ¹⁵×SO(3)×ℝ³):
x = [R ∈ SO(3), p ∈ ℝ³, v ∈ ℝ³, bω ∈ ℝ³, ba ∈ ℝ³, g ∈ ℝ³, R_IL ∈ SO(3), t_IL ∈ ℝ³]
| 변수 | 의미 |
| R, p | 회전, 위치 |
| v | 속도 |
| bω, ba | gyro/accel bias |
| g | gravity vector (온라인 추정) |
| R_IL, t_IL | LiDAR-IMU extrinsic 온라인 캘리브레이션 |
State propagation은 manifold 위에서:
xᵢ₊₁ = xᵢ ⊞ (Δt · f(xᵢ, uᵢ, wᵢ))
Motion distortion 보정: 각 LiDAR point의 정확한 캡처 타임스탬프에 맞게 IMU back-propagation으로 포즈 보간 → 고속 회전에서도 point cloud 왜곡 없음.
LiDAR-IMU Extrinsic 온라인 캘리브레이션: R_IL, t_IL을 state vector에 포함시켜 odometry와 동시 추정. 별도 오프라인 캘리브레이션 없이 초기값만 대략 주면 실주행 중 수렴.
Evaluation: 19 Sequences, 5 Datasets
| 데이터셋 | LiDAR 타입 | 채널 수 | 환경 |
| LILI | Livox Horizon (solid-state) | 비균일 | 실내/실외 |
| LIO-SAM | VLP-16 (spinning) | 16ch | 실외 large-scale |
| UTBM/ULHK | HDL-32E (spinning) | 32ch | 도시 주행 |
| NCLT | HDL-32E (spinning) | 32ch | 장기간 UGV |
정확도: 19개 시퀀스 중 대다수에서 LOAM, LIO-SAM, LINS, LILI-OM 대비 최저 RMSE.
속도: LIO-SAM 대비 8배 이상 빠름. Intel i7-8550U에서 100Hz 유지.
임베디드 동작: ARM Cortex-A73 (Khadas VIM3, 4GB RAM)에서도 실시간 동작 확인. 드론 온보드 적용 현실화.
Key Experiments
극한 조건 테스트
고속 회전 내성: 1000 deg/s까지 안정적 pose 추정. LOAM 계열은 이 속도에서 발산. IMU back-propagation이 핵심 역할.
UAV 탑재 검증: Livox Avia + DJI Manifold 2-C 조합으로 쿼드로터에서 실시간 odometry. 모터 진동 + 급기동 환경에서도 안정.
Sensor Agnosticism: Livox Horizon(비균일 solid-state 스캔)과 VLP-16(균일 spinning 스캔) 모두 동일 코드, 동일 파라미터로 처리. Feature 기반 방법은 LiDAR 종류별로 파라미터 재튜닝이 불가피한데, FAST-LIO2는 이 비용이 없음.
ikd-Tree vs. 기존 자료구조 비교
논문에서 ikd-Tree를 octree, R*-tree, nanoflann k-d tree와 비교:
- 삽입/삭제 속도: ikd-Tree > 나머지
- 검색 속도: ikd-Tree ≈ nanoflann (최적화 static k-d tree)
- 동적 업데이트 지원: ikd-Tree만 가능
Limitations — 현장 엔지니어 관점
논문이 인정하는 한계:
- 루프 클로저 없음: 순수 odometry 시스템. 장거리 이동 시 drift 누적, 출발점으로 돌아와도 map이 닫히지 않음
- 로컬 맵 크기 제한: 일정 범위를 벗어난 포인트는 삭제. Large-scale persistent mapping에 부적합
- 정지 초기화 필요: IMU bias 추정을 위해 시작 시 정지 상태 필요. 이동 중 cold start 불가
현장 관점 추가:
- Degenerate geometry: 긴 복도, 열린 광장처럼 법선벡터가 한 방향으로 집중된 환경에서 추정 불안정 — FAST-LIO2도 geometry degeneracy에는 취약. 실제 배포 시 별도 degeneracy 감지 로직 권장
- Dense map 없음: ikd-Tree는 odometry용 sparse point map. 3D dense reconstruction·voxel occupancy map이 필요하면 R3LIVE, FAST-LIVO2 같은 확장 시스템 필요
- Solid-state 스캔 패턴 엣지케이스: Livox의 비균일 패턴에서 한 프레임 내 nearest neighbor 탐색 결과가 불균형해지는 케이스 존재
- 파라미터 완전 자유는 아님: feature 추출은 없지만 ikd-Tree의 α-balance/α-delete 파라미터, point downsampling resolution은 환경마다 최적값이 다름
The Lineage — FAST-LIO2가 열어준 것
| 시스템 | FAST-LIO2에서 가져간 것 |
| FAST-LIVO (2022) | ikd-Tree backbone + RGB 카메라 융합 |
| R3LIVE (2022) | FAST-LIO2 백엔드 + dense color map reconstruction |
| FAST-LIVO2 (2024) | Direct LiDAR-IMU-Visual 융합, 더 robust한 tight coupling |
| Point-LIO (2023) | Point-by-point 처리로 더 높은 주파수 대응, IMU 모델 개선 |
| Faster-LIO (2022) | Incremental voxel map으로 ikd-Tree 대체 시도 |
| HKU-MARS 전 계열 | LiDAR-visual-inertial fusion 연구의 공통 backbone |
FAST-LIO2가 증명한 것: Feature 추출 없는 direct registration + incremental map = 빠름 + 정확함 + 범용성. 이 조합이 이후 모든 HKU-MARS LIO 계열의 baseline이 됩니다.
Summary — Key Takeaways
- Feature 추출을 버려도 더 정확하다 — Direct raw point registration이 환경 적응성과 정확도 모두 향상. "Feature 기반이 더 안정적"이라는 통념을 뒤집음
- ikd-Tree가 100Hz의 비결 — O(log n) 실시간 map 업데이트로 latency spike 없는 고주파 odometry 달성. Parallel rebalancing이 핵심
- LiDAR-IMU extrinsic을 state로 포함 — 별도 캘리브레이션 없이 실주행 중 자동 수렴. 운용 편의성 대폭 향상
- ARM 임베디드에서 실시간 동작 — Khadas VIM3 (ARM, 4GB) 검증. 드론·모바일 로봇 온보드 배포 현실화
- 루프 클로저는 없다 — 장거리 SLAM에는 SC-FAST-LIO2 같은 확장 필요. "odometry" 임을 명확히 인지하고 사용해야
📄 Paper: arXiv:2107.06829 🐙 GitHub: hku-mars/FAST_LIO
다음 포스트: Octo — 오픈소스 범용 로봇 정책, VLA 민주화의 시작