[SLAM] 27. ORB SLAM 논문 리뷰

https://arxiv.org/pdf/1502.00956.pdf

• ORB-SLAM1 : monocular
• ORB-SLAM2 : + stereo, RGB-D까지
• ORB-SLAM3 : imu 사용해서 tracking, factor graph 그리는 과정이 많이 달라졌다

0. Abstrack

• 실시간 feature-based monocular SLAM system
• small, large, 실내, 실외 다 가능
• motion cluter robust, loop closure, relocalization, 초기화 가능
• tracking, mapping, relocalization, loop closing을 하는데 사용되는 feature 동일
• mapping을 할 때 사용되는 point, keyframe 중에서 좋은 것만 남기는 전략이 있다 -> robustness, compact, trackable map

---

1. Introduction

bundle adjustment(BA)

• correspond가 잘 주어져있고 초기값이 잘 정해진 경우, camera localization과 sparse geometrical reconstruction(map)를 잘 추론한다
• 지금까지는 실시간으로 쓰기 어렵다고 생각했다. 실시간으로 되기 위해서는 아래와 같은 조건이 필요하다
  • 선별된 좋은 frame에만 써야한다 (keyframe)
  • keyframe이 많아지면 계산량만 많아진다
  • keyframe은 잘 spread 되어 있어야하고 signiiticant parallax, loop closure 되는 match가 많아야한다
  • 초기값이 있어야한다
  • local map에 대한 optimization 필요
  • global optimization(pose graph로 pose만 loop를 만들어서 최적화)

ORB-SLAM의 특징

• PTAM에 place recognition, scaled-aware loop closing, convisibility information for large scale operation을 추가

• tracking, mapping, relocalization, loop closing을 하는데 사용되는 feature 동일 (ORB feature 사용)
• covisibility graph를 사용해서 tracking, mapping 가능
• 실시간 loop closing : Essential graph 사용
• 실시간 camera relocalization 가능
• 새로운 automatic, robust 초기화

• direct 방식보다 성능이 좋다
• feature-based가 더 좋다

---

2. Related Work

A. place Recognition

• FAB-MAP
  • 이미지의 appearance based method
• DBoW2
  • bags of word
  • local feature을 모아서 global feature로
• ORB를 기반으로 DBoW2를 사용해서 place recognition를 한다

B. Map Initialization

• monocular SLAM은 inital map이 필요하다

intial map 구하는 방법 (reconstruction method)

• 방법 1. 두개의 이미지에서 바라본 feature point들로부터 correspondence를 구하여 honography 모델을 구해 inital map 생성 (map point가 하나의 평면에 존재)
• 방법 2. 5 point 알고리즘을 사용하여 essential matrix을 계산하여 R,t 계산

• low parallax 경우 안된다
• planar scene이 카메라 center와 가까이 있으면 twofold ambiguity가 나타날 수 있다
  • twofold ambiguity : 이미지 속의 사각형이 실제로 어떤 방향의, 어떤 모양인지 알 수 없다

• 방법 3. non-planar 상황에서 8 point 기반의 fundamental matrix를 사용해서 구할 수 있다

• planar의 경우 : homography 사용
• nonplanar의 경우 : fundamental 사용
• 이를 고민할 필요없이 automatic approach를 통해 구해준다.
• 둘다 계산해보고 좋은 방법을 알려준다

C. Monocular SLAM

filtering 또는 key frame 방식

• fitering 방식이 많이 사용되었다
• 매 frame마다 map, pose를 동시에 최적화했다
  • 이는 계산량 낭비
  • EKF의 경우 linearization error 누적으로인해 drift가 생길 수 있다
• key frame을 사용하는 방법
  • BA를 사용하여 정확하게 하지만 시간이 걸린다
• key frame이 filtering 보다 같은 계산량으로 정확도가 높다
• loop closure : pose graph 7DoF로 최적화를 계산 -> scale drift 해결

double window optimization

• 내부의 작은 window에서는 pose, landmark를 전부 최적화해준다
• 외부의 큰 window는 pose만 최적화

LSD-SLAM (direct)

• direct 방법을 사용하여 large scale semi-dense(line의 정보)를 생성
• feature를 사용하지 않았다
• 실시간
• loop detection을 위해서는 feature가 필요하다

semi-direct

• SVO
• feature를 처음에 뽑은 후 direct 방법 사용
• loop closure가 없다 (당연히...VO니까...)

ORB-SLAM의 방법

