Merge pull request #302 from JeffLIrion/graphslam

Add Graph SLAM documentation and SE(2) example

SLAM/GraphBasedSLAM/LaTeX/.gitignore

@@ -0,0 +1,3 @@
+# Ignore LaTeX generated files
+graphSLAM_formulation.*
+!graphSLAM_formulation.tex

SLAM/GraphBasedSLAM/LaTeX/graphSLAM.bib

@@ -0,0 +1,30 @@
+@article{blanco2010tutorial,
+  title={A tutorial on ${SE}(3)$ transformation parameterizations and on-manifold optimization},
+  author={Blanco, Jose-Luis},
+  journal={University of Malaga, Tech. Rep},
+  volume={3},
+  year={2010},
+  publisher={Citeseer}
+}
+
+@article{grisetti2010tutorial,
+  title={A tutorial on graph-based {SLAM}},
+  author={Grisetti, Giorgio and Kummerle, Rainer and Stachniss, Cyrill and Burgard, Wolfram},
+  journal={IEEE Intelligent Transportation Systems Magazine},
+  volume={2},
+  number={4},
+  pages={31--43},
+  year={2010},
+  publisher={IEEE}
+}
+
+@article{thrun2006graph,
+  title={The graph {SLAM} algorithm with applications to large-scale mapping of urban structures},
+  author={Thrun, Sebastian and Montemerlo, Michael},
+  journal={The International Journal of Robotics Research},
+  volume={25},
+  number={5-6},
+  pages={403--429},
+  year={2006},
+  publisher={SAGE Publications}
+}

SLAM/GraphBasedSLAM/LaTeX/graphSLAM_formulation.tex

