11.20 리 군 기반 시각적 관성 항법(VIO)

1. 시각적 관성 항법의 개요

시각적 관성 항법(Visual-Inertial Odometry, VIO)은 카메라와 관성 측정 장치(IMU)의 측정을 결합하여 운동 중인 카메라의 자세와 위치를 추정하는 기술이다. 카메라는 환경에 대한 풍부한 시각 정보를 제공하지만 깊이 정보가 부족하고 동적 운동에 민감하며, IMU는 자기 운동을 직접 측정할 수 있지만 적분 오차가 누적된다. 두 센서의 상호 보완적인 특성을 결합하면 강건하고 정확한 자세 추정이 가능하다. 리 군 이론은 이러한 결합 추정에 자연스러운 수학적 틀을 제공하며, 현대 VIO 알고리즘의 기초이다.

2. VIO의 수학적 구조

2.1 상태 변수

VIO의 상태는 다음을 포함한다.

카메라 또는 IMU의 자세: $\mathbf{T} \in SE(3)$
속도: $\mathbf{v} \in \mathbb{R}^3$
자이로 바이어스: $\mathbf{b}_g \in \mathbb{R}^3$
가속도계 바이어스: $\mathbf{b}_a \in \mathbb{R}^3$

추가적으로 환경의 3차원 점들이 포함될 수 있다.

2.2 측정값

VIO의 측정값은 두 가지이다.

IMU 측정: 각속도와 선형 가속도
시각 측정: 이미지 특징점의 픽셀 좌표

2.3 다양체 구조

자세 변수가 $SE(3)$ 의 원소이므로, 추정 알고리즘은 다양체 구조를 활용해야 한다. 리 군 기반 접근법이 표준이다.

3. IMU 측정 모형

3.1 자이로 측정

자이로스코프의 측정 모형은 다음과 같다.

$\boldsymbol{\omega}_{\text{meas}} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g$

여기서 $\boldsymbol{\omega}$ 는 참 각속도, $\mathbf{b}_g$ 는 자이로 바이어스, $\mathbf{n}_g$ 는 측정 잡음이다.

3.2 가속도계 측정

가속도계의 측정 모형은 다음과 같다.

$\mathbf{a}_{\text{meas}} = \mathbf{R}^T(\mathbf{a} - \mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$

여기서 $\mathbf{a}$ 는 본체의 선형 가속도(세계 좌표계), $\mathbf{g}$ 는 중력 벡터, $\mathbf{R}$ 은 자세, $\mathbf{b}_a$ 는 가속도계 바이어스, $\mathbf{n}_a$ 는 측정 잡음이다.

3.3 바이어스의 동역학

바이어스는 임의 보행 모형을 따른다.

$\dot{\mathbf{b}}_g = \mathbf{n}_{bg}, \quad \dot{\mathbf{b}}_a = \mathbf{n}_{ba}$

여기서 $\mathbf{n}_{bg}$ 와 $\mathbf{n}_{ba}$ 는 바이어스 구동 잡음이다.

4. 운동 방정식

4.1 자세의 동역학

자세 $\mathbf{R}$ 의 동역학은 다음과 같다.

$\dot{\mathbf{R}} = \mathbf{R}[\boldsymbol{\omega}_{\text{meas}} - \mathbf{b}_g - \mathbf{n}_g]_\times$

4.2 속도의 동역학

속도 $\mathbf{v}$ 의 동역학은 다음과 같다.

$\dot{\mathbf{v}} = \mathbf{R}(\mathbf{a}_{\text{meas}} - \mathbf{b}_a - \mathbf{n}_a) + \mathbf{g}$

4.3 위치의 동역학

위치 $\mathbf{p}$ 의 동역학은 단순히 속도이다.

$\dot{\mathbf{p}} = \mathbf{v}$

4.4 통합된 형태

이러한 동역학을 결합하면 다음과 같은 미분 방정식 시스템이 얻어진다.

$\dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}, \mathbf{n})$

여기서 $\mathbf{x}$ 는 상태, $\mathbf{u}$ 는 IMU 측정, $\mathbf{n}$ 은 잡음이다.

5. 시각 측정 모형

5.1 카메라 모형

카메라는 핀홀 모형으로 표현된다. 3차원 점 $\mathbf{p}_w$ 가 세계 좌표계에서 카메라 좌표계로 변환된 후 이미지 평면에 투영된다.

$\mathbf{p}_c = \mathbf{R}_c^T(\mathbf{p}_w - \mathbf{p}_c^w)$

$\mathbf{u} = \pi(\mathbf{K}, \mathbf{p}_c) = \mathbf{K}\frac{\mathbf{p}_c}{p_{c,z}}$

