FAST-LIVO2: LiDAR-IMU-Visual 완전 통합 — Sparse-Direct + Sequential ESIKF로 10~100배 정확도 향상

TL;DR

FAST-LIVO2는 HKU MARS Lab이 개발한 타이트 커플링 LiDAR-IMU-Visual Odometry 시스템입니다. FAST-LIO2의 LiDAR-IMU 파이프라인에 Sparse-Direct Visual Odometry를 완전 통합한 완성형으로, 세 센서가 하나의 voxel map과 하나의 ESIKF에서 순차적으로 업데이트됩니다. 핵심 혁신: (1) LiDAR 평면을 prior로 활용한 affine photometric residual, (2) 노출 시간 자동 추정으로 조명 변화 대응, (3) On-demand voxel raycasting으로 LiDAR 블라인드존 처리. 결과: Hilti'22 벤치마크에서 R3LIVE 대비 10~100배 낮은 RMSE, ARM 임베디드에서도 실시간 동작. IEEE T-RO 게재 예정.

Background: LiDAR만으로는 부족한 이유

FAST-LIO2는 뛰어났습니다. 하지만 현장에서 쓰다 보면 한계가 드러납니다.

LiDAR의 약점:

• Geometry degeneracy: 긴 복도, 열린 광장 — 법선벡터가 한 방향으로 쏠리는 환경에서 roll/pitch 추정 불안정
• Low-texture + featureless 동시 발생 시 최악: 지하 주차장, 마인, 안개 낀 실외
• 반사 이상: 유리, 금속, 물 표면에서 점 구름 왜곡

카메라만으로도 부족:

• 조명 변화, 모션 블러, 고속 이동에서 추적 실패
• 스케일 모호성 (단안 카메라 기준)
• 깊이 정보 없음 → 3D 맵 구성에 비용 큼

답은 셋을 묶는 것. LiDAR가 기하학 구조를 잡고, 카메라가 텍스처로 정밀 정렬하고, IMU가 고주파 모션을 채웁니다.

기존 융합 시스템들의 문제:

• R3LIVE: 두 단계(LiDAR → Visual)로 분리, 중간 오차 누적
• LVI-SAM: Loosely-coupled, factor graph 기반 — 무겁고 실시간성 제한
• FAST-LIVO (v1): FAST-LIO2 + 기본 비주얼 — dimension mismatch 문제 미해결

FAST-LIVO2는 이 모든 걸 하나의 프레임워크로 통합합니다.

Core Architecture

Fig.1. FAST-LIVO2 전체 파이프라인 — IMU propagation → LiDAR ESIKF update → Voxel map → Visual ESIKF update, 세 센서가 하나의 상태 추정기에서 순차적으로 처리

Sequential ESIKF: 하나의 State, 두 번의 Update

State vector (19차원, SO(3)×ℝ¹⁶ manifold):

x = [R_IG, p_IG, v_IG, b_g, b_a, g_G, τ]ᵀ

FAST-LIVO v1과의 결정적 차이: τ를 state에 포함시켜 조명 변화를 필터로 직접 추정합니다. 별도 조명 보정 없이 어두운 환경, 급격한 조명 전환에서도 안정적 동작.

업데이트 순서:

1. IMU 전파 (forward propagation)
2. LiDAR scan → point-to-plane residual로 ESIKF update
3. Voxel map 구성/업데이트
4. Visual patch → photometric residual로 ESIKF update

LiDAR update가 먼저 geometry를 잡고, visual update가 그 위에서 정밀 정렬. 두 업데이트가 같은 state를 공유하기 때문에 진정한 tight coupling입니다.

Sparse-Direct Visual Residual

Fig.2. Affine warping — LiDAR 평면 normal을 prior로 활용해 이미지 패치를 현재 프레임으로 변환

Feature 추출(ORB, FAST) 없이 8×8 픽셀 패치의 photometric error를 직접 최소화합니다:

