[2023-07-17] 최소자승법

최소자승법

Least Squares(최소자승법)

• 최적화 방법론 중 하나
  • 최적화 방법론 : 어떤 방정식들이 있고, 방정식에 완벽하게 떨어지는 정답값을 찾지 못하는 상황일 때 모든 방정식에 대해서 가장 적은 에러를 갖는 최적의 값을 찾는 수학적 기법과 방법론
• 에러 값을 제곱하여 취급하고 제곱 값이 최소가 되게 하는 방법
  • 비교적 높은 값을 갖는 에러는 제곱이 되어 더 큰 에러로 취급됨
• over-determined system(미지수의 개수보다 방정식의 개수가 더 많은 경우)의 해를 푸는 방법
  • SLAM은 거의 항상 over-determined system 문제
    • 풀려고 하는 state의 개수(랜드마크의 수, 카메라 포즈의 수)보다 센서 데이터가 항상 더 많기 때문

over-determined system

• 하나의 state x를 추정하기 위해 real sensor measurements z1, z2, … ,zn가 있음
  • z들은 feature 하나하나마다 sensor measurement라고 부를 수 있음

• 추정한 state x가 좋은 값이라고 하면 수식상으로 sensor 값들이 어떤 형태로 나와야 하는지 알 수 있음 -> 그런 수식을 f1(x), f2(x), …, fn(x)으로 표현

• f1(x), f2(x), …, fn(x)에 state 값을 넣어서 나온 추정한 센서의 값들을 z_hat_1, z_hat_2, …, z_hat_n 으로 표현(hat은 추정치를 의미)

• 수식에 state를 넣어 기대되는 값인 z_hat과 실제 센서 값인 z 값에는 차이가 있음

  • 센서에는 항상 노이즈가 있음

  • 수식상에 모델링 에러가 있을 수 있음 (실제 물리적인 현상과 약간의 차이가 있을 수 있음)

    e.g. Triangulation을 할 때 2개의 3d ray가 완벽하게 교차하지 않으니 F와 G의 점을 더하고 2를 나눠 H를 구하는 과정은 계산을 간단하게 하기 위해 실제 물리적인 현상을 무시한 부분으로 이러한 부분에서 모델링 에러가 조금씩 나타남

• 실제 센서 값인 z와 기대 센서 값은 z_hat의 차이는 error 값

• Least Squares는 모든 error 값들의 합이 최소화가 되는 x의 값이 무엇인지 찾아내는 것

Maximum-a-posteriori(MAP) estimation in SLAM

• 센서의 종류에는 2가지 : proprioceptive sensor, exteroceptive sensor
  • proprioceptive sensing 수식이 되는 Motion model
  • exteroceptive sensing 수식이 되는 Obeservation model

Motion model

• 현재 위치에 관련된 state를 추정할 수 있게 해주는 수식
• x_k-1: 이전 프레임의 state
• u_k : control(odometry)
  • 이전 state와 현재 state 위치의 차이
  • control : 제어의 관점 (전방으로 0.5m를 움직여라)
  • odometry : 측량의 관점 (odometry 센서가 0.5m를 움직였다 받아들임)

• e_u : motion noise

• 현재 state는 이전 frame의 state에 이동치를 더하고 노이즈 값까지 더한 것

Obeservation model

• 랜드마크의 위치와 관련한 state를 추정할 수 있게 해주는 수식
• x_k : 현재 로봇의 state
• l_i : 랜드마크의 state(랜드마크가 로봇을 기준으로 어디에 위치해 있는지, 상대적인 위치)
• e_z : sensor noise

SLAM state and Uncertainty

