|
| 1 | +\documentclass{article} |
| 2 | + |
| 3 | +\usepackage{amsfonts} |
| 4 | +\usepackage{amsmath,amssymb,amsfonts} |
| 5 | +\usepackage{textcomp} |
| 6 | +\usepackage{fullpage} |
| 7 | +\usepackage{setspace} |
| 8 | +\usepackage{float} |
| 9 | +\usepackage{cite} |
| 10 | +\usepackage{graphicx} |
| 11 | +\usepackage{caption} |
| 12 | +\usepackage{subcaption} |
| 13 | +\usepackage[pdfborder={0 0 0}, pdfpagemode=UseNone, pdfstartview=FitH]{hyperref} |
| 14 | + |
| 15 | +\DeclareMathOperator*{\argmax}{arg\,max} |
| 16 | +\DeclareMathOperator*{\argmin}{arg\,min} |
| 17 | + |
| 18 | +\def\keyterm{\textit} |
| 19 | + |
| 20 | +\newcommand{\transp}{{\scriptstyle{\mathsf{T}}}} |
| 21 | + |
| 22 | + |
| 23 | + |
| 24 | +\begin{document} |
| 25 | + |
| 26 | +\title{Graph SLAM Formulation} |
| 27 | +\author{Jeff Irion} |
| 28 | +\date{} |
| 29 | + |
| 30 | +\maketitle |
| 31 | +\vspace{3em} |
| 32 | + |
| 33 | + |
| 34 | +\section{Problem Formulation} |
| 35 | + |
| 36 | +Let a robot's trajectory through its environment be represented by a sequence of $N$ poses: $\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_N$. Each pose lies on a manifold: $\mathbf{p}_i \in \mathcal{M}$. Simple examples of manifolds used in Graph SLAM include 1-D, 2-D, and 3-D space, i.e., $\mathbb{R}$, $\mathbb{R}^2$, and $\mathbb{R}^3$. These environments are \keyterm{rectilinear}, meaning that there is no concept of orientation. By contrast, in $SE(2)$ problem settings a robot's pose consists of its location in $\mathbb{R}^2$ and its orientation $\theta$. Similarly, in $SE(3)$ a robot's pose consists of its location in $\mathbb{R}^3$ and its orientation, which can be represented via Euler angles, quaternions, or $SO(3)$ rotation matrices. |
| 37 | + |
| 38 | +As the robot explores its environment, it collects a set of $M$ measurements $\mathcal{Z} = \{\mathbf{z}_j\}$. Examples of such measurements include odometry, GPS, and IMU data. Given a set of poses $\mathbf{p}_1, \ldots, \mathbf{p}_N$, we can compute the estimated measurement $\hat{\mathbf{z}}_j(\mathbf{p}_1, \ldots, \mathbf{p}_N)$. We can then compute the \keyterm{residual} $\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j)$ for measurement $j$. The formula for the residual depends on the type of measurement. As an example, let $\mathbf{z}_1$ be an odometry measurement that was collected when the robot traveled from $\mathbf{p}_1$ to $\mathbf{p}_2$. The expected measurement and the residual are computed as |
| 39 | +% |
| 40 | +\begin{align*} |
| 41 | + \hat{\mathbf{z}}_1(\mathbf{p}_1, \mathbf{p}_2) &= \mathbf{p}_2 \ominus \mathbf{p}_1 \\ |
| 42 | + \mathbf{e}_1(\mathbf{z}_1, \hat{\mathbf{z}}_1) &= \mathbf{z}_1 \ominus \hat{\mathbf{z}}_1 = \mathbf{z}_1 \ominus (\mathbf{p}_2 \ominus \mathbf{p}_1), |
| 43 | +\end{align*} |
| 44 | +% |
| 45 | +where the $\ominus$ operator indicates inverse pose composition. We model measurement $\mathbf{z}_j$ as having independent Gaussian noise with zero mean and covariance matrix $\Omega_j^{-1}$; we refer to $\Omega_j$ as the \keyterm{information matrix} for measurement $j$. That is, |
| 46 | +\begin{equation} |
| 47 | + p(\mathbf{z}_j \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) = \eta_j \exp \left( (-\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^\transp \Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j) \right), \label{eq:observation_probability} |
| 48 | +\end{equation} |
| 49 | +where $\eta_j$ is the normalization constant. |
| 50 | + |
| 51 | +The objective of Graph SLAM is to find the maximum likelihood set of poses given the measurements $\mathcal{Z} = \{\mathbf{z}_j\}$; in other words, we want to find |
| 52 | +% |
| 53 | +\begin{equation*} |
| 54 | + \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z}) |
| 55 | +\end{equation*} |
| 56 | +% |
| 57 | +Using Bayes' rule, we can write this probability as |
| 58 | +% |
| 59 | +\begin{align} |
| 60 | + p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z}) &= \frac{p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) p(\mathbf{p}_1, \ldots, \mathbf{p}_N) }{ p(\mathcal{Z}) } \notag \\ |
| 61 | + &\propto p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N), \label{eq:bayes} |
| 62 | +\end{align} |
| 63 | +% |
| 64 | +since $p(\mathcal{Z})$ is a constant (albeit, an unknown constant) and we assume that $p(\mathbf{p}_1, \ldots, \mathbf{p}_N)$ is uniformly distributed \cite{thrun2006graph}. Therefore, we can use \eqref{eq:observation_probability} and \eqref{eq:bayes} to simplify the Graph SLAM optimization as follows: |
| 65 | +% |
| 66 | +\begin{align*} |
| 67 | + \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z}) &= \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) \\ |
| 68 | + &= \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \prod_{j=1}^M p(\mathbf{z}_j \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) \\ |
| 69 | + &= \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \prod_{j=1}^M \exp \left( -(\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^\transp \Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j) \right) \\ |
| 70 | + &= \argmin_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \sum_{j=1}^M (\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^\transp \Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j). |
| 71 | +\end{align*} |
| 72 | +% |
| 73 | +We define |
| 74 | +% |
| 75 | +\begin{equation*} |
| 76 | + \chi^2 := \sum_{j=1}^M (\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^\transp \Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j), |
| 77 | +\end{equation*} |
| 78 | +% |
| 79 | +and this is what we seek to minimize. |
| 80 | + |
| 81 | + |
| 82 | +\section{Dimensionality and Pose Representation} |
| 83 | + |
| 84 | +Before proceeding further, it is helpful to discuss the dimensionality of the problem. We have: |
| 85 | +\begin{itemize} |
| 86 | + \item A set of $N$ poses $\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_N$, where each pose lies on the manifold $\mathcal{M}$ |
| 87 | + \begin{itemize} |
| 88 | + \item Each pose $\mathbf{p}_i$ is represented as a vector in (a subset of) $\mathbb{R}^d$. For example: |
| 89 | + \begin{itemize} |
| 90 | + \item[$\circ$] An $SE(2)$ pose is typically represented as $(x, y, \theta)$, and thus $d = 3$. |
| 91 | + \item[$\circ$] An $SE(3)$ pose is typically represented as $(x, y, z, q_x, q_y, q_z, q_w)$, where $(x, y, z)$ is a point in $\mathbb{R}^3$ and $(q_x, q_y, q_z, q_w)$ is a \keyterm{quaternion}, and so $d = 7$. For more information about $SE(3)$ parameterizations and pose transformations, see \cite{blanco2010tutorial}. |
| 92 | + \end{itemize} |
| 93 | + \item We also need to be able to represent each pose compactly as a vector in (a subset of) $\mathbb{R}^c$. |
| 94 | + \begin{itemize} |
| 95 | + \item[$\circ$] Since an $SE(2)$ pose has three degrees of freedom, the $(x, y, \theta)$ representation is again sufficient and $c=3$. |
| 96 | + \item[$\circ$] An $SE(3)$ pose only has six degrees of freedom, and we can represent it compactly as $(x, y, z, q_x, q_y, q_z)$, and thus $c=6$. |
| 97 | + \end{itemize} |
| 98 | + \item We use the $\boxplus$ operator to indicate pose composition when one or both of the poses are represented compactly. The output can be a pose in $\mathcal{M}$ or a vector in $\mathbb{R}^c$, as required by context. |
| 99 | + \end{itemize} |
| 100 | + \item A set of $M$ measurements $\mathcal{Z} = \{\mathbf{z}_1, \mathbf{z}_2, \ldots, \mathbf{z}_M\}$ |
| 101 | + \begin{itemize} |
| 102 | + \item Each measurement's dimensionality can be unique, and we will use $\bullet$ to denote a ``wildcard'' variable. |
| 103 | + \item Measurement $\mathbf{z}_j \in \mathbb{R}^\bullet$ has an associated information matrix $\Omega_j \in \mathbb{R}^{\bullet \times \bullet}$ and residual function $\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j) = \mathbf{e}_j(\mathbf{z}_j, \mathbf{p}_1, \ldots, \mathbf{p}_N) \in \mathbb{R}^\bullet$. |
| 104 | + \item A measurement could, in theory, constrain anywhere from 1 pose to all $N$ poses. In practice, each measurement usually constrains only 1 or 2 poses. |
| 105 | + \end{itemize} |
| 106 | +\end{itemize} |
| 107 | + |
| 108 | + |
| 109 | +\section{Graph SLAM Algorithm} |
| 110 | + |
| 111 | +The ``Graph'' in Graph SLAM refers to the fact that we view the problem as a graph. The graph has a set $\mathcal{V}$ of $N$ vertices, where each vertex $v_i$ has an associated pose $\mathbf{p}_i$. Similarly, the graph has a set $\mathcal{E}$ of $M$ edges, where each edge $e_j$ has an associated measurement $\mathbf{z}_j$. In practice, the edges in this graph are either unary (i.e., a loop) or binary. (Note: $e_j$ refers to the edge in the graph associated with measurement $\mathbf{z}_j$, whereas $\mathbf{e}_j$ refers to the residual function associated with $\mathbf{z}_j$.) For more information about the Graph SLAM algorithm, see \cite{grisetti2010tutorial}. |
| 112 | + |
| 113 | +We want to optimize |
| 114 | +% |
| 115 | +\begin{equation*} |
| 116 | + \chi^2 = \sum_{e_j \in \mathcal{E}} \mathbf{e}_j^\transp \Omega_j \mathbf{e}_j. |
| 117 | +\end{equation*} |
| 118 | +% |
| 119 | +Let $\mathbf{x}_i \in \mathbb{R}^c$ be the compact representation of pose $\mathbf{p}_i \in \mathcal{M}$, and let |
| 120 | +% |
| 121 | +\begin{equation*} |
| 122 | + \mathbf{x} := \begin{bmatrix} \mathbf{x}_1 \\ \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_N \end{bmatrix} \in \mathbb{R}^{cN} |
| 123 | +\end{equation*} |
| 124 | +% |
| 125 | +We will solve this optimization problem iteratively. Let |
| 126 | +% |
| 127 | +\begin{equation} |
| 128 | + \mathbf{x}^{k+1} := \mathbf{x}^k \boxplus \Delta \mathbf{x}^k = \begin{bmatrix} \mathbf{x}_1 \boxplus \Delta \mathbf{x}_1 \\ \mathbf{x}_2 \boxplus \Delta \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_N \boxplus \Delta \mathbf{x}_2 \end{bmatrix} \label{eq:update} |
| 129 | +\end{equation} |
| 130 | +% |
| 131 | +The $\chi^2$ error at iteration $k+1$ is |
| 132 | +\begin{equation} |
| 133 | + \chi_{k+1}^2 = \sum_{e_j \in \mathcal{E}} \underbrace{\left[ \mathbf{e}_j(\mathbf{x}^{k+1}) \right]^\transp}_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\mathbf{e}_j(\mathbf{x}^{k+1})}_{\bullet \times 1}. \label{eq:chisq_at_kplusone} |
| 134 | +\end{equation} |
| 135 | +% |
| 136 | +We will linearize the residuals as: |
| 137 | +% |
| 138 | +\begin{align} |
| 139 | + \mathbf{e}_j(\mathbf{x}^{k+1}) &= \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \notag \\ |
| 140 | + &\approx \mathbf{e}_j(\mathbf{x}^{k}) + \frac{\partial}{\partial \Delta \mathbf{x}^k} \left[ \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \right] \Delta \mathbf{x}^k \notag \\ |
| 141 | + &= \mathbf{e}_j(\mathbf{x}^{k}) + \left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right) \frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k} \Delta \mathbf{x}^k. \label{eq:linearization} |
| 142 | +\end{align} |
| 143 | +% |
| 144 | +Plugging \eqref{eq:linearization} into \eqref{eq:chisq_at_kplusone}, we get: |
| 145 | +% |
| 146 | +\small |
| 147 | +\begin{align} |
| 148 | + \chi_{k+1}^2 &\approx \ \ \ \ \ \sum_{e_j \in \mathcal{E}} \underbrace{[ \mathbf{e}_j(\mathbf{x}^k)]^\transp}_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\mathbf{e}_j(\mathbf{x}^k)}_{\bullet \times 1} \notag \\ |
| 149 | + &\hphantom{\approx} \ \ \ + \sum_{e_j \in \mathcal{E}} \underbrace{[ \mathbf{e}_j(\mathbf{x^k}) ]^\transp }_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN} \underbrace{\Delta \mathbf{x}^k}_{cN \times 1} \notag \\ |
| 150 | + &\hphantom{\approx} \ \ \ + \sum_{e_j \in \mathcal{E}} \underbrace{(\Delta \mathbf{x}^k)^\transp}_{1 \times cN} \underbrace{ \left( \frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k} \right)^\transp}_{cN \times dN} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)^\transp}_{dN \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN} \underbrace{\Delta \mathbf{x}^k}_{cN \times 1} \notag \\ |
| 151 | + &= \chi_k^2 + 2 \mathbf{b}^\transp \Delta \mathbf{x}^k + (\Delta \mathbf{x}^k)^\transp H \Delta \mathbf{x}^k, \notag |
| 152 | +\end{align} |
| 153 | +\normalsize |
| 154 | +% |
| 155 | +where |
| 156 | +% |
| 157 | +\begin{align*} |
| 158 | + \mathbf{b}^\transp &= \sum_{e_j \in \mathcal{E}} \underbrace{[ \mathbf{e}_j(\mathbf{x^k}) ]^\transp }_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN} \\ |
| 159 | + H &= \sum_{e_j \in \mathcal{E}} \underbrace{ \left( \frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k} \right)^\transp}_{cN \times dN} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)^\transp}_{dN \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN}. |
| 160 | +\end{align*} |
| 161 | +% |
| 162 | +Using this notation, we obtain the optimal update as |
| 163 | +% |
| 164 | +\begin{equation} |
| 165 | + \Delta \mathbf{x}^k = -H^{-1} \mathbf{b}. \label{eq:deltax} |
| 166 | +\end{equation} |
| 167 | +% |
| 168 | +We apply this update to the poses via \eqref{eq:update} and repeat until convergence. |
| 169 | + |
| 170 | + |
| 171 | + |
| 172 | +\bibliographystyle{acm} |
| 173 | +\bibliography{graphSLAM}{} |
| 174 | + |
| 175 | +\end{document} |
0 commit comments