여기서 $\mathbf{K}$ 는 카메라 내부 매개변수 행렬이다.

5.2 측정 모형

시각 측정 모형은 다음과 같다.

$\mathbf{u}_{ij} = \pi(\mathbf{K}, \mathbf{T}_i, \mathbf{p}_j) + \mathbf{n}_{\text{visual}}$

여기서 $\mathbf{u}_{ij}$ 는 $i$ 번째 이미지에서 측정된 $j$ 번째 특징점의 픽셀 좌표이다.

5.3 재투영 오차

재투영 오차는 측정된 픽셀 좌표와 예측된 픽셀 좌표 사이의 차이이다.

$\mathbf{r}_{\text{visual}, ij} = \mathbf{u}_{ij} - \pi(\mathbf{K}, \mathbf{T}_i, \mathbf{p}_j)$

이 오차의 제곱합이 시각 비용 함수이다.

6. VIO의 두 접근법

6.1 필터 기반 VIO

필터 기반 VIO는 EKF나 UKF를 사용하여 상태를 점진적으로 갱신한다. 측정이 도착할 때마다 즉시 처리된다.

6.1.1 MSCKF

MSCKF(Multi-State Constraint Kalman Filter)는 대표적인 필터 기반 VIO 알고리즘이다. 카메라 자세를 상태에 추가하지만, 3차원 점은 상태에 포함시키지 않고 마진화한다.

6.1.2 ROVIO

ROVIO(Robust Visual Inertial Odometry)는 직접 광도 측정을 사용하는 필터 기반 VIO이다.

6.2 최적화 기반 VIO

최적화 기반 VIO는 키프레임의 자세와 측정을 그래프로 표현하고, 전체 그래프를 최적화한다.

6.2.1 VINS-Mono

VINS-Mono는 단안 카메라와 IMU를 사용하는 최적화 기반 VIO이다. 슬라이딩 윈도우 방식으로 동작한다.

6.2.2 OKVIS

OKVIS(Open Keyframe-based Visual-Inertial SLAM)는 키프레임 기반 비주얼-관성 SLAM이다.

6.2.3 ORB-SLAM3

ORB-SLAM3은 ORB-SLAM의 후속 버전이며, 시각, 시각-관성, 다중 지도 등을 지원한다.

7. 미리 적분(Preintegration)

7.1 동기

IMU 측정값은 매우 빠르게(100~1000 Hz) 도착한다. 모든 측정을 키프레임 변수에 직접 결합하면 비효율적이며, 자세가 변할 때마다 모든 IMU 측정을 다시 적분해야 한다.

7.2 미리 적분의 정의

미리 적분은 두 키프레임 사이의 IMU 측정값을 누적하여 단일 측정값으로 표현하는 기법이다. 자세, 속도, 위치의 변화로 표현된다.

$\Delta\mathbf{R}_{ij} = \prod_{k=i}^{j-1}\exp([(\boldsymbol{\omega}_k - \mathbf{b}_g)\Delta t]_\times)$

$\Delta\mathbf{v}_{ij} = \sum_{k=i}^{j-1}\Delta\mathbf{R}_{ik}(\mathbf{a}_k - \mathbf{b}_a)\Delta t$

$\Delta\mathbf{p}_{ij} = \sum_{k=i}^{j-1}\!\left[\Delta\mathbf{v}_{ik}\Delta t + \frac{1}{2}\Delta\mathbf{R}_{ik}(\mathbf{a}_k - \mathbf{b}_a)\Delta t^2\right]$

7.3 다양체 위의 미리 적분

Forster 등의 미리 적분은 자세를 다양체 위에서 정확히 처리한다. 자세 변화가 $SO(3)$ 의 원소로 표현되며, 곱셈으로 누적된다.

7.4 바이어스의 갱신

미리 적분 항은 사용된 바이어스의 함수이다. 바이어스가 갱신되면 미리 적분 항도 1차 근사로 갱신된다. 이는 효율적인 처리를 가능하게 한다.

7.5 잔차

미리 적분 잔차는 키프레임 사이의 자세 변화의 측정값과 예측값의 차이이다.

$\mathbf{r}_{\text{IMU}, ij} = \begin{bmatrix} \log(\Delta\mathbf{R}_{ij}^T\mathbf{R}_i^T\mathbf{R}_j)^\vee \\ \mathbf{R}_i^T(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) - \Delta\mathbf{v}_{ij} \\ \mathbf{R}_i^T(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\Delta t_{ij} - \frac{1}{2}\mathbf{g}\Delta t_{ij}^2) - \Delta\mathbf{p}_{ij} \end{bmatrix}$

