Optimization for SLAM

OpenJR·2023년 2월 9일

Optimization

모든 Optimzation method들은 어떠한 함수 $f(x)$ 가 최소가 되는 $x$ 의 값을 찾는 것을 목표로 한다.
보통 최적화 방법들은 Iterate하게 동작한다. 아래의 공식 처럼 $i$ 번 째 $x$ 의 함수 값 $f(x_i)$ 가 어디로, 얼만큼 이동해야 현재의 값보다 작아질지 구하고( $\bigtriangleup x$ ), 이동해 $f(x)$ 가 최소가 되는 $x$ 값을 찾는다.

x_{i+1} = x_i + \bigtriangleup x, ~~~~f(x_{i+1}) < f(x)

여기서 $\bigtriangleup x$ 를 어떻게 구하냐에 따라 method들이 파생된다.

Gradient Descent Method

Gradient Descent(GD)은 최적화 기법 중 하나로, 가장 간단한 방법이다.
컨셉은 간단하게 현재의 위치에서 기울기를 구해 (일차 미분), 아래를 향하는 방향으로 이동하는 방식이다.
여기서 기울기란, 어디로 이동할지 방향의 정보만을 제공하며 얼만큼 이동해야 하는지에 대한 정보는 주지 못한다.

J(x) = \frac{d}{dx}f(x)

x_{i+1} = x_i + (-\lambda J(x_i)), ~~~ \bigtriangleup x=-\lambda J(x_i)

$x_i$ 에서 $x_{i+1}$ 로 이동하기위해선 1) 어느 방향으로, 2) 얼만큼 이동할 것인지에 대한 정보가 필요하다.
GD는 단순히 현재 위치에서 내려가는 방향만을 가지고서 다음 step으로 이동하기 때문에 얼만큼 이동할 것인지( $\lambda$ )는 사용자가 결정해주어야 한다.
이 때, $\lambda$ 를 딥러닝 진영에서는 Learning rate라고 부른다.

Newton's Method

Gradient Descent Method의 다음 스텝에 얼마 만큼 가야하는지 모른다는 단점이 있었다. 하지만 Newton's Method(뉴튼법) 부터는 방향과 크기를 모두 알 수 있다.
뉴튼법의 원형은 $f(x)$ 가 최소일 때의 $x$ 값을 구하는 것이 아니라, $f(x)$ 가 0 일 때의 $x$ 값을 찾는 method이기 때문에 뉴튼법을 Optimzation문제에 적용하기 위해 Least Square를 적용해야 한다.
즉, $f(x)$ 가 최소일 때의 $x$ 를 구하기 위해 $f(x)^Tf(x)$ 을 미분했을 때, 기울기가 0이 되는 문제로 재해석 해야한다.
이제부터, $f(x)^Tf(x)$ 를 $F(x)$ 로 정의하겠다.
먼저, $F(x)$ 를 2차 테일러 급수로 근사화한다.

F(x+\bigtriangleup x)\approx F(x)+J(x)^T\bigtriangleup x+\frac{1}{2}\bigtriangleup x^TH(x)\bigtriangleup x, ~~~J(x)=F'(x), ~~H(x)=F''(x)

$f(x^\ast)$ 가 최소인 $x^\ast$ 에서 $F(x^\ast)$ 또한 최소이며, 이 때 $F(x)$ 는 일종의 2차 함수 이므로 $F'(x^\ast)=0$ 이다.

F'(x+\bigtriangleup x)\approx J(x)^T + \bigtriangleup x^TH(x) = 0

\bigtriangleup x = - H(x)^{-1}J(x), ~~~~H(x)^T=H(x)

Gauss-Newton's Method

뉴튼법에서는 Hessian행렬( $H(x)$ )을 사용해 방향과 크기를 알았지만, Hessian행렬을 구하는 것은 굉장히 Computational Cost가 많이 든다.
Gauss-Newton's Method(가우스 뉴튼법)은 Hessian행렬을 바로 구하지 않고 유사 Hessian행렬을 사용해 연산량을 줄인 방식이다.
테일러 급수를 사용하는 것은 뉴튼법과 동일하지만 $F(x)$ 를 근사화 하는 것이 아니라 $f(x)$ 를 근사화 하여 문제를 해결한다.

f(x+\bigtriangleup x)\approx f(x)+J(x)^T\bigtriangleup x, ~~~J(x)=f'(x)

F(x+\bigtriangleup x)= f(x+\bigtriangleup x)^Tf(x+\bigtriangleup x)=(\bigtriangleup x^TJ(x)+f(x)^T)(f(x)+J(x)^T\bigtriangleup x)

F(x)= f(x)^Tf(x)+2f(x)J(x)^T\bigtriangleup x+\bigtriangleup x^TJ(x)J(x)^T\bigtriangleup x , ~~~J(x)^T\bigtriangleup x=\bigtriangleup x^TJ(x)

F'(x)= 2f(x)J(x)^T+2H(x)\bigtriangleup x , ~~~H(x)=J(x)J(x)^T

뉴튼법과 마찬가지로

\bigtriangleup x = - H(x)^{-1}J(x)f(x)

이며, Hessian행렬을 구하지 않고, 단순히 Jacobian만을 사용해 연산량을 줄인 방법이다.

Levenberg-Marquardt method

이제야 이글의 본론이라고 할 수 있는 Levenberg-Marquardt Method (LM)이다. LM은 SLAM의 Backend Optimization에서 사용하는 method로 가우스 뉴튼법과 Gradient Method를 합친 방법이다.
원래는 Trust Region 개념도 설명해야 제대로 LM을 설명한다고 할 수 있지만, 단순히 유저 입장에선 가우스 뉴튼법과 Gradient Method를 합친 방식이라고 이해하고 넘어가면 된다.
식으로 바로 넘어가보자

Levenberg-Marquardt

\bigtriangleup x = - (H(x)+\lambda diag(H(x)))^{-1}J(x)f(x)

Gradient descent

\bigtriangleup x=-\lambda J(x)

Guass-Newton

\bigtriangleup x = - H(x)^{-1}J(x)f(x)

가우스 뉴튼법과 GD의 아이디어가 공식에 합쳐진 것을 볼 수 있다.
LM은 curvature 즉, 근사화 한 것의 곡률( $\rho$ )을 계산하여, 곡률이 크면(변화가 크면) GD를, 곡율이 작으면(변화가 작으면) 가우스 뉴튼법을 사용하는 방식이다.

\rho=\frac{f(x+\bigtriangleup x)-f(x)}{J(x)^T\bigtriangleup x}

LM의 Pesudo Code

for i in max_iteration:
	if rho > upper_threshold:
    	λ = 2λ
    else if rho < upper_threshold:
    	λ = 0.5λ
    calulate dx
    x_current = x_before + dx
    if x_current == x_before:
    	return x_current
    else:
    	x_before = x_current

OpenJR

Jacob

이전 포스트

Geocentric, Geodetic 차이

다음 포스트