Slam 4-2강 (Graph-based SLAM with Landmarks) 요약

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

0. Today’s Goal

• The graph with Landmarks
• Landmarks Observation
• Landmark를 통한 로봇 위치 파악

1. 강의 및 강의 자료

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

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

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

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

• Graph-based SLAM with Landmarks (Cyrill Stachniss)
• SLAM DUNK Season 2 - Graph-Based SLAM Using Pose Graph

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

2. The graph with Landmarks

SLAM 4-1강 (Graph-based SLAM using Pose Graphs)에서 배운 Graph-based SLAM에서는 로봇의 pose만을 node로 생각하고 최적화를 진행했다.

간단히 Pose graph SLAM에 대해서 이야기해보면, graph를 활용해서 SLAM의 문제를 해결하려는 방법이다. 모든 node는 로봇의 pose를 의미하고, 모든 edge는 node 사이에 observation값을 표현하고 constraints이다. 하지만 edge에도 불확실성이 있기 때문에 이를 잘 판단하여 최적화를 진행해줘야 한다. 불확실성은 Covariance matrix를 통해서 판단했다.

그렇다면 로봇의 Pose 뿐만 아니라 map에 있는 Landmark를 Graph로 표현하려면 어떻게 해야할까?

위 사진은 실제 환경에서 Lamdmark를 활용하여 Map을 만든 예시이다. 유명한 Victoria Park dataset의 한 장면이다. 자동차가 공원 주변을 돌아다니면서 차량에 부착되어 있는 laser 센서를 활용하여 나무들을 tracking하고 나무들의 위치를 추정한 것을 나타낸다.

Landmark까지 고려된 Graph를 간략하게 그림으로 표현하면 아래와 같다.

파란색 화살표인 로봇 pose를 의미하는 것이고 위치정보(2차원인 경우 X,Y 정보, 3차원인 경우 X,Y,Z 정보)와 방향정보를 포함한다. 다만 검은색 별로 표신된 Landmark의 경우 위치정보만을 포함하고 있다.

그러므로 pose와 pose 사이를 연결하는 edge와 pose와 feature(Landmark) 사이를 연결하는 edge는 각각 서로 다른 정보를 가지고 있다.

정리를 해보면 다음과 같다.

Node로 포현할 수 있는 것

• Robot pose(위치 정보 + 방향 정보)
• Landmark의 위치

Edge로 표현할 수 있는 것

• Landmark observations (관찰값)
• Odometry measurements (측정값)

따라서 이 Grpah에서는 최적화 해야할 대상이 Landmarks와 robot poses, 크게 두 가지가 있다.

3. Landmarks Observation

Landmarks가 node로 추가되면서 pose와 feature(Landmark) 사이를 연결하는 edge는 어떻게 구성되어 있을까?

Landmarks observation에는 두 가지가 존재한다.

• $(x,y)$ sensor observation : 위치 정보에 대한 edge
• Bearing Only Observations : 방향 정보에 대한 edge

강의에서 설명을 할 때 2차원 평면을 기준으로 설명을 진행했다.

$(x,y)$ sensor observation

$(x,y)$ sensor observation을 수식으로 쓰면 다음과 같다.

Pose와 pose 사이를 잇는 edge와 형태는 유사하지만, 벡터의 차원이 다른 것에 주의해야한다.

2D 평면을 기준으로 Robot pose는 방향과 위치를 나타낼 때 $(3,1)$ 벡터로 나타내고, Landmark의 경우 위치만 나타내기 때문에 $(2,1)$로 나타낼 수 있다.

따라서 Robot pose에서 translation($t_i$)에 대한 값만 가져온 다음, landmark($x_j$)와의 차이를 구하고 Robot pose의 rotation matrix의 역행렬($R^T_i$)만큼 곱해주는 방식으로 Edge를 만들게 된다.

Edge가 다르게 정의됨에 따라 Error function도 다르게 정의되게 되는데 수식은 아래와 같다.

우리가 예측한 observation $\hat{z_{ij}}$에 실제로 관찰된 observation $z_{ij}$의 차이만큼을 error로 만들게 된다.

Bearing Only Observations

Bearing Only Observations는 방향 정보를 가지고 있는 edge이다. Landmark를 바라볼 때 Robot이 어떤 방향으로 바라봐야 하는지 알려준다고 이해하면 된다.

수식적으로 표현하면 아래와 같다.

2D 평면을 기준으로 방향에 대한 정보 $\theta$만 필요하기 때문에, 1차원 벡터로 표현할 수 있다. 수식에서 $(x_j - t_i).y$는 landmark($x_j$)와 Robot pose에서 translation($t_i$)에 $y$성분에 차이를 의미하고, $(x_j - t_i).x$는 landmark($x_j$)와 Robot pose에서 translation($t_i$)에 $x$성분에 차이를 의미한다.

