[SLAM] 20. Modern VSLAM

1. 이전 내용 복습

3D rotations

• 오일러 angle : roll, pitch, yaw로 회전 표헌
• axis angle : 회전 방향을 알려주는 axis가 있고 얼만큼 회전하는지에 대한 magnitude 값이 있다
• 쿼터니언 : 오일러의 짐벌락 문제 해결 / 복소수를 이용하여 4차원으로 표현
• SO(3) : 3x3 matrix로 회전을 표현하는 방법

• translation vector : x, y, z로 이동한다
• SE(3) : 4x4 matrix / SO(3) matrix에 translation vector를 합치고 homogeneous coordinate로 표현한 방법

coordinate system

• 유클리디안 space + cartesian coordiantes (x, y, z)
  • 현재 살고있는 물리적인 세상
• projective space + homogeneous coordinates (x, y, z, scale)
  • 유클리디안 space를 포함하는 조금 더 큰 차원
  • scale 정보 차원이 추가되었다

핀홀 카메라 projection

• 3D point에서 2D point로 매핑하는 과정
• intrinsic matrix
  • focal length (fx, fy) : normalized 이미지 plane을 픽셀에 대한 scale로 바꿔주는 역할
  • principle point (cx, cy) : normalized 이미지 plane 중앙에서 픽셀의 원점을 왼쪽 위 (0,0)으로 만들어 주는 목적
• extrinsic matrix
  • 3D rotation + translation
  • world에서 camera로의 transformation

Feature detection

1. keypoint detection
   • 2D local feature의 pixel position 찾기
   • 코너와 같은 점들이 keypoint가 된다
   • moravec -> harris -> SIFT -> FAST- -> ORB -> AKAZE 등등
2. descriptor extraction
   • 뽑힌 keypoint 주변 patch가 어떤 정보를 가지고 있는지 압축된 정보
   • SIFT -> BRIEF -> rBRIEF -> 등등
   • floating point descriptor(SIFT의 경우) vs binary descriptor(BRIEF의 경우)
3. correspondence matching
   • 여러개의 descriptor가 있을 때 같은 landmark를 가르키고 있는지
   • FLANN-based matcher : FLANN 라이브러리 사용
   • Brute-Force matcher

eipipolar geometry

• 2 view geometry
• epipolar line, epipole, epipolar plane, epipolar pencil
• essential matrix
  • 2D 이미지 간의 상대적인 rotation과 translation
  • normalized image plane에서 사용되는 것이기 때문에 픽셀단위에 적용하기 위해서는 fundamental matrix가 필요
• fundamental matrix
  • pixel plane

RANSAC

• framework 알고리즘
• minimum solver 함수가 같이 필요하다
• PROSAC : VSLAM에서 유용, descriptor distance응 prior로 활용
• Lo-RANSAC : inner RANSAC + optimization
• early stop : 좋은 결과가 나오면 빨리 끝낸다

• 과정

1. minimal dataset을 랜덤하게 추출
2. model 추정
3. model 평가
4. score를 비교하며 반복

trianglation

• 두 카메라 사이의 rotation과 translation 값 존재
• 연속되는 두 이미지에서 feature를 뽑고, descriptor를 뽑아서 매칭을 한 다음에 매칭이 된 feature를 가지고 RANSAC + essential/fundamenatl matrix를 통해 rotation과 translation matrix를 구한다
• 이 두개를 알고있고, 2D feature matching 정보를 알고 있을 때 3D landmark의 위치를 알아낼 수 있다
• general triangulation
  • 각각의 feature에 대해 ray가 존재
  • ray가 triangulation된다면 교차해야하지만 노이즈 때문에 교차하지 않는다
  • 따라서 최적의 F, G를 찾고 그 중간인 H를 교차하는 점이하고 근사하여 사용
• stereo triangulation
  • 쉽게 풀 수 있다
  • disparity(두 feature 사이의 거리)을 알고 있기 때문에 이를 활용

PnP

• trianglation를 통해서 3D landmark 즉, map을 만들었다
• 카메라가 map을 바라보고 있을때 나오는 이미지에서 뽑은 feature가 map point와 매칭이 되었을 때 카메라 pose를 알 수 있는 방법
• map을 보고 현재 위치 추정
• localization에서 사용
• P3P

Least squares

