\documentclass[11pt]{article}
\usepackage{latexsym}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{amsthm}
\usepackage{epsfig}
\usepackage[tight]{subfigure}
\usepackage{multirow}
\usepackage{url}

\newcommand{\handout}[5]{
  \noindent
  \begin{center}
  \framebox{
    \vbox{
      \hbox to 5.78in { {#1} \hfill #2 }
      \vspace{4mm}
      \hbox to 5.78in { {\Large \hfill #5  \hfill} }
      \vspace{2mm}
      \hbox to 5.78in { {\em #3 \hfill #4} }
    }
  }
  \end{center}
  \vspace*{4mm}
}

\newcommand{\lecture}[5]{\handout{#1}{#2}{#3}{#4}{#5}}

\newtheorem{theorem}{Theorem}
\newtheorem{corollary}[theorem]{Corollary}
\newtheorem{lemma}[theorem]{Lemma}
\newtheorem{observation}[theorem]{Observation}
\newtheorem{proposition}[theorem]{Proposition}
\newtheorem{definition}[theorem]{Definition}
\newtheorem{claim}[theorem]{Claim}
\newtheorem{fact}[theorem]{Fact}
\newtheorem{assumption}[theorem]{Assumption}

% 1-inch margins, from fullpage.sty by H.Partl, Version 2, Dec. 15, 1988.
\topmargin 0pt
\advance \topmargin by -\headheight
\advance \topmargin by -\headsep
\textheight 8.9in
\oddsidemargin 0pt
\evensidemargin \oddsidemargin
\marginparwidth 0.5in
\textwidth 6.5in

\parindent 0in
\parskip 1.5ex
%\renewcommand{\baselinestretch}{1.25}

\begin{document}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Document header: change the lecture number, date, scribe name, and
% lecture topic.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\lecture{Statistical Techniques in Robotics (16-831, F10)}
{Lecture\#26-27 (November 18-23, 2010)}
{Lecturer: Drew Bagnell}
{Scribes: Justin Haines, Neal Seegmiller \footnotemark}
{SLAM, Fast SLAM and Rao-Blackwellization}

% Be sure to acknowledge all previous scribes.
\footnotetext{Some content adapted from previous scribe: Bryan Wagenknecht}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Document body: Scribe your notes here.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\section{About SLAM}
Simultaneous localization and mapping (SLAM) is one of the canonical problems in robotics. A robot set down in an unknown environment with noisy odometry and sensing needs to figure out where it is in the world and learn details about the world at the same time. It is a sort of chicken-egg problem where the mapping problem is relatively easy given exact knowledge of the robot's location, and localizing the robot is relatively easy given a good map of the environment. The challenge is found in performing these tasks simultaneously.

\subsection{Variations on the SLAM problem}
SLAM can be approached in two ways:
\begin{itemize}
\item{Batch: Given all data, $y_{1:T}$, up front, solve for robot path $x_{1:T}$ and map $M$ all at once (we won't cover this)}
\item{Online/filtering: compute $\left\{x_t, m_t\right\}$ at each timestep as data $y_t$ is received (we focus on this)}
\subitem{Alternatively, if you are interested in the robot trajectory, calculate $\left\{x_{1:t}, m_t\right\}$ at each timestep}
\end{itemize}
The mapping part of SLAM can also be approached in two ways:
\begin{itemize}
\item{Filling an occupancy grid $M = \left\{m_1, m_2, ...\right\}$ (we'll come back to this)}
\item{Tracking landmarks $l_i = (l_{xi}, l_{yi})$ (focus on this for now)}
\end{itemize}

\section{Basic Approaches to SLAM}
\subsection{Na\"{\i}ve approach: PF over joint}
The first potential solution is to build a Particle Filter to sample from the joint distribution \[p(r_x, r_y, l_{x1}, l_{y1}, l_{x2}, l_{y2}, ...)\] This becomes bad very quickly due to the high dimensionality of the state space when landmark position is included (this example has dimension $2+2*l$.)
\begin{itemize}
\item{PF doesn't scale well with dimensionality - particle sparsity means a particle that is good at describing one landmark is probably bad at describing others}
\item{Observation distributions/densities get very sharp due to multiplying distributions}
\end{itemize}

\subsection{X-KF over joint}
The next potential solution (often called the ``classical solution'') might be to use your favorite Kalman Filter (X-KF here could mean EKF, UKF, etc.) to track all variables in one joint distribution \[p(r_x, r_y, l_{x1}, l_{y1}, l_{x2}, l_{y2}, ...)\] This results in a big covariance matrix with the marginal gaussian distribution of each landmark contained in blocks along the covariance diagonal. Off-diagonal terms are correlations from multiple landmarks appearing in the robot's view at once.

The motion update would look like:
\[
\left[
\begin{array}{c}
r_x \\
r_y \\
l_{x1} \\
l_{y1} \\
\vdots \end{array}
\right]_{t+1}
=
\left[
\begin{array}{cc}
	\left[
	\begin{array}{cc}
	\multicolumn{2}{c}{\multirow{2}{*}{$A_r$}} \\
	& \end{array}
	\right]_{2x2} & 0\\
0 & \left[
	\begin{array}{ccc}
	&&\\
	\multicolumn{3}{c}{I}\\
	&&\\ \end{array}
	\right]
\end{array}\right]
\left[
\begin{array}{c}
r_x \\
r_y \\
l_{x1} \\
l_{y1} \\
\vdots \end{array}
\right]_{t}
+
\left[
\begin{array}{c}
\epsilon_x \\
\epsilon_y \\
\\
0\\
\\\end{array}
\right]
\]

where $A_r$ is the Jacobian of the motion model of the robot and $\epsilon_x$ and $\epsilon_y$ are gaussian noise.  The identity block represents the assumption that landmarks do not move over time.

The observation model would look like:
\[
\left[
\begin{array}{c}
range_i \\
bearing_i \end{array}
\right]
= 
\left[
\begin{array}{c}
\sqrt{(r_x - l_{xi})^2 + (r_y - l_{yi})^2} \\
atan2((r_y - l_{yi}),(r_x - l_{xi})) \end{array}
\right]
+
\left[
\begin{array}{c}
\epsilon_{range} \\
\epsilon_{bearing} \end{array}
\right]
\]

KEY ASSUMPTION: The robot must be able to differentiate landmarks

\subsubsection{Potential problems with this approach}
\begin{itemize}
\item{The solution could be multimodal, especially with bad odometry}
\item{Data association: If the robot cannot distinguish between landmarks, problems can arise with ``loop closure'' where the robot returns to a location it has already mapped.  In this case, the robot needs to match the landmarks with those previously observed.}
\subitem{Maximum Likelihood data association with hard thresholds is often used for this}
\item{Scales as $O(l^2)$ where $l$ is number of landmarks}
\subitem{Although this is better than the complexity of the particle filter SLAM}
\item{What to use for initial landmark location, $\mu_{x0}$? Any guess could be arbitrarily wrong and take a long time to correct}
\end{itemize}

\subsubsection{Some hacks that could be helpful}
\begin{itemize}
\item{Use submaps (segment the large world map into smaller maps) and work with smaller chunks of the covariance matrix.}
\item{When a landmark is seen for the first time, initialize its $\mu_x$ to the observed location}
\item{Throw out confusing landmarks}
\end{itemize}

\subsection{Alternative: Information Filter for SLAM}
\begin{itemize}
\item{Information filter implementation is $O(l^3)$ in the number of landmarks}
\item{However, the sparse nature of the $lxl$ matrix inversion allows the algorithm run in closer to $O(l\log(l))$}
\end{itemize}

\section{SLAM as a Bayes Net}
If we draw the Bayes Network representation of SLAM we get something like Figure \ref{BayesNet}. Here the data associations are presumed to be known and fixed.

	\begin{figure}[ht]
      \centering
      \includegraphics[bb=0 0 417 386,width=3in]{BayesNet.png}
      \caption{Bayes Network representation of SLAM. Robot states $r_i$ form a Markov chain and data associations (which landmarks $l_i$ are in view at each observation $y_i$) are presumed known and fixed. Observe that the landmark locations are conditionally independent given the robot states.}
      \label{BayesNet}
   \end{figure}

\subsection{Fast SLAM factorization}
Investigation of the Bayes Net shows us that we can make the landmark locations independent by conditioning on the robot locations. This allows us to factorize the distribution as
\[p(y_{1:t}|z_{1:t},u_{1:t}) = p(x_{1:t}|z_{1:t},u_{1:t}) \cdot \prod_i p(l_i|x_{1:t},z_{1:t})\]
Thus we can track the landmark locations independently by conditioning on the assumed robot position.

Fast SLAM is an algorithm that runs a particle filter in robot location space using augmented particles. Each particle carries a history of its past locations $r_1...r_t$ and an individual Kalman filter (consisting of $\mu_{l_i}$ and $\Sigma_{l_i}$) for each landmark $l_i$ it sees. This is visualized in Figure \ref{AugmentedParticle}.

	\begin{figure}[ht]
      \centering
      \includegraphics[bb=0 0 581 469, width=3in]{AugmentedParticle.png}
      \caption{Fast SLAM particles include a history of their positions and individual Kalman filters for each landmark.}
      \label{AugmentedParticle}
   \end{figure}

\section{Smarter Approach: Fast SLAM}
\subsection{Fast SLAM 1.0}
The first version of Fast SLAM runs a particle filter with augmented particles, then applies importance weighting to the particles using the latest estimates of any landmarks within view. The landmarks are tracked independently in Kalman filters within each particle.

\subsubsection{Fast SLAM 1.0 Algorithm}
\begin{enumerate}
	\item{Motion update: for all $particle_i$, move $r_i$ according to motion model (leave KF's for landmarks alone)
	\[ r_t \sim p(r_t|r_{t-1},u_t)\]}
	\item{Observation update: weight $particle_i$ by $p(y|associations\:l, r_i) \sim N(\mu_y,\Sigma_{yy})$
	\[ p(y|associations\:l, r_i) =  \frac{1}{\sqrt{(2\pi)^2\left|\Sigma_{yy}\right|}}\exp\left[-\frac{1}{2}(y-\mu_y)^T\Sigma_{yy}(y-\mu_y)\right] \]}
	\item{Run KF update on each particle's estimate of landmark $l$}
	\item{Resample particles}
\end{enumerate}

\subsubsection{Timestep Complexity: $O(l)$}
\begin{itemize}
\item{Data association is $O(l)$ (use KD tree)}
\item{Memory copy is $O(l)$}
\end{itemize}

\subsubsection{Occupancy Grids}
\begin{itemize}
\item{In theory each particle should carry its own map grid (instead of KF's)}
\item{Practically this is too memory intensive, use data structure tricks: DP-SLAM}
\subitem{Use a single map grid with data for all relevant particles in each cell}
\end{itemize}

\subsection{Fast SLAM 2.0}
The problem with Fast SLAM is paricle deprivation, especially for loop closure. The key is to have a smarter motion model (sampling distribution).  The Fast SLAM 2.0 motion model is conditioned on the observation $z_t$ in addition to the control, $u_t$.

\subsubsection{Fast SLAM 2.0 Algorithm}
\begin{enumerate}
\item{Motion update: Sample robot position from Kalman filter $\tilde{p}(x_{t+1}|y_{t+1},x_t,z_t)$}
\item{Observation update: Weight $particle_i$ by
\[ w_i \propto \frac{p(y_{t+1}|associations\:l, r_i)}{\tilde{p}(x_{t+1}|y_{t+1},x_t)} \]}
\end{enumerate}

\section{Wrap-up Topics}
\subsection{Data association}
If we want to be lazy with data association (or we can't label the landmarks a priori) then we can track associations within the particle. Wrong assignments are corrected in retrospect for free through particle resampling.
\begin{itemize}
\item{For each landmark $l_i$, sample associations from $p(y|l_i,r)$}
\end{itemize}

\subsection{Rao-Blackwell theorem}
The Rao-Blackwell theorem says running an algorithm that samples from a distribution on one variable $p(x_1)$ then determines another variable $x_2$ analytically conditioned on $x_1$ will do no worse than an algorithm that samples from the joint distribution $p(x_1,x_2)$.
\[ var\left[sampler(x_1,x_2) \right] \geq var\left[sampler(x_1) + does\:x_2|x_1\:analytically\right] \]
Fast SLAM is an example of Rao-Blackwellization since by using KF's inside particles, we don't have to sample landmark info from the whole joint distribution. In general Rao-Blackwellized filters make for the most efficient SLAM algorithms.


%%% added by Neal
\section{Occupancy Grid SLAM}
\subsection{DP SLAM}
Murphy initially proposed Fast SLAM using occupancy grid maps\footnote{Kevin Murphy, ``Bayesian Map Learning in Dynamic Environments,'' NIPS 1999. (Neural Info. Proc. Systems)}. In the simplest implementation, a complete map is stored for each particle; however, this is too memory intensive (and computationally expensive to copy the maps). Researchers at Duke University proposed DP SLAM which uses dynamic programming to reduce the memory/computational requirements\footnote{\url{http://www.cs.duke.edu/~parr/dpslam/}}.

The basic idea: If two particles share a common ancestor, both their maps will agree with the observations of that common ancestor. A single occupancy grid map is stored with an observation tree at each cell (storing the observations of all particles that affect that cell).

Efficient maintenance of this data structure is non-trivial.

	\begin{figure}[ht]
      \centering
      \includegraphics[bb=0 0 581 469, width=3in]{particleAncestry.png}
      \caption{Example of particle ancestry for DP SLAM. Note that all particles at $t_3$ are descended from the root $a$ as well as $b$ at $t_1$. All particles at $t_3$ will share the observations of $b$ and its predecessors, so these sections of the map need only be stored once.}
      \label{particleAncestry}
   \end{figure}

\subsubsection{Loss of diversity}
Ideally, in order to close loops diversity must be maintained on the order of the largest loop that will ever be traversed. However, DP SLAM typically only maintains diversity on the order of 2-3 meters (i.e. all current particles are descended from a single particle 2-3 meters back). Despite this, DP SLAM still works due to high localization accuracy. 

	\begin{figure}[ht]
      \centering
      \includegraphics[bb=0 0 581 469, width=3in]{lossOfDiversity.png}
      \caption{Illustration of loss of diversity in DP SLAM. In this example, correcting the map after closing the loop around the rectangular obstacle would require diversity of paths all the way back to the root; however, the map can only be corrected over length $d$ because the maps of all current particles share the observations of the common ancestor.}
      \label{lossOfDiversity}
   \end{figure}
   
\subsection{3D Fast SLAM}

Nathaniel Fairfield's research on DEPTHX\footnote{Nathaniel Fairfield, George Kantor, and David Wettergreen, ``Real-Time SLAM with Octree Evidence Grids for Exploration in Underwater Tunnels,'' Journal of Field Robotics 2007.} is an example of efficient occupancy grid SLAM in 3D. An octree data structure is used to maintain hundreds of maps required by the particle filter. The octree structure exploits spatial locality and temporal shared ancestry between particles to reduce the processing and storage requirements.

\section{Stochastic Gradient Descent approach to SLAM}
Olsen proposed as fast gradient descent approach to recovering a time sequence of robot poses\footnote{Edwin Olson, John Leonard, and Seth Teller, ``Fast Iterative Alignment of Pose Graphs with Poor Initial Estimates,'' ICRA 2006}. The steps are:
\begin{enumerate}
	\item{Marginalize out the landmarks}
	\item{Use stochastic gradient descent to optimize pose graph}
	\item{(if desired) Perform mapping using the optimized pose data}
\end{enumerate}


\subsection{Marginalize out the landmarks}
Observations of landmark location relative to robot pose are converted to constraints between poses from which the same landmark was observed. This results in fewer variables (no landmark variables) but more constraints. The resulting pose graph will have two types of constraints:
\begin{enumerate}
	\item{temporal (odometry) constraints between poses at adjacent timesteps}
	\item{landmark constraints between poses from which the same landmark was observed}
\end{enumerate}

	\begin{figure}[ht]
      \centering
      \includegraphics[bb=581 0 1162 469, width=2in]{landmarkConstraint.png}
      \caption{Illustration of marginalizing out a landmark. The observations of landmark $l_1$ at poses $x_2$ and $x_4$ (left) are converted to a single pose constraint (right).}
      \label{landmarkConstraint}
   \end{figure}
   
\subsection{Critical trick: represent the state variables as pose deltas}
At each iteration a single constraint is chosen at random for the update.

Instead of representing the state variables as robot poses, represent them as the difference in pose between timesteps.

\[
\left[
\begin{array}{c}
x_0 \\
x_1 \\
x_2 \\
\vdots \end{array}
\right]
\rightarrow
\left[
\begin{array}{c}
x_0 \\
\Delta x_1 \\
\Delta x_2 \\
\vdots \end{array}
\right]
\]

Robot pose is calculated by summing the pose deltas, so a change in any one pose delta affects the robot pose at all subsequent timesteps.
Alternatively, this change in representation can be thought of as a change in the distance metric used by gradient descent.

	\begin{figure}[ht]
      \centering
      \includegraphics[bb=581 0 1162 469, width=2in]{poseDelta.png}
      \caption{(left, original representation) Consider $x_3$ undergoes a large update due to a landmark constraint. Likely many iterations will pass before $o_2$ or $o_3$ are selected, finally updating the neighboring poses. (right, pose delta representation) Because a change in pose delta affects all subsequent poses, sequences of poses will update together.}
      \label{poseDelta}
   \end{figure}
   
\subsection{Other notes}
\begin{itemize}
	\item{it's magic! tends to jump over local minima}
	\item{be clever about the learning rate (increase for loop closure, etc.)}
	\item{this is a batch algorithm, but it can be implemented online using a sliding window/lag filter}
\end{itemize}
\end{document}