@@ -0,0 +1,175 @@
+\documentclass{article}
+
+\usepackage{amsfonts}
+\usepackage{amsmath,amssymb,amsfonts}
+\usepackage{textcomp}
+\usepackage{fullpage}
+\usepackage{setspace}
+\usepackage{float}
+\usepackage{cite}
+\usepackage{graphicx}
+\usepackage{caption}
+\usepackage{subcaption}
+\usepackage[pdfborder={0 0 0}, pdfpagemode=UseNone, pdfstartview=FitH]{hyperref}
+
+\DeclareMathOperator*{\argmax}{arg\,max}
+\DeclareMathOperator*{\argmin}{arg\,min}
+
+\def\keyterm{\textit}
+
+\newcommand{\transp}{{\scriptstyle{\mathsf{T}}}}
+
+
+
+\begin{document}
+
+\title{Graph SLAM Formulation}
+\author{Jeff Irion}
+\date{}
+
+\maketitle
+\vspace{3em}
+
+
+\section{Problem Formulation}
+
+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.  
+
+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
+%
+\begin{align*}
+    \hat{\mathbf{z}}_1(\mathbf{p}_1, \mathbf{p}_2) &= \mathbf{p}_2 \ominus \mathbf{p}_1 \\
+    \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),
+\end{align*}
+%
+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,
+\begin{equation}
+    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}
+\end{equation}
+where $\eta_j$ is the normalization constant.
+
+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 
+%
+\begin{equation*}
+    \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z}) 
+\end{equation*}
+%
+Using Bayes' rule, we can write this probability as
+%
+\begin{align}
+    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 \\
+    &\propto p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N), \label{eq:bayes}
+\end{align}
+%
+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:
+%
+\begin{align*}
+    \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) \\
+    &= \argmax_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \prod_{j=1}^M p(\mathbf{z}_j \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) \\
+    &= \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) \\
+    &= \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).
+\end{align*}
+%
+We define
+%
+\begin{equation*}
+    \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),
+\end{equation*}
+%
+and this is what we seek to minimize.
+
+
+\section{Dimensionality and Pose Representation}
+
+Before proceeding further, it is helpful to discuss the dimensionality of the problem.  We have:
+\begin{itemize}
+  \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}$
+  \begin{itemize}
+    \item Each pose $\mathbf{p}_i$ is represented as a vector in (a subset of) $\mathbb{R}^d$.  For example:
+    \begin{itemize}
+      \item[$\circ$] An $SE(2)$ pose is typically represented as $(x, y, \theta)$, and thus $d = 3$.
+      \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}.
+    \end{itemize}
+    \item We also need to be able to represent each pose compactly as a vector in (a subset of) $\mathbb{R}^c$.
+    \begin{itemize}
+      \item[$\circ$] Since an $SE(2)$ pose has three degrees of freedom, the $(x, y, \theta)$ representation is again sufficient and $c=3$.  
+      \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$.
+    \end{itemize}
+    \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.
+  \end{itemize}
+  \item A set of $M$ measurements $\mathcal{Z} = \{\mathbf{z}_1, \mathbf{z}_2, \ldots, \mathbf{z}_M\}$
+  \begin{itemize}
+    \item Each measurement's dimensionality can be unique, and we will use $\bullet$ to denote a ``wildcard'' variable.
+    \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$.
+    \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.  
+  \end{itemize}
+\end{itemize}
+
+
+\section{Graph SLAM Algorithm}
+
+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}.
+
+We want to optimize
+%
+\begin{equation*}
+    \chi^2 = \sum_{e_j \in \mathcal{E}} \mathbf{e}_j^\transp \Omega_j \mathbf{e}_j.
+\end{equation*}
+%
+Let $\mathbf{x}_i \in \mathbb{R}^c$ be the compact representation of pose $\mathbf{p}_i \in \mathcal{M}$, and let
+%
+\begin{equation*}
+    \mathbf{x} := \begin{bmatrix} \mathbf{x}_1 \\ \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_N \end{bmatrix} \in \mathbb{R}^{cN}
+\end{equation*}
+%
+We will solve this optimization problem iteratively.  Let
+%
+\begin{equation}
+    \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}
+\end{equation}
+%
+The $\chi^2$ error at iteration $k+1$ is
+\begin{equation}
+    \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}
+\end{equation}
+%
+We will linearize the residuals as:
+%
+\begin{align}
+    \mathbf{e}_j(\mathbf{x}^{k+1}) &= \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \notag \\
+    &\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 \\
+    &= \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}
+\end{align}
+%
+Plugging \eqref{eq:linearization} into \eqref{eq:chisq_at_kplusone}, we get:
+%
+\small
+\begin{align}
+    \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 \\
+    &\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 \\
+    &\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 \\
+    &= \chi_k^2 + 2 \mathbf{b}^\transp \Delta \mathbf{x}^k + (\Delta \mathbf{x}^k)^\transp H \Delta \mathbf{x}^k,  \notag
+\end{align}
+\normalsize
+%
+where
+%
+\begin{align*}
+    \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} \\
+    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}.
+\end{align*}
+%
+Using this notation, we obtain the optimal update as
+%
+\begin{equation}
+    \Delta \mathbf{x}^k = -H^{-1} \mathbf{b}.  \label{eq:deltax}
+\end{equation}
+%
+We apply this update to the poses via \eqref{eq:update} and repeat until convergence.
+
+
+
+\bibliographystyle{acm}
+\bibliography{graphSLAM}{}
+
+\end{document}

SLAM/GraphBasedSLAM/data/README.rst

@@ -0,0 +1,6 @@
+Acknowledgments and References
+==============================
+
+Thanks to Luca Larlone for allowing inclusion of the `Intel dataset <https://lucacarlone.mit.edu/datasets/>`_ in this repo.
+
+1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization <https://arxiv.org/pdf/1211.3063.pdf>`_. IEEE Transactions on Robotics, 30(2), pp.475-492.

SLAM/GraphBasedSLAM/graphslam/__init__.py

@@ -0,0 +1,9 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+"""Graph SLAM solver in Python.
+
+"""

SLAM/GraphBasedSLAM/graphslam/edge/__init__.py

@@ -0,0 +1,5 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam

SLAM/GraphBasedSLAM/graphslam/edge/edge_odometry.py