residual = τ_k · I_k(u_i + Δu) - τ_r · I_r(u_i' + A_ir · Δu)

• I_k: 현재 이미지
• I_r: 참조 이미지 (reference patch)
• A_ir: Affine warping matrix (평면 normal로 계산)
• τ_k, τ_r: 현재/참조 역 노출 시간

핵심: Affine matrix A_ir를 계산할 때 LiDAR가 이미 추정한 평면 normal을 사용합니다. 기존 방법들이 constant-depth(깊이 일정)를 가정하는 것과 달리, 실제 기하학적 정보로 affine 변환을 계산 → 정확한 패치 정렬.

3단계 코어스-투-파인 피라미드로 수렴 속도와 정확도 동시 확보.

Voxel Map: Hash Table + Octree

FAST-LIO2의 ikd-Tree 대신 해시 테이블 + Octree 2단계 구조를 채택:

Hash table (0.5m voxels)
    └── Octree (3 layers, variable-size sub-voxels)
            ├── Plane normal + center + uncertainty
            ├── Raw LiDAR points
            └── Visual patches (selected LiDAR points)

• Ring-buffer 방식으로 로컬 맵 크기 고정 → 메모리 바운드 보장
• LiDAR와 Visual이 동일한 map structure를 공유 → 중복 자료구조 없음
• ikd-Tree 대비 혼합 쿼리(기하학 + 비주얼)에 효율적

Fig.3. Ring-buffer 방식 로컬 맵 슬라이딩 — 플랫폼이 이동할 때 오래된 voxel은 자동 해제, 새 voxel 삽입

On-Demand Voxel Raycasting

Fig.4. On-demand raycasting — LiDAR가 닿지 않는 영역(블라인드존)에서 카메라 ray로 visual map point 탐색

LiDAR의 블라인드존(최소 측정 거리 내부, FOV 밖)에서 카메라가 보는 영역을 어떻게 처리할까?

해결책: 이미지를 30×30 픽셀 그리드로 분할 → 각 셀에서 ray를 쏘아 비어있는 voxel을 탐색 → 맵에 있는 가장 가까운 plane으로 투영.

이 방식으로 LiDAR 포인트 없이도 visual constraint 획득 가능. 근거리 물체, 드론 아래쪽 영역 등 LiDAR가 닿지 않는 곳에서도 카메라가 기여합니다.

동적 Reference Patch 선택

기존 방법: 가장 최근 프레임을 reference로 사용 → 시점 변화에 취약.

FAST-LIVO2: 다음 조건으로 최적 reference 동적 선택:

1. Parallax: 현재 시점과의 각도 차이가 적당한 프레임
2. Texture richness: 패치 내 gradient 강도
3. Photometric similarity: 노출 보정 후 유사도

→ 시점이 크게 바뀌어도 항상 가장 정보량 많은 reference를 씁니다.

Evaluation

Fig.5. 평가 플랫폼 — 핸드헬드, UAV(ARM RB5 온보드), 지상 차량

테스트 데이터셋

정확도 비교 (Hilti'22, RMSE)

복도 시퀀스에서 LVI-SAM 대비 94배 낮은 오차. Stairs에서 R3LIVE 대비 49배 낮은 오차.

임베디드 동작

ARM Qualcomm Kryo585 (RB5, 8GB RAM)에서 UAV 온보드로 실시간 동작 검증. LiDAR + IMU + Camera 전체 파이프라인이 임베디드에서 실시간.

Key Experiments

LiDAR Degeneracy 환경에서의 강건성

긴 복도 시퀀스에서 FAST-LIO2(LiDAR only)는 drift 누적 → 카메라가 없으면 geometry degeneracy로 발산. FAST-LIVO2는 동일 시퀀스에서 0.067m RMSE 유지. 카메라가 LiDAR의 degeneracy를 상호 보완하는 것이 실제 데이터로 검증.

조명 변화 대응 (τ 추정)

야외→실내→야외 전환이 있는 시퀀스에서:

• τ 없이: 조명 전환 시 패치 매칭 실패, 포즈 점프 발생
• τ 있이: 부드러운 전환, 안정적 추적

Ablation: 각 컴포넌트의 기여

모든 컴포넌트가 유의미하게 기여함을 확인.

같은 파라미터, 모든 시퀀스

66.9분, 20개 시퀀스 (마인, 복도, 지하, 실외)를 동일 파라미터로 처리. 환경별 재튜닝 없이 범용 동작 달성.

Limitations — 현장 엔지니어 관점

논문이 인정하는 한계:

1. 사전 캘리브레이션 필수: LiDAR-Camera extrinsic, 타임스탬프 오프셋을 미리 캘리브레이션해야 함. 온라인 캘리브레이션 미지원
2. Rolling shutter 미지원: 글로벌 셔터 카메라 전제. 롤링 셔터 왜곡은 τ가 일부 보상하지만 완전하지 않음
3. 평면 의존성: 모든 visual map point가 LiDAR 평면 위에 있다고 가정. 점, 선 특징이 많은 환경(철골 구조물 등)에서 map point 부족 가능
4. 고속 이동 한계: 빠른 모션에서 패치 blur, 매칭 실패 가능성

현장 관점 추가:

• 센서 타임스탬프 동기화가 핵심: LiDAR-IMU-Camera 하드웨어 동기화가 안 되어 있으면 논문 결과 재현 어려움. 하드웨어 설계 단계부터 트리거 동기화 고려 필수
• 카메라 해상도 선택: 고해상도일수록 patch 매칭 정확도 높아지지만 처리 비용 급증. MARS-LVIG의 2448×2048는 임베디드에서 실시간 어려울 수 있음
• 초기화 민감도: 정지 상태 초기화(IMU bias) + 충분한 LiDAR 평면 확보 필요. 움직이면서 cold start는 여전히 취약
• Loop closure 없음: 순수 odometry. 장거리 탐사에서 drift 누적. SC-FAST-LIO2처럼 별도 루프 클로저 추가가 현장 배포의 현실적 요구

The Lineage — FAST-LIVO2의 위치

FAST-LIVO2가 증명한 것: Sparse-direct visual + LiDAR plane prior + sequential ESIKF = 두 센서가 서로의 약점을 커버하는 진정한 tight fusion. 이 설계 원칙이 이후 멀티센서 odometry의 기준이 됩니다.

Summary — Key Takeaways

1. 진정한 Tight Coupling — 하나의 state, 하나의 ESIKF에서 LiDAR와 Visual이 순차 업데이트. Loosely-coupled 방식의 중간 오차 누적 없음
2. LiDAR가 카메라를 돕고, 카메라가 LiDAR를 돕는다 — LiDAR 평면 prior로 accurate affine warping, Visual이 LiDAR degeneracy 구간을 커버. 진정한 상호 보완
3. 노출 시간을 state로 추정 — 조명 변화를 외부 조건이 아닌 필터의 내부 변수로 처리. 야간, 터널, 조명 전환 환경에서 강건함
4. 같은 파라미터로 20개 시퀀스 처리 — 마인, 복도, 실외, 지하를 재튜닝 없이 통과. 범용성 실증
5. Loop closure는 여전히 없다 — 장거리 SLAM 응용에는 별도 루프 클로저 레이어 필요. 현장 배포 시 odometry임을 인지하고 시스템 설계해야

📄 Paper: arXiv:2408.14035 🐙 GitHub: hku-mars/FAST-LIVO2

다음 포스트: OpenVLA — 오픈소스 Vision-Language-Action 모델의 또 다른 축