이는 9차원 벡터이며, 자세, 속도, 위치 잔차를 포함한다.

8. 비용 함수와 최적화

8.1 통합된 비용 함수

VIO의 비용 함수는 시각 잔차와 IMU 잔차의 결합이다.

$J = \sum_{i, j}\lVert\mathbf{r}_{\text{visual}, ij}\rVert^2_{\Sigma_{\text{visual}}} + \sum_i\lVert\mathbf{r}_{\text{IMU}, i, i+1}\rVert^2_{\Sigma_{\text{IMU}}}$

여기서 가중치는 각 측정의 공분산의 역이다.

8.2 최적화 변수

최적화 변수는 키프레임의 자세, 속도, 바이어스, 그리고 3차원 점이다. 자세의 매개변수화에 리 군 이론이 활용된다.

8.3 슬라이딩 윈도우

실시간 동작을 위해 슬라이딩 윈도우 방식이 사용된다. 가장 최근의 $N$ 개 키프레임만 최적화에 포함되고, 나머지는 마진화된다.

8.4 마진화

오래된 키프레임을 마진화할 때 정보가 보존된다. 이는 슈어 보충(Schur complement)을 사용한 효율적인 처리이다.

9. 자코비안의 계산

9.1 IMU 잔차의 자코비안

미리 적분 잔차의 자세에 대한 자코비안은 리 군의 좌측/우측 야코비안을 사용하여 유도된다.

9.2 시각 잔차의 자코비안

재투영 오차의 자세에 대한 자코비안은 카메라 모형의 미분과 자세의 미분을 결합한다.

9.3 자동 미분

복잡한 자코비안의 계산은 자동 미분을 사용하여 단순화할 수 있다. Ceres Solver 등의 라이브러리는 리 군 변수의 자동 미분을 지원한다.

10. 초기화

10.1 시각적 초기화

VIO의 시작 시 시각적 초기화가 수행된다. 카메라의 첫 몇 프레임에서 특징점을 추출하고 매칭하여 초기 3차원 구조를 추정한다.

10.2 관성 초기화

IMU 측정으로부터 중력 방향, 초기 속도, 바이어스 등이 추정된다. 시각적 정보와 결합되어 일관된 초기화가 이루어진다.

10.3 견고성

초기화는 VIO의 안정성에 매우 중요하다. 잘못된 초기화는 전체 추정에 영향을 준다.

11. 회로 폐쇄

11.1 회로 폐쇄 검출

VIO에 회로 폐쇄가 추가되면 비주얼-관성 SLAM이 된다. 회로 폐쇄는 누적 오차를 보정한다.

11.2 알고리즘

DBoW2, NetVLAD 등의 위치 인식 알고리즘이 회로 폐쇄 검출에 사용된다.

11.3 그래프 최적화

검출된 회로 폐쇄는 그래프에 새로운 엣지로 추가되고, 전체 그래프가 다시 최적화된다.

12. 강건성

12.1 이상치 처리

특징점 매칭에 이상치가 포함될 수 있다. RANSAC과 강건 추정 방법이 사용된다.

12.2 동적 환경

동적 객체는 잘못된 매칭을 유발할 수 있다. 동적 객체 검출과 제외가 필요하다.

12.3 빠른 운동

빠른 운동에서는 이미지 흐림과 IMU 포화가 발생할 수 있다. 강건한 알고리즘이 이를 처리한다.

13. 정확도

13.1 절대 정확도

VIO의 절대 위치 정확도는 일반적으로 이동 거리의 1%~5% 정도이다. 회로 폐쇄가 있으면 더 향상된다.

13.2 상대 정확도

짧은 시간 동안의 상대 정확도는 매우 높다. 이는 매니퓰레이션과 빠른 제어에 충분하다.

13.3 평가 데이터셋

EuRoC, KITTI, TUM-VI 등의 데이터셋이 VIO 알고리즘의 평가에 사용된다.

14. 응용 분야

14.1 무인 항공기

소형 드론과 무인 항공기에서 VIO는 GPS가 없는 환경에서의 항법에 핵심이다. 실내 비행, 빌딩 사이의 비행 등에 활용된다.

14.2 증강 현실

스마트폰과 헤드 마운트 디스플레이의 증강 현실에서 VIO는 시점 추적의 표준이다. Apple ARKit, Google ARCore 등이 VIO 기술을 사용한다.

14.3 모바일 로봇

GPS가 없는 실내 환경에서 모바일 로봇의 위치 추정에 VIO가 활용된다.

14.4 자율 주행

자율 주행 차량에서 VIO는 GPS와 결합하여 정확한 위치 추정을 제공한다.