@@ -0,0 +1,180 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+r"""A class for odometry edges.
+
+"""
+
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+#: The difference that will be used for numerical differentiation
+EPSILON = 1e-6
+
+
+class EdgeOdometry:
+    r"""A class for representing odometry edges in Graph SLAM.
+
+    Parameters
+    ----------
+    vertices : list[graphslam.vertex.Vertex]
+        A list of the vertices constrained by the edge
+    information : np.ndarray
+        The information matrix :math:`\Omega_j` associated with the edge
+    estimate : graphslam.pose.se2.PoseSE2
+        The expected measurement :math:`\mathbf{z}_j`
+
+    Attributes
+    ----------
+    vertices : list[graphslam.vertex.Vertex]
+        A list of the vertices constrained by the edge
+    information : np.ndarray
+        The information matrix :math:`\Omega_j` associated with the edge
+    estimate : PoseSE2
+        The expected measurement :math:`\mathbf{z}_j`
+
+    """
+    def __init__(self, vertex_ids, information, estimate, vertices=None):
+        self.vertex_ids = vertex_ids
+        self.information = information
+        self.estimate = estimate
+        self.vertices = vertices
+
+    def calc_error(self):
+        r"""Calculate the error for the edge: :math:`\mathbf{e}_j \in \mathbb{R}^\bullet`.
+
+        .. math::
+
+           \mathbf{e}_j = \mathbf{z}_j - (p_2 \ominus p_1)
+
+
+        Returns
+        -------
+        np.ndarray
+            The error for the edge
+
+        """
+        return (self.estimate - (self.vertices[1].pose - self.vertices[0].pose)).to_compact()
+
+    def calc_chi2(self):
+        r"""Calculate the :math:`\chi^2` error for the edge.
+
+        .. math::
+
+           \mathbf{e}_j^T \Omega_j \mathbf{e}_j
+
+
+        Returns
+        -------
+        float
+            The :math:`\chi^2` error for the edge
+
+        """
+        err = self.calc_error()
+
+        return np.dot(np.dot(np.transpose(err), self.information), err)
+
+    def calc_chi2_gradient_hessian(self):
+        r"""Calculate the edge's contributions to the graph's :math:`\chi^2` error, gradient (:math:`\mathbf{b}`), and Hessian (:math:`H`).
+
+        Returns
+        -------
+        float
+            The :math:`\chi^2` error for the edge
+        dict
+            The edge's contribution(s) to the gradient
+        dict
+            The edge's contribution(s) to the Hessian
+
+        """
+        chi2 = self.calc_chi2()
+
+        err = self.calc_error()
+
+        jacobians = self.calc_jacobians()
+
+        return chi2, {v.index: np.dot(np.dot(np.transpose(err), self.information), jacobian) for v, jacobian in zip(self.vertices, jacobians)}, {(self.vertices[i].index, self.vertices[j].index): np.dot(np.dot(np.transpose(jacobians[i]), self.information), jacobians[j]) for i in range(len(jacobians)) for j in range(i, len(jacobians))}
+
+    def calc_jacobians(self):
+        r"""Calculate the Jacobian of the edge's error with respect to each constrained pose.
+
+        .. math::
+
+           \frac{\partial}{\partial \Delta \mathbf{x}^k} \left[ \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \right]
+
+
+        Returns
+        -------
+        list[np.ndarray]
+            The Jacobian matrices for the edge with respect to each constrained pose
+
+        """
+        err = self.calc_error()
+
+        # The dimensionality of the compact pose representation
+        dim = len(self.vertices[0].pose.to_compact())
+
+        return [self._calc_jacobian(err, dim, i) for i in range(len(self.vertices))]
+
+    def _calc_jacobian(self, err, dim, vertex_index):
+        r"""Calculate the Jacobian of the edge with respect to the specified vertex's pose.
+
+        Parameters
+        ----------
+        err : np.ndarray
+            The current error for the edge (see :meth:`EdgeOdometry.calc_error`)
+        dim : int
+            The dimensionality of the compact pose representation
+        vertex_index : int
+            The index of the vertex (pose) for which we are computing the Jacobian
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of the edge with respect to the specified vertex's pose
+
+        """
+        jacobian = np.zeros(err.shape + (dim,))
+        p0 = self.vertices[vertex_index].pose.copy()
+
+        for d in range(dim):
+            # update the pose
+            delta_pose = np.zeros(dim)
+            delta_pose[d] = EPSILON
+            self.vertices[vertex_index].pose += delta_pose
+
+            # compute the numerical derivative
+            jacobian[:, d] = (self.calc_error() - err) / EPSILON
+
+            # restore the pose
+            self.vertices[vertex_index].pose = p0.copy()
+
+        return jacobian
+
+    def to_g2o(self):
+        """Export the edge to the .g2o format.
+
+        Returns
+        -------
+        str
+            The edge in .g2o format
+
+        """
+        return "EDGE_SE2 {} {} {} {} {} ".format(self.vertex_ids[0], self.vertex_ids[1], self.estimate[0], self.estimate[1], self.estimate[2]) + " ".join([str(x) for x in self.information[np.triu_indices(3, 0)]]) + "\n"
+
+    def plot(self, color='b'):
+        """Plot the edge.
+
+        Parameters
+        ----------
+        color : str
+            The color that will be used to plot the edge
+
+        """
+        xy = np.array([v.pose.position for v in self.vertices])
+        plt.plot(xy[:, 0], xy[:, 1], color=color)