• over-determined system을 푸는 방법
  • over-determined system : SLAM은 항상
• motion model & observatioin model을 구하고 이를 joint optimization함으로써 SLAM 문제를 풀 수 있다
• 수식으로 표현하는 대신에 graph 형태(factor graph)로 문제를 쉽게 표현하고 풀 수 있다
• incremental SLAM vs batch SLAM
  • incremental : 최종단만 풀어내면서 실시간으로
  • batch : graph 전체를 한번에
• visual odometry vs visual SLAM
  • loop closure
  • odometry는 loop closure가 없다
  • SLAM : loop closure가 생겼을 때 loop를 닫고 loop안의 drift 해소, global consistancy를 갖는다

bundle adjustment

• landmark와 camera pose를 통합하여 optimization한다
• 수많은 ray들을 bundles of light ray라고 하고 이를 조정한다
• 이를 조정하기 위해 reprojection error를 cost function에 사용
• non-linear optimization (gauss-newton)
• 라이브러리를 사용하여 쉽게 쓸 수 있다 (cerres-solver, g2o, GTSAM)

Vocabs

local feature vs global feature

• local feature
  • 국소적인 것
  • keypoint와 descriptor : 이미지의 특정 부분을 대표하는 것 (전체x)
• global feature
  • descriptor들을 모아서 하나로 표현한 것
  • BoW와 같은 방법이 있다
• loop closure를 수행할 때 현재 이미지가 이전에 있던 이미지인지 확인하기 위해 global feature를 계산해서 사용한다

feature-based method vs direct method

• feature-based method
  • ORB를 추출해서 matching을 하고 ORB를 기준으로 landmark을 생성해서 map을 tracking하는 방식
• direct method
  • 이미지 자체에서 나타나는 밝기값으로 gradient를 사용
  • 처음부터 optimization을 잘해야하고 초심자에게는 어렵다
  • 학회, 논문에서 많이 사용되지만 실제로 사용하지에는 카메라 calibration도 복잡하고 제한적이라 쉽지 않다

Sparse vs Semi-dense vs Dense SLAM

• sparse SLAM
  • 우리가 사용하는 것
  • local feature들이 이미지 안에서 이어져서 나타나는 것이 아니기 때문에 sparse할 수 밖에 없다
• Semi-dense SLAM
  • 정보를 조금 더 많이 추출한다
  • line을 사용하는 경우부터 Semi-dense라고 이야기하기 시작 (point보다는 이어져있으니까)
• Dense SLAM
  • 이미지 데이터 전부를 사용 (모든 픽셀 사용)
  • 굉장히 무겁지만 outlier에 강인
  • 메모리 효율성, 계산 효율성에 대한 고민이 많이 필요하다
  • GPU를 사용하는 노력을 한다

---

2. Modern VSLAM Pipeline

기대하는 것

1. 여러개의 센서를 fusion할 수 있다
2. factor graph를 확장시킬 수 있는 다양한 전략이 있다
3. motion과 observation model을 동시에 최적화

3 types of computations

1. frame-to-frame tracking
2. sliding-window local map optimization
3. global map optimization / loop closure

• 1, 2의 경우는 시간 제약이 있다
• frame to frame 적어도 33ms에 끝나야하고
• local map도 적당히 빨리 돌아야 drift가 쌓이지 않는다고 해서 50~500ms에 한번씩 돌아야한다
• 하지만!global은 얼마나 걸릴지 모른다(그래프가 커질수록 오래걸린다) -> 동일한 시간에 끝내는 것을 기대하지 않는다 -> thread 하나에서 계속 돌리고 있는다

frame-to-frame tracking

