[Paper Review] VINS-mono 요약 및 설명

Visual Inertial SLAM 중 유명한 VINS-mono를 요약해보자.

최근 VIO, VI-SLAM 쪽 연구를 진행하려고 하고 유명한 논문을 먼저 읽고 공부해 보고자 한다.
Visual Inertial쪽에서 제일 인용이 많이 된 논문 중 하나인 VINS-mono를 읽어보자.

공부를 하는 과정에서 포스팅을 했기 때문에 부족한 점이 많습니다. 문제점 및 오타가 발견되면 댓글로 알려주시면 감사드리겠습니다.

논문은 아래 링크에서 원본을 확인할 수 있다.

• VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator (ArXiv , IEEE)
• Github : HKUST-Aerial-Robotics/VINS-Mono

1. Introduction

VINS-mono는 Monocular camera와 IMU를 결합했을 때 단순히 카메라를 썻을 때보다, 여러 장점이 있다고 소개한다.

• Scale 값을 얻을 수 있다. (Roll, pitch angle도 얻을 수 있다.)
• 카메라쪽에서 Tracking하기 어려운 환경(illumination change, textureless area, or motion blur)에서도 IMU 센서의 도움을 받아 Tracking을 수행할 수 있다.
• Camera, IMU 모두 저가로 구입할 수 있는 센서이다.

이러한 장점이 있는 반면 큰 Issue들도 존재한다.

• 초기화 과정이 힘들다.

  직접 거리를 측정할 수 있는 센서가 없기 때문에, visual structure 와 inertial measurements의 결합이 어렵다.

• VINS(visual-inertial system)은 Non-linear한 System이다.

  대부분의 경우 시스템은 고정 위치에서 시작해야 하며 처음에는 천천히 조심스럽게 움직여야 하므로 실제로 사용이 제한된다.

• VIO(Visual-inertial odometry)의 경우, drift현상이 필연적이다.

따라서 이런 Issue들을 해결하기 위해 VINS-mono에서는 다음과 같은 Contribution이 존재한다.

• 초기 상태(initial states)를 모르더라도 초기화 할 수 있는 방법을 제안
• Tightly coupled 방식의 sensor fusion, Optimization-based Estimation 방법
• 실시간이 보장된 Relocalization and 4 degrees-of-freedom (DOF) global pose graph optimization
• pose graph를 저장하고 불러오고 다른 local pose graph와 합칠 수 있음

2. OverView

Related work 부분은 생략을 할 예정이다.

VINS-mono의 전반적인 system은 아래 그림과 같다.

VINS-mono는 우선 measurement preprocessing 과정을 거친다. 이미지로부터 feature를 뽑아내고, Tracking을 하면서 연속적인 두 이미지 사이에 IMU measurments를 preintegrated하는 과정을 거친다.

Measurement preprocessing 과정이 끝나면 초기화(initialization) 과정을 거친다. 추후 nonliear optimization을 할 때 필요한 값들을 모두 구한다. 예를 들어, pose, velocity, gravity vector, gyroscope bias, and (3-D) 특징 점의 위치 등등을 구하게 된다.

초기화 값으로 구한 값을 가지고 Relocalization Module이 있는 Visual-Inertial Odometry Module은 preintegrated 된 IMU 측정값과, feature observations를 tightly coupeld 방법으로 fusion을 진행한다.

VINS-mono에서는 Monocular camera와 IMU값을 이용하여 초기화하는 방법, Keyframe을 고르는 방법, tracking을 잘 수행하는 방법등을 제안했으며, Loop closure과 pose graph reuse 모듈까지 만들어 전체적인 SLAM system을 구축했다.

이 논문에서 사용하는 Notation은 다음과 같다.

• $()^c$ : camera frame
• $()^w$ : world frame
• $()^b$ : IMU frame
• $q$ : 쿼터니언, $R$ : Rotation Matirx
• $p$ : Translation vector
• $b_k$ : k번째 이미지에서의 IMU(Body) frame
• $c_k$ : k번째 이미지에서의 Camera frame
• $\otimes$ : 쿼터니언끼리의 곱
• $\hat{()}$ : 추정 값 or noisy measurement

3. Measurement preprocessing

Visual measurment와 IMU measurement의 전처리 과정을 자세하게 살펴보자.