SLAM/GraphBasedSLAM/graphslam/graph.py

@@ -0,0 +1,265 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+r"""A ``Graph`` class that stores the edges and vertices required for Graph SLAM.
+
+"""
+
+
+from collections import defaultdict
+from functools import reduce
+import warnings
+
+import numpy as np
+from scipy.sparse import SparseEfficiencyWarning, lil_matrix
+from scipy.sparse.linalg import spsolve
+import matplotlib.pyplot as plt
+
+
+warnings.simplefilter("ignore", SparseEfficiencyWarning)
+warnings.filterwarnings("ignore", category=SparseEfficiencyWarning)
+
+
+# pylint: disable=too-few-public-methods
+class _Chi2GradientHessian:
+    r"""A class that is used to aggregate the :math:`\chi^2` error, gradient, and Hessian.
+
+    Parameters
+    ----------
+    dim : int
+        The compact dimensionality of the poses
+
+    Attributes
+    ----------
+    chi2 : float
+        The :math:`\chi^2` error
+    dim : int
+        The compact dimensionality of the poses
+    gradient : defaultdict
+        The contributions to the gradient vector
+    hessian : defaultdict
+        The contributions to the Hessian matrix
+
+    """
+    def __init__(self, dim):
+        self.chi2 = 0.
+        self.dim = dim
+        self.gradient = defaultdict(lambda: np.zeros(dim))
+        self.hessian = defaultdict(lambda: np.zeros((dim, dim)))
+
+    @staticmethod
+    def update(chi2_grad_hess, incoming):
+        r"""Update the :math:`\chi^2` error and the gradient and Hessian dictionaries.
+
+        Parameters
+        ----------
+        chi2_grad_hess : _Chi2GradientHessian
+            The ``_Chi2GradientHessian`` that will be updated
+        incoming : tuple
+            TODO
+
+        """
+        chi2_grad_hess.chi2 += incoming[0]
+
+        for idx, contrib in incoming[1].items():
+            chi2_grad_hess.gradient[idx] += contrib
+
+        for (idx1, idx2), contrib in incoming[2].items():
+            if idx1 <= idx2:
+                chi2_grad_hess.hessian[idx1, idx2] += contrib
+            else:
+                chi2_grad_hess.hessian[idx2, idx1] += np.transpose(contrib)
+
+        return chi2_grad_hess
+
+
+class Graph(object):
+    r"""A graph that will be optimized via Graph SLAM.
+
+    Parameters
+    ----------
+    edges : list[graphslam.edge.edge_odometry.EdgeOdometry]
+        A list of the vertices in the graph
+    vertices : list[graphslam.vertex.Vertex]
+        A list of the vertices in the graph
+
+    Attributes
+    ----------
+    _chi2 : float, None
+        The current :math:`\chi^2` error, or ``None`` if it has not yet been computed
+    _edges : list[graphslam.edge.edge_odometry.EdgeOdometry]
+        A list of the edges (i.e., constraints) in the graph
+    _gradient : numpy.ndarray, None
+        The gradient :math:`\mathbf{b}` of the :math:`\chi^2` error, or ``None`` if it has not yet been computed
+    _hessian : scipy.sparse.lil_matrix, None
+        The Hessian matrix :math:`H`, or ``None`` if it has not yet been computed
+    _vertices : list[graphslam.vertex.Vertex]
+        A list of the vertices in the graph
+
+    """
+    def __init__(self, edges, vertices):
+        # The vertices and edges lists
+        self._edges = edges
+        self._vertices = vertices
+
+        # The chi^2 error, gradient, and Hessian
+        self._chi2 = None
+        self._gradient = None
+        self._hessian = None
+
+        self._link_edges()
+
+    def _link_edges(self):
+        """Fill in the ``vertices`` attributes for the graph's edges.
+
+        """
+        index_id_dict = {i: v.id for i, v in enumerate(self._vertices)}
+        id_index_dict = {v_id: v_index for v_index, v_id in index_id_dict.items()}
+
+        # Fill in the vertices' `index` attribute
+        for v in self._vertices:
+            v.index = id_index_dict[v.id]
+
+        for e in self._edges:
+            e.vertices = [self._vertices[id_index_dict[v_id]] for v_id in e.vertex_ids]
+
+    def calc_chi2(self):
+        r"""Calculate the :math:`\chi^2` error for the ``Graph``.
+
+        Returns
+        -------
+        float
+            The :math:`\chi^2` error
+
+        """
+        self._chi2 = sum((e.calc_chi2() for e in self._edges))
+        return self._chi2
+
+    def _calc_chi2_gradient_hessian(self):
+        r"""Calculate the :math:`\chi^2` error, the gradient :math:`\mathbf{b}`, and the Hessian :math:`H`.
+
+        """
+        n = len(self._vertices)
+        dim = len(self._vertices[0].pose.to_compact())
+        chi2_gradient_hessian = reduce(_Chi2GradientHessian.update, (e.calc_chi2_gradient_hessian() for e in self._edges), _Chi2GradientHessian(dim))
+
+        self._chi2 = chi2_gradient_hessian.chi2
+
+        # Fill in the gradient vector
+        self._gradient = np.zeros(n * dim, dtype=np.float64)
+        for idx, contrib in chi2_gradient_hessian.gradient.items():
+            self._gradient[idx * dim: (idx + 1) * dim] += contrib
+
+        # Fill in the Hessian matrix
+        self._hessian = lil_matrix((n * dim, n * dim), dtype=np.float64)
+        for (row_idx, col_idx), contrib in chi2_gradient_hessian.hessian.items():
+            self._hessian[row_idx * dim: (row_idx + 1) * dim, col_idx * dim: (col_idx + 1) * dim] = contrib
+
+            if row_idx != col_idx:
+                self._hessian[col_idx * dim: (col_idx + 1) * dim, row_idx * dim: (row_idx + 1) * dim] = np.transpose(contrib)
+
+    def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
+        r"""Optimize the :math:`\chi^2` error for the ``Graph``.
+
+        Parameters
+        ----------
+        tol : float
+            If the relative decrease in the :math:`\chi^2` error between iterations is less than ``tol``, we will stop
+        max_iter : int
+            The maximum number of iterations
+        fix_first_pose : bool
+            If ``True``, we will fix the first pose
+
+        """
+        n = len(self._vertices)
+        dim = len(self._vertices[0].pose.to_compact())
+
+        # Previous iteration's chi^2 error
+        chi2_prev = -1.
+
+        # For displaying the optimization progress
+        print("\nIteration                chi^2        rel. change")
+        print("---------                -----        -----------")
+
+        for i in range(max_iter):
+            self._calc_chi2_gradient_hessian()
+
+            # Check for convergence (from the previous iteration); this avoids having to calculate chi^2 twice
+            if i > 0:
+                rel_diff = (chi2_prev - self._chi2) / (chi2_prev + np.finfo(float).eps)
+                print("{:9d} {:20.4f} {:18.6f}".format(i, self._chi2, -rel_diff))
+                if self._chi2 < chi2_prev and rel_diff < tol:
+                    return
+            else:
+                print("{:9d} {:20.4f}".format(i, self._chi2))
+
+            # Update the previous iteration's chi^2 error
+            chi2_prev = self._chi2
+
+            # Hold the first pose fixed
+            if fix_first_pose:
+                self._hessian[:dim, :] = 0.
+                self._hessian[:, :dim] = 0.
+                self._hessian[:dim, :dim] += np.eye(dim)
+                self._gradient[:dim] = 0.
+
+            # Solve for the updates
+            dx = spsolve(self._hessian, -self._gradient)  # pylint: disable=invalid-unary-operand-type
+
+            # Apply the updates
+            for v, dx_i in zip(self._vertices, np.split(dx, n)):
+                v.pose += dx_i
+
+        # If we reached the maximum number of iterations, print out the final iteration's results
+        self.calc_chi2()
+        rel_diff = (chi2_prev - self._chi2) / (chi2_prev + np.finfo(float).eps)
+        print("{:9d} {:20.4f} {:18.6f}".format(max_iter, self._chi2, -rel_diff))
+
+    def to_g2o(self, outfile):
+        """Save the graph in .g2o format.
+
+        Parameters
+        ----------
+        outfile : str
+            The path where the graph will be saved
+
+        """
+        with open(outfile, 'w') as f:
+            for v in self._vertices:
+                f.write(v.to_g2o())
+
+            for e in self._edges:
+                f.write(e.to_g2o())
+
+    def plot(self, vertex_color='r', vertex_marker='o', vertex_markersize=3, edge_color='b', title=None):
+        """Plot the graph.
+
+        Parameters
+        ----------
+        vertex_color : str
+            The color that will be used to plot the vertices
+        vertex_marker : str
+            The marker that will be used to plot the vertices
+        vertex_markersize : int
+            The size of the plotted vertices
+        edge_color : str
+            The color that will be used to plot the edges
+        title : str, None
+            The title that will be used for the plot
+
+        """
+        plt.figure()
+
+        for e in self._edges:
+            e.plot(edge_color)
+
+        for v in self._vertices:
+            v.plot(vertex_color, vertex_marker, vertex_markersize)
+
+        if title:
+            plt.title(title)
+
+        plt.show()