• SLAM은 state estimation 문제

  • 각 시점 마다 state를 갖게 되는데 SLAM state에는 하나의 시점에서 한 개의 로봇 포즈와 다수의 관측한 랜드마크 위치를 담고 있음

  • 시간이 지나면서 로봇이 움직이면서 로봇의 Pose가 바뀌고 랜드마크들에 대한 관측 값도 바뀌게 됨 -> SLAM State가 더 추가로 생성
    • 이때 생기는 Noise를 확률적으로 잘 분석하여 최적의 로봇 Pose와 랜드마크 Position을 계산하는 것이 SLAM의 목적
  • Motion과 Observation에 노이즈가 있어, 시간이 지날수록 Motion Model과 Observation Model에 대한 수식이 많아질 수록 Noise는 누적됨
    • State Uncertainty가 시간이 지날수록 늘어난다고 말함

• State Uncertainty가 증가하는 과정

Maximum-a-posteriori(MAP) Estimation

• Noise 패턴을 분석하면서 가장 최적의 로봇 Pose와 랜드마크의 위치 값을 추정하려고 함
  • Motion 모델과 Observation 모델의 정보를 기반으로 Noise 누적에도 불구하고 안정적으로 최적의 로봇 Pose와 랜드마크의 위치 값을 구하려고 하는 것
  • 통계적인 단어로 belief 라고 함
  • 센서 데이터는 관측 후에 나타는 결과(사후 데이터)이므로 사후 데이터를 가장 잘 표현하는 확률을 찾아야 함 (최대 사후 확률을 구하는 문제를 풀어야 함)
    • 최대 사후 확률 : Maximum-a-posteriori
• MAP Estimation을 풀기 위해서는 Motion Model과 Observation Model의 확률 분포를 찾고 확률 분포를 정확하게 표현하는 state의 값을 찾아야 함
  • e 가 0에 수렴한다고 가정하면 아래처럼 나옴
    • x_k - f(x_k-1, u_k) 가 0에 수렴하게 되고 e와 동일하게 됨 (e도 0에 수렴하니깐)
    • z_k,j - h(x_k, l_j) 가 0에 수렴하게 되고 e와 동일하게 됨 (e도 0에 수렴하니깐)
  • Map Estimation을 풀기 위해 e 값이 최소화가 되는 지점을 찾는 것이 목표임
    • motion model과 observation model의 total error가 최소가 되는 x0, …,xk 모든 state를 구함
    • Map Estimation을 해결하는 Least Squares 방법론 최적화 수식 : 그림의 맨 아래 식

Motion Model Only

• 예시로 Motion Model의 에러 값만 최적화 함
• Odometric Trajetory를 다음과 같이 그릴 수 있음
  • 각각의 Robot Pose들에서 Observation 식을 보고 Landmark의 위치들을 그려보면, Landmark의 위치들이 동일하지 않은 것을 확인할 수 있음

Motion Model + Observation Model

• 여러 개의 Cost function을 한번에 최적화해서 state를 구하는 것을 Joint Optimization이라고 함 (여러 파라미터를 동시에 최적화)

• Motion 모델도 정확해졌고 Observation 모델도 정확해짐

• Noise를 완전하게 정확하게 제거하진 못했지만, 최소화된 에러치를 가지는 state들(robot pose, 랜드마크 위치)을 계산해냄

• SLAM 기술의 코어

  • 동시적 위치 추정 및 지도 작성

    • 기존의 Localization 기술은 Observation이 완벽하다는 전제로 Observation 식을 고정한 상태에서 Motion Model만 최적화를 함

    • 반대로 Mapping 기술은 Motion이 완벽하다는 전제로 Motion 식을 고정한 상태로 Observation Model만 최적화를 함
    • SLAM은 Motion 모델과 Observation 모델을 Joint Optimization을 해서 훨씬 더 안정적인 결과를 냄

Graph-based SLAM

• SLAM에는 2가지 방법론이 있음
  • Incremental
  • Batch

Incremental

• Online SLAM

• 주로 필터 기반의 알고리즘 사용

  • Particle filter
  • Extended Kalman filter

