[논문 리뷰] VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

* 아직 공부를 하고 있는 비전문가이기 때문에 잘못 이해하였거나 잘못 번역한 부분이 있을 수 있습니다. 해당 부분이 있다면 댓글 부탁드립니다.

[ Abstract ]

monocular camera와 IMU를 사용하면 가장 적은 센서 형태(크기, 무게 등에서)로 6 DOF state estimation 분야에 적용할 수 있다. 비선형 최적화 방법은 preintegrated IMU measurement와 특징점 관찰을 결합해 얻은 정밀한 visual-inertial odometry를 이용한다. loop detection은 추가적으로 global consistency를 향상시키기 위해 4 DOF pose graph optimization을 진행한다. 또한 효율적인 방법으로 지도를 저장하고, 불러와 재사용을 할 수 있다. 현재와 과거의 지도는 global pose graph optimization으로 병합될 수 있다.

[ Introduction ]

state estimation은 여러 분야(로봇, 자율주행, AR, VR)에서 중요한 부분이고, 그중 monocualr camera는 저렴하고 작은 장점으로 많이 사용된다. 하지만 monocular camera는 metric scale의 부재라는 문제를 갖고 있는데, 이를 IMU와의 결합으로 해결할 수 잇다.

하지만 monocular-inertial system에는 몇 가지 문제가 있다. 우선 metric scale의 부재로 인한 초기화(결합)의 어려움이다. 또한 VINS는 매우 비선형적이기 때문에 estimator initialization에 어려움이 있다. 또한 long-term drift의 누적이 불가피하다는 점이 있다.

이것을 해결하기 위해 VINS-Mono를 제안하고, 이것의 contribution은 아래와 같다.

- 초기 상태를 모르더라도 초기화할 수 있는 방법 제안

- tightly coupled 방식의 sensor fusion, optimization-based estimation 방법 제안

- 실시간 relocalization 및 4 DOF global pose graph optimizaiton

- pose graph 저장 및 재사용

[ Overview ]

논문에서 사용할 표기법 정리

- 각 frame(world, body, camera)에 대한 표기법(+ 중력은 z축)

- rotation matrix R과 quaternion q

- q_bw, p_bw는 rotation과 translation from body_frame to world_frame

- b_k, c_k는 k번째 image의 body_frame과 camera_frame

- (^)은 특정 값에 대한 추정

[ Measurement Preprocessing ]

1. Vision-only Frontend

GoodFeatureToTrack으로 feature를 추출하고 이것은 KLT sparse optical flow algorithm에 의해 tracking된다.

이때 keyframe도 선택이 되는데 그 기준은 아래의 두 가지이다.

1) last keyframe과의 픽셀 차이(average parallax)가 threshold를 초과

이때 rotation-only motion에서는 triangulation을 할 수 없는데, 이러한 상황을 방지하기 위해 parallax를 계산할 때 gyroscope의 short-term integration을 사용한다. 이 값은 오직 keyframe 선정에만 사용되고 버려진다.

2) tracked feature의 수가 threshold를 초과

이것은 tracking loss를 방지하기 위한 것이다.

2. IMU Preintegration

이전의 연구인 continuous-time quaternion-based derivation of IMU preintegration을 사용한다. 이것은 [19] on-manifold preintegration for real-time visual-inertial odometry 방법과 유사하지만 조금 다르다.

1) IMU noise and bias

IMU measurement는 accelerometer bias(b_a)와 gyroscope bias(b_w), 추가적인 noise의 영향을 받는다. 그래서 선형 가속도 a_t와 각속도 w_t는 위와 같이 표현된다. 이때 noise는 gaussian이라 가정하며, bias의 미분도 gaussian으로 가정한다.

2) preintegration

연속되는 frame b_k와 b_{k+1} 사이에는 여러 inertial measurements(position, velocity, orientation)이 존재하고, 그 값을 local_frame b_k로 통합한다.

translation(p), velocity(v), quaternion(q)

이때 reference frame을 world_frame에서 local_frame b_k로 바꾸면, 선형 가속도와 각속도와 관련된 부분만 preintegration 할 수 있다.

이때 α, β, γ는 아래와 같다.

IMU의 translation(α), velocity(β), quaternion(γ)