SLAM/GraphBasedSLAM/graphslam/load.py

@@ -0,0 +1,66 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+"""Functions for loading graphs.
+
+"""
+
+
+import logging
+
+import numpy as np
+
+from .edge.edge_odometry import EdgeOdometry
+from .graph import Graph
+from .pose.se2 import PoseSE2
+from .util import upper_triangular_matrix_to_full_matrix
+from .vertex import Vertex
+
+
+_LOGGER = logging.getLogger(__name__)
+
+
+def load_g2o_se2(infile):
+    """Load an :math:`SE(2)` graph from a .g2o file.
+
+    Parameters
+    ----------
+    infile : str
+        The path to the .g2o file
+
+    Returns
+    -------
+    Graph
+        The loaded graph
+
+    """
+    edges = []
+    vertices = []
+
+    with open(infile) as f:
+        for line in f.readlines():
+            if line.startswith("VERTEX_SE2"):
+                numbers = line[10:].split()
+                arr = np.array([float(number) for number in numbers[1:]], dtype=np.float64)
+                p = PoseSE2(arr[:2], arr[2])
+                v = Vertex(int(numbers[0]), p)
+                vertices.append(v)
+                continue
+
+            if line.startswith("EDGE_SE2"):
+                numbers = line[9:].split()
+                arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64)
+                vertex_ids = [int(numbers[0]), int(numbers[1])]
+                estimate = PoseSE2(arr[:2], arr[2])
+                information = upper_triangular_matrix_to_full_matrix(arr[3:], 3)
+                e = EdgeOdometry(vertex_ids, information, estimate)
+                edges.append(e)
+                continue
+
+            if line.strip():
+                _LOGGER.warning("Line not supported -- '%s'", line.rstrip())
+
+    return Graph(edges, vertices)