• 가장 최종 state(newest state)만 추정함

  • Markov chain assumption : 이전 state들의 정보는 지속적으로 새로운 state에 녹아듬
  • 새로운 state를 계산하기 위해서 이전의 state 정보, control input, observation 정보만 있으면 됨
    • 계산에 들어가는 정보가 적으므로 굉장히 빠르게 작동하는 장점이 있음
    • 바로 최신 정보를 주므로 실시간으로 사용할 수 있어 Online SLAM이라고 불렀음

Batch

• Offline SLAM

• 주로 그래프 최적화 알고리즘 사용

• 여러 시점의 state들을 한번에 추론함

  • 동시에 여러 시점의 정보를 독립적인 확률 분포를 갖고 추론하므로 state들간의 drift error는 잘 해소가 됨
  • 많은 정보를 계산해서 전체적인 계산량이 많음

  • 실시간 처리가 불가능하다고 Offline SLAM이라고 불렀음

  • 다양한 트릭을 사용해서 현대에서는 Batch SLAM을 실시간으로 사용함

위 정보를 토대로는 Incremental SLAM이 Batch Slam보다는 빠르지만 drift error에 취약하고 부정확하다고 생각할 수 있음 하지만 Batch SLAM에서 사용하는 그래프 최적화 방법론이 Incremental SLAM에서 사용하는 필터 방법론 보다 동일 정확도를 얻어내기 위한 계산량을 비교했을 때 그래프 최적화 방법론이 훨씬 더 효율적이라는 논문으로 발표됨.

그 후부터 대부분의 VSLAM들이 Batch SLAM 방식을 사용함

Factor graph

• 그래프를 다루는 여러가지 방법들이 연구가 되다가, 현재로써는 Factor graph가 SLAM의 그래프를 다루는 방식으로 자리 잡음

• 노드와 엣지로 이루어진 그래프
  • 노드 : 로봇 state, 랜드마크 state
  • 엣지 : motion 모델, observation 모델의 정보 -> Factor라고 부름
• 이 그래프를 최적화 한다는 것은 특정 노드에 연결되어있는 Factor들인 motion 모델, observation 모델의 에러치를 이용해서 최적화 한다는 것을 의미함
• 굉장히 직관적인 구조
• SLAM에서 가장 유명한 표현 방식이며 다른 로보틱스 분야에서도 많이 사용됨

Graph-based SLAM

• Factor graph의 사용이 보편화되면서 Graph-based slam이 VSLAM이 주로 사용되는 방법이 됨
• Graph-based SLAM를 통해서 SLAM 문제의 Least Squares를 쉽게 적용할 수 있음
• 그래프로 표현하면 로봇의 Pose와 Observation에 대한 링크를 쉽게 파악할 수 있는 장점이 있음
  • 이를 이용해서 그래프 안에 루프가 생기게 된 경우 해당 루프에 대한 그래프 최적화를 함으로써 루프 속에서 누적된 uncertainty를 제거해주고 루프 안에 있는 모든 노드들(로봇 pose, 랜드마크 position)에 대한 최적의 값을 찾을 수 있게 됨 -> loop closure 기술
  • loop closure 기술로 인해 더 큰 환경에서 구동될 수 있을 정도로 노이즈를 효과적으로 해소할 수 있는 방법이 생기게 됨

Program flow of Graph-based SLAM

• SLAM 기술에 파이프라인이 생김

  • Frontend
    • Graph의 새로운 node와 edge를 추가하는 작업을 수행 (Graph Construction)
    • Feature을 뽑고 Relative Motion을 구하고 Triangulation 진행 (Triangulation을 한다는 것이 node를 생성한다는 것)
  • backend
    • loop closure가 생기거나 그래프 최적화 알고리즘 수행 (Graph optimization)
    • Bundle Adjustment, Loop Closure

Visual-Odometry vs Visual-SLAM

• 로봇 분야에서 Odometry와 SLAM에 차이를 두고 있음
  • Odometry는 누적되는 drift를 직접 제거할 수 있는 방법(글로벌 scale로써 graph optimization을 할 방법)이 없음
  • SLAM은 누적되는 drift를 직접 제거할 수 있는 방법(loop closure)이 있음