마찬가지로 Error function도 바뀌게 되는데 수식으로는 아래와 같다.

4. Landmark를 통한 로봇 위치 파악

Pose graph와 마찬가지로 Gauss-newton 방법을 사용하여 최적화를 수행하게 된다. 따라서 자연스럽게 Matrix $H$를 구하게 된다.

$H$는 Jacobian matrix와 Information matrix의 곱으로 만들어지는데, 수식은 $H = J^T\Omega J$로 정의하게 된다. 표현의 통일성을 위해 SLAM 2강 - Gauss netwon method 부분에서 쓰던 정의 및 표기를 그대로 사용했다. (강의에서도 마찬가지다.)

여기서 Cyrill 교수님읜 Matrix $H$의 rank에 초점을 맞추었다.

Edge를 어떻게 정의하느냐에 따라, 즉 Observation에 따라 행렬의 차원에 차이가 존재한다.

2D 평면을 기준으로 (x,y) sensor observation의 경우 로봇 pose($x,y,\theta$)와 Landmark($x,y$)에 변수 5개와 edge에서의 변수 2개($x,y$)로 Matrix $H$를 구성하기 때문에 $(5*2)$로 표현된다.

이 때 Matrix $H$의 rank는 2이다.

2D 평면을 기준으로 Bearing Only observation의 경우 로봇 pose($x,y,\theta$)와 Landmark($x,y$)에 변수 5개와 edge에서의 변수 1개($\theta$)로 Matrix $H$를 구성하기 때문에 $(5*1)$로 표현된다.

이 때 Matrix $H$의 rank는 1이다.

Rank가 중요한 이유를 예시를 들어 설명해보자.

Robot이 Landmark를 관찰했다고 했을 때, Landmark 주변으로 Robot이 어디있는지 어떻게 판단할 수 있을까?

이것 또한 Edge의 종류(Observation의 종류)에 따라 달라진다.

$(x,y)$ sensor observation에 대한 하나의 값이 존재할 경우 아래 그림처럼 빨간색 원(Robot)이 검은색 원(Landmark)를 기준으로 어디에나 존재할 수 있게된다. (observation이 상대적인 값이기 때문이다.)

Bearing Only observation에 대한 하나의 값이 존재할 경우는 아래 그림처럼 2차원 평면 어디서나 존재할 수 있게 된다. 마찬가지로 observation이 로봇을 기준으로 상대적인 값이기 때문이다.

Levenberg Marquardt method를 통한 최적화

두 경우 마찬가지로 하나의 observation에 대해서는 정확하게 로봇의 위치를 정확히 파악하기 어렵다. 따라서 추가적인 정보가 필요하다.

그렇다면 정확한 로봇의 위치를 파악하기 위해선 얼마나 많은 observation이 필요할까? 이를 Rank를 통해 판단한다. 우리가 unique한 solution을 구하려면 System이 Full rank를 가지고 있어야 하기 때문이다.

따라서 system을 Full rank로 만들기 위하여 Gauss newton 방법에서 선형화를 진행할 때, "damping factor $\lambda$"를 Matrix $H$에 더하고 해를 구하게 된다.

$H \varDelta x = -b$가 아니라 $(H+ \lambda I) \varDelta x = -b$의 해를 구하게 된다.
damping factor를 작게 만들어 system이 많이 변하게 하지 않게 하면서 system을 full rank로 만드는 작업을 진행한다.

이렇게 damping factor을 추가하는 방법을 Steepest Descent Approach라고 한다.

Steepest Descent는 9.Steepest descent 방법 포스팅을 참고해서 이해하면 된다.

또한 Gauss-newton method에 Steepest Descent Approach를 추가한 방법을 Levenberg Marquardt method라고 한다.

Levenberg Marquardt의 pseudo code는 아래와 같다. 코드에 대한 설명은 SLAM DUNK Season 2 - Levenberg Marquardt 설명을 참고해보자.

Levenberg Marquardt는 Visual SLAM에서 자주 등장하는 용어인 Bundle Adjustment에도 사용하는 방법이다.

Bundle Adjustment는 서로 다른 viewpoints를 가진 이미지와 environment에서 3차원 feature points들 간의 관계를 찾을 때 사용한다. Camera에 대한 odometry를 모른다고 가정할 때 이미지에서 보이는 feature와 environment에서 3차원 feature points의 대응관계(correspondence)를 구하고, 결과적으로 camera의 pose와 3차원 feature points의 위치를 구하게 된다.

Bundle Adjustment는 2D 이미지에서 reprojection error를 사용하여 이 error를 최소화하는 것이 목표이다. Bundle Adjustment의 경우 많은 내용을 다뤄야 하기 때문에 추후 다른 포스팅에서 이에 대한 내용을 정리해볼 예정이다.