Slam 4-3강 (Hierarchical Pose Graphs for SLAM) 요약

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

0. Today’s Goal

• Hierarchical Pose-Graph이란?
• Hierarchical Pose-Graph 설명
• Hierarchical Pose-Graph의 장점 및 고려사항

1. 강의 및 강의 자료

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

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

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

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

• Hierarchical Pose Graphs for SLAM (Cyrill Stachniss)
• SLAM DUNK Season 2 - Hierarchical Pose Graphs for SLAM

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

2. Hierarchical Pose-Graph이란?

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

Graph-based SLAM은 크게 Front-end와 back-end로 구분할 수 있다.

Front-end쪽에서는 센서에서 raw data를 받아, graph를 만드는 node나 edge를 만들고, back-end쪽에서는 front-end쪽에서 만든 node나 edge를 최적화하는 일을 진행한다.

Front-end와 Back-end는 서로 상호작용(Interplay)를 하면서 정교한 Graph를 만들어나간다.

그렇다면 Hierarchical Pose-Graph는 무엇일까?

영어 단어 Hierarchical(계층적)에서도 알 수 있듯이 Graph를 계층적으로 만들고 간략화하여 더 적은 수의 node와 edge로 최적화를 빠르게 수행하기 위한 구조이다.

위 그림이 Hierarchical Pose-Graph를 시각화한 그림이다. 먼저 Robot이 돌아다니면서 Input data로 부터 Graph를 만든 것을 bottom layer라고 하면, First layer에서는 bottom layer보다 node와 edge 수를 간략화하여 grah로 표현한 것이다. 마찬가지로 second layer에서는 node와 edge의 수를 더 간략화하여 graph를 표현한다.

Hierarchical Pose-Graph의 기본적인 Idea는 node와 edge의 수가 적으면 적을수록 최적화를 할 때 빠르며, Global한 map을 최적화할 때는 node의 수를 줄여 핵심적인 node와 edge 몇 개들만 최적화를 해도 충분하다는 것이다.

간단하지만 Key insight 중 하나는 우리가 하나의 observation이 관찰되었을 때, 모든 graph를 최적화시킬 필요가 없다는 것이다. Loop closing을 할 때도, 재방문을 한 node 주변 node들만 최적화를 진행하면된다. 얼마나 많은 주변 node들을 최적화 할 것인지는 주변 node들은 covariance matrix를 이용해 판단한다.

Hierarchical Pose-Graph의 방법을 사용할 때 주의해야할 점은 Robot이 순간이동(teleported)하지 않고, 자연스럽게 움직여야 한다는 것이다.

3. Hierarchical Pose-Graph 설명

그렇다면 어떻게 Hierarchical Pose-Graph이 동작하는지 강의 자료를 통해 차근차근 알아보자.

우선, 아래 그림처럼 Sensor data로부터 Dense한 graph를 만들어야 한다.

그 다음, 주변에 있는 node들끼리 local connectivity를 기준으로 그룹화를 진행한다. Local connectivity의 경우 Robot의 상황마다 일관된 기준을 사용하여 적용한다. 예를 들어, node와 node 사이가 50m 이상 차이가 날 때, 다른 그룹으로 취급을 하는 방식으로 그룹화를 진행하는 것이다.

그룹화를 진행한 다음에는 대표 node를 하나 정한다. 아래 그림에는 빨간색으로 표신된 것이 대표 node이다.

그 다음 상위 계층의 graph를 대표 node를 이용하여 다시 만든다. 이때 edge를 다시 만들어야 하는데, 상위 계층에서의 edge는 직접적으로 관찰된 값으로 만드는 것이 아니라, 그룹화한 node들끼리 연결성을 활용하여 계산을 진행한다.

이렇게 상위 계층의 graph에서는 조금 더 적은 node와 edge로 최적화가 가능해진다. 계층을 많이 만들고 싶으면 지금까지 했던 방법을 반복적으로 진행하면 된다.

그 다음 상위 계층의 graph에서 최적화를 진행한 모습을 아래 그림과 같이 나타낼 수 있다.

우리가 최적화한 빨간색 node는 하위 계층을 대표하는 node였기 때문에 최적화를 진행했을 경우, 최적화 결과를 하위 계층의 node들로도 전파(propagte)를 시켜줘야 한다.

여기까지 설명을 듣는다면 두 가지 궁금증이 생긴다.

1. 어떻게 상위 계층에서 edge를 계산하는 것일까?
2. 최적화 결과를 어떻게 하위 계층으로 전파를 시켜주는 것일까?

상위 계층에서의 edge 결정하기

앞서 설명을 할 때, “그룹화한 node들끼리 연결성을 활용하여 계산을 진행한다.”라고 설명을 했다. 구체적으로 어떻게 계산을 진행하는지 알아보자.

