Slam 4-4강 (Robust Least Squares for Graph-Based SLAM) 요약

Cyrill 교수님의 SLAM 강의를 듣고 정리해보자!

0. Today’s Goal

• Least Squares approach의 단점
• MaxMixtures or Dealing with Multiple Modes
• Dynamic Covariance Scaling
• Least Squares with Robust Kernels
• Summary

with Robust Kernels

1. 강의 및 강의 자료

강의 List는 SLAM KR의 SLAM DUNK seson 2에서 정해준 강의를 따라 정리를 할 것이다.

SLAM DUNK season 2의 Reference는 여기서 확인해 볼 수 있다.

SLAM KR에서 이 강의를 듣고 요약을 세미나 형식으로 준비를 해준다!

이번 포스팅은 다음의 강의들을 참고하여 요약을 해보았다.

• Robust Least Squares for Graph-Based SLAM (Cyrill Stachniss)
• SLAM DUNK Season 2 - Robust Least Squares for Graph-Based SLAM

Cyrill Stachniss의 강의 자료는 pdf로 다운 받을 수 있다.

2. Least Squares approach의 단점

이번 강의를 이해하기 위해선 SLAM 4-1강 (Graph-based SLAM using Pose Graphs)에서 배운 Graph-based SLAM에 대해서 이해를 하고 있어야 한다.

우리가 지금까지 배웠던 Least squares 방법은 error의 제곱의 합에 해당하는 값을 최소화시키는 방법이다.

이러한 방법에는 크게 두 가지 단점이 존재한다.

1. Outlier에 굉장히 취약하다는 것
2. Edge로 만드는 constraints가 Gaussian distribution을 따른다고 가정한다는 것

센서 모델의 경우 이러한 Gaussian distribution을 따르는 것이 나쁜 결과를 초래하진 않지만, 문제는 Data association이 애매모호한 경우, 이러한 Gaussian distribution은 현실 세계(real world)와는 다르기 때문에, 우리가 원치 않은 값으로 최적화가 이뤄질 수 있다.

Data association이 모호하게 나타나는 경우는 다음과 같은 경우에서 빈번하게 나타난다.

1. Landmark들이 모두 비슷하게 보일 때
2. 장소가 굉장히 유사한 곳을 mapping할 때
3. GPS의 신호를 활용하여 Data association을 만들 때

위 사진은 Data association이 모호하게 나타나는 경우의 한 예이다. 로봇이 비슷한 모양의 Landmark(검은색 직육면체)로 둘러쌓여 있을 때, 로봇의 pose에 대한 확률이 여러 곳에서 높게 나타난 것을 볼 수 있다.

따라서 이번 강의에서는 Data association이 모호할 때, 우리가 어떻게 Outlier를 처리하고 Robust한 Least squares approach를 적용할 것인지 다룰 것이다. 다양한 방법론에 대해 소개하는 식으로 강의가 진행된다.

3. MaxMixtures or Dealing with Multiple Modes

그 첫번째 방법은 바로 MaxMixtures or Dealing with Multiple Modes이다.

간단히 이 방법에 대해서 설명하면 Model을 정의할 때 하나의 Gaussian Model로 정의하는 것이 아니라 여러 모드로 일반화할 수 있는 방법이다.

우리가 앞서 배웠던 Pose graph에서 최적화할 부분을 Gaussian Model로 정의하고 수식으로 풀어쓰면 다음과 같다.

우리가 최적화를 할 때 자주 보던 $e^T_{ij} \Omega_{ij} e_{ij}$ 값에 Gaussian distribution만 적용한 것이다. $\eta$는 Gaussian distribution 식을 만들 때 사용한 상수값을 나타낸 것이다.

Error minimization을 진행할 때는 Log likelihood를 사용하기 때문에 식은 아래와 같이 변형된다.

지수(exponential)꼴로 복잡하게 정리됐던 식이 원소들의 덧셈으로 나름 아름답게(?) 정리가 된다.

우리가 multiple mode로 나타낼 수 있는 첫번째 방법은 서로 다른 Gaussian distribution을 합하는 방법이다.

그럼 식은 아래와 같이 달라진다.

위 식과의 차이점은 서로 다른 $k$개의 Gaussian distribution에 대해서 Weight인 $w_k$를 부여하고 모두 합쳐서 식을 정의했다는 것이다.