• 그래서 key frame 방법을 사용한다
• 7DoF pose graph optimization의 loop closing 방법 사용하여 Essential Graph에 적용
• double window optimization을 사용하지는 않지만 pose optimization과 covisibility 기반의 local map 사용
• SLAM을 할 때 최대한 많은 point를 가지고 있는 것은 좋지만 쓸데없는 key frame은 버리는게 좋다

---

3. System Overview

A. Feature Choice

• mapping, tracking, place recognition, frame-rate relocalization에 같은 feature를 사용
• 30fps 실시간으로 돌기 위해서 feature를 뽑는데 33ms 이하여야한다
  • SIFT 300ms
  • SURF 300ms
  • A-KAZE 100ms
  • 그래서 ORB 사용
• ORB
  • oriented multi-scale FAST corner
  • 256bits BRIEF descriptor
  • ORB 생성, 매칭이 빠르다
  • 좋은 invariance to viewpoint : 다른 각도에서 봐도 정확

B. 3 threads : tracking, local mapping, loop closing

tracking thread

• 매 frame마다 카메라 pose 구하고 keyframe인지 아닌지 확인
• 이전 frame과 feature matching을 간단히 한 후
• motion-only BA(motion만 최적화) 방법으로 pose 최적화
  • landmark는 고정, 최적화를 하여도 위치가 바뀌지 않는다
• tracking이 끊기는 경우
  • place recognition model 작동
  • DBoW2를 사용하여 global relocalization 수행
  • 잃어버린 위치를 다시 찾는다

• 첫 camera pose와 feature matching 찾으면 covisibility graph에서 지금까지의 모든 graph의 node와 edge를 가져와서 계산한다
  • covisibility graph : landmark를 보고 있는 모든 keyframe의 정보, key frame이 보고 있는 모든 landmark 정보들을 나타낸다
• 모든 가능성을 이미지에 reprojection하여 매칭을 수행하고 이를 다시 최적화
• keyframe인지 아닌지 구분

local mapping thread

• 새로운 keyframe 추가하고 local BA 수행
• 새로운 keyframe이 들어왔으니 아직 다른 keyframe들과 매칭이 안된 ORB feature를 찾아서 매칭을 만들어주고 새 map point를 만든다
• 3개의 keyframe 동안 여러 test를 수행하고 이 test를 통과하지 못하면 map point를 제거한다(culling)
• 또한 좋은 정보를 갖고 있지 않은 keyframe도 제거해 준다

loop closing

• 매 keyframe마다 loop인지 아닌지 확인
• loop인 경우
  • similiarity transformation(7DoF)을 찾는다 -> scale값을 확인할 수 있다, drift 누적을 알 수 있다
  • similiarity constraint를 가지고 loop fusion을 하고 pose graph 최적화를 한다
  • 이를 essential graph에서 수행한다
• 모든 최적화는 g2o에 있는 levenberg-marquardt 알고리즘을 사용
  • g2o : graph 형태의 nonlinear 함수들을 최적화할 수 있는 프레임워크

C. Map Points, KeyFrames and their Selection

• map point, keyframe 생성은 관대하게 삭제는 자비없이

map point $p_i$가 담고 있는 정보

• 3D position $X_{w,i}$
• viewing direction $n_i$
  • map point가 만들어졌을 때 어디서 보고 있었는지
  • keyframe을 저장해 놓는다 (ray)
• ORB descriptor $D_i$
• maximum $d_{max}$ and minimum $d_{min}$ 거리
  • ORB 피라미드를 사용했기 때문에 볼 수 있는 최대, 최소 거리

keyframe $K_i$가 담고 있는 정보

• camera pose $T_{iw}$
• camera instrinsic
• frame에서 추출한 모든 ORB feature

D. Covisibility Graph and Essential Graph

• covisibility 정보
  • 두 개의 keyframe이 있을 때 같은 landmark를 바라보고 있는지
• factor graph
  • node : keyframe
  • edge
    • 같은 map point를 보고 있을 때 keyframe 사이에 edge가 생긴다
    • 몇 개의 map point를 같이 보고 있는지에 따라 weight($\theta$)가 있다
• loop가 생겼을 때 loop를 최적화하기 위해 pose만 최적화하는 pose graph 생성
  • 이때 essential graph를 사용한다
• essential graph
  • covisibility graph인데 edge가 적다
  • 적어도 최소 100개 이상($\theta_{min}$)의 landmark를 동시에 바라보고 있는 경우만 남긴다

E. Bags of Words Place Recognition

• DBoW2 사용하여 loop detection과 relocalization 수행
• visual vocabulary
  • 사전에 offline에서 만들어진다
  • image dataset에서 ORB descriptor를 추출해서 만든다