SLAM/GraphBasedSLAM/graphslam/pose/__init__.py

@@ -0,0 +1,5 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam

SLAM/GraphBasedSLAM/graphslam/pose/se2.py

@@ -0,0 +1,171 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+r"""Representation of a pose in :math:`SE(2)`.
+
+"""
+
+import math
+
+import numpy as np
+
+from ..util import neg_pi_to_pi
+
+
+class PoseSE2(np.ndarray):
+    r"""A representation of a pose in :math:`SE(2)`.
+
+    Parameters
+    ----------
+    position : np.ndarray, list
+        The position in :math:`\mathbb{R}^2`
+    orientation : float
+        The angle of the pose (in radians)
+
+    """
+    def __new__(cls, position, orientation):
+        obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)], dtype=np.float64).view(cls)
+        return obj
+
+    # pylint: disable=arguments-differ
+    def copy(self):
+        """Return a copy of the pose.
+
+        Returns
+        -------
+        PoseSE2
+            A copy of the pose
+
+        """
+        return PoseSE2(self[:2], self[2])
+
+    def to_array(self):
+        """Return the pose as a numpy array.
+
+        Returns
+        -------
+        np.ndarray
+            The pose as a numpy array
+
+        """
+        return np.array(self)
+
+    def to_compact(self):
+        """Return the pose as a compact numpy array.
+
+        Returns
+        -------
+        np.ndarray
+            The pose as a compact numpy array
+
+        """
+        return np.array(self)
+
+    def to_matrix(self):
+        """Return the pose as an :math:`SE(2)` matrix.
+
+        Returns
+        -------
+        np.ndarray
+            The pose as an :math:`SE(2)` matrix
+
+        """
+        return np.array([[np.cos(self[2]), -np.sin(self[2]), self[0]], [np.sin(self[2]), np.cos(self[2]), self[1]], [0., 0., 1.]], dtype=np.float64)
+
+    @classmethod
+    def from_matrix(cls, matrix):
+        """Return the pose as an :math:`SE(2)` matrix.
+
+        Parameters
+        ----------
+        matrix : np.ndarray
+            The :math:`SE(2)` matrix that will be converted to a `PoseSE2` instance
+
+        Returns
+        -------
+        PoseSE2
+            The matrix as a `PoseSE2` object
+
+        """
+        return cls([matrix[0, 2], matrix[1, 2]], math.atan2(matrix[1, 0], matrix[0, 0]))
+
+    # ======================================================================= #
+    #                                                                         #
+    #                                Properties                               #
+    #                                                                         #
+    # ======================================================================= #
+    @property
+    def position(self):
+        """Return the pose's position.
+
+        Returns
+        -------
+        np.ndarray
+            The position portion of the pose
+
+        """
+        return np.array(self[:2])
+
+    @property
+    def orientation(self):
+        """Return the pose's orientation.
+
+        Returns
+        -------
+        float
+            The angle of the pose
+
+        """
+        return self[2]
+
+    @property
+    def inverse(self):
+        """Return the pose's inverse.
+
+        Returns
+        -------
+        PoseSE2
+            The pose's inverse
+
+        """
+        return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]), self[0] * np.sin(self[2]) - self[1] * np.cos([self[2]])], -self[2])
+
+    # ======================================================================= #
+    #                                                                         #
+    #                              Magic Methods                              #
+    #                                                                         #
+    # ======================================================================= #
+    def __add__(self, other):
+        r"""Add poses (i.e., pose composition): :math:`p_1 \oplus p_2`.
+
+        Parameters
+        ----------
+        other : PoseSE2
+            The other pose
+
+        Returns
+        -------
+        PoseSE2
+            The result of pose composition
+
+        """
+        return PoseSE2([self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]), self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])], neg_pi_to_pi(self[2] + other[2]))
+
+    def __sub__(self, other):
+        r"""Subtract poses (i.e., inverse pose composition): :math:`p_1 \ominus p_2`.
+
+        Parameters
+        ----------
+        other : PoseSE2
+            The other pose
+
+        Returns
+        -------
+        PoseSE2
+            The result of inverse pose composition
+
+        """
+        return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (self[1] - other[1]) * np.sin(other[2]), (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])], neg_pi_to_pi(self[2] - other[2]))