마찬가지로 Error minimization을 진행할 때는 Log likelihood를 사용하기 때문에 식은 아래와 같이 변형된다.

$\Sigma_k$가 있기 때문에 식을 정리하기가 까다롭다.

따라서 이 식을 그대로 사용하는 것이 아닌 대략적인 값을 활용하여 식을 재구성한다. 강의에서는 가장 중요한 mode의 값만 남겨두는 방식으로 식을 만든다고 이야기한다.

$\Sigma_k$ 연산자를 사용하는 것이 아닌 $max_k$ 연산자를 사용하여 Log likelihood를 사용할 때 식을 정리하기 수월하게끔 한다.

우리가 $max$ 연산자를 사용하기 때문에 방법의 이름이 “Max mixture” 이다.

위 그림 중 왼쪽은 두 Gaussian distribution을 나타낸 것, 가운데는 $max$ 연산자를 사용했을 때 Gaussian distribution, 오른쪽은 $\Sigma$ 연산자를 사용했을 때 Gaussian distribution이다. 오차가 조금 존재하지만 식을 간단하게 할 수 있기 때문에 $max$ 연산자를 사용해서 Gaussian distribution를 만든다.

따라서 Max mixture 식에서 Log Likelihood를 적용한 식은 아래와 같다.

Max mixture 방법의 장점

"Max mixture" 을 사용하면 우리가 기존에 해왔던 Error minimization을 서로 다른 Gaussian distribution에 따라서만 값을 바꿔주고 최소값을 찾으면 되기 때문에 우리가 기존에 사용했던 Error minimization식을 그대로 활용할 수 있다는 장점이 있다.

위 그림에서도 알 수 있듯이, 시간적인 측면에서도 기존의 Error minimization 방법과 차이가 얼마 나지 않는다는 것을 알 수 있다.

또한 Outlier에 굉장히 강인하게 작동할 수 있다. 서로 다른 모드를 적용하여 하나는 주요 constraints값으로 분포를 만들고, 다른 하나는 이상적인 Gaussian 분포로 만들어 outlier를 제거하는 방식을 사용한다. 따라서 Data Association이 모호할 때 사용하면 굉장히 효과적이다.

위와 같이 다양한 mode (빨간색, 파란색)에 대해서 서로 다른 분포를 만들고 Max mixture 방법을 사용한다.

4. Dynamic Covariance Scaling

두번째 방법은 Dynamic Covariance Scaling 방법이다.

간단히 설명하면 이 방법은 constraints를 구성할 때 다양한 weight값들을 활용하여 outlier에 강인하도록 Error minimization을 해 나아가는 과정이다.

보통의 Least squares는 아래와 같은 식을 활용한다.

Dynamic Covariance Scaling 방법은 이 식에 Scale term ($s_{ij}$)을 추가하여 아래와 같이 식을 만드는 것이다. scale($s_{ij}$)도 다음과 같이 정의한다.

각각의 observation마다 scale term을 추가하게 될 경우 outlier에 대한 값은 scale term을 통해 Error minimization을 하는데 적은 영향을 끼치게 할 수 있다. $s_{ij}$를 정의하는데 사용한
$\chi^2_{ij}$ 는 기존의 Least squares에서 사용하는 값을 나타내고,

$ \Phi$ 는 사전에 정의한 parameter의 값이다. $ min $ 함수를 통해 1과 계산된 값에 더 작은 값을 $ s_{ij} $ 으로 정의한다.

Error function을 시각화하면 아래와 같다.

검은색 line은 기존의 Error function, 파란색 line은 scale factor, 빨간색 line은 Dynamic Covariance Scaling 방법을 적용한 Error function이다.

파란색 값이 1일 경우, 기존의 검은색 line과 빨간색 line은 같은 값을 가지게 되지만, 파란색 값이 작아질 경우(중심에서 멀어진 값들), Error에 대한 값도 작아져 outlier에 강인하도록 식이 설계됐다.

달라진 기울기는 선형화, Jacobian을 만들 때도 영향을 미치게 된다.

강의에서는 Dynamic Covariance Scaling 방법은 다음에 소개할 방법인 Robust least squares estimation에 특별한 방법으로 이해하면 된다고 했다.

5. Least Squares with Robust Kernels

