FAST-LIVO2

Zheng 2024 · 논문

한 줄 요약 — FAST-LIVO2는 이종의 LiDAR 및 이미지 측정값 사이의 차원 불일치를 해결하는 순차적 업데이트를 갖춘 오차 상태 반복 칼만 필터를 통해 IMU, LiDAR, 카메라를 융합하며, 온보드 컴퓨팅 자원에서의 직접 LVI 오도메트리 기준을 세운다.

문제

LiDAR 스캔은 업데이트마다 수천 개의 저차원 기하학적 잔차를 제공하는 반면, 카메라 프레임은 완전히 다른 구조의 광도계 잔차를 제공한다—이 둘을 하나의 칼만 스텝에서 공동으로 업데이트하는 것은 어색하며, FAST-LIVO의 단순화(어파인 워핑을 위한 패치당 상수 깊이, 노출 처리 없음, LiDAR 포인트가 부족할 때의 폴백 없음)는 정확도와 강인성을 미완성으로 남겨두었다. FAST-LIVO2는 완전히 온보드에서의 실시간 로봇 활용에 충분히 엄밀하고 효율적이도록 융합을 재설계한다.

방법 및 아키텍처

19차원 매니폴드 $\mathcal{M} = SO(3)\times\mathbb{R}^{16}$ 위의 상태: $\mathbf{x} = [{^G}\mathbf{R}_I^T\ {^G}\mathbf{p}_I^T\ {^G}\mathbf{v}_I^T\ \mathbf{b}_g^T\ \mathbf{b}_a^T\ {^G}\mathbf{g}^T\ \tau]^T$이며, 여기서 $\tau$는 역카메라 노출 시간(첫 프레임 기준 상대값으로, 관측 가능성을 위해 $\tau_0 = 1$로 고정)이고 랜덤 워크로 모델링된다. 스캔 재조합은 LiDAR 스트림을 카메라 샘플링 시점에서 다시 스캔으로 분할하여, 두 센서가 동일한 10Hz로 업데이트되도록 한다.

• 순차적 ESIKF 업데이트: 사후 확률을 다음과 같이 인수분해하면

$$p(\mathbf{x} \mid \mathbf{y}_l, \mathbf{y}_c) \propto p(\mathbf{y}_c \mid \mathbf{x})\, \underbrace{p(\mathbf{y}_l \mid \mathbf{x})\, p(\mathbf{x})}_{\propto\, p(\mathbf{x} \mid \mathbf{y}_l)},$$

필터는 먼저 IMU로 전파된 사전 분포에 대해 LiDAR 업데이트를 반복하고, 그다음 LiDAR로 수렴한 상태와 공분산에 대해 시각 업데이트를 반복한다—두 노이즈가 독립일 때 결합 업데이트와 이론적으로 동치이지만, 각 모듈은 자신만의 반복과 구조를 유지한다. 각 단계는 표준 반복 칼만 이득 $\mathbf{K} = ((\mathbf{H}^\kappa)^T\mathbf{R}^{-1}\mathbf{H}^\kappa + \widehat{\mathbf{P}}^{-1})^{-1}(\mathbf{H}^\kappa)^T\mathbf{R}^{-1}$을 사용한다.

• LiDAR 측정 모델: 원시(시간적으로 1:3 다운샘플링된) 포인트가, 해시 인덱싱된 복셀 맵(0.5m 루트 복셀, 3계층 옥트리)에 저장된 평면에 대해 포인트-투-평면 잔차 $\mathbf{0} = \mathbf{n}_j^T({^G}\mathbf{T}_I\,{^I}\mathbf{T}_L\,{^L}\mathbf{p}_j - \mathbf{q}_j)$를 형성하며, 측거·방향·그리고 레이저 빔 발산을 모델링하는 노이즈를 가진다—노이즈는 평면에 대한 입사각이 커질수록 증가한다.
• 시각 측정 모델 (희소-직접, 원-스텝 프레임-투-맵): 각 시각 맵 포인트는 패치 피라미드를 가진 LiDAR 포인트이며, 잔차는 현재 프레임과 참조 프레임 사이의 노출-정규화된 패치를 비교한다,