우리가 기본적으로 pose graph에서 edge를 결정하는 방법은 두 가지가 존재한다. 조금 더 자세한 내용은 SLAM 4-1강에서 “Graph-based SLAM 파헤치기” 부분을 참고하자.

여기서는 Observation-Based edge를 활용하여 edge를 결정한다.

아래의 그림에서 virtual observation $Z$와 두 node간에 Information matrix $\Omega$를 찾는 것이 우리의 목표이다. 어떻게 local node edge 정보를 결합하여 계산하는 것일까?

우선 node에는 기본적으로 각각의 pose들이 저장되어 있다. 따라서 빨간색 node에 있는 pose들을 활용하여 두 node의 상대적인 Transformation을 표현한다.

기본적으로 edge를 만들 때는 Information matrix $\Omega$를 활용해서 불확실성 정도를 표현했다.

여기서도 마찬가지로 두 node간의 uncertainty를 구해야 하는데, 이 방법도 SLAM 4-1강에서 “Uncertainty에 대하여” 부분에서 다룬 내용이다. 이와 같은 방법으로 Information matrix $\Omega$도 계산한다.

여기서 Information matrix $\Omega$, 선형화할 때 필요한 Matrix $H$, 그리고 covariance matrix와의 관계를 알고 있어야 이해를 쉽게 할 수 있다.

Information matrix $\Omega$는 두 node의 불확실성을 표현하기 위한 행렬이고, Matrix의 $H$를 정의할 때 Information matrix $\Omega$가 들어간다. 그리고 Information matrix $\Omega$의 역행렬이 covariance matrix이다.

따라서 강의자료에서는 두 node $x_a,x_b$의 Information matrix $\Omega_{ab}$를 아래처럼 표현하고 있다.

node $x_a$에서 바라본 불확실성을 Matrix의 $H^{-1}$의 (b,b)번째 성분을 값을 통해 알아내고 이를 다시 Information matrix 값으로 바꿔주기 위해 전체의 성분 값에 Inverse값을 구하게 된다.

이렇게 해서 상위 계층간에 edge를 계산하는 방법에 대해서 알아봤다. node들에는 robot의 pose가 각각 저장되어 있으므로, 그 값을 활용해서 기존의 pose graph의 edge를 만드는 방법과 동일하게 edge를 결정한다는 것을 알 수 있다.

최적화 결과를 하위 계층으로 전파하기

상위 계층에서 node들은 하위 계층에서 대표 node들이고, 대표 node들의 최적화는 자연스럽게 다른 node들에도 영향을 미친다. 최적화를 진행하면 node들의 이동(Shifting)이 생기게 되는데, 전파를 시킬때는 단순하게 대표 node들의 이동(Shifting)을 그대로 받아드려, 같은 그룹의 하늘색 node들에도 대표 node들의 이동만큼 움직이게 된다.

그 결과 아래 그림처럼 하늘색 node들끼리의 거리가 가까워질 수 있다.

물론 이 과정에서 추가적인 error가 발생할 수 있지만 여기서는 고려하지 않는다. 대표 node들의 움직임이 그렇게 크지 않기 때문에 critical한 문제는 발생하지 않기 때문이다. 물론 움직임이 얼마나 큰지 한번 알아봐야 할 필요는 있다.

4. Hierarchical Pose-Graph의 장점 및 고려사항

우리가 결국 정확한 Map을 만드려면 Lowest level에서 최적화를 진행하야 한다. 그럼에도 불구하고 이런 Hierarchical Pose-Graph를 활용하면 이점이 여러가지가 존재한다.

1. Initial guess가 좋지 않을 경우, high level graph에서 최적화를 진행하기 때문에 전체적인 수렴 속도를 빠르게 할 수 있다.
2. 모든 node들을 최적화를 하는 것이 아니기 때문에 계산량이 확실이 줄어든다.

실제 예시는 강의에서 설명을 하고 있으니 참고해보자.

우리가 Hierarchical(계층) 구조를 활용해서 graph를 만들 경우, 이는 전체적인 graph를 간략하게 한 것이기 때문에 가장 lowest graph와 얼마나 차이가 있는지 알아봐야 한다.

위 그림에서 왼쪽은 가장 lowest graph에서의 불확실성 정도이고, 오른쪽은 higher level graph에서 불확실성 정도이다. 표에서도 알 수 있듯이 higher level의 graph에서는 불확실성이 더 크다는 것을 알 수 있다.

따라서 Higher level graph는 approximation(근사)를 진행한 Graph라는 것을 명심해야 한다!

지금까지 말한 Hierarchical Pose-Graph를 한 장의 슬라이드로 정리하면 아래와 같다.