우선 Error function을 만들 때 이차식(squared term)으로 만드는 것이 문제라고 지적한다. Model이 Gaussian distribution이 아니라면 이차식으로 outlier에 강인하게 Error minimization을 하는 것이 힘들기 때문이다.

따라서 첫번째로 Robust M-estimator를 설계한다.

확률밀도함수(PDF)를 이용해 function을 재정의하고 이를 Log likelihood를 활용해 최적화를 진행한다.

Gaussian distribution일 때는 $\rho(e_i(x))$ 함수를 우리가 기존에 사용했던 이차식으로 사용하고 그렇지 않을 때는 다른 함수를 사용한다. 즉, 우리가 사용하던 식을 일반화 한 것이다.

따라서 이차식 이외에도 다양한 function들을 적용한다. 함수들의 종류는 아래와 같다.

• L1 norm
• Huber function
• Tukey, Cauchy…

L1 norm의 경우 절대값을 사용한다.

Huber function의 경우 아래와 같이 정의한다. 이차식과 선형식을 혼합한 모양이다.

다양한 함수를 시각화한 모습은 아래 강의자료에서 확인할 수 있다.

Robust estimation as Weighted Least Squares

다양한 robust estimation function은 기존의 Least squares 방법에서 Weight를 추가한 방법과 연관지어 생각해볼 수 있다.

우선 함수의 모양을 살펴보면 아래와 같다.

그리고 위 두 식의 편미분값을 구하고, 비교를 한번 해보자.

위 식에서 빨간색 부분을 제외하곤 모든 부분이 같다는 것을 알 수 있고, $\rho$ 함수를 잘 설계하면 Weighted Least Squares와 비슷하게 설계를 할 수 있다는 것을 알 수 있다.

여기서 내린 결론은 Weight를 잘 설계하면 Weighted Least Squares의 방법을 사용하여 Robust estimation을 활용할 수 있다는 것이다! 즉, Robust estimation과 Weighted Least Squares을 비슷하게 생각해도 무방하다.

Robust Least Squares를 정리하면 다음과 같다.

• Weighted Least Squares의 방법을 사용하여 Robust estimation을 활용할 수 있다.
• Kernel(즉, function의 모양)은 Jacobian에 영향을 준다.
• Kernel의 선택에 따라 Outlier를 잘 제거할 수 있다.

Generalized Robust Kernels

그렇다면 여기서 생기는 궁금증은 어떤 kernel을 선택하냐는 것이다.

답은 Outlier의 Type에 따라 최적에 kernel이 달라지기 때문에 Outlier의 분포를 보고 결정해야 한다.

첫번째 접근 방법은 Iteration을 돌 때마다, outlier을 제거하는 전략을 다르게 가져가는 것이다.

1. $N_1$ iteration에서는 확률밀도함수(PDF)의 끝 부분(tail)도 영향을 많이 끼치는 것을 고려(strong tail)를 해서 outlier 제거를 많이 진행한다.
2. $N_2$ iteration에서는 확률밀도함수(PDF)의 끝 부분(tail)도 영향을 적게 끼치는 것을 고려(weak tail)를 해서 outlier 제거를 적게 진행한다.
3. 어느정도 outlier를 제거했다고 가정하고 Gaussian distribution를 사용할 때 사용한 함수나 Huber function을 사용하여 최적화를 진행한다.

하지만 첫번째 방법은 별로 좋은 방법이 아니다. $N_1,N_2$를 어떻게 결정하는지도 Issue가 될 것이고 outlier 제거가 얼만큼 됐는지도 heuristic하게 결정하기 때문이다.

따라서 여기서 CVPR 2019에 accepted된 “A General and Adaptive Robust Loss Function” 방법을 소개한다.

이 방법에 대해서는 “A General and Adaptive Robust Loss Function” Youtube 영상 (in English)을 참고하면 조금 더 이해하기 쉬울 것이다.

이 논문에서는 $\rho$ function을 다음과 같이 제안한다.

$e$는 우리가 기존에 봤던 error 값을 의미하는 것이고,
$c$를 통해 우리가 어느 지점에서 이차식에서 어떤 function으로 바꿀지 결정하는 것이고,
$\alpha$를 통해 다양한 function을 선택할 수 있게 된다.