$$\mathbf{0} = \tau_k\,\mathbf{I}_k(\mathbf{u}_i + \Delta\mathbf{u}) - \tau_r\,\mathbf{I}_r(\mathbf{u}'_i + \mathbf{A}^r_i\,\Delta\mathbf{u}),$$

여기서 $\mathbf{A}^r_i$는 FAST-LIVO의 상수-깊이 가정 대신 LiDAR 평면 사전 정보(선택적으로 다중 패치 광도계 최적화로 정제된 법선)로부터 계산된 어파인 워프이다. 역합성(inverse compositional) 공식화는 포즈 증분을 참조 측에 두어 야코비안이 한 번만 계산되도록 하며, 정렬은 세 개의 피라미드 레벨에 대해 조-대-세밀로 실행된다. $\tau_k$가 잔차에 나타나므로 노출은 동일한 업데이트에서 추정된다.

• 통합 복셀 맵: LiDAR가 기하(평면)를 구축하고 업데이트한다; 시각은 LiDAR 포인트에 패치 피라미드를 부착한다. 참조 패치는 형제 패치와의 NCC 유사도 및 뷰 각도 직교성으로 재점수화되며; 새 패치는 >20 프레임 또는 >40 픽셀의 모션 이후 추가되고; 새 맵 포인트는 최고 그래디언트를 가진 후보로 30×30 픽셀 빈 그리드 셀을 채운다.
• 요청 기반 복셀 레이캐스팅: LiDAR가 포인트를 거의 반환하지 않을 때(근접 사각지대, FoV 불일치), 비어 있는 이미지 그리드 셀을 통해 광선을 쏘아, 맵 포인트가 회수될 때까지 깊이를 따라 복셀을 샘플링한다—시각 업데이트를 제약된 상태로 유지한다.

실험 결과

• 벤치마크 (25개 시퀀스: NTU-VIRAL, Hilti’22, Hilti’23; LVI-SAM은 루프 클로저 제거): 평균 절대 병진 RMSE 0.044m—2위인 FAST-LIVO(0.137m)보다 약 3배 정확하며, FAST-LIO2(0.151m), R3LIVE(0.278m), LVI-SAM(1.928m, 9개 시퀀스에서 실패), SDV-LOAM(7.416m)보다 훨씬 앞선다. FAST-LIO2가 밀리미터 단위로 더 나은 어둡고 흐릿한 두 시퀀스를 제외하면 거의 모든 시퀀스에서 최고이다.
• 제거 실험(ablation): 온라인 노출 추정을 제거하면 평균 정확도가 6mm 나빠진다; 참조 패치 업데이트를 제거하면 44mm 나빠진다; 법선 정제는 ~1mm를 추가한다(조명이 좋은 구조화된 장면에서만 도움).
• 런타임 (i7-10700K): 프레임-쌍당 평균 30.03ms (LiDAR 17.13ms + 이미지 12.90ms)—여유를 두고 10Hz 실시간; ARM RB5 (Qualcomm Kryo585)에서 78.44ms로, 여전히 실시간이다. FAST-LIO2는 이미지를 전혀 처리하지 않으면서도 겨우 ~10.35ms 빠르다.
• 매핑: 실시간 색상 포인트 맵의 클로즈업은 실제 RGB 이미지와 유사하다; 시연된 매핑 시퀀스에서 종단 위치 오차가 0.01m 미만이다; 노출-정규화된 채색은 과노출된 맵 포인트를 대부분 제거한다.
• 응용: 완전히 온보드로 이루어지는 자율 UAV 내비게이션(선구적인 실제 환경에서의 LIV 기반 폐루프 비행이며, 레이캐스팅이 벽면 근접 사각지대에서 위치 추정을 안정적으로 유지), 항공 매핑, 그리고 밀집 색상 맵으로부터의 메쉬/텍스처링과 NeRF/3D 가우시안 스플래팅 모델 생성. 코드, 데이터셋, 응용 사례가 모두 오픈소스이다.

SLAM에서의 의미

FAST-LIVO2는 가장 강력한 오픈소스 직접 LVI 오도메트리로 널리 평가받는다—HKU MARS 라인(FAST-LIO2 → FAST-LIVO → FAST-LIVO2)의 정점이다. 그 순차적 업데이트 트릭은 차원과 구조가 크게 다른 측정값을 가진 센서를 융합하는 데 일반적으로 유용한 패턴이며, 그 실증된 UAV 배치는 직접적인 삼중 융합이 엣지 컴퓨팅 예산에서도 프로덕션에 준비되어 있음을 보여준다. 오늘날 로봇을 위한 현대적인 LVI 시스템을 선택한다면, 이것이 넘어서야 할 기본 후보이다.

실습

• FAST-LIVO2 실행하기

관련 문서

• FAST-LIVO — 이 시스템이 융합을 엄밀하게 만든 선행 연구
• FAST-LIO2 — 직접 LiDAR-관성 코어
• R3LIVE++ — 온라인 노출 추정이라는 사고방식을 공유
• Direct LiDAR-camera alignment — 기저의 원리
• LVI-SAM — LVI 설계 공간에서의 팩터 그래프 대응 시스템
• NeRF — 이 시스템의 밀집 맵이 공급하는 렌더링 파이프라인 중 하나