• 이를 활용하여 이미지에서 ORB descriptor를 뽑고 clustering하여 히스토그램을 만들어서 하나의 이미지를 bag of visual words라고 표현

• keyframe마다 visual word를 dataset에 저장해놓기 때문에 database query를 할 때 효율적으로 할 수 있다
  • query : frame에서 BoW vector를 만들고 dataset의 BoW들 중에 가장 가까운 것을 얻는 것
• 문제는 가까운 keyframe이 너무 가깝다(view가 비슷하다) -> 단 하나의 high score가 나오지 않을 수 있다
  • keyframe 간에는 overlap이 존재하기 때문
• 이를 개선하기 위해
• covisibility graph 데이터를 전부 가지고 와서 75%가 넘는 match 결과를 하나로 취급한다

• ORB matching을 할 때 brute force 방식(전부 비교)이 아닌 ORB vector가 어떤 BoW vector에 속하는지 확인하고 비교하는 ORB vector에서도 BoW에 속하는 subset과만 비교를 수행 -> 좀 더 빠르다
• 새 point의 triangulating을 수행할 때, loop detection, relocalization을 수앻할 때 이 trick를 사용
  • 그 이유는 BoW vector를 만드는 것이 keyframe을 만들때만 추출하기 때문

---

4. Automatic Map Initialization

• map 초기화를 하는 이유
  • 두개의 frame 사이의 상대적인 pose를 통해 triangulation해서 첫 map point를 생성
• 두개의 geometrical model을 동시에 계산하는 것 제안
  • homography : planar scene
  • fundamental matrix : non-planar scene
• 이 둘 중에 하나를 고른다
• 좋은 결과가 나올 때까지 initailize를 허용하지 않는다
• low parallax, well-known twofold planar ambiguity 다 걸러낸다

avoiding to initialize a corrupt map

1. find initial correspondence
   • feature matching
2. parallel computation of the two models
   • homography : normalized DLT 방법
   • fundamental matrix : 8 point 알고리즘
   • RANSAC에 적용하여 계산
   • 각각 score 계산 -> high score은 keep
3. model selection
   • 
   • $R_H$ > 0.45이면 homography 선택
   • 특별한 원리가 있는 것이 아니라 실험적으로 만든 방법
4. motion and strcuture from motion recovery
   • homography의 경우
     • 8개의 결과가 나온다 -> cheriality test를 통해 하나의 결과를 뽑는다 -> 하지만 우리는 이 방법을 사용하지 않는다
     • 8개의 결과를 모두 triangulate한 후 가장 좋은 결과를 사용
   • fundamental의 경우
     • essential matrix로 변환 (calibration K matrix를 사용하여)
     • 4개의 결과가 나온다
     • 4개 전부 triangulate한 후 가장 좋은 결과를 사용
5. Bundle Adjustment

• full BA : landmark, pose 모두 최적화
• initialize map을 정확하게 뽑는다

---

5. Tracking

A. ORB Extraction

• 8개의 scale로 이미지 피라미드를 만들어서 FAST corner 추출
  • scale factor : 1.2
• 이미지 해상도에 따라 추출하는 corner 개수를 지정
  • 이미지 해상도가 512x384 ~ 752x480의 경우 1000개의 corner 추출
  • 그 이상이면 2000개
• 이미지 전체에서 골고루 feature를 뽑기 위해 grid를 나누고 cell마다 최소 5개의 corner 검출
• 그 후 corner orientation, ORB descriptor 계산

B. Initial Pose Estimation from Previous Frame

• constant velocity motion model 사용 (등속 운동)
  • 이를 통해 camera pose 예측
  • map point들을 reprojection해서 matching
• matching된 것을 활용하여 pose 최적화

C. Initial Pose Estimation via Global Relocalization

• 만약 tracking이 끊겼으면 keyframe을 바꾸고 BoW를 구해 database에 query를 해서 gloval localization 수행
• global relocalization 수행 -> 3D point 추출
• 이 3D point와 2D image와의 match를 통해 PnP 알고리즘을 RANSAC에 수행
• 그 결과 pose를 구하고 pose 최적화 수행

D. Track Local Map

• camera pose estimation을 얻고 나서 더 정확하게 trakcing하기 위해 더 많은 map point correspondence 구해서 pose 최적화 수행
• local map을 현재 fraem에 projection
  • 현재 보고 있는 map point를 가지고 있는 keyframe, 이것과 연관된 covisibility에서 연결되어있는 keyframe을 가지고 와서 모든 map point를 이미지에 reprojection 한다
  • 현재 frame에서 BoW를 구하고 database에서 BoW의 score가 가장 높은 keyframe을 선택 -> 이것과 관련된 map point 사용