Visual measurment에서는 연속적인 이미지에서의 Tracking을 시도하고, 현재 frame에서 새로운 특징점을 찾는다. IMU measurements에서는 연속적인 두 이미지에서 preintegration과정을 거친다.

Vision Processing Front End

새로운 이미지가 들어오면, KLT sparse optical flow algorithm을 수행한다. feature를 찾을 때는 GoodFeatureToTrack() 함수를 사용한다. 이는 OpenCV에 있는 함수이다. 한 이미지당 featrue의 갯수는 100개~300개 정도를 유지한다. Detector는 feature간 너무 붙어 있지 않도록 uniform feature distribution를 적용한다.

RANSAC 알고리즘을 이용하여 Oulier 제거를 거친 후, 특징이 추출된 이미지를 unit sphere(단위 구)에 투영을 시킨다.

이 과정에서 KeyFrame 선별도 하게 되는데 두 가지 기준을 가지고 KeyFrame을 선별한다.

• last(최근) frame과 current(현재) frame간에 특징점들 끼리의 픽셀 차이(parallax)가 일정 threshold 이상일 경우 새로운 keyframe으로 구분
• Tracking quality에 따라 구분

특징점 끼리의 픽셀 차이를 여기서 parallax(시차)라고 표현했다. 이러한 기준을 선정한 이유는 triangulate를 진행할 때 충분한 feature 수를 확보하기 위해서이다. 또한 parallax를 구할 때 Rotation만 일어났을 때만을 대비하여 IMU measurement에서 측정된 short-term integration of gyroscope measurements를 보상하여 parallax를 계산한다.

Tracking quality는 visual feature point의 수로 판단을 할 수가 있는데, tracking이 되고 있는 feature point의 수가 일정 threshold보다 떨어지면 새로운 feature들이 많이 생기고, 새로운 상황(이미지 및 장면)을 맞이했다고 이해할 수 있다. 따라서 새로운 상황이라고 판단하면 KeyFrame을 선별한다고 이해하면 된다.

• [2023.03.09 문장 수정] : 김범수님의 도움을 받아 문장을 수정하였습니다. 😆

IMU pre-integration

IMU pre-integration이란 연속적인 카메라 이미지 데이터에 IMU 정보를 결합하여 더 정교한 pose를 계산하기 위한 작업이다. 보통 아래의 그림처럼 IMU가 들어오는 간격이 카메라 이미지보다 더 빠르기 때문에 카메라 이미지 사이에 들어오는 IMU measurement들을 하나로 이미지와 싱크를 맞추는 작업을 거친다.

IMU가 어떻게 동작하는지 우선 이해가 필요하다. 나도 이번 기회를 통해서 조금 깊게 파보려고 공부중인데 다음과 같은 링크가 도움이 되었다.

• [IMU] IMU의 개념 및 활용법
• INERTIAL NAVIGATION PRIMER

기본적으로 IMU는 gyroscope(각속도)와 accelerometer(가속도)를 측정하는 센서이다. 각각의 값들은 Noise와 bias가 존재하고 VINS-mono에서는 IMU를 다음과 같이 모델링 했다.

$a_t$는 linear acceleration 값을 나타내고, $w_t$는 angular velocity이다.

각각 노이즈는 Gaussian white noise를 가정한다. 그리고 Bias값도 미분을 하게 되면 Gaussian white noise이 나오도록 모델링했다.

[k, k+1] image frame 사이에 IMU의 translation 값 $p$, velocity 값 $v$, quaternion 값 $q$는 아래와 같다.

여기서 $g^w$는 world 좌표계에서의 중력 값이 아닌, IMU body frame으로 투영 된 중력 값이다.

여기서 우리는 world 좌표계가 기준이 아닌 $b_k$(k번째 이미지에서의 IMU body frame)을 기준으로 계산을 해야하므로 좌표계 변환이 필요하다.

따라서 결론적으로 구해야 하는 IMU의 translation 값 \(p = \alpha^{b_k}_{b_{k+1}}\)

IMU의 velocity 값 \(v = \beta^{b_k}_{b_{k+1}}\)

IMU의 quaternion 값 \(q = \gamma^{b_k}_{b_{k+1}}\)

이라고 하면 아래와 같이 정리할 수 있다.

