Slam 4-1강 (Graph-based SLAM using Pose Graphs) 요약
Cyrill 교수님의 SLAM 강의를 듣고 정리해보자!
0. Today’s Goal
- Graph-based SLAM이란?
- Graph-based SLAM 파헤치기
- 1D example of Graph-based SLAM
- Uncertainty에 대하여
1. 강의 및 강의 자료
강의 List는 SLAM KR의 SLAM DUNK seson 2에서 정해준 강의를 따라 정리를 할 것이다.
SLAM DUNK season 2의 Reference는 여기서 확인해 볼 수 있다.
SLAM KR에서 이 강의를 듣고 요약을 세미나 형식으로 준비를 해준다!
이번 포스팅은 다음의 강의들을 참고하여 요약을 해보았다.
- Graph-based SLAM using Pose Graphs (Cyrill Stachniss)
- SLAM DUNK Season 2 - Graph-Based SLAM Using Pose Graph
Cyrill Stachniss의 강의 자료는 pdf로 다운 받을 수 있다.
2. Graph-based SLAM이란?
전반적인 SLAM system은 아래와 같은 슬라이드로 나타낼 수 있다.
센서에서 raw data를 받아 처리를 하는 부분을 front-end, front-end에서 만든 정보들을 활용해서 최적화를 진행하는 부분을 back-end라고 이해하면 된다.
Graph-based SLAM을 간단히 설명하면 SLAM back-end에서 Graph를 활용해서 최적화를 시키는 방법을 이야기한다.
Graph는 예전 학부 수업 자료구조 시간에 들어봤었는데, 그 때 사용한 graph와 같은 의미이다.
즉, SLAM system을 node와 edge의 결합으로 생각하고 이를 정교하게 풀어나아가는 것이라고 이해를 했다.
최적화를 수행할 때는 우리가 앞서 배웠던 Least squares 방법을 사용한다.
최근 SOTA를 찍은 SLAM 알고리즘들을 살펴보면 모두 이 graph-based slam을 사용한다. 그럼 이렇게 graph를 활용하여 slam system을 최적화를 진행하게 된다면 가지는 장점은 무엇일까?
강의 초반에서는 2가지의 장점이 있다고 이야기한다.
- Observation에 대해 유연하게 대처할 수 있고, 관리하기 쉽다.
- Pose를 활용해서 graph를 만든다면 랜드마크(Land mark) 없이 map을 만들 수 있다.
간단한 모바일 로봇의 예로 graph-based slam을 이해해보자.
위 그림처럼 모바일 로봇이 environment를 움직인다고 했을 때, 기준이 되는 특정 시간마다 로봇의 pose를 표현 (파란색 삼각형) 할 수 있고, pose와 pose 사이를 constraints (주황색 점선 화살표)을 활용해서 표현할 수 있다.
이 예제에서 constraints, 즉 변하지 않는 값은 모바일 로봇의 회전 바퀴 수(wheel odometry)로 설명을 했다.
2D로 가정을 할 때, XY 좌표값과 로봇의 heading point가 robot pose에 대한 정보가 된다. robot pose는 variable로 변수로 취급을 하지만, constraints는 변하지 않는 상수 취급을 한다. 하지만 constraints인 wheel odometry가 불확실성이 있다는 것도 여기서 고려해야 하는 것 중 하나이다.
로봇이 이렇게 environment를 돌아다니게 되면 위와 같이 이전에 방문했던 곳을 다시 방문하는 상황이 나타날 있게 된다.
이러한 상황이 SLAM system에서 굉장히 중요한 요소인데, 이전에 지났던 장소를 다시 지나게 되면서 같은 지역을 재방문하면서 새로운 pose와 constraints 간의 관계를 만들 수 있다.
이러한 과정을 Loop Closing이라고 한다.
이렇게 재방문하면서 우리는 가상의 constraints를 만들 수 있다. 예를 들어 지난 시간에 배웠던 ICP algorithm을 활용해서 map point들을 alignmemt하는 과정에서 pose들을 계산할 수 있고, 현재 설명하고 있는 모바일 로봇의 같은 경우, 현재 pose에서 내가 원하는 pose까지 얼마나 가야하는지 wheel odometry를 계산할 수 있기 때문이다.
지금까지 설명한 Graph-based SLAM을 한장의 슬라이드로 표현하면 아래와 같다.
Graph base SLAM의 목적은 edge 값들을 활용해서 node 값들을 정교하게 계산하는 것이다. 다시 말해서 edge값을 이용해서 error function을 만들고 앞서 살펴본 Least squares 접근법을 활용해서 최적화를 진행하게 된다.
3. Graph-based SLAM 파헤치기
Edge를 만드는 방법
이 강의에서 사용하고 있는 notation에 대해 간단히 설명하면 $n$개의 node가 있다고 했을 때 $x =x_{1:n}$으로 node들을 표현하고, i번째 node인 $x_i$는 $i$번째 time에서의 robot의 pose를 의미한다.
Edge는 2가지 방법으로 만들 수 있다.
- Odometry-Based edge
- Observation-Based edge
Odometry-Based edge는 위 예제에서 설명한 방법과 같다. odometry 정보를 활용해서 $x_i$에서 $x_{i+1}$로 갈 때 constraints를 가지고 edge를 만든다.
Observation-Based edge는 Lidar나 카메라와 같은 센서를 활용해서 로봇이 다른 위치인 $x_i$, $x_j$에서 같은 환경에 대하여 observation값을 구하고 observation 값을 겹쳐서 $x_i$, $x_j$의 관계를 계산하고 이를 통해 edge를 만드는 방법이다.
위 설명한 두번째 방법을 강의 슬라이드에 나와있는 그림으로 표현하면 아래와 같다. 강의에서는 2D lidar 센서를 활용해서 observation 값을 얻어내는 방법으로 예를 들었다.
$x_i$, $x_j$에서 Laser scan을 활용해 검정색 scan과 파란색 scan 값을 얻은 후,
두 scan값을 겹쳐 $x_i$, $x_j$의 관계를 계산한 값으로 edge를 만든다.
Transformation을 표현할 때는 Homogenous Coordinates를 이용한다.
Homogenous coordinate를 활용하면 edge를 하나의 행렬로 비교적 간단히 표현할 수 있다는 장점이 있다. 예를 들어 3차원 공간에서 로봇의 pose를 4x4로 표현하고 pose들의 관계는 역행렬을 활용해서 계산할 수 있다.
앞서 설명했다시피 Edge는 constraints이다. 하지만 불확실성이 존재한다. 따라서 Information matrix (강의에서 $\Omega$로 표현)를 활용해서 edge값에 대한 신뢰도를 알려준다. 즉, Information matrix값이 크면 edge 값이 비교적 정확하다는 뜻이고, information matrix값이 작으면 edge 값이 부정확하다는 뜻이다.
Information matrix는 covariance matrix의 역으로 구한다.
Pose graph
위 그림을 이해한다면 pose graph를 이해한 것이다. 😄
현재 graph에서 $x_i$와 $x_j$는 각각 node들을 의미한다. 두 node들을 잇는 edge를 계산하기 위해 ICP와 같은 알고리즘을 사용해서 observation 값을 구하게 된다.
edge에 들어가있는 $<z_{ij},\Omega_{ij}>$에 대해서 설명해보자. $z_{ij}$는 observation 값을 통해 $x_i$에서 바라본 $x_j$의 값이다. 이때 불확실성이 있기 때문에 Information matrix $\Omega_{ij}$를 활용해서 불확실성 정도를 표현했다.
여기서 우리가 최소화 해야할 값이 생긴다. 우리가 Observation을 통해 구한 값이 기존에 node $x_j$와 차이가 발생했기 때문이다. 이에 대한 Error를 $e_{ij}(x_i, x_j)$로 표현했다.
우리가 최소화 해야할 error function을 수학적으로 쓰면 아래와 같다.
이때 Least squares 방법을 활용하여 최적화를 진행한다. $e_{ij}$를 구성하는데 단지 2개의 node들을 활용하면 되기 때문에 변수가 적고, SLAM system에서 각각 node들과 edge들이 독립적이기 때문에 효율적으로 최적화를 진행할 수 있다.
여기서 state vector $x^T$는 다음과 같이 정의한다.
\(x^T = (x^T_1, x^T_2, x^T_3 x^T_4 ... x^T_n)\)
하나의 원소 $x^T_n$는 하나의 node를 의미하고 이는 로봇의 pose에 해당한다.
Error function을 조금 더 뜯어보면 다음과 같다.
$Z_{ij}$는 측정값으로 센서를 통해 $x_i$에서 바라본 $x_j$의 값을 의미하고, $(X^{-1}_i X_j)$는 node에 저장된 값을 활용해서 $x_i$에서 바라본 $x_j$의 값을 의미한다.
$t2v$는 Transformation to vector라는 함수를 의미한다.
Gauss-Newton 방법을 활용한 최적화
Error minimization을 할 때는 Gauss-newton 방법을 활용한다. 과정을 살펴보면 다음과 같다.
- Error function 정의
- Error function 선형화(Linearize)
- Jacobian을 활용하여 미분값 계산
- 미분값 0으로 만드는 값 찾기
- 선형 시스템 해 구하기
- Iteration을 통해 최적의 값 찾기
하나의 error function은 단지 두 개의 node와 하나의 edge를 활용해 만들어지기 때문에 우리가 계산한 Jacobian은 sparse하게 값을 가지게 되고 대부분 0의 성분을 가지고 있게 된다.
시각화를 위해 0인 성분을 파란색으로, 0이 아닌 성분을 빨간색으로 표현하면 아래와 같다.
이것이 중요한 이유는 Sparse한 Jacobian은 Sparse한 $H$와 $b$를 만들어내기 때문이다. $H,b$는 우리가 선형화할 때 이용하는 값을 의미한다. 그림으로 표현하면 아래와 같다.
지금까지 하나의 원소에 대해서만 이야기를 했고, 모든 원소를 더하면 아래와 같이 나타낼 수 있다.
Matix $H$가 sparse하기 때문에 우리는 Sparse Cholesky decomposition, Conjugate gradients 등 다양한 방법을 활용해서 보다 효과적으로 계산을 할 수 있다.
4. 1D example of Graph-based SLAM
지금까지 설명한 Graph-based SLAM의 전체적인 방법을 간단한 예로 이해해보자.
위 그림과 같이 node $x_1$에서 node $x_2$까지 로봇이 1m 움직였다고 가정해보자. 따라서 $z_{12} = 1$로 쓸 수 있다.
초기값을 $x = (0, 0)$으로 두고 필요한 값들을 계산하면 아래의 슬라이드와 같다.
자세한 설명은 이 부분의 강의를 직접 듣는 것을 추천한다.
여기서 의아한 점은 $\varDelta x$를 구하려면 $H^{-1}$의 값이 필요한데, $det(H) = 0$이 나온다는 것이다!
이런 상황이 발생한 이유는 $z_{12}$의 값이 상대적인 constraint만 표현하기 때문이다.
즉, Global coordinates에서 볼 때, 기준이 하나 있어야 한다. $x_1$이 어디있는지 아니면 $x_2$가 어디있는지 알아야 한다.
첫번째 node를 fix하기 위해 constraints를 더해준다. 이 말은 첫번째 node가 중요도가 더 높다는 것을 수식적으로 표현한 것이다.
[SLAM] Graph-based SLAM (Pose graph SLAM)을 참고해보면 “information을 더해서 첫번째 node의 uncertainty를 낮춰준다.”라고 설명하고 있다.
따라서 $\varDelta x$는 아래와 같이 구할 수 있다.
Global coordinates을 정해주는 것이 중요하고, Global coordinates를 바로 정하지 못할 경우, 하나의 Node에 대해 uncertainty를 낮춰주는 작업이 굉장히 중요하다는 것을 알 수 있다.
5. Uncertainty에 대하여
지금까지 기본적인 Graph-based SLAM에 대해서 알아봤다. 우리는 이전에 알고 있는 정보(priori)를 활용해서 system을 만들어야 한다.
바로 앞 예제에서도 살펴보았듯이 node $x_i$의 global cooridinate에서의 위치나 node $x_i$의 Information matrix 값을 조금 더 높게 한다는지 등등 어떠한 variable에 대해선 Fix된 값을 사용해야 한다.
수식적으로 이러한 문제는 간단히 Matrix를 만들 때 해당하는 행과 열을 지우고 최적화를 진행하면 된다. 예를 들어, node $x_0$의 값을 Fix하고 Gauss-newton method를 진행할 때, $H,b$를 만들 때 $x_0$값과 연관된 행과 열을 지우고, 부분적인 $H,b$를 만들어 최적화를 진행하는 것이다.
이렇게 되면 고정된 node $x_0$을 기준으로 graph optimization이 이뤄질 것이다.
Uncertainty를 계산할 때는 $H$ matrix를 이용한다. $H$의 역행렬이 covariance matrix로 생각할 수 있기 때문이다. ($H$를 만들 때 Information matrix 를 이용하기 때문이라고 생각한다.)
따라서 $H$의 역행렬의 대각 성분을 통해 각각 variables의 uncertainties를 알 수 있다.
지금까지 이야기 했던 것들을 통해 node $x_i$와 node $x_j$의 uncertainty를 구할 수 있다.
- Full Matrix $H$를 계산한다.
- node $x_i$을 기준으로 삼기 위해서 $x_i$의 행과 열을 제거한다.
- $x_i$의 행과 열을 제거된 Matrix $H$의 역행렬을 구하고, $(j,j)$ 성분 값을 계산한다.
이러한 과정은 Loop closure에서 이용할 수 있다.
이렇게 해서 Graph-based SLAM using pose-graph와 관련된 포스팅을 마무리하겠다. 이를 통해 pose graph based SLAM이 어떻게 작동하는지 간단히 이해할 수 있었다. (힘들었다.. 😄)
댓글남기기