[VSLAM]최소자승법

Least Squares(최소자승법)

SLAM

정의(복습)

동시적 위치 추정 및 지도 생성

Least Squares

정의

SLAM에서 사용하는 최적화 방법론 중 하나이다

최적화는 우리가 정답을 찾지 못하는 상황에서 모든 방정식에 대해 가장 적은 error 값을 찾는 수학적 기법이다

 

error 값을 제곱한다

비교적 높은 error 값들은 더 크게 다뤄지므로 

제곱 값을 줄이는 기법이 최소자승법이다

 

최소자승법은 over-determined system의 해를 푸는 방법으로

미지수의 개수보다 방정식의 개수가 더 많을 경우를 나타낸다

SLAM은 항상 이러한 시스템에 있다

 

 

하나의 상태 x를 추정하기 위해 실제 측정 값 z를 통해 

f(x)를 예측하고 이 값을 통해 상태를 예측한다

센서로부터 발생한 노이즈와 측정한 값으로부터 예측하는 모델이 물리 세계와는 달라

실제 값과는 차이가 있을 수 있다

 

실제 센서 값인 z와 기대하는 센서 값인 z Hat 값의 차이의 합이 x 값이 무엇인지 찾는 과정이다

 

 

Maximum A Posteriori(MAP, 최대사후확률) Estimation in SLAM

Motion/Observation model

Motion model

- Proprioceptive sensing의 수식으로 사용된다

현재의 위치와 관련된 상태를 추정할 수 있는 식이며 아래와 같은 식을 가진다

Xk-1은 이전 프레임의 state, Uk는 이전 state와 현재 state의 차이, 

이전 프레임의 state에 차이를 더하고 노이즈 값까지 더하면 현재 프레임 값을 알 수 있다

 

 

 

하나의 state는 x, y, theta 세 개의 변수로 표현이 가능하며 

각 시점마다 state가 존재한다

 

k-1=0일 때 세 변수가 모두 0일 때

(1,1,45도)입력을 준다

노이즈가 발생한 경구 (1.1, 1.1, 47도)와 같이 나타날 수 있다

Observation model

- Exteroceptive sensing의 수식으로 사용된다

observation model은 landmark의 위치와 관련한 state를 구한다

Xk는 현재 로봇의 state, Li는 landmark로부터 로봇과의 상대적인 위치인 landmark의 state, Ez는 error 값이다

 

2D LiDAR의 경우

관측 값으로 Distance와 Bearing을 가질 수 있다 

Distance의 경우 (x,y)의 거리와 error 값을 더해서 구한다

Bearing의 경우 x 값과 y 값에 대한 거리를 구한 후 arctan 함수를 통해 방향을 구하고

로봇의 현재 진행 방향 만큼 뺀 후 error 값을 더해서 구한다

SLAM State and Uncertainty

SLAM problem

SLAM은 state estimation으로 

각 state마다 하나의 로봇 pose 값과 여러 개의 landmark 위치 값을 가지고 있다

 

state 관점으로 

한 개의 motion state와 다수의 landmark state들이 매핑되어 있다

 

시간이 지나면서 로봇의 pose가 바뀌고 landmark의 위치 측정 값이 바뀐다

이때 SLAM 값이 더 생기는데

확률적으로 분석하여 최적의 로봇 pose와 landmark의 위치를 예측해야한다

 

SLAM은 motion과 observation 모두 노이즈가 있으므로

시간이 지날수록 수식이 많아져 노이즈가 누적되어 state의 불확실성이 커진다 

 

빨간색 영역이 uncertainty 영역으로 시간이 지날수록 더 커진다

motion에 대한 uncertainty와 observation에 대한 uncertainty가 서로 증가한다

 

Maximum A Posteriori(MAP) Estimation

SLAM scenario

센서 데이터들로부터 확률적으로 가장 믿을 수 있는 값을 belief라 한다

센서 데이터는 사후 데이터이므로 이 데이터를 가장 잘 표현하는 확률을 찾아야 한다

최대 사후 확률(MAP)을 구한다

 

 

MAP estimation을 풀기 위해

motion model과 observation model을 확률 분포를 찾고

확률 분포를 정확하게 나타내는 state를 찾는 것이 목표이다

 

- proprioceptive measurement

motion model에서 e가 0에 수렴한다는 전제 하에

state의 값과 측정 값이 매우 유사하게 나올 것이다

 

 