위 수식에 대한 정리는 Formula Derivation and Analysis of the VINS-Mono에서 IMU PREINTEGRATION 파트를 보면 자세히 나와있다.

컴퓨터는 discrite time을 가지고 있기 때문에 위에 나온 preintegration term을 discrite time에서 나타내면 식은 아래와 같다. 이때 우리는 Mid point method를 사용한다.

$i$는 $[t_k, t_{K+1}]$에서 discrete moment를 나타낸다.

$\delta_t$는 $i,i+1$에서의 time interval이다.

[… 하아 일단 수식이 너무 어려움으로 나중에 다시 봐야겠다…]

논문에 의하면 $k$번째 Image에서의 IMU measurement를 $b_k$라고 하고 $k+1$번째 Image에서의 IMU measurment를 $b_{k+1}$이라고 할 때, 그들의 관계를 나타내는 covariance $P$는 아래와 같이 나타낼 수 있다.

4. Estimator Initialization

VINS(visual-inertial system)에서 초기화는 굉장히 어려운 Issue이다. 카메라(이미지)와 IMU 센서를 tightly coupled로 결합 방식을 택한다면 더욱 더 복잡한 non-linear system이 된다.

따라서 VINS-mono에서는 초기화 과정을 크게 두 가지 과정으로 거쳐 문제를 풀려고 한다.

1. Visual only SFM(Structure from Motion)
2. Visual-Inertial Alignment

먼저 Visual 정보만으로 대략적인 초기 값을 구한 다음, IMU measurement 값을 algin(정렬)시키는 방법이다.

논문에서는 아래의 그림으로 표현하고 있다.

Visual only SFM의 경우 monocular camera이기 때문에 scale을 모르기 때문에 IMU pre-integration 부분과 차이가 날 수 있다.

하지만 처음부터 Visual과 IMU가 Aligned된 초기값을 구하는 것이 어려움으로 이렇게 2 step으로 나누어 진행한 것으로 생각한다.

또한 특이하게도 Initialization을 할 때는 accelerometer bias term을 무시하고 초기화를 진행한다. accelerometer bias term의 경우 중력의 영향을 받기 때문에 초기화 단계에서는 정확하게 accelerometer bias term을 구하기 힘들기 때문이다.

Visual only SFM

Visual only SFM으로 scale을 제외한 camera pose와 3D feature position을 구하게 된다.

논문에서는 “Sliding Window Vision-Only SfM”이라는 Part에 나와있는데 Sliding window란 N개의 keyframe 정보를 기반으로 SfM을 수행한다고 이해하면 될 것 같다. VINS-mono를 설명하는 Monocular Visual-Inertial SLAM - Shaojie Shen의 자료에 따르면 1초당 10frame 정도 Widnow를 설정한다고 했다.

Five point algorithm으로 두 이미지 간에 Rotation과 Translation의 값을 구한 다음, 임의의 scale 값을 추가하여 Triangulation을 수행한다.(Monocular camera로는 아직 정확한 scale값을 모르기 때문이다.)

Triangulation은 PnP 알고리즘을 활용하며 Window 안에 있는(N개의 keframe) 추정된 모든 camera pose에 대해서 수행하게 된다.

그 다음 Window에 있는 모든 estimated camera pose와 3D feature points에 대해서 Bundle adjustment를 수행한다.

아직 World frame에 대한 정보가 없으므로 첫번째 camera frame($()^{c0}$)를 기준으로 위 과정을 수행하며 Camera와 IMU간의 extrinsic parameter를 알고 있는 경우에 좌표계 변환을 이용하여 camera frame $c0$ 부터 $k$번째 IMU body frame $b_k$의 quternion $q$와 Translation $p$를 계산한다.

위 식에서 $s$는 scale factor로 아직까지는 unknown parameter로 Visual-Inertial Alignment를 수행하며 값을 계산을 하게 된다.

Visual-Inertial Alignment

Visual only SFM으로 카메라간의 Roation과 Translation을 구했다면 이제 IMU preintegration 값과 align을 통해 scale factor $s$와 VIO initialization을 진행해보자.

첫번째 과정으로 Gyroscope Bias calibration을 진행한다. Visual only SFM 과정으로 \(q^{c0}_{b_{k+1}}, q^{c0}_{b_{k}}\)