SLAM/GraphBasedSLAM/graphslam/util.py

@@ -0,0 +1,78 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+"""Utility functions used throughout the package.
+
+"""
+
+import numpy as np
+
+
+TWO_PI = 2 * np.pi
+
+
+def neg_pi_to_pi(angle):
+    r"""Normalize ``angle`` to be in :math:`[-\pi, \pi)`.
+
+    Parameters
+    ----------
+    angle : float
+        An angle (in radians)
+
+    Returns
+    -------
+    float
+        The angle normalized to :math:`[-\pi, \pi)`
+
+    """
+    return (angle + np.pi) % (TWO_PI) - np.pi
+
+
+def solve_for_edge_dimensionality(n):
+    r"""Solve for the dimensionality of an edge.
+
+    In a .g2o file, an edge is specified as ``<estimate> <information matrix>``, where only the upper triangular portion of the matrix is provided.
+
+    This solves the problem:
+
+    .. math::
+
+       d + \frac{d (d + 1)}{2} = n
+
+    Returns
+    -------
+    int
+        The dimensionality of the edge
+
+    """
+    return int(round(np.sqrt(2 * n + 2.25) - 1.5))
+
+
+def upper_triangular_matrix_to_full_matrix(arr, n):
+    """Given an upper triangular matrix, return the full matrix.
+
+    Parameters
+    ----------
+    arr : np.ndarray
+        The upper triangular portion of the matrix
+    n : int
+        The size of the matrix
+
+    Returns
+    -------
+    mat : np.ndarray
+        The full matrix
+
+    """
+    triu0 = np.triu_indices(n, 0)
+    triu1 = np.triu_indices(n, 1)
+    tril1 = np.tril_indices(n, -1)
+
+    mat = np.zeros((n, n), dtype=np.float64)
+    mat[triu0] = arr
+    mat[tril1] = mat[triu1]
+
+    return mat