- exteroceptive measurement

e가 0에 수렴한다는 전제 하에

Zk,j는 h(x,l)과 유사하게 나올 것이다

 

 

목표는 measurement의 오차 값이 0에 수렴하기 위해

최소 값의 성분(state)을 구하는 것이다

왼쪽 항은 motion의 e의 제곱의 합, 오른쪽 항은 observation의 e의 제곱의 합이다

이 둘의 합은 총 error로 이를 제일 작게 하는 state를 구한다

 

 

 

Motion model only

Motion model만 최적화했을 경우

그린 경로가 완전히 닫히지 않고 예측한 랜드마크의 state가 서로 상이하다

Trusting both motion model and observation model

motion과 observation 모두 최적화 했을 경우

깔끔한 경로가 나타나고 랜드마크의 state가 안정적으로 나타난다

최소한의 error가 나타난다

이 기능이 SLAM의 핵심이라 할 수 있다

 

 

 

Graph-Based SLAM

Incremental vs Batch SLAM

How to implement least squares in SLAM

SLAM의 두 가지 방법론이 있다

- Incremental SLAM(Online SLAM): 주로 필터 기반 알고리즘을 사용한다

- Batch SLAM(Offline SLAM): 주로 그래프 최적화 알고리즘을 사용한다

Incremental SLAM

가장 최종 state만 추측하는 SLAM으로

Markov chain assumption을 전제 조건으로 한다

 

가정의  뜻은 이전 state의 정보는 새로운 state 정보에 녹아든다는 의미를 가진다

새로운 state를 구하기 위해서는 이전의 state 정보와 control 입력, observation 정보만 있으면 된다

 

계산량이 작으므로 빠르게 작동한다

최신 정보를 수집하므로 실시간으로 사용할 수 있다

 

Batch SLAM

여러 시점의 state를 한 번에 추론한다

동시에 여러 시점의 정보를 독립적인 확률 분포로 추론하기 때문에 

state 간의 drift error를 잘 해결할 수 있다 

하지만 더 많은 정보를 계산해야 하므로

과거에는 실시간 처리가 불가능 했다

 

Incremental vs Batch SLAM

Incremental SLAM은 빠르고 drift error 에 취약하고 부정확하다

 

2012년 새롭게 발표된 Visual SLAM: why filter? 논문에서

그래프 방법론이 훨씬 효율적이다는 결과를 발표했다

이후로 그래프 최적화 방식을 사용했다

strasdat_etal_ivc2012.pdf (ic.ac.uk)

 

Graph-based SLAM

Factor graph

그래프를 다루는 여러 방법이 연구된 후 이 방식이 자리 잡았다

로봇의 state와 랜드마크의 state가 node에 저장되고  

edge에는 motion model의 정보와 observation model의 정보가 저장된다

edge에 저장된 정보를 factor라 하며

node와 edge로 구성된 factor graph를 구한다

 

이 그래프를 최적화 하는 것은 특정 노드에 연결된 factor들의 error를 최소화하는 것을 의미한다

 

Graph-based SLAM의 정의

least 문제를 쉽게 해결할 수 있었다

장점은 로봇 pose와 observation의 link를 쉽게 파악할 수 있다

이를 통해 graph 내에 loop가 생긴 경우 

해당 loop에 대한 graph 최적화를 통해 

loop 내에서 생긴 uncertainty를 제거하고 

loop 내의 모든 node(로봇 pose와, landmark pose) 에 대한 최적의 값을 찾을 수 있다

 

loop closure를 통해 노이즈를 제거하여 SLAM의 범위가 매우 커졌다

 

Program flow of graph-based SLAM

SLAM의 파이프라인으로 

프론트엔드에서는 graph의 새로운 node와 edge를 추가한다

백엔드에서는 loop closure가 생기는 등의 graph 최적화를 수행한다

 

프론트엔드에서는 feature를 뽑고, motion을 구해 triangulation을 통한 노드 생성을 수행한다

백엔드에서는 최적화를 진행한다

 

 

 

Visual Odometry vs Visual SLAM

odometry는 누적된 drift를 직접 제거할 수 있는 방법이 없지만

SLAM은  loop closure등을 통해 최적화 할 수 있는 방법이 있다

Graph-based SLAM 시각화

SLAM을 실제로 돌릴 때 나타나는 값은 

계산을 통해 구한 값과 factor graph 표현하는 방식처럼 나타난다