이것으로 preintegration term(α, β, γ)은 k번째 body frame을 기준으로 하는 imu 측정치만으로 얻을 수 있는 것을 알 수 있다.

3) bias correction

bias에 대한 추정치가 조금 변하면, α, β, γ를 각각에 대한 첫 근사치(by their first-order approximations)를 통해 수정한다. 하지만 크게 변하면, bias를 다시 전송한다. 이것은 IMU measurement를 계속 전파(propagate)할 필요가 없기 때문에 optimization-based algorithm을 위해 상당한 양의 계산량을 절약한다.

[ Estimator Initialization ]

Monocular VIO는 매우 비선형 시스템이라 정확한 초기화가 필요하다. 이때 미리 통합한 IMU 측정값을 vision-only 방식과 느슨하게 정렬하여 초기값을 얻는다. Metric 정보를 갖고 있는 IMU 측정값과 정렬함으로써 대략적인 scale, gravity, velocity, bias를 회복할 수 있다.

1. vision-only SfM in sliding window

계산 복잡성을 제한하기 위해 sliding window를 유지한다. 먼저 latest frame과 모든 previous frame 간의 correspondence를 검사한다. 만약 latest frame과 sliding window의 다른 frame 사이에서 안정적으로 tracking이 되고(more than 30 tracked features) 충분한 parallax(more than 20 pixels)이 발견된다면, 두 frame 사이에서 5-points algorithm을 사용하여 상대적인 회전과 스케일에 맞는 이동을 회복한다. 그리고 임의의 scale을 설정하여 나머지 모든 feature에 대해 triangulation을 진행하고 window 안의 모든 frames의 pose estimation을 위해 triangulated feature로 perspective-n-point(PnP) 방법을 사용한다. 마지막으로 global BA로 최적화를 한다. 이때 아직 world_frame에 대한 정보가 없기 때문에 첫 번째 camera_frame으로 설정하고, 이것은 SfM의 reference frame이 된다. camera와 IMU 사이의 extrinsic parameter가 주어지면, camera_frame과 body_frame 사이의 pose를 아래와 같이 변환할 수 있다.

s는 scaling parameter로 metric scale로 변경해주는 역할을 하고, 이것을 찾는 것이 initialization의 핵심이다.

2. visual-inertial alignment(아직 조금 더 공부가 필요)

1) gyroscope bias calibration

연속되는 두 frame b_k, b_{k+1}이 window 안에 있다고 가정하면, visual SfM으로 c0로부터 각각에 대한 회전(q)을 얻고, preintegration으로 두 프레임 간의 γ(IMU의 회전 측정값)를 얻을 수 있다. 이후 아래의 비용 함수를 최소화하여 gyroscope bias(b)의 initial calibration을 얻는다.

2) velocity, gravity vector, metric scale initialization

앞의 과정을 하고 나서 주요 state 선형화를 진행한다.

이때 모든 body frame의 velocity, visual reference frame에 대한 gravity vector, scale parameter를 구할 수 있다.

(내용 생략)

3) gravity refinement

4) completing initialization

[ Tightly-Coupled Monocular VIO ]

1. Formulation

sliding window의 모든 state는 아래와 같이 정의된다.

visual-inertial BA(bundle adjustment)를 사용하여 최대 사후 추정(MAP)을 얻기 위해 모든 measurement residuals의 prior와 Mahalanobis norm의 합을 최소화한다.

r_B와 r_C는 각각 imu와 visual measurement의 residual     r_p와 H_p는 marginalization의 prior information

이때 ρ는 Huber norm으로 아래와 같다.

nonlinear problem을 위해 Ceres Solver를 사용한다.

2. IMU measurement residual

연속되는 프레임 b_k와 b_{k+1}에 대해 residual for preintegration은 아래와 같다.

여기에서 [.]_xyz 는 오류 상태 표현(error state representation)을 위해 quaternion의 vector part(허수부)를 추출한 것이고, δθ 는 quaternion의 3차원 오류 상태 표현이다. α^hat, β^hat, γ^hat은 연속되는 두 frame 사이의 preintegrated IMU measurements이다.

3. visual measurement residual

기존의 pinhole 모델과 달리 여기에서는 unit sphere에 camera measurement residual을 정의한다.

i번째 프레임에서 처음 관찰된 feature에 대해 j번째 프레임에서 관찰한 residual은 아래와 같다.