또한 , Preintegration 과정에서 \(\gamma^{b_k}_{b_{k+1}}\) 을 구했다.

이론상으로는 Visual only SFM 과정으로 구한 쿼터니언 값과 Preintegration 과정에서 구한 쿼터니언 값을 쿼터니언 곱셈을 하게 되면 $(qx, qy, qz, qw) = (0, 0, 0, 1)$ 값이 나와야 한다.

앞서 말했지만 IMU의 quaternion 값 \(q = \gamma^{b_k}_{b_{k+1}}\)

이렇게 정의했었다.

하지만 real world에서는 noise 및 bias 값이 있기 때문에 위 식이 $(qx, qy, qz, qw) = (0, 0, 0, 1)$ 값이 아닌 다른 값이 나오게 되고 이를 최소화를 시켜줘야 정교한 값을 구할 수 있게된다.

이 때 위 식에 영향을 미치는 Term이 바로 Gyroscope Bias($b_w$)이다. 왜냐하면 Preintegration 과정에서 IMU의 quaternion 값을 선형화를 진행하는데 그 때 아래의 식을 사용하기 때문이다.

따라서 아래의 식을 통해 Gyroscope Bias($b_w$)을 최소화한다.

그 다음 새로 구한 Gyroscope Bias($b_w$)을 이용해 IMU pre-integration terms를 다시 계산한다. (논문에서는 re-propagate라고 표현)

두번째 과정으로 아래의 state vector를 초기화를 진행하게 된다.

IMU body frame $[b_0, b_n]$까지의 velocity 값과 첫번째 camera frame $c_0$에서의 gravity 값, 그리고 스케일 값 $s$로 이루어진 vector이다.

state vector는 Window안에 있는 linear measurement model을 통해서 최적화가 이루어지게 된다.

IMU preintegration에서 구한 값을 활용하여 world frame에서 첫번째 camera frame으로 좌표계 변환을 해주면 아래의 식이 나오게 된다.

선형화를 했으니 Matrix decomposition이 가능하다 따라서 아래와 같이 행렬화를 시켜줄 수 있다.

논문에서는 위에 있는 식에있는 가운데 있는 Matrix를 아래(식.11)처럼 $H$로 정의했다.

따라서 IMU sensor 를 통해 linear acceleration을 측정할 수 있고 측정한 것($\hat{z}$)을 아래와 같이 표현할 수 있다.

이상(Ideal)한 값과 측정(Measurement) 값에는 오차가 발생함으로 이를 Least squares method로 최적의 해를 찾을 수 있다. 여기서 찾는 최적의 해는 state vector이다.

세번째 과정으로 gravity refinement를 진행한다.

우리가 state vector를 초기화하면서 gravity term$(g^{c0})$을 계산했다.

이 gravity term을 활용하여 아래의 소개하는 알고리즘을 활용하여 gravity vector를 계산할 것이다.

gravity vector를 구하게 되면 world to camera의 Rotation 정보를 구할 수 있게 된다.

우리는 중력 가속도 상수가 9.81로 고정되어 있다는 것을 알고 있으므로, gravity vector를 2 DOF로 표현이 가능하다.
중력 벡터의 수직인 평면에서의 (tangent space) 직교하는 벡터만 구하면 되기 때문이다.

논문에서는 Figure를 통해 이해를 돕고있다.

논문에서는 알고리즘을 아래와 같이 소개하고 있다.

$b_1, b_2, \hat{g}$는 첫번째 camera frame에서의 좌표계를 나타낸 것이라 이해하면 되는데(주관적인 해석), IMU Preintegration을 할 때 나왔던 식인 아래의 식 (17)을 보면 알겠지만 $[b_k,b_{k+1}]$ 사이에 gravity term$(g^{c0})$이 존재한다.

Window 안에 있는 첫번째 frame부터 마지막 frame까지 가지고 있는 모든 gravity term$(g^{c0})$를 사용하면서 $\hat{g}$가 $[0,0,1]$로 수렴할 때까지 알고리즘을 수행한다.

앞서 말했지만, gravity vector를 통해서 world to camera의 Rotation 정보를 구할 수 있게 되고, world 좌표계를 기준으로 모든 성분을 표현할 수 있게 된다.

