본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이 글에서는 Least square를 이용하여 실제로 어떻게 SLAM에 적용이 되는지 알아볼 것이다. 아직 least squre에 대해서 익숙하지 않다면 이전 글을 참고하기 바란다. 이번 글에서의 표기법은 이전 글의 표기법을 그대로 사용한다.
Graph-based SLAM
Graph-based SLAM은 로봇의 위치와 움직임을 graph처럼 node와 edge로 표현함을 의미한다.
Node: Graph의 node는 로봇의 pose를 의미한다.
Edge: 두 node사이의 edge는 로봇의 위치 사이의 odometry정보이며 constraint라고 한다.
로봇이 움직이며 odometry 정보를 이용하여 node와 constraint를 생성한다.
로봇이 위 그림과 같이 이전에 방문했던 지역을 다시 방문할 경우 주변 환경에 대한 정보를 이용하여 같은 위치임을 인식하고 연속적이지 않은(non-successive) node사이에 constraint를 추가하고, graph를 최적화 함으로써 measurement에 최적화된 로봇의 위치를 계산할 수 있다.
아래의 youtube 동영상은 pose graph SLAM을 이용한 SLAM의 예이다.
동영상에서 차량이 움직이는 경로의 실선은 constraint이며, 실선 중간에 생성되는 네모는 graph의 node이다. 위 application은 도로 상의 marking정보를 이용하여 장소를 인식하고 non-successive node사이의 constraint를 추가함으로써 odometry의 error를 보정한다. 동영상에서 이러한 constraint를 추가함으로써 계속적으로 graph가 최적화 되는 모습을 볼 수 있다.
Overall SLAM System
Graph SLAM은 아래 그림과 같이 크게 Front-end와 Back-end로 나눌 수 있다.
위의 동영상을 예로 들면, Front-end는 camera로 부터 얻어지는 이미지로 부터 데이터 처리를 통해 sub-map을 만들고, 생성된 sub-map을 통해서 다시 방문한 장소를 인식함으로써 각 node사이의 constraint를 생성하는 과정을 말한다. 이렇게 생성된 constraint들과 node들의 정보를 이용하여 최적화된 node의 위치를 계산하는 과정은 graph SLAM의 back-end에서 수행한다. 최근 많이 사용되는 graph SLAM의 back-end로는 iSAM과 g2o가 있다.
Graph
이 글에서 로봇이 이동하는 환경은 2-dimension 으로 가정하며, 로봇의 위치는 다음과 같이 3 DOF로 표현한다. \[\mathbf{x}_i = (x_i, y_i, \theta_i)\]
Graph는 이러한 로봇의 위치를 표현하는 $\mathbf{x}_i$로 구성된 vector이다. \[\mathbf{x} = \mathbf{x}_{1:n}\]
Graph를 구성하는 노드 $\mathbf{x}i$와 $\mathbf{x}{i+1}$ 사이에는 odometry 센서로부터 얻어진 measurement가 constraint/edge로 생성된다.
만약 로봇이 같은 장소를 다시 방문했을 때 두 로봇의 위치에서 얻은 measurement를 이용하여 두 노드 사이의 상대적인 위치(relative pose)를 계산할 수 있다. 아래 그림은 다른 위치에서 획득한 같은 물체의 센서 데이터를 보여준다.
두 센서데이터의 매칭을 통해서 두 node사이의 상대위치를 계산할 수 있고, 이 상대위치가 두 node사이의 constraint가 된다. 이때 직접적으로 node사이의 관계를 구한 것이 아닌 센서 measurement로부터 계산하였기 때문에 virtual measurement라고 부른다.
Pose Graph
아래 그림은 Pose graph SLAM에서 measurement에 대한 error를 그림으로 표현하여 보여준다.
$\mathbf{x}i$와 $\mathbf{x}_j$는 현재 graph에서 두 node의 위치이다. $<\mathbf{z}{ij},\Omega_{ij}>$는 $\mathbf{x}i$의 위치에서 센서를 이용하여 측정한 $\mathbf{x}_j$의 위치이다. 센서로 측정한 node의 위치와 현재 graph상의 위치의 차이를 $\mathbf{e}{ij}$로 표현한다. Graph optimization은 이러한 error를 최소화 시키는 graph를 계산하는 것이다. 여기서 $\Omega_{ij}$가 센서 measurement의 information matrix임을 기억하자.
Error function ($\mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j)$)을 homogeneous coordinate로 표현하면 다음과 같이 표현할 수 있다. \[\mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j) = \text{t2v}(\mathbf{Z}_{ij}^{-1}(\mathbf{X}_i^{-1}\mathbf{X}_j))\]
homogeneous coordinate은 로봇의 translation과 rotation을 하나의 matrix로 표현하는 방법이며, 이러한 표현방법에 대해서는 이글을 참고하라.
위 식에서 $\mathbf{Z}_{ij}$는 i에서 바라본 j의 measurement이며, $\mathbf{X}_i^{-1}\mathbf{X}_j$는 현재 graph에서 i를 기준으로 j의 위치를 의미한다. t2v함수는 homogeneous coordinate를 vector form으로 바꾸는 transform 함수이다.
Graph optimization식을 전체 state를 표현하는 $\mathbf{x}$를 이용하여 표현하면 다음과 같다. \[\begin{aligned} x^* &=\text{argmin}_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}^T(\mathbf{x}_i,\mathbf{x}_j) \mathbf{\Omega}_{ij} \mathbf{e}_{ij}(\mathbf{x}_i,\mathbf{x}_j)\\ &=\text{argmin}_{\mathbf{x}} \sum_k \mathbf{e}_k^T(\mathbf{x}) \mathbf{\Omega}_k \mathbf{e}_k(\mathbf{x}) \end{aligned}\]
여기서 state vector $\mathbf{x}$는 graph의 node가 각각의 block을 구성하는 vector이다. \[\mathbf{x}^T = \begin{pmatrix}\mathbf{x}_1^T & \mathbf{x}_2^T & \cdots & \mathbf{x}_n^T \end{pmatrix}\]
error function을 선형화하면 Jacobian으로 표현할 수 있다. \[\begin{aligned} \mathbf{e}_{ij}(\mathbf{x}+\triangle\mathbf{x}) &\approx \mathbf{e}_{ij}(\mathbf{x}) + \mathbf{J}_{ij}(\mathbf{x})\triangle \mathbf{x} \end{aligned}\]
이전 글에서 optimization 과정을 통해서 최적화된 $\mathbf{x}^*$를 계산하였다. 최적화 식을 만족하는 state를 계산하기 위해서는 $\mathbf{b}^T$와 $\mathbf{H}$를 계산해야 한다. \[\begin{aligned} \mathbf{b}^T &= \sum_{ij} \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\\ \mathbf{H} &= \sum_{ij} \mathbf{J}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij} \end{aligned}\]
Jacobian matrix $\mathbf{J}_{ij}$가 sparse matrix이기 때문에 information matrix인 $\mathbf{H}$도 sparse matrix가 된다. Jacobian matrix의 sparse함이 각 파라미터에 어떻게 영향을 미치는지 그림으로 보자.
위에서 설명한 과정을 통해서 최적의 $\mathbf{x}$를 계산하는 과정을 정리하면 다음과 같다.
위에서 설명한 과정을 통해 모든 measurement에 대한 information matrix($\mathbf{H}$)와 $\mathbf{b}$를 계산하고, 두 값을 이용하여 state의 변화량인 $\triangle \mathbf{x}$를 계산한다($\triangle \mathbf{x} = -\mathbf{H}^{-1}\mathbf{b}$). $\mathbf{H}$는 크기가 큰 matrix이므로 inverse과정이 높은 계산량을 요구하므로, 이를 쉽게 계산하기 위해서 Cholesky factorization 방법을 사용한다. 이제 계산된 변화량을 이용하여 state를 업데이트하고($\mathbf{x} = \mathbf{x}+\triangle \mathbf{x}$) state가 수렴할 때 까지 반복하여 최적화된 state를 계산한다.
Example of Pose Graph
이해를 돕기위해 graph의 optimization과정을 통한 state update과정의 예를 통해 설명할 것이다.
위 그림과 같이 현재 2개의 state가 있으며, 센서를 통해 측정한 두 node 사이의 odometry정보는 1이다. 처음에는 모든 state의 값을 모르기 때문에 모두 0으로 설정한다. \[\begin{aligned} \mathbf{x} &= \begin{pmatrix} 0 & 0 \end{pmatrix}^T\\ \mathbf{z}_{12} &= 1\\ \mathbf{\Omega} &= 2\\ \mathbf{e}_{12} &= \mathbf{z}_{12} -(x_2 - x_1) = 1-(0-0) = 1\\ \mathbf{J}_{12} &= \begin{pmatrix}1 -1\end{pmatrix}^T\\ \mathbf{b}_{12} &= \mathbf{e}_{12}^T \mathbf{\Omega}_{12} \mathbf{J}_{12} = \begin{pmatrix} 2 & -2 \end{pmatrix}\\ \mathbf{H}_{12} &= \mathbf{J}_{12}^T \mathbf{\Omega} \mathbf{J}_{12} = \begin{pmatrix} 2 & -2 \\ -2 & 2\end{pmatrix}\\ \triangle \mathbf{x} &= -\mathbf{H}_{12}^{-1} b_{12} \end{aligned}\]
센서 measurement의 information은 2로 설정하였다. state update를 위해 \(\mathbf{b}, \mathbf{H}\)를 계산하였다. 이때 \(\triangle\mathbf{x}\)를 계산하기 위해서는 \(\mathbf{H}\)의 inverse를 계산해야 하지만 현재 \(det(\mathbf{H})\)는 0이므로 계산할 수 없다. 이것은 모든 node의 uncertainty가 같아 graph가 floating상태이기 때문이다. 이를 해결하기 위해서 첫번째 node를 고정(fix)시켜 줘야한다. 이를 위해 첫번째 node의 uncertainty를 낮춰준다(즉, information을 더해서 크게 해준다). \[\begin{aligned} \mathbf{H} &= \begin{pmatrix} 2 & -2 \\ -2 & 2 \end{pmatrix} + \begin{pmatrix} 1 & 0 \\ 0 & 0 \end{pmatrix}\\ \triangle \mathbf{x} &= -\mathbf{H}^{-1} b_{12}\\ \triangle \mathbf{x} &= \begin{pmatrix} 0 & 1 \end{pmatrix}^T \end{aligned}\]
따라서 error를 최소화 하는 state의 변화량은 $\begin{pmatrix} 0 & 1 \end{pmatrix}^T$가 되며 state를 업데이트하면 최종적으로 $\mathbf{x} = \mathbf{x} + \triangle \mathbf{x} = \begin{pmatrix} 0 & 1 \end{pmatrix}^T$가 된다.
Hierarchical Approach to Least Square SLAM
Hierarchical Approach는 Graph SLAM의 속도를 향상시키기 위한 한가지 방법이다.
맨 왼쪽 그림은 원래의 pose graph를 보여주고 있다. Hierarchical Approach는 이름에서 알 수 있듯이 여러개의 계층구조로 구성되어 있으며 아래의 layer일수록 많은 node를, 위로 갈수록 적은 수의 node를 갖는 graph로 구성되어 있다. Optimization 문제는 node의 갯수가 많을 수록 계산시간이 오래 걸리기 때문에 이러한 계층구조를 이용하여, 적은 수의 node의 graph에서(sampling된 node로 이루어진 graph) 최적의 node위치를 계산 후, 계산된 node를 기준으로 주변 node의 위치를 다시 계산하는 방법을 이용한다. Hierarchical Approach에 대해서는 자세히 언급하지 않겠다. 이 부분에 대해서 자세히 알고 싶으면 Robot Mapping 강의 16강을 참고하기 바란다.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
SLAM은 로봇의 위치를 추정함과 동시에 주변 환경에 대한 맵도 같이 생성하는 기술이다. SLAM의 방법은 크게 3가지로 나눌 수 있다.
Kalman Filter
Particle Filter
Graph-based
이번 글에서 부터는 Graph-based SLAM에 대해서 설명할 것이며, 이 글에서는 Graph-based SLAM의 접근 방법인 Least square에 대해서 설명한다.
Least Square
Least square는 “overdetermined system”의 해를 구하기 위한 방법이다. overdetermined system이란 미지수의 갯수보다 식의 수가 더 많기 때문에, 모든 식을 만족하는 해가 존재하지 않는 시스템을 말한다.
Least square는 에러의 제곱합(sum of the squared error)을 최소화 하는 방법으로 해를 구한다.
Problem of Least Square
$\mathbf{x}$는 state vector
$\mathbf{z}_i$는 실제 measurement(실제 센서 측정값)
$\hat{\mathbf{z}}_i=f_i(x)$는 현재 state vector $\mathbf{x}$에서 예상되는 measurement값
목표: 센서로부터 실제 얻어진 measurement($\mathbf{z}_i$)에 가장 적합한 $\mathbf{x}$ 추정
위 그림은 least square를 그림으로 표현하였다. Least square는 센서로부터 측정된 measurement에 가장 적합한 로봇의 state를 계산하는 방법이다. Sensor observation model처럼 로봇의 상태를 알고 있을 때 예상되는 measurement를 예상할 수 있고($\mathbf{\hat{z}}_i$) 실제 measurement와 예상되는 measurement와의 차이를 최소화 함으로써 최적화된 로봇의 state를 계산할 수 있다.
Error Function
Error($\mathbf{e}_i$)는 실제 measurement와 예상되는 measurement와의 차이를 나타내며, measurement와 같은 dimension을 갖는 vector이다. \[\mathbf{e}_i(\mathbf{x}) = \mathbf{z}_i - f_i(\mathbf{x})\]
위 식에서 measurement의 noise는 평균이 0이며, Gaussian 분포를 따른다고 가정한다.
Information matrix($\Omega_i$)는 measurement의 covariance matrix의 inverse이며, measurement($\mathbf{z}_i$)와 dimension의 크기가 같은 matrix이다.
squared error($e_i(\mathbf{x})$)는 아래와 같으며, vector가 아닌 scalar값을 갖는다. \[e_i(\mathbf{x}) = \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x})\]
Find Minimum of Error Function
로봇의 최적화된 state를 계산하는 문제는, 주어진 모든 센서 measurement를 이용한 error function의 합이 최소화 되는 state를 찾는 문제이다. 식으로 표현하면 다음과 같다. \[\begin{aligned} x* &= \text{argmin}_{\mathbf{x}} F(\mathbf{x})\\ &=\text{argmin}_{\mathbf{x}} \sum_i e_i(\mathbf{x})\\ &=\text{argmin}_{\mathbf{x}} \sum_i \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x}) \end{aligned}\]
위 식에서 각 element들에 대해서는 error function에서 설명했다. 이 식을 보고 넘어갈 때 각 항들의 dimension이 어떻게 되는지 확인하고 넘어가도록 하자. 만약 센서 measurement의 dimension이 n일 때 $\mathbf{e}_i$는 $n\times1$, $\mathbf{\Omega}_i$는 $n\times n$의 dimension을 갖는다.
위와 같은 optimization 문제는 복잡하며, closed form(미지수의 해가 수식으로 정리되는 형태)이 아니기 때문에 수학적인 접근방법으로 해를 구한다.
Optimization 방법으로 최적의 로봇 state($\mathbf{x}^*$)를 구하는 순서는 다음과 같다.
현재의 state를 기준으로 error term($\mathbf{e(x)}$)의 선형화
선형화된 error term으로 계산되는 squared error function($e_i(\mathbf{x})$)의 1차 미분계산
squared error function은 quadratic form이므로 1차 미분값이 0이 되는 점이 최소값이므로 미분값이 0인 solution을 계산
계산된 solution을 이용하여 state를 update
위의 과정을 수렴할 때까지 반복
위의 과정을 통해 iterative하게 optimal한 state를 계산할 수 있다. 각 단계의 과정을 자세히 살펴보도록 하자.
1. Error Term의 선형화
vector의 형태를 갖는 error term은 현재 measurement와 예상되는 measurement의 차이이다. \[\mathbf{e}_i(\mathbf{x}) = \mathbf{z}_i - f_i(\mathbf{x})\]
이때 $f_i(\mathbf{x})$는 observation model로 많은 경우에 비선형형태를 갖는다. observation model에 대해 익숙하지 않다면 Motion Model과 Observation Model에 대한 이전 글을 참고하기 바란다. 따라서 식을 풀기 위해서는 현재 state기준으로 선형화를 수행한다. 선형화 방법으로는 EKF와 마찬가지로 Tayor expansion방법을 사용한다. \[\begin{aligned} \mathbf{e}_i(\mathbf{x}+\triangle\mathbf{x}) &\approx \mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})((\mathbf{x}+\triangle \mathbf{x})-\mathbf{x})\\ &= \mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})\triangle \mathbf{x} \end{aligned}\]
계산된 squared error는 위와 같으며, 마지막 식에서 $c_i, \mathbf{b}_i, \mathbf{H}_i$는 다음과 같다. \[\begin{aligned} c_i &= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{e}_i\\ \mathbf{b}_i^T &= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i\\ \mathbf{H}_i &= \mathbf{J}_i^T \mathbf{\Omega}_i \mathbf{J}_i \end{aligned}\]
$\mathbf{H}_i$는 선형화 후의 information matrix이다.
위에서 계산된 $e_i(\mathbf{x}+\triangle\mathbf{x})$는 1개의 measurement에 대한 error function이므로 모든 measurement에 대한 error function은 다음과 같다. \[\begin{aligned} F(\mathbf{x}+\triangle \mathbf{x}) &\approx \sum_i (c_i + 2 \mathbf{b}_i^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H}_i)\triangle \mathbf{x}\\ &= \sum_i c_i + 2 (\sum_i \mathbf{b}_i^T) \triangle \mathbf{x} + \triangle \mathbf{x}^T (\sum_i \mathbf{H}_i) \triangle \mathbf{x}\\ &= c + 2 \mathbf{b}^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H} \triangle \mathbf{x} \end{aligned}\] \[\begin{aligned} \mathbf{b}^T &= \sum_i \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i\\ \mathbf{H} &= \sum_i \mathbf{J}_i^T \mathbf{\Omega}_i \mathbf{J}_i \end{aligned}\]
그렇다면 이제 squared error function의 마지막 식을 살펴보자. \[\begin{aligned} F(\mathbf{x}+\triangle \mathbf{x}) &\approx c + 2 \mathbf{b}^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H} \triangle \mathbf{x} \end{aligned}\]
위의 식은 $\triangle \mathbf{x}$에 대해서 quadratic한 형태를 띄고 있다. quadratic form은 일반 2차방정식의 형태로 생각할 수 있다. 즉, 함수의 극대 혹은 극소값은 함수의 미분이 0이 되는 지점을 계산함으로써 계산할 수 있다.
그럼 이제 위의 식의 1차 미분을 계산한다. 일반적으로 quadratic form의 미분은 다음과 같다(matrix cookbook, section 2.2.4참고). \[\begin{aligned} f(x) &= \mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{b}^T\mathbf{x}\\ \frac{\partial f}{\partial x} &= (\mathbf{H} + \mathbf{H}^T)\mathbf{x} +\mathbf{b} \end{aligned}\]
따라서 함수의 1차 미분은 다음과 같다. \[\frac{\partial F(\mathbf{x}+\triangle \mathbf{x})}{\partial \triangle \mathbf{x}} = 2 \mathbf{b} + 2 \mathbf{H}\triangle \mathbf{x}\]
3. Global Error Function의 최소값 계산
2번단계에서 global error function의 1차 미분값을 계산하였다. global error function은 quadratic form이므로 미분값이 0인 지점이 함수의 극대 혹은 극소값이다. 우리가 계산하고 있는 least square의 목적은 error를 최소로 만드는 state를 계산하는 것이므로 계속해서 error function의 최소값을 찾아 나간다. 최소값을 찾기 위하여 미분값을 0으로 만드는 $\mathbf{x}$를 계산한다. \[\frac{\partial F(\mathbf{x}+\triangle \mathbf{x})}{\partial \triangle \mathbf{x}} = 2 \mathbf{b} + 2 \mathbf{H}\triangle \mathbf{x} = 0\]
따라서 optimal한 state는 다음과 같다. \[\triangle \mathbf{x}^* = -\mathbf{H}^{-1}\mathbf{b}\]
이때 $\mathbf{H}$는 크기가 큰 matrix이므로 inverse를 계산 시 많은 계산이 소요된다. 이러한 문제를 해결하기 위해서 Cholesky factorization, 혹은 QR decomposition과 같은 방법을 사용한다. Cholesky factorization은 matrix $\mathbf{H}$를 하 삼각행렬(lower triangular matrix)의 곱으로 표현하는 방법이다($\mathbf{H}= \mathbf{L}\mathbf{L}^T$). Triangular matrix는 일반 matrix에 비해 inverse의 계산이 간단하므로 이렇게 분해함으로써 inverse를 더 빠르게 계산할 수 있다. \[\begin{aligned} \triangle \mathbf{x}^* &= -\mathbf{H}^{-1}\mathbf{b}\\ &= -\mathbf{L}^{-T}\mathbf{L}^{-1}\mathbf{b} \end{aligned}\]
4. State Update
3번 과정에서 global error function을 최소로 만드는 state인 $\triangle \mathbf{x}$를 계산하였다. 이제 이 결과를 이용하여 state를 update한다. \[\mathbf{x} = \mathbf{x} + \triangle \mathbf{x}\]
업데이트 완료되면 업데이트된 state를 기준으로 다시 선형화를 하여 위 과정을 반복한다. 여러번 반복함으로써 계속 최적화된 state로 업데이트하고, state가 수렴하면 계산을 중지하고 optimization과정을 종료한다.
위와 같이 quadratic form을 계산함으로써 optimization 문제를 해결하는 방법을 Gauss-Newtom optimization 이라고 한다.
Least Square와 Gaussian의 관계
위에서 least square의 계산과정은 모두 설명하였다. Least square문제는 아래의 식을 품으로써 최적의 state를 계산하였다. \[\begin{aligned} x* &=\text{argmin}_{\mathbf{x}} \sum_i \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x}) \end{aligned}\]
하지만 왜 이런 형태의 식을 만들어서 풀게 되었을까? 이것은 Gaussian 분포와 관계가 있다. Bayes filter의 posterior의 식은 다음과 같다. \[p(x_{0:t} \mid z_{1:t}, u_{1:t}) = \eta p(x_0) \prod_t [p(x_t \mid x_{t-1}, u_t)p(z_t \mid x_t)]\]
여기서 prior와, motion model 그리고 observation model은 모두 gaussian 분포를 따른다. 따라서 log likelihood를 계산하면 다음과 같다. \[log p(x_{0:t} \mid z_{1:t}, u_{1:t}) = \text{const.} + log p(x_0) + \sum_t [log p(x_t \mid x_{t-1}, u_t)+ log p(z_t \mid x_t)]\]
여기서 모든 확률은 Gaussian 분포를 따르는데, Gaussian 분포의 log는 다음과 같다. \[log \mathcal{N}(x,\mu,\Sigma) = \text{const.} -\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\]
계산된 Gaussian의 log형태가 매우 익숙하다. 위의 형태는 우리가 최적화에 사용하였던 함수와 같은 형태임을 알수 있다. 따라서 posterior를 error function으로 표현하면 다음과 같다. \[\begin{aligned} log p(x_{0:t} \mid z_{1:t}, u_{1:t}) &= \text{const.} + log p(x_0) + \sum_t [log p(x_t \mid x_{t-1}, u_t)+ log p(z_t \mid x_t)]\\ &= \text{const.} - \frac{1}{2} e_p(\mathbf{x}) -\frac{1}{2} \sum_t [e_{u_t}(\mathbf{x})+e_{z_t}(\mathbf{x})] \end{aligned}\]
따라서, posterior의 최대값을 찾는 문제는 error function의 최소값을 찾는 문제가 된다. \[\begin{aligned} argmax \ \ log p(x_{0:t} \mid z_{1:t}, u_{1:t}) &= argmin \ \ e_p(\mathbf{x})+ \sum_t [e_{u_t}(\mathbf{x})+e_{z_t}(\mathbf{x})] \end{aligned}\]
이러한 Gaussian과의 관계에 의해서 위에서와 같이 error function이 정의됨을 기억하자.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이번 글에서는 Particle filter를 이용한 localization방법에 대해서 설명한다. EKF나 EIF와 같은 filter들은 Gaussian 분포만을 이용하여 시스템을 모델링하였다. Particle filter의 목적은 아래 그림과 같이 Gaussian이 아닌 임의의 분포를 다루기 위한 접근 방법이다.
Particle Selection
가중치를 갖는 particle들의 set은 다음과 같이 정의된다. \[\chi = \{<x^{[j]},w^{[j]}>\}_{j=1,2,\cdots, J}\]
$x^{[j]}$는 particle의 위치, $w^{[j]}$는 particle의 weight를 의미한다. Posterior는 다음과 같이 표현된다. \[p(x) = \sum_{j=1}^{J} w^{[j]}\delta_{x^{[j]}}(x)\]
위 식에서 $\delta$는 dirac delta 함수로 함수의 인자로 주어진 값 이외의 출력은 0이며 전체 합(Integral)은 1인 함수이다.
왼쪽 그림은 Gaussian 분포를, 오른쪽 그림은 임의의 분포를 보여준다.
이러한 분포에서 sample은 어떻게 추출될까?
Gaussian 분포의 경우는 위와 같이 $-\sigma$와 $\sigma$사이에서 12개의 임의의 값을 뽑고 합을 구함으로써 Gaussian 분포에 맞는 sample을 추출할 수 있다. 하지만 임의의 분포의 경우에는 위의 방법으로 sample을 추출할 수 없다.
위 분포는 Gaussian 분포가 아닐 때 sampling 하는 방법을 설명한다. proposal(x)는 실제 분포인 target(x)를 Gaussian 분포로 근사한 함수이다. 근사된 Gaussian 분포인 proposal(x) 함수에서 sample을 추출하고, 각 sample에 weight를 할당함으로써 실제 분포와 유사한 의미를 부여할 수 있다. 이때 할당되는 weight 는 \[w = \frac{f}{g}\]
이며, f는 target, g는 proposal의 함수값이다. 위 그림에서와 같이 실제 sample의 수는 Gaussian 분포를 따르지만 weight가 실제 함수의 확률을 반영한다.
Particle Filter Algorithm and Monte Carlo Localization
위의 sampling 방법을 적용한 particle filter 알고리즘은 다음과 같다.
이제 particle filter를 localization에 적용시켜보자. Monte Carlo 방법은 난수를 이용하여 함수의 값을 확률적으로 계산하는 알고리즘이다. 계산하려는 값이 close form(정확한 수식으로 계산되는 형태)이 아니거나 너무 복잡하여 근사적으로 구해야 할 때 주로 사용된다. particle filter는 이러한 Monte Carlo 방법이다. 이제 particle filter를 이용한 localization 알고리즘은 알아보자.
달라진 부분은 sample을 구하는 3번과, weight를 계산하는 4번이다. sample을 구하는 3번식은 로봇의 motion model이 되며, 각 particle의 weight는 센서의 observation model이다. 7번부터 10번까지는 resampling 과정이다.
Resampling
Resamping 과정은 확률이 작은 particle은 제거하고, 확률이 큰 파티클은 여러개로 나눈다. Resampling을 하는 이유는 우리가 사용할 수 있는 point의 갯수가 한정적이기 때문이며(computation문제와 memory문제 때문에) resampling 전과 후의 particle 갯수는 같아야 한다. 또한 resampling 전에 particle들의 weight는 다르지만, resampling 후에는 모두 같아진다. 아래 그림은 resampling을 하는 두가지 방법을 보여준다.
우선 노란색 원은 각 particle의 weight에 맞게 영역을 할당하여 원으로 표현한 것으로 생각할 수 있다. 즉 particle의 weight가 클수록 차지하는 영역이 크므로 랜덤하게 선택될 가능성이 크다.
Roulette wheel 방법의 경우 한번의 1개씩 random하게 particle을 추출한다. Stochastic universal sampling 방법은 random하게 추출함과 동시에 일정한 간격으로 particle을 추출한다. stochastic universal sampling 방법의 장점으로는 계산시간이 선형적이며, 모든 particle의 weight가 같은 경우 이전과 같은 형태로 particle이 resampling 된다는 점이다.
Particle Filter Localization Example
particle filter localization에서 초기상태는 아래 그림과 같다. 모든 particle은 균일하게 분포되어 있으며 동일한 weight를 갖는다.
control input에 의해서 모든 particle은 motion update가 진행된다.
로봇의 센서로부터 주변 환경에 대한 데이터를 수집한다.
센서 정보를 이용하여 각 particle의 weight를 update한다.
계산된 weight를 기준으로 resampling과정을 거친다.
resampling된 particle들을 이용하여 다시 motion update를 한다.
로봇 센서로부터 주변환경에 대한 데이터를 수집한다.
데이터로 부터 각 particle의 weight를 update한다.
update된 weight를 기준으로 다시 resampling을 한다.
resampling된 particle을 기준으로 motion이 update된다.
위 그림과 같이 Particle filter는 motion update, measurement, weight update, resampling과정을 반복하며 로봇의 위치를 추정해 나간다.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이번 글에서는 EKF와 EIF등과 같이 landmark나 feature를 기반의 map표현 방법이 아닌, 센서의 raw 데이터를 이용하여 volumetric하게 map을 표현하는 방법에 대해서 설명하도록 한다.
Feature
EKF, EIF, SEIF는 feature 기반의 map을 사용한다.
센서 데이터를 그대로 사용하는 것이 아닌 compact한 표현 방법을 사용하여 map을 나타낸다.
많은 feature의 관측은 landmark의 위치 추정 정확도를 높인다.
Grid Map
공간을 cells로 나누어 표현한다.
grid로 표현된 공간 구조는 고정이다.
각 cell은 occupied space와 free space로 구분된다.
non-parametric model이다. 즉 센서데이터를 이용하여 landmark를 표현할 수 있는 parameter를 계산하지 않고 map을 표현한다.
compact한 parameter로 map을 표현하지 않기 때문에 상당한 memory가 필요하다.
feature detector에 의존하지 않는다.
위 그림은 grid map을 보여준다.
Grid Map의 가정 1
첫번째 가정: cell을 구성하는 영역은 완전히 binary random variable로 occupied이거나 free이다.
cell이 occupied: $p(m_i) = 1$
cell이 free: $p(m_i) = 0$
cell상태를 알수 없음: $p(m_i) = 0.5$
Grid Map의 가정 2
두번째 가정: map은 움직이지 않는다. 즉 static world이다. 대부분의 mapping system은 이러한 가정을 한다.
Grid Map의 가정 3
세번째 가정: map을 이루는 각 cell은 서로 독립적(independent)이다. 따라서 전체 map의 확률은 각 cell 확률의 곱으로 계산되어진다. \[p(m) = \prod_i p(m_t)\]
위 식에서 m은 map전체를 의미하며, $m_i$는 각 cell을 의미한다.
Data를 이용한 Map의 추정
주어진 센서 데이터 \(z_{1:t}\)와 로봇의 위치 \(x_{1:t}\)를 이용하여 map을 추정하면 다음과 같다. \[p(m \mid z_{1:t}, x_{1:t}) = \prod_i p(m_i \mid z_{1:t},x_{1:t})\]
여기서 각 cell을 의미하는 \(m_i\)는 binary random variable이다.
각 cell의 확률을 계산하면 다음과 같다.
위 식의 유도과정에는 bayes rule과 markov assumption이 사용된다. 위에서 계산된 $p(m_i \mid z_{1:t},x_{1:t})$는 cell이 occupied일 확률이며, cell이 free space일 확률을 구하면 다음과 같다.
이제 위에서 구한 식을 이용하여 cell이 occupied확률을 계산해보자. 아래 식의 binary random variable의 특징을 보자. \[\begin{aligned} \frac{p(x)}{1-p(x)} &= Y \\ p(x) &= Y - Yp(x)\\ p(x) (1+Y) &= Y\\ p(x) &= \frac{Y}{1+Y}\\ p(x) &= \frac{1}{1+\frac{1}{Y}} \end{aligned}\]
즉 binary random variable의 두 확률의 ratio를 이용하면 probability를 계산할 수 있다. 이러한 특징을 이용하여 cell의 occupied확률을 계산해 보자. \[\frac{p(m_i \mid z_{1:t}, x_{1:t})}{p(\lnot m_i \mid z_{1:t}, x_{1:t})} = \frac{p(m_i \mid z_{1:t}, x_{1:t})}{1-p(m_i \mid z_{1:t}, x_{1:t})}\]
즉 probability를 정리하면 3개의 항으로 생각할 수 있다. 가운데 항은 recursive term으로 볼 수 있으며, 오른쪽 항은 맵에대한 prior term으로 볼 수 있다.
여기서 우리가 계산해야 하는 cell의 확률 $p(m_i \mid z_{1:t},x_{1:t})$을 효율적으로 계산하기 위해 Log odds notation을 이용한다.
Log Odds Notation
log odds notation은 다음과 같이 정의된다. \[l(m_i \mid z_{1:t},x_{1:t}) = log\big( \frac{p(m_i \mid z_{1:t},x_{1:t})}{1 - p(m_i \mid z_{1:t},x_{1:t})}\big)\]
log odds notation을 이용하여 식을 정리하면 다음과 같다. \[l(m_i \mid z_{1:t},x_{1:t}) = l(m_i \mid z_t,x_t) + l(m_i \mid z_{1:t-1},x_{1:t-1}) - l(m_i)\]
여기서 $l(m_i \mid z_{1:t-1},x_{1:t-1})$은 recursive하게 계산되는 항이며, $l(m_i)$은 prior 확률이다. 따라서 inverse sensor model인 $l(m_i \mid z_t,x_t)$가 계산된다면 효율적으로 단순한 더하기로 $l(m_i \mid z_{1:t},x_{1:t})$를 계산할 수 있다. 이렇게 계산된 log odds notation으로 부터 cell의 확률을 아래와 같이 계산할 수 있다. \[p(m_i \mid z_{1:t},x_{1:t}) = 1- \frac{1}{1+ exp(l(m_i \mid z_{1:t},x_{1:t}))}\]
전체적인 grid map의 algithm은 다음과 같다.
Inverse Sensor Model
앞에서 grid map의 update algorithm에 대해서 살펴보았으며, inverse sensor model인 $p(m_i \mid z_t, x_t)$를 알아야 grid map을 생성할 수 있다. $p(m_i \mid z_t, x_t)$은 로봇의 위치와 센서 관측값이 있을 때 각 cell의 occupied 되어 있을 확률이다. 여기서는 2가지 모델을 예를 보여준다.
1. Sonar Sensor Model
첫번째 sensor는 sonar 센서이다. sonar센서는 상대적으로 거리 측정값에 대한 noise가 크다. 아래 그림에서 $z$는 센서의 측정값이다. $z-d_1$보다 가까운 구간은 free space로 생각하며, $z$ 부터 $z+d_3$구간은 object가 있는 영역으로 생각한다. $z+d_3$보다 먼 영역은 알수 없는 영역이므로 확률은 0.5이다. sonar센서는 측정값의 noise가 크기 때문에 $d$가 상대적으로 크다.
아래 그림은 sonar센서를 이용하여 생성한 grid map이다.
2. Laser Range Finder(LiDAR sensor)
두번째 센서는 Lidar sensor이다. Lidar는 sonar에 비해 거리측정 오차가 매우 적다. 따라서 occupied로 생각하는 영역이 sonar모델에 비해 매우 좁다.
아래 그림은 LiDAR센서를 이용하여 생성한 Grid map이다. Sonar를 이용한 map에 비해 상대적으로 매우 정확한 것을 알 수 있다.
Occupancy Grid Map 정리
Occupancy Grid Map은 공간을 독립적인 cell로 분리하여 생각한다.
각 cell은 binary random variable이다.
정확한 로봇의 위치를 알고 있다면 mapping은 상대적으로 쉽다. 하지만 실제 모델에서는 motion noise 때문에 map이 일그러진다. 이러한 문제를 해결하기 위해서 센서 데이터간의 align을 통해 상대 위치를 계산해 나가는 scan-matching방법을 사용한다. Iterative Closest Point(ICP) 알고리즘이 scan-matching 방법 중에 하나이다.
Filter의 연산량 개선을 위한 Sparse Extended Information Filter(SEIF)를 이용한 SLAM의 계산과정을 설명한다.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이번 글에서는 이전 글에서 설명한 Sparse Extended Information Filter(SEIF) SLAM의 과정을 차근차근 한단계씩 이해해 보려고 한다. SEIF에 대해서 설명한 이전 글에서 언급했듯이 SEIF는 적은 갯수의 큰 값을 갖는 information matrix의 sparsification과정을 통해 filter의 일정한 계산속도(constant computation time)를 보장하는 방법이다. SEIF의 과정은 전체적으로 개념은 EIF와 유사하나, 각 단계마다 계산량을 줄이기위한, 즉 일정한 계산속도를 보장하기 위한 테크닉이 들어있다. 이는 large scale의 환경에서 SLAM을 구현하기 위한 꼭 필요한 테크닉이므로 정확히 이해하고 넘어가는 것이 좋다.
앞으로 각 단계를 하나씩 유도해 볼 것이며, 각 단계에서 constant한 computation time을 위해 어떤 테크닉을 사용하였는지 설명하도록 하겠다.
SEIF Motion Update
Matrix Inversion Lemma
sparse한 matrix의 inverse를 빠르게 계산하기 위해서 matrix inversion lemma를 이용한다. 이 lemma는 많은 부분에서 사용되니 기억해 두면 좋다. \[(R+PQP^T)^{-1} = R^{-1} - R^{-1}P(Q^{-1}+P^T R^{-1}P)^{-1}P^TR^{-1}\]
위 식에서 R은 크기가 큰 matrix, Q는 크기가 작은 matrix이며 P가 low dimension matrix를 high dimension으로 변형하는 직사각 행렬이다. 이때 만약 큰 matrix인 R의 inverse를 간단하게 계산할 수 있으면 전체 matrix의 inverse도 쉽게 구할 수 있다.
Motion Update step(prediction step)
목적: 이전에 iteration에서 추정된 $\xi_{t-1}, \Omega_{t-1}, \mu_{t-1}$와 control input을 이용하여 $\bar{\xi}{t-1}, \bar{\Omega}{t-1}, \bar{\mu}_{t-1}$를 계산한다.
방법: Information matrix의 sparseness를 활용하여 효율적으로 계산한다. Information matrix가 제한된 수의 non-zero off-diagonal을 갖는 sparse matrix로 가정한다.
우선 EKF SLAM에서 prediction 단계를 다시 보도록 하자. velocity-based model의 경우 위와같이 정의할 수 있다.
편의를 위하여 vector와 matrix를 $\delta, \triangle$로 표현한다.
Prediction step에서의 information matrix를 계산하기 위해 EKF의 5번 식에서 부터 계산을 시작한다. \[\begin{aligned} \bar{\Omega}_t &= \bar{\Sigma}_t^{-1}\\ &= [G_t\Omega_{t-1}^{-1}G_t^T + R_t]^{-1}\\ &= [\Phi_t^{-1} + R_t]^{-1} \end{aligned}\]
우리가 최종적으로 계산하고자 하는 information matrix는 위와 같이 계산할 수 있다. prediction step에서 가장 큰 계산량을 차지하는 부분은 $\Omega_{t-1}^{-1}$을 계산하여 $\bar{\Omega}_t$를 계산하는 과정이다. 여기서 $\Phi_t$는 다음같이 정의된다. \[\begin{aligned} \Phi_t &= [G_t \Omega_{t-1}^{-1} G_t^T]^{-1}\\ &= [G_t^T]^{-1} \Omega_{t-1} G_t^{-1} \end{aligned}\]
이제 위에서 설명한 matrix inversion lemma를 이용하여 전개하면 다음과 같다. \[\begin{aligned} \bar{\Omega}_t &= [\Phi_t^{-1} + R_t]^{-1}\\ &= [\Phi_t^{-1} + F_x^TR_t^x F_x]^{-1}\\ &= \Phi_t - \Phi_t F_x^T({R_t^{x}}^{-1}+F_x \Phi_t F_x^T)^{-1} F_x \Phi_t\\ &= \Phi_t - \kappa_t \end{aligned}\]
이제 우리가 prediction step에서 구하고자 하는 information matrix를 계산하는 식을 자세히 들여다 보도록 하자. $F_x$는 $3\times3$ block을 제외하고는 전부 0인 matrix이다. 그리고 inverse 계산을 해야하는 ${R_t^{x}}^{-1}+F_x \Phi_t F_x^T$는 $3\times3$ matrix이므로 연산량이 많지 않다. 따라서 만약
$\Phi_t$의 computation time이 constant하다면 전체 process의 computation time도 constant해 진다.
그러면 이제 $\Phi_t$의 계산과정을 들여다 보도록 하자. \[\begin{aligned} \Phi_t &= [G_t \Omega_{t-1}^{-1} G_t^T]^{-1}\\ &= [G_t^T]^{-1} \Omega_{t-1} G_t^{-1} \end{aligned}\]
$\Phi_t$를 계산하는 과정에서 가장 계산량이 많은 부분은 $G_t^{-1}$을 계산하는 부분이다. 이 부분을 자세히 살펴보도록 하자. \[\begin{aligned} G_t^{-1} &= (I+F_x^T \triangle F_x)^{-1}\\ &= \begin{pmatrix} \triangle + I_3 & 0 \\ 0 & I_{2N} \end{pmatrix}^{-1}\\ &= \begin{pmatrix} (\triangle + I_3)^{-1} & 0 \\ 0 & I_{2N} \end{pmatrix} \end{aligned}\]
즉 위 계산결과를 보면 $G_t^{-1}$의 계산속도에 영향을 주는 부분은 $(\triangle + I_3)^{-1}$이다. 이 부분은 전체 matrix의 크기에 상관없이 항상 $3\times3$ matrix이기 때문에 $G_t^{-1}$의 계산속도는 일정하다는 것을 알 수 있다. 위 식을 조금 더 정리하면 다음과 같다. \[\begin{aligned} G_t^{-1} &= (I+F_x^T \triangle F_x)^{-1}\\ &= \begin{pmatrix} \triangle + I_3 & 0 \\ 0 & I_{2N} \end{pmatrix}^{-1}\\ &= \begin{pmatrix} (\triangle + I_3)^{-1} & 0 \\ 0 & I_{2N} \end{pmatrix}\\ &= I_{3+2N} + \begin{pmatrix} (\triangle + I_3)^{-1} - I_3 & 0 \\ 0 & 0 \end{pmatrix}\\ &= I+ F_x^T[(I+\triangle)^{-1} - I]F_x\\ &= I + \Psi_t \end{aligned}\]
위의 정의를 이용하여 $\Phi_t$를 정리해 보면 \[\begin{aligned} \Phi_t &= [G_t^T]^{-1} \Omega_{t-1} G_t^{-1}\\ &= (I+\Psi_t^T) \Omega_{t-1} (I+\Psi_t)\\ &= \Omega_{t-1} +\Psi_t^T \Omega_{t-1} +\Omega_{t-1}\Psi_t + \Psi_t^T \Omega_{t-1} \Psi_t\\ &= \Omega_{t-1} + \lambda_t \end{aligned}\]
여기서 $\lambda_t$는 일정개수의 요소들만 non-zero의 값을 갖는 matrix이다.
따라서 $G_t^{-1}$의 계산속도가 constant하고 information matrix가 sparse하므로 $\Phi_t$의 계산속도는 constant하며, $\Phi_t$의 계산속도가 constant하므로 최종적으로 prediction step의 information matrix 계산량도 constant하다.
위에서 설명한 대로 일정한 계산속도를 유지하며 information matrix를 계산하는 과정을 정리하면 다음과 같다.
1. $\Psi_t$ 계산
2. $\Psi_t$를 이용하여 $\lambda_t$ 계산
3. $\lambda_t$를 이용하여 $\Phi_t$ 계산
4. $\Phi_t$를 이용하여 $\kappa_t$ 계산
5. 마지막으로 $\kappa_t$와 $\Phi_t$를 이용하여 $\bar{\Omega}_t$ 계산
위의 계산 방법을 통해 matrix크기에 상관없이 constant한 계산속도로 information matrix를 계산할 수 있다.
이제 information matrix를 구했으니 information vector를 구해야 한다. information vector는 아래의 유도과정을 통해 계산할 수 있다.
Information vector를 계산하기 위해 사용되는 값들의 대부분이 Information matrix를 구할 때 계산된 값들이며, inverse의 계산이 없기 때문에 계산량이 크지 않다.
따라서 SEIF의 prediction step에서 information matrix가 sparse 할 때, constant computation time으로 information vector와 information matrix를 계산할 수 있음을 증명하였다.
SEIF Measurement update(correction step)
SEIF의 correction step은 EIF와 마찬가지로 계산량이 크지않다. SEIF의 measurement update과정은 다음과 같다.
SEIF의 measurement update과정은 EKF와 대부분 동일하다. SEIF에서 바뀐 부분은 information vector와 matrix를 계산하는 12, 13번 과정이다. 큰 matrix의 inverse와 같은 많은 계산량을 요구하는 부분이 없기 때문에 constant computation time을 보장한다($Q_t^{-1}$는 observation의 noise로 matrix의 크기가 작으며 미리 계산할 수 있는 matrix이다). 위의 계산과정이 이해가 되지 않는다면 이전 글인 EKF SLAM을 참고하기 바란다.
SEIF Updated State Estimate
SEIF의 세번째 단계는 state의 평균값(mean, $\mu_{t}$)을 계산하는 과정이다. 계산된 평균값은 다음의 과정에서 사용된다.
Motion model의 선형화
Measurement model의 선형화
Sparsification 단계
이전 두 단계에서 information vector와 matrix를 계산하였기 때문에 mean값을 쉽게 계산할 수 있다. \[\mu = \Omega^{-1} \xi\]
하지만 이와 같이 단순한 정의에 의해서 계산하게 되면 한가지 문제가 발생한다. $\Omega$는 매우 큰 matrix이므로 이 matrix의 inverse과정은 매우 많은 계산량을 요구한다.
따라서 mean값을 계산하기 위해 optimization 방법으로 접근한다. \[\begin{aligned} \hat{\mu} &= \text{argmax} \ \ p(\mu)\\ &= \text{argmax}_{\mu} \ \ exp(-\frac{1}{2} \mu^T\Omega\mu + \xi^T\mu) \end{aligned}\]
SEIF의 이전 두 단계에서 information matrix와 vector를 계산하였기 때문에 이를 이용하여 Gaussian 분포를 표현할 수 있다. 이 Gaussian의 분포에서 mean값은 함수의 최대값을 갖는 위치 이므로 위와 같이 optimization 방법으로 mean값을 계산하는데, 함수를 미분하여 미분값이 0이 되는 $\mu$를 구하면 된다.
Optimization 방법으로 mean값을 구하는 과정에서, 로봇의 위치와 active landmark의 mean값만을 구함으로써 더 효율적으로 계산 가능하고, constant computation을 보장할 수 있다.
SEIF Sparsification
마지막은 Information matrix의 sparsification 과정이다. sparsification은 matrix의 non-zero off-diagonal의 수가 일정하게 유지되도록, 즉 sparse matrix가 유지되도록 만드는 과정이다. SEIF의 이전 step에서 constant computation을 보장하기 위한 조건이 information matrix의 sparseness이었으므로 이 과정이 꼭 필요하다.
Sparsification은 SEIF 에서 언급했던것 처럼 로봇과 landmark간의 link을 제거함으로써(conditional independence하다고 가정함으로써) non-zero off-diagonal의 수를 유지하는 것이다.
landmark는 2가지 종류로 분류할 수 있다. 현재 관측하거나 관측하고 있다고 생각하는 landmark(즉, direct link가 있는)는 active landmark이며, direct link가 없는 landmark를 passive landmark라고 한다.
sparsification 과정을 위해서 landmark를 다시 3가지로 분류한다. $m^{+}$는 현재 active landmark이며
계속 active landmark이다. $m^0$는 현재는 active landmark이지만 passive landmark로 만들 대상이다. $m^-$는 현재도 passive landmark이며 계속 passive landmark이다. \[\begin{aligned} p(x_t,m \mid z^t, u^t) &= p(x_t, m^+, m^0, m^- \mid z^t, u^t)\\ &= p(x_t \mid m^+, m^0, m^-, z^t, u^t)p(m^+, m^0, m^- \mid z^t, u^t)\\ &= p(x_t \mid m^+, m^0, m^- = 0, z^t, u^t)p(m^+, m^0, m^- \mid z^t, u^t) \end{aligned}\]
위 식에서 마지막 단계에서 우리가 만약에 $m^+$과 $m^0$에 대해서 알고있다면 $x_t$는 $m^-$에 independent하다는 사실을 이용하였다. 따라서 $m^-$의 값을 임의의 값으로 설정할 수 있으며, 여기서는 단순하게 0으로 설정하였다. 이제 위 식으로 부터 $m^0$를 제거함으로써 근사화 시킬 수 있다. passive landmark가 되는 $x^0$는 이제 로봇의 위치인 $x_t$와 independent하기 때문에 제거 가능하다. \[\begin{aligned} \tilde{p}(x_t,m \mid z^t, u^t) &= p(x_t \mid m^+, m^- = 0, z^t, u^t)p(m^0, m^+, m^- \mid z^t, u^t) \\ &= \frac{p(x_t, m^+ \mid m^- = 0, z^t, u^t)}{p(m^+ \mid m^- = 0, z^t, u^t)}p(m^0, m^+, m^- \mid z^t, u^t) \end{aligned}\]
$\tilde{}$는 sparsification을 통해 근사화를 했음을 의미한다. 이제 $\tilde{p}(x_t,m \mid z^t, u^t)$의 information matrix를 계산하는 과정이 sparsification 과정을 의미한다. \[\begin{aligned} \Omega_t &= \text{information matrix of } p(x_t, m^+, m^0, m^- \mid z^t, u^t) \\ \Omega_t' &= \text{information matrix of } p(x_t, m^+, m^0, \mid m^- = 0, z^t, u^t) \end{aligned}\]
맨 먼저 $\Omega_t’$를 계산하는 것으로 시작한다. bayes rule을 이용하여 전개하면 다음과 같다. \[p(x_t, m^+, m^0, \mid m^- = 0, z^t, u^t) = \frac{p(x_t, m^+, m^0, m^- \mid z^t, u^t)}{p(m^- = 0 \mid z^t, u^t)}\]
따라서 $\Omega_t’$는 $p(x_t, m^+, m^0, m^- \mid z^t, u^t)$의 information matrix에서 $m^-$에 해당하는 information 항을 0으로 만든 information matrix이다(information form을 이용한 Gaussian 분포 식을 생각하자). 이 matrix를 projection matrix $S$를 이용하여 표현하면 다음과 같다. \[\Omega_t' = S_{x,m^+,m^0}S_{x,m^+,m^0}^T \Omega_t S_{x,m^+,m^0} S_{x,m^+,m^0}^T\]
여기서 Projection marix는 남기고자 하는 부분이 identity로 구성된 matrix이다. 예를들어 로봇의 포즈와 첫번째 landmark만 남기고 모두 0으로 만드는 projection matrix는 다음과 같다. \[S_{x,m1} = \begin{bmatrix} I_3 & 0 & 0 & \cdots & 0\\0 & I_2 & 0 & \cdots & 0 \end{bmatrix}^T\]
따라서 $\Omega_t’$을 계산함으로써 $m^-$에 해당하는 row와 column은 0이 된다. 이 부분이 잘 이해가 되지 않는다면 matlab을 이용해서 계산해보면 쉽게 이해할 수 있다.
을 구성하는 각 항의 information matrix를 구해본다. \[\begin{aligned} \Omega_t^1 &= \text{information matrix of } p(x_t, m^+ \mid m^- = 0, z^t, u^t) \\ \Omega_t^2 &= \text{information matrix of } p(m^+ \mid m^- = 0, z^t, u^t) \\ \Omega_t^3 &= \text{information matrix of } p(m^0, m^+, m^- \mid z^t, u^t) \end{aligned}\]
각 information matrix는 information form의 marginalization을 이용하여 계산한다. information form의 marginalization은 EIF 글에 정리되어 있다. 이를 통해 각각의 information matrix를 계산하면 다음과 같다. \[\begin{aligned} \Omega_t^1 & = \Omega_t' - \Omega_t' S_{m^0}(S_{m^0}^T \Omega_t' S_{m^0})^{-1}S_{m^0}^T \Omega_t'\\ \Omega_t^2 & = \Omega_t' - \Omega_t' S_{x_t, m^0}(S_{x_t, m^0}^T \Omega_t' S_{x_t, m^0})^{-1}S_{x_t, m^0}^T \Omega_t'\\ \Omega_t^3 & = \Omega_t - \Omega_t S_{x_t}(S_{x_t}^T \Omega_t S_{x_t})^{-1}S_{x_t}^T \Omega_t\\ \end{aligned}\]
이제 계산된 information matrix를 이용하여 sparsification 된 information matrix를 계산할 수 있다. \[\begin{aligned} \tilde{\Omega}_t &= \Omega_t^1 - \Omega_t^2 + \Omega_t^3\\ &=\Omega_t - \Omega_t' S_{m^0}(S_{m^0}^T \Omega_t' S_{m^0})^{-1}S_{m^0}^T \Omega_t' \\ & \ \ + \Omega_t' S_{x_t, m^0}(S_{x_t, m^0}^T \Omega_t' S_{x_t, m^0})^{-1}S_{x_t, m^0}^T \Omega_t' \\ & \ \ - \Omega_t S_{x_t}(S_{x_t}^T \Omega_t S_{x_t})^{-1}S_{x_t}^T \Omega_t \end{aligned}\]
이제 sparsification을 통해 근사화된 information matrix가 계산되었다. 이제 information vector를 계산해야 되는데 다음과 같이 간단하게 계산할 수 있다. \[\begin{aligned} \tilde{\xi}_t &= \mu_t^T \tilde{\Omega}_t\\ &= \mu_t^T(\Omega_t - \Omega_t + \tilde{\Omega}_t)\\ &= \mu_t^T\Omega_t + \mu_t^T(\tilde{\Omega}_t - \Omega_t )\\ &= \xi_t + \mu_t^T(\tilde{\Omega}_t - \Omega_t ) \end{aligned}\]
SEIF SLAM Vs EKF SLAM
SEIF는 roughly constant time complexity, EKF는 quadratic complexity
SEIF는 linear memory complexity, EKF는 quadratic memory complexity
SEIF는 계산량의 이점을 위해 근사화를 하였기 때문에 EKF에 비해 덜 정확하다.
landmark의 갯수가 증가(matrix 크기 증가)함에 따른 계산시간의 변화를 보여준다. SEIF는 크게 변하지 않지만 EKF는 계산시간이 크게 증가한다.
landmark의 갯수 증가에 따른 메모리 크기변화를 보여준다. SEIF는 선형적으로 증가하지만 EKF는 quadratic하게 증가한다.
landmark의 갯수 증가에 따른 error를 보여준다. 근사화를 하였기 때문에 SEIF가 EKF에 비해 error는 높다.
Active landmark의 갯수에 따른 update 시간을 비교하였다. Active landmark의 갯수가 적을수록 계산시간은 빨라진다.
Active landmark의 갯수에 따른 error의 변화를 보여준다. Active landmark의 갯수가 적을수록 error는 증가한다.