• 매 frame마다 camera pose 계산
• construct map 만들지 않고 camera pose만 추정한다
• map은 고정되어 있다고 생각하고 그 map 기반으로 pose 추정 / 이전 frame의 pose와 현재 frame의 pose가 어떻게 이어질까에 대해 사용되는 경우가 많다
• 30fps라고 하면 33ms안에 모든 처리가 끝나야한다
• step
  1. color conversion (RGB -> Gray)
  2. resize immage : 연산을 빨리 하기 위해 / 보통 640x480으로
  3. detect feature : fearure, descriptor 뽑기
  4. match features with
     • previous frame (첫번째 방법)
       • 2D-2D correspondence -> E/F matrix 구한다.
       • 이를 활용하여 rotation과 translation 얻어낸다
       • 그 결과 이전과 현재 프레임의 이동치 즉, motion을 구할 수 있다
       • 최신 SLAM에서는 잘 사용하지 않는다
     • keyframe (두번째 방법)
       • 단순히 바로 전 frame과 비교하는 것이 아니라 이전의 key frame 사용
       • key frame을 생성하면 map point가 생성 -> map point란 이미 정확하게 3D landmark를 추정했다는 의미 -> 신뢰도가 높다
       • 3D map point의 descriptor와 현재 프레임의 2D descriptor간의 correspondence 구한다 -> PnP를 사용하여 pose 예측 (motion 추정)
       • 속도 빠르다
       • 최근에는 이 방식 많이 사용

sliding-window local map optimization

• 로봇이 움직이면서 factor 그래프는 시간이 지날수록 커진다
• H matrix가 엄청 커지고 이는 실시간으로 풀 수 없다
• SLAM에서는 실시간이 중요한데 PnP로 pose를 빨리 찾을 수 있지만 error가 잘 쌓인다
• 완벽하지 않기 때문에 보정이 필요 -> 그래서 bundle adjustment를 해야하는데 모든 frame을 다 하기에는 데이터가 너무 많다
• 그래서 최신 몇 frame만 사용하는 것은 어떨까

• 이전 N개의 keyframe에서 bundle adjustment(BA) 진행
• drift가 없지는 않지만 계속 최적화를 해주기 때문에 drift가 작다
• 새로운 keyframe(n개의 새로운 keyframe)이 들어올 때마다 수행
• 보통 50~500ms에 한번 (1초에 한두번)

• step
  1. 현재 이미지가 keyframe인지 확인
     • keyframe이라면 triangulate을 해서 새로운 3D landmark 생성
  2. 카메라 pose와 landmark를 통합하여 최적화
     • BA, non-linear optimization 사용

Global map optimization

• 딱 2가지 경우에서만 작동
  • loop closure 찾았을 때
    • drift가 쌓이고 있고 과거에 왔던 곳을 왔다면
    • 그때까지의 내용을 최적화 -> drift 해소, map과 pose에 대한 정보가 최적화
  • SLAM이 종료될 때 -> 전체 그래프를 최적화해서(유저는 기다린다) 보정해준다

• step
  1. loop 탐지
     • Bag-of-visual-words를 사용해서 loop 탐지
  2. pose 추정
     • loop를 찾았으면 과거의 위치와 동일한 현재의 위치가 얼만큼 차이나는지
     • PnP 사용
  3. loop 최적화
     • BA를 통해 최적화

---

3. Example (Mini SLAM)

초기화

• 카메라 on
  • 모든 이미지는 gray로 변환
  • resize : 640 x 480
• 2 image frame 선택
  • 유저가 직접 3D 공간을 다른 view에서 사진을 찍어서 만든다
  • 찍은 사진에서 AKAZE feature, descriptor를 뽑는다
  • FLANN-based matcher로 3D to 2D 매칭을 수행
  • RANSAC을 넣어서 fundamental matrix 추정
  • fundamental을 활용해서 상대적인 rotation, translation을 구하고 이를 가지고 triangulation 통해 첫번째 3D points들에 대한 map을 만들어 준다

frame-to-frame tracking

• 메 프레임마다 AKAZE feature, descriptor를 뽑는다
• 가장 최근에 뽑힌 key frame에 FLANN-based matching을 해서 2D-3D correspondence를 만들고자 한다
• P3P + RANSAC을 통해 카메라 pose 추정
  • 첫프레임이 항상 key frame이 된다
  • key frame에서의 3D map point에 대한 descriptor과 현재 이미지의 descriptor간에 수행

sliding wondow local map optimization

• 매번 30cm 움직었으면 그때마다 keyframe 생성
• 새로운 keyframe이 생성되면, 지난 keyframe, triangulation을 통해 새로운 landmark를 만들어준다

global window local map optimization

• loop closure할 때
• loop를 찾았으면 P3P와 RANSAC을 이용해서 카메라 pose를 추정
• 후에 loop를 만할 수 있다
• P3P : 과거의
• loop closure 수행
  • ceres-solver : BA 사용
  • gauss-newton method