14.5 우주 탐사

우주 탐사 로봇(예: 화성 탐사 로버)에서 GPS를 사용할 수 없으므로 VIO와 시각적 SLAM이 핵심 항법 기술이다.

15. 라이브러리와 시스템

15.1 VINS-Fusion

VINS-Mono의 확장으로 단안, 스테레오, 다중 카메라를 지원한다.

15.2 OKVIS

키프레임 기반 비주얼-관성 SLAM 시스템이다.

15.3 ORB-SLAM3

비주얼, 비주얼-관성, 다중 지도 SLAM을 지원한다.

15.4 MSCKF-VIO

MSCKF 알고리즘의 오픈소스 구현이다.

15.5 Open VINS

확장 가능한 비주얼-관성 항법 시스템이다.

16. 학습 기반 접근

16.1 종단 학습

전체 VIO 파이프라인을 학습하는 종단 학습 방법이 연구되고 있다. 다만 안정성과 일반화가 도전이다.

16.2 부분 학습

특정 모듈(예: 특징점 검출, 매칭)을 학습으로 대체하는 방법이 더 안정적이다.

16.3 신경망과 최적화의 결합

신경망의 표현력과 최적화의 정확성을 결합한 하이브리드 방법이 활발히 연구되고 있다.

17. 미래의 발전 방향

17.1 효율성 향상

VIO의 효율성을 향상시키는 연구가 계속되고 있다. 임베디드 시스템에서의 동작이 목표이다.

17.2 강건성 향상

다양한 환경 조건에서 강건한 VIO가 필요하다. 어두운 환경, 텍스처가 부족한 환경 등이 도전이다.

17.3 의미 통합

객체 정보와 의미 분할을 VIO에 통합하는 연구가 진행되고 있다.

17.4 다중 센서 융합

VIO에 라이다, 레이더 등 추가 센서를 융합하는 방법이 발전하고 있다.

18. 학습의 가치

리 군 기반 시각적 관성 항법을 깊이 이해하는 것은 다음과 같은 이점을 제공한다.

자율 주행, 드론, 증강 현실의 핵심 기술을 이해할 수 있다.
다양한 VIO 알고리즘의 원리와 차이를 비교할 수 있다.
실제 시스템을 구현하고 디버깅할 수 있다.
학술 연구에 기여할 수 있다.
새로운 응용을 개발할 수 있다.

19. 응용 예시: 드론의 실내 비행

GPS가 없는 실내 환경에서 드론은 VIO를 사용하여 자율 비행한다. 카메라와 IMU의 융합으로 정확한 위치 추정이 가능하며, 장애물 회피와 경로 따르기가 수행된다.

20. 응용 예시: 증강 현실 게임

스마트폰의 증강 현실 게임에서 VIO는 가상 객체의 안정적인 표시를 가능하게 한다. 사용자가 움직여도 가상 객체가 실제 환경에 고정되어 보인다.

21. 응용 예시: 자율 주행 시뮬레이션

자율 주행 시뮬레이션에서 VIO 알고리즘이 평가된다. 다양한 환경 조건에서의 성능이 측정되며, 개선이 이루어진다.

22. 응용 예시: 로봇 매니퓰레이션

매니퓰레이터에 부착된 카메라가 작업 환경을 추적할 때 VIO가 활용된다. 카메라의 자세를 정확히 알아야 정확한 매니퓰레이션이 가능하다.

23. 참고 문헌

Forster, C., Carlone, L., Dellaert, F., & Scaramuzza, D. (2017). “On-manifold preintegration for real-time visual-inertial odometry.” IEEE Transactions on Robotics, 33(1), 1–21.
Qin, T., Li, P., & Shen, S. (2018). “VINS-Mono: A robust and versatile monocular visual-inertial state estimator.” IEEE Transactions on Robotics, 34(4), 1004–1020.
Mourikis, A. I., & Roumeliotis, S. I. (2007). “A multi-state constraint Kalman filter for vision-aided inertial navigation.” IEEE International Conference on Robotics and Automation, 3565–3572.
Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., & Furgale, P. (2015). “Keyframe-based visual-inertial odometry using nonlinear optimization.” International Journal of Robotics Research, 34(3), 314–334.
Campos, C., Elvira, R., Rodríguez, J. J. G., Montiel, J. M. M., & Tardós, J. D. (2021). “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM.” IEEE Transactions on Robotics, 37(6), 1874–1890.
Bloesch, M., Burri, M., Omari, S., Hutter, M., & Siegwart, R. (2017). “Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback.” International Journal of Robotics Research, 36(10), 1053–1072.

version: 1.0