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

\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}

\lecture{Statistical Techniques in Robotics (16-831, F10)}{Lecture \#5 (09/07/2010)}{Lecturer: Drew Bagnell}{Scribe: Brian Coltin, based on notes by Mehmet R. Dogar}{Mapping}

\section{Occupancy Mapping}

When solving the localization problem, we knew a map of the world and tried to find the position of the robot in it. Now we will solve the opposite problem: given the position of the robot (perhaps through some sensor such as GPS), find the map of the world. Similar to localization, we will solve the mapping problem as a filtering problem.

We need two components to solve this filtering problem: a state and a measurement model. The state is the map of the world which we are trying to find. For example, it could be a pixelized grid of cells or a map of landmarks. The measurement model is $p(z_t | m, l_t)$, or the probability of making an observation $z_t$ given a map $m$ and a location on the map $l_t$. We do not need to concern ourselves with the robot's controls since we have the position of the robot.

For our first solution to the mapping problem, we will partition the world into cells, with each being one of two states: filled or empty.  The individual grid cells are labeled $m_i$, and $\vec{m}$ is the vector of all grid cells.  Unfortunately, the curse of dimensionality prevents us from filtering $\vec{m}$, since there are $2^{\left|\vec{m}\right|}$ possible states.  Instead, we filter each cell independently, assuming that they are in fact independent.  In truth, this is a very bad assumption to make, but it is practical. We then reconstruct the entire map from the marginal probability of each cell.

This is called ``occupancy mapping''.

\subsection{Derivation}

Let $X^i$ represent the state of a grid cell $m_i$. The state is either $x$, meaning filled, or $\bar{x}$, meaning empty. We look at the probability that a cell is filled given the measurements.

\begin{eqnarray}
  p(x|z_{1:t}) & = & \frac{p(z_t|x,z_{1:t-1})p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})}\\
  p(x|z_{1:t}) & = & \frac{p(z_t|x)p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})}
\end{eqnarray}

We made the Markov assumption that $p(z_t|x,z_{1:t-1}) = p(z_t|x)$. However, this is totally bogus! This is a statement about a single cell, but the observation does not depend on just a single cell.

We expand the equation once again using Baye's rule to get

$$p(x|z_{1:t}) = \frac{p(x|z_t) p(z_t)}{p(x)} \frac{p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})}$$

Now $p(x|z_{1:t})$ is based on the inverse sensor model, $p(x|z_t)$---
the probability of being in a grid cell given the current sensor
readings.

Using the same proof, we can derive a matching update rule for
$p(\bar{x}|z_{1:t})$:

$$p(\bar{x}|z_{1:t}) = \frac{p(\bar{x}|z_t) p(z_t)}{p(\bar{x})}
\frac{p(\bar{x}|z_{1:t-1})}{p(z_t|z_{1:t-1})}\mbox{.}$$

Next, we divide the two update rules.

\begin{eqnarray*}
\frac{p(x|z_{1:t})}{p(\bar{x}|z_{1:t})} &=&
\frac{p(x|z_t)}{p(\bar{x}|z_t)} \frac{p(\bar{x})}{p(x)}
\frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})}\\
&=& \frac{p(x|z_t)}{p(x)}\frac{p(\bar{x})}{{p(\bar{x}|z_t)}}
\frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})}
\end{eqnarray*}

This eliminates the term $p(z_t|z_{1:t-1})$.
Now we have a recurisve update rule. If the probability of $\bar{x}$
decreases with the observation $z_t$, then $p(\bar{x})>p(\bar{x}|z_t)$,
which causes the belief on $x$ to increase relative to $\bar{x}$.

Next, we take the log of this update rule to find the log odds
of the belief the square is filled over the belief it isn't
filled, which we label $l_t(x)$. Using the log odds
reduces numerical errors from multiplying miniscule floating point
numbers.

\begin{eqnarray*}
\log \frac{p(x|z_{1:t})}{p(\bar{x}|z_{1:t})} &=&
\log \frac{p(x|z_t)}{p(\bar{x}|z_t)} \frac{p(\bar{x})}{p(x)}
\frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})}\\
l_t(x) &=& \log \frac{p(x|z_t)}{p(\bar{x}|z_t)} +
\log \frac{p(\bar{x})}{p(x)} + l_{t-1}(x)
\end{eqnarray*}

For this update rule, we need only to specify $p(x|z_t)$, the
sensor model, and $p(x)$, the prior. $p(\bar{x}|z_t)$ and $p(\bar{x})$
are the complements of these two terms.

An example $z_{t}$ here can be pass-through / hit information.

The algorithm derived above uses the \emph{inverse sensor model} $p(x|z_{t})$, instead of the familiar forward model $p(z_{t}|x)$. The inverse sensor model specifies a distribution over the (binary) state variable as a function of the measurement $z_{t}$.

\subsection{Problems with the Markov Assumption}

\begin{center}
\includegraphics[width=10cm]{img/occGrid.pdf}
\end{center}

At the beginning of this derivation we used the Markov assumption to
claim that $p(z_t|x,z_{1:t-1}) = p(z_t|x)$. This would have been
reasonable had $x$ represented the state of the complete map.
However, $x$ represents the state of a single grid cell. The Markov
assumption in this context doesn't make much sense: we can't say that
an observation $z_t$ is independent of all prior observations given
only the state of a single cell. The above figure shows an example
using a wide laser beam where this Markov assumption fails.

\section{Density Mapping}

In occupancy grid mapping every grid cell is one of two states:
filled or empty. 
But in some situations it makes sense for a cell to be partially filled.
This may occur when only part of the grid cell is filled and the rest
is empty, or when the objects that ``fill'' the grid cell have
special characteristics: we may want a grid filled with vegetation to
be ``less filled'' than a grid filled with a solid rock.

Furthermore, occupancy grids have trouble dealing with semi-transparent
obstacles such as glass and vegetation. These obstacles may return
hits to the laser rangefinder about half of the time, but eventually
the occupancy grid will converge to either filled or not filled,
both of which are incorrect.

To address this problem, we use a density filter where the state
$x \in [0, 1]$ is the probability a beam passes through rather than a
binary filled / empty state. Assuming that a beam returns with a probability
$x$ according to a Bernoulli distribution, we model this probability
using a Beta distribution. If $\alpha$ is the number of beams
that pass through and $\beta$ is the number of beams that hit, the
most probable value of a cell's density is

$$\frac{\alpha}{\alpha + \beta}$$

\noindent and we use this as the value in the density grid. Note
that density grids make the same bogus Markov assumption
as occupancy grids, but do not assume the cells can only be
completely full or completely empty.

\end{document}