이렇게 초기화 과정을 마무리할 수 있게 되고, 초기화에서 구한 값을 추후 Tightly Coupled Monocular VIO에서 쓸 수 있게 된다.

5. Tightly Coupled Monocular VIO

초기화 방식을 마치게 되면 Sliding window 방식의 VIO로 최적화를 수행하게 된다. 즉 N개의 Keyframe을 Window로 설정하고, 그 N개의 해당되는 Keyframe 및, 3D feature(계산을 할 때는 Inverse depth를 사용)들, IMU measurement와 Visual measurement만을 최적화를 수행하는 것이다. 만약 3D feature들을 보고 있는 다른 Loop closure key frame도 존재하면, 그 역시 최적화를 수행한다.

그림으론 간단하게 아래와 같이 나타낼 수 있다.

먼저 최적화를 수행할 대상인 state vector는 아래와 같다.

• $x_0, x_1, …, x_k, …, x_n$ : $k$번째 IMU state (단, $k$는 이미지 frame을 셀 때 사용)
• $\lambda_0, \lambda_1, … , \lambda_n$ : 3D feature points의 Inverse depth값
• $x_k$에는 world 좌표계를 기준으로 Body frame($b_k$)까지의 Translation $p$, Velocity $v$, Rotation (quternion) $q$, 그리고 bias 값들이 존재
• Camera와 Body frame의 extrinsic parameter도 존재

여기서 state estimation을 구하는 것을 Maximum A Posteriori (MAP) 문제로 바꿔서 푼다. 사용하는 식은 아래와 같다.

1번은 Marginalization에서 나온 식이고, 2번은 IMU Measurement Residual 그리고 3번은 Visual Measurement Residual에서 나온 식이다.

IMU Measurement Residual에서는 우리가 preintegration 때 구했던 IMU에 관한 식과 우리가 실제로 움직임에 의해 관찰된 값의 차이(Residual)을 최소화하는 식으로 식이 구성되었다. 자세한 식은 아래와 같다.

다음은 Visual Measurement Residual을 살펴보자.

3D feature point $l$을 $i$번째 이미지에서 바라봤을 때 이미지 plane에서 x좌표, y좌표가 존재할 것이다. 논문에서는 $[u^{c_i}_l , v^{c_i}_l]$ 로 표현했다.

또한 3D feature point $l$을 $j$번째 이미지에서 바라봤을 때 이미지 plane에서 x좌표, y좌표가 존재할 것이다. 논문에서는 $[u^{c_j}_l , v^{c_j}_l]$ 로 표현했다.

back projection function $\pi^{-1}$을 활용하여 픽셀 좌표계로 부터 tangent plane으로 투영시킬 수 있고 좌표계를 동일하게 맞추어($P^{c_j}_l$ 을 계산할 때 $R$을 여러번 곱하고, $p$를 여러번 더한 이유) 그 차이를 계산한 것이 Visual measurement 관점에서 차이(Residual)이다.

투영을 시킬 때 기존의 pinhole camera가 Image plane에 투영을 했던 것과는 달리 Unit sphere에서의 Tangent plane에 투영을 시키는 것에 주의하자.

1번식은 Marginalization에 관한 것이라 했는데 논문에 설명이 조금 부족한 것 같다.. 코드를 통해 이해해 봐야 할 것 같다.

Marginalization 전략

Marginalization은 VIO의 계산 복잡도를 줄이기 위해 필요한 전략이다. 전략은 간단하다. Window에서 두번째로 최근에 들어온 frame을 기준으로 그 frame이 KeyFrame이면 살리고 가장 Old한 KeyFrmae을 제거한다.

만약 Window에서 두번째로 최근에 들어온 frame이 KeyFrame이 아니라면 이를 제거하는 방식이다.

하지만 IMU와 관련된 inertial measurement와 관련된 factor들은 제거하지 않는다.

Marginalization은 Schur complement를 사용한다. (연산량을 줄이기 위해 사용하는 기법이라고 이해하면 된다.)

이를 그림으로 표현하면 아래와 같다.

그 다음 논문에서 설명하고 있는 E. Motion-Only Visual-Inertial Optimization for Camera-Rate State Estimation 와 F. IMU Forward Propagation for IMU-Rate State Estimation는 간단히 설명하고 넘어가겠다.

