ORB-SLAM 2

Title : ORB-SLAM 2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras

Authors : Raul Mur-Artal and Juan D. Tardos

 

*모든 이미지 및 caption은 위 논문을 참고하였음

 

RGB-D input: keyframes and dense pointcloud of a room scene with one loop closure. The pointcloud is rendered by backprojecting the sensor depth maps from estimated keyframe poses. No fusion is performed.

[1] Introduction

• Monocular, stereo 그리고 RGB-D SLAM을 위한 첫 번째 open-source SLAM system
• Bundle Adjustment (BA) 만을 사용하여 local, global optimization
  • ICP + BA 기반의 RGB-D Mapping 보다 더 정확하며 가벼움
• ORB feature를 사용함 (기존의 ORB-SLAM과 동일)

[2] ORB-SLAM 2

ORB-SLAM2 is composed of three main parallel threads: tracking, local mapping and loop closing, which can create a fourth thread to perform full BA after a loop closure. The tracking thread pre-processes the stereo or RGB-D input so that the rest of the system operates independently of the input sensor. Although it is not shown in this figure, ORB-SLAM2 also works with a monocular input

 

[2.1] Overview

• 3개의 main parallel threads와 1개의 sum thread (full BA)로 구성되어 있음
  • Tracking : 매 frame마다 camera의 pose를 추정함. 관측된 feature points를 local map에 match 시키고 re-projection error를 최소화 (motion-only BA)
  • Local mapping : local map을 manage 및 optimize (local BA)
  • Loop closing : large loop를 detect하고 pose-graph optimization을 통해 축적된 drift를 correct (4번째 full BA thread를 launch 시킴)
• Tracking failure 발생 시 re-localization 또는 이미 생성된 map에서 re-initialization을 위한 Place Recognition module로 DBoW2를 사용함
• Tracking과 local mapping을 locally 수행시키고 pose graph시 graph 구성을 위해 다음과 같은 자료구조 이용
  • 동일한 feature point를 같이 공유하는 두 keyframe을 연결하기 위하여 covisibility graph 이용
  • 모든 keyframe들을 연결하기 위하여 minimum spanning tree 구조 이용

[2.2] Keypoints

• ORB-SLAM2 에서는 keypoint (feature point)를 monocular keypoints와 stereo keypoints로 구분하고 있음
• Monocular keypoint는 일반적인 feature point와 같음
• Stereo keypoints는 close keypoints와 far keypoints로 구분됨
  • Stereo keypoints $\textbf{x}_s = (u_L, v_L, u_R)$ 이며 stereo rectified image를 가정함(epipolar line이 horizontal)
  • RGB-D camera의 경우에는 depth 정보를 이용하여 $u_R=u_L-\frac{f_xb}{d}$ 로 함. 이 때, $f_x$는 horizontal focal length이고 $b$는 baseline임
  • Depth가 baseline의 40배 미만일 경우 close, 이상일 경우 far keypoint로 구분
  • Close keypoints의 경우 depth가 정확하다고 가정하며 scale, translation, rotation 등을 정확하게 추정하는데 좋음
  • Far keypoints의 경우 scale과 translation을 추정하는데는 좋지 않지만 rotation을 정확하게 추정하는데 좋음

[2.3] System Bootstrapping

• Depth 정보를 이용함으로써 structure from motion initialization이 필요 없음
• 첫 번째 frame을 keyframe으로 할 수 있으며 해당 frame에서의 pose를 origin으로 설정
• 모든 stereo keypoints에서 initial map 생성 가능

[2.4] Bundle Adjustment with Monocular and Stereo Constraints

• ORB SLAM 2에서는 다음과 같이 세 종류의 Bundle Adjustment (BA) module이 존재함
  • Motion-only BA : tracking thread에서 camera pose 추정을 위한 BA
  • Local BA : keyframes들의 local window (sliding window) 와 local mapping thread에서의 points 들의 optimization과 관련된 BA
  • Full BA : loop closure detection 이후 모든 keyframes optimization
• Motion-only BA
  • Camera orientation $\textbf{R} \in SO(3)$ 와 position $\textbf{t} \in \mathbb{R}^3$ 을 optimize
  • World coordinates 에서의 3D point $\textbf{X}^i \in \mathbb{R}^3$ 과 monocular keypoint $\textbf{x}_m^i \in \mathbb{R}^2$ 그리고 stereo keypoint $\textbf{x}_s^i \in \mathbb{R}^3$가 match 되었을 때 다음과 같은 최적화 문제를 푼다.
    • $\{ \textbf{R}, \textbf{t} \} = \underset{\textbf{R}, \textbf{t}}{\arg\min} \sum_{i \in \mathcal{X}} \rho (|| \textbf{x}{(\cdot)}^i-\pi{(\cdot)}(\textbf{R}\textbf{X}^i+\textbf{t}) ||_{\Sigma}^2)$
    • $\mathcal{X}$ 는 the set of all matches, $\rho$ 는 robust Huber cost function, 그리고 $\Sigma$ 는 keypoint의 scale과 관련된 covariance matrix이다.
    • $\pi_{(\cdot)}$는 projection function이고 monocular, rectified stereo projection function은 다음과 같이 정의한다.$(f_x, f_y)$ 는 focal length 이고 $(c_x, c_y)$는 principal point 이며 $b$는 baseline이다.
    • $\pi_m \Bigg( \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \Bigg) =$ $\begin{bmatrix} f_x \frac{X}{Z} + c_x \\ f_y \frac{Y}{Z} + c_y \end{bmatrix}$ , $\pi_s \Bigg( \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \Bigg) =$ $\begin{bmatrix} f_x \frac{X}{Z} + c_x \\ f_y \frac{Y}{Z} + c_y \\ f_x \frac{X-b}{Z} + c_x \end{bmatrix}$
• Local BA
  • Covisible한 keyframes $\mathcal{K}_L$ 과 이 keyframes들에서의 모든 keypoints $\mathcal{P}_L$ 을 optimize (sliding window 방식)
  • $\mathcal{P}_L$ 이 visible 하지만 $\mathcal{K}_L$ 에 속하지 않은 다른 keyframes $\mathcal{K}_F$ 는 cost function에 기여는 하되 optimization 과정에서 update 되지 않는다(fixed).
  • $\mathcal{X}_k$ 를 $\mathcal{P}_L$ 와 keyframe $k$ 간의 matches로 정의하면 Local BA는 다음과 같은 최적화 문제를 푼다고 할 수 있다.
    • $\{ \textbf{X}^i, \textbf{R}_l, \textbf{t}_l | i \in \mathcal{P}_L, l \in \mathcal{K}_l \} =$ $\underset{\textbf{X}^i, \textbf{R}_l, \textbf{t}l}{\arg\min}$ $\sum{k \in \mathcal{K}_L \cup \mathcal{K}F}$$\sum{j \in \mathcal{X}k} \rho \bigg(E{kj} \bigg)$
    • $E_{kj} = || \textbf{x}{(\cdot)}^j-\pi{(\cdot)}(\textbf{R}k\textbf{X}^j+\textbf{t}k) ||{\Sigma}^2$
• Full BA
  • Full BA는 Local BA와 식은 같고 모든 keyframes이 optimization에 참여한다.

 

참고자료

[1] Mur-Artal, R., & Tardós, J.D. (2017). ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras. IEEE Transactions on Robotics, 33, 1255-1262.