SLAM/GraphBasedSLAM/graphslam/vertex.py

@@ -0,0 +1,67 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+#
+# This file originated from the `graphslam` package:
+#
+#   https://github.com/JeffLIrion/python-graphslam
+
+"""A ``Vertex`` class.
+
+"""
+
+import matplotlib.pyplot as plt
+
+
+# pylint: disable=too-few-public-methods
+class Vertex:
+    """A class for representing a vertex in Graph SLAM.
+
+    Parameters
+    ----------
+    vertex_id : int
+        The vertex's unique ID
+    pose : graphslam.pose.se2.PoseSE2
+        The pose associated with the vertex
+    vertex_index : int, None
+        The vertex's index in the graph's ``vertices`` list
+
+    Attributes
+    ----------
+    id : int
+        The vertex's unique ID
+    index : int, None
+        The vertex's index in the graph's ``vertices`` list
+    pose : graphslam.pose.se2.PoseSE2
+        The pose associated with the vertex
+
+    """
+    def __init__(self, vertex_id, pose, vertex_index=None):
+        self.id = vertex_id
+        self.pose = pose
+        self.index = vertex_index
+
+    def to_g2o(self):
+        """Export the vertex to the .g2o format.
+
+        Returns
+        -------
+        str
+            The vertex in .g2o format
+
+        """
+        return "VERTEX_SE2 {} {} {} {}\n".format(self.id, self.pose[0], self.pose[1], self.pose[2])
+
+    def plot(self, color='r', marker='o', markersize=3):
+        """Plot the vertex.
+
+        Parameters
+        ----------
+        color : str
+            The color that will be used to plot the vertex
+        marker : str
+            The marker that will be used to plot the vertex
+        markersize : int
+            The size of the plotted vertex
+
+        """
+        x, y = self.pose.position
+        plt.plot(x, y, color=color, marker=marker, markersize=markersize)