Motion-Only Visual-Inertial Optimization for Camera-Rate State Estimation에서는 Device에서 Computing power가 별로 좋지 않을 경우, 실시간성 보장을 위해 최적화하는 대상을 줄이겠다는 의미이다. 기존에 Tightly Coupled Monocular VIO를 푸는 것은 같지만, 최적화하는 대상을 최근에 들어온 몇개의 frame에 대해서 pose, velocity, 몇개의 IMU constraints들만으로 제한한다.

IMU Forward Propagation for IMU-Rate State Estimation는 간단히 말해서 카메라보다 빠르게 들어오는 IMU state들에 대해 먼저 pre-processing 과정을 진행하고 이를 Loop-closure에서 활용하겠다는 의미이다.

6. Relocalization

Relocalization은 Drift 현상을 피하기 위해 필요한 단계이다. VINS-MONO에서는 따로 Vocabulary file을 사용하지 않고, 이전에 봤던 이미지들에 대한 feature들 및 descriptor를 모두 저장해 두고, 비슷한 장소가 발견 되었을 때, Loop detection 및 Relocalization을 수행한다.

기존의 Visual SLAM과 비슷한 전략으로 Loop detection은 DBoW2라는 Bag-of-word library를 사용한다. 새로 들어오는 sliding window에서 feature 및 descriptor를 구했을 때, 기존에 유사한 feature 및 descriptor가 있으면 후보군을 DBoW2가 return해주는 방식이다.

후보군이 제시되었으면 feature-level에서 Retrival을 수행한다. 이 때 2단계에 걸쳐 Outlier를 제거한다.

1. 2-D–2-D outlier rejection : Fundamental matrix test with RANSAC
2. 3-D–2-D outlier rejection : PnP test with RANSAC

Outlier를 제거한 이후에도 유사도가 높다면 Loop detection을 성공했다고 판단하고 Relocalization을 수행한다.

Relocalization이란 현재의 Sliding window를 이전에 계산했던 Pose들과 Align 시키는 일을 말한다.
따라서 이전의 계산했던 Pose는 constant로 두고 Sliding window 방식의 optimization을 수행한다. 최적화를 수행하는 식은 기존의 Tightly Coupled Monocular VIO 파트에서 본 식과 유사하다. 여기에 Loop closure frame에 대한 Term이 추가되었다고 이해하면 된다.

Loop closure를 수행할 때 하나의 constraints만 있는 것보다 여러개의 constraints를 가지고 있는게 훨씬 더 높은 정확도를 보였다고 한다. 그림으로 나타내면 아래와 같다.

7. Global Pose Optimization

Relocalization을 수행하고 난 후, Local window에서의 past pose graph가 Global한 pose graph로 유지되는데 필요한 Optimization을 수행한다. 여기서 보이는 VINS-MONO의 특징은 6-DOF (x,y,z,roll,pitch,yaw)를 모두 최적화 시키는 것이 아니라 4-DOF (x,y,z,yaw) 값만 최적화 시킨다는 것이다.

그 이유는 IMU sensor에서 나온 측정값으로 gravity의 방향을 측정할 수 있고, 이를 활용해 Roll, pitch 값을 얻을 수 있으므로, 굳이 최적화를 시키지 않은 것이다.

논문에서는 아래와 같은 그림으로 설명하고 있다.

Pose graph는 KeyFrame을 Vertex로 두고, 연속적인 frame일 경우 sequential edge를 Loop connected frame일 경우 Loop-closure edge로 정의한다.

그리고 아래와 같은 cost function을 정의하고 최적화를 수행한다.

앞에 있는 1번식은 연속적인 frame에서 얻은 sequential edge에 대한 Term이고 2번식은 Loop connected frame에서 얻은 Loop-closure edge에 대한 Term이다.

1번식 같은 경우, 기존의 Tightly Coupled Monocular VIO 수행시 outlier rejection을 수행했기 때문에 따로 Huber norm을 추가하지 않았다.

또한 Pose graph optimization 과 relocalization은 서로 다른 2개의 쓰레드에서 동작한다는 특징이 있다.

---

이렇게 대략적으로 VINS-MONO에 대한 포스팅을 마무리하겠다.
뒤로 갈 수록 포스팅이 부실해지는 느낌인데, 코드 리뷰를 통해 전체적인 구조를 한번 다시 파악해야겠다..😅