과정

1. 현재 frame에서 map point를 projection한 $x$ 계산
   • 이미지 밖으로 벗어난 경우는 버린다
2. 현재 viewing ray $v$와 map point mean viewing direction $n$ 간의 각도 계산 -> 60도 보다 크면 버린다
3. camera center와 map point 사이의 거리 $d$ 계산
   • d가 최소 거리~최대 거리 안에 속하지 않으면 버린다
4. $d/d_{min}$ 비율을 통해 frame의 scale 계산
5. 아직 matching이 안된 ORB feature를 matching하여 correspondence를 만들어준다

• camera pose를 최종적으로 최적화
• 그 결과 relocalization, 다시 map correspond tracking도 유지할 수 있다

E. New Keyframe Decision

• keyframe인지 아닌지 구분

keyframe 조건

1. 마지막 global relocatlization을 하고 20 frame은 지나야한다
2. local mapping이 놀고 있거나(idle) 20 frame이 지난 경우
3. 현재 frame이 적어도 50 point를 track
4. 현재 frame이 reference keyframe의 90% 미만의 것을 track (새로운 것을 보고 있어야한다)

• 이 4가지 조건을 모두 통과하면 keyframe이 되고 covisibility graph에 들어간다
• local mapping이 진행중인 경우 새 keyframe이 나타나면 BA를 멈추고 keyframe 등록 -> 새 keyframe이 나타날때마다 새로운 BA를 한다

---

6. Local Mapping

A. Keyframe Insertion

• keyframe을 실제로 graph에 추가
• keyframe에 대한 BoW 계산

B. Recent Map Points Culling

• map points가 생성되고 나서 첫 3개의 keyframe 동안 restrictive test를 통과해야한다
• keyframe이 제거되면서 map point가 적어도 3개 이상의 keyframe에서 안보이면 그때 제거된다

2가지 조건

1. map point가 최소 25% 이상 다른 keyframe에서 보여야한다
2. map point가 생성되고 나서 적어도 3개 이상의 keyframe에서 보여야한다

C. New Map Point Creation

• 새 map point 생성
• covisibility graph에 있는 keyframe들끼리 triangulating하여 새로운 ORB landmark 생성
• epipolar line을 만족하지 못하면 제거
• positive depth, parallax, reprojection error, scale consistency가 확인된다

D. Local Bundle Adjustment

• covisibility에 있는 모든 keyframe과 그 모든 map point는 local BA
  

• 서로 연결되어있는 keyframe과 map point는 보정되고, map point를 바라보고 있지만 아직 연결되지 않은 keyframe은 최적화 계산에서는 포함되지만 보정되지는 않는다
• 최적화하면서 outlier는 버린다

E. Local Keyframe Cullint

• keyframe을 버리는 작업
• keyframe이 보고 있는 map point 중에 90% 정도가 다른 3개의 keyframe에서 보고 있다고 하면 제거 (겹치는 정보가 많기 때문)

---

7. Loop Closing

A. Loop Candidates Detection

• BoW를 계산하여 covisibility에서 연결되어 있는 것들(neighbors) 간의 similarity 계산하고 그중에서 가장 낮은 score $s_{min}$ 얻는다
• 이 $s_{min}$를 가지고 query
• $s_{min}$보다 낮은 keyframe은 버린다

B. Compute the Similarity Transformation

• 7DoF의 motion값을 구한다
  • 3 rotation, 3 translation, 1 scale
• query해서 가져온 keyframe의 3D map 정보와 현재 keyframe의 3D map 정보룰 통해 correspondense 계산해서 loop candidate를 구할 수 있다
• RANSAC를 진행하면서 similarity transformation을 구한다
• 현재 상황
  

C. Loop Fusion

• 1번을 2번으로 옮기는 작업
• 현재 3D point는 과거에 계산한 3D point이다
• 중복된 map point를 하나로 합치고 새 edge를 넣어준다

• loop keyframe에 있는 정보와 covisibility에 엮여있는 모든 keyframe에 대한 모든 map point를 reprojection하고(현재 loop keyframe에) match
• match된 것들은 같은 거라고 생각하고 합친다

D. Essential Graph Optimization

• essential graph를 가지고 pose graph 최적화만 하면된다
• 그럼 scale drift가 해소된다

---

8. 결과

• loop closure가 없으면 실시간에서는 좋지 않다