예를 들어, $\alpha$에 따라 함수가 L2 norm이 될 수도 있고, Huber function이 될 수도 있으며, 앞서 간단히 그림으로 살펴봤던 Tukey function이 될 수도 있다.

아래 그림은 $\rho$ function과 weight function을 시각화 한 그림이다. $\alpha$에 따라 function의 모양이 달라지는 것을 확인할 수 있다.

아래 그림의 Outlier distribution을 파란색 histogram(residuals)이라고 할 때, 다양한 $\alpha$에 따라 최적의 function을 만들 수 있는 것이 이 방법의 핵심이다. 최적의 function은 노란색 line으로 표현됐다.

따라서 위의 function을 활용하여 Least squares 방법과 결합하면 Robust한 Least squares approach를 제안할 수 있다!

이러한 방법 또한 최근 이뤄지고 있는 연구 중 하나이다. ICRA 2021 with RA-L에 소개된 논문이다. 마찬가지로 “Adaptive Robust Kernels for Non-Linear Least Squares Problems” Youtube 영상을 참고하면 이해가 더 잘될 것이다.

이 방법론에서는 최적화할 식을 아래와 같이 다시 설계한다.

앞서 $\rho$ function에서는 $c$도 parameter 였는데, $c$는 $\alpha$에 따라 결정되는 implicit parameter라고 설명한다.

위 함수를 활용해 확률 밀도 함수를 만들면 다음과 같다.

하지만 $Z(\alpha)$를 정의할 때 $(-\infty , \infty)$의 범위를 적분할 하게 되는데 $\alpha$의 값이 양수일 때는 수렴을 하지만, 음수일 때는 값이 발산하기 때문에 문제가 된다.

따라서 적분을 할 때 범위를 유한한 값으로 정해주는 trick을 사용한다.

이 방법을 사용하여 Least Squares를 실행할 경우, 실제로는 다음과 같은 문제들도 생각해야 한다.

• 새로운 Jacobian이 계산된다.
• $\alpha$ 값을 바꾸는 것이 parameter 추정을 하는데 많은 영향을 미친다.
• 초기값에 민감하다.

따라서 또 다른 방법을 제안한다.

EM-based optimization with Adaptive Robust Kernel이라는 방법인데 이 방법은 크게 2가지 step으로 이루어져 있다.
EM은 expectation maximization의 약자이다.

• E-step : parameter를 추정하는 단계이다.
• M-step : E-step에서 추정한 parameter를 활용하여 weighted least squares 문제를 최소화한다.

강의 슬라이드로 표현하면 아래와 같다.

이러한 Robust kernel 방법을 SLAM problem, ICP 알고리즘이나 Bundle adjustment에 적용하면 outlier에 강인하도록 알고리즘을 설계할 수 있다.

Cyrill 교수님의 말에 의하면 이러한 EM-based optimization with Adaptive Robust Kernel이 가장 outlier를 잘 제거하는 결과를 가져왔다고 이야기하고 있다.

6. Summary

강의가 굉장히 길었기 때문에 요약을 하자면 이번 강의에서 다룬 내용은 다음과 같다.

가장 중요한 것은 Outlier distribution의 모양은 무엇인가? 이다.

Outlier를 제거하기 위해 사용하는 여러가지 방법 중 첫번째 방법은 “MaxMixture”이다. 이 방법은 여러 Gaussian distribution을 합치고 계산의 편의성을 위해 $max$ 연산자를 사용하는 방법이다.

두번째 방법은 “Dynamic Covariance Scaling”이다. 이 방법은 kernel을 하나로 채택하고 고정해야 할 때 사용하면 좋은 방법이다. 이 방법은 Robust least squares estimation에 특별한 방법이다.

마지막으로는 “Adaptive robust kernel을 활용한 Least squares” 방법이다. 보통의 경우 outlier distribution을 모르기 때문에 이 방법이 제일 유연하게 여러 상황에 대처할 수 있는 방법이라고 설명한다. 또한 마지막에 설명했던 EM-based optimization이 가장 최근에 연구되고 제시된 방법이다.

이번 강의에서는 기본적인 내용 뿐만 아니라 최신에 연구되고 있는 논문까지 소개해주셨는데 강의의 난이도가 조금 있었지만, 알아두면 좋은 내용인 것 같다. 😊😊