thesis-ch03-lazysp.tex

\chapter{Fast Pathfinding on Graphs via Lazy Evaluation}
\label{chap:lazysp}

The roadmap methods described in Chapter~\ref{chap:roadmaps}
create a discretization of the configuration space using
a graph $G = (V,E)$.
This allows the motion planning problem to be solved by way of
a pathfinding algorithm on $G$
-- and there are a large variety of such algorithms available
in the literature to choose from.
However,
the computational efficiency of suitable algorithms
depends intimately on the underlying problem domain.

In this chapter,
we consider the general shortest path problem
with a particular focus on domains (such as robot motion planning)
where evaluating the edge weight function
dominates algorithm running time.
Inspired by lazy approaches in robotics,
we define and investigate the \emph{Lazy Shortest Path} class of
algorithms which is differentiated by the choice of
an \emph{edge selector} function.
We show that several algorithms in the literature are equivalent to
this lazy algorithm for appropriate choice of this selector.
Further, we propose various novel selectors inspired by
sampling and statistical mechanics,
and find that these selectors outperform
existing algorithms on a set of example problems.

\begin{figure}
\centering
\begin{tikzpicture}
   \tikzset{>=latex} % arrow heads
   %\draw[step=1cm,black!10,very thin] (0,0) grid (8,4);
   \node[draw,align=center,minimum height=1.0cm,thick]
      at (2.5,2.5) {Graph\\$G=(V,E)$};
   \node[draw,align=center,minimum height=1.0cm,thick]
      at (5.5,2.5) {Weight Function\\$w:E \rightarrow [0,+\infty]$};
   \node[draw,align=center,minimum height=1.0cm,minimum width=3cm,thick]
      (alg) at (4,1) {Shortest Path\\Algorithm};
   \node[draw,align=center,shape=document,minimum width=1.5cm,ultra thin]
      (query) at (1,1) {Query $u$};
   \node[draw,align=center,shape=document,minimum width=1.5cm,ultra thin]
      (path) at (7,1) {Path $p^*$};
   \draw[->] (3,2) -- (3,1.5);
   \draw[->] (5,2) -- (5,1.5);
   \draw[->] (query.east) -- (alg.west);
   \draw[->] (alg.east) -- (path.west);
\end{tikzpicture}
\caption{While solving a shortest path query,
   a shortest path algorithm incurs computation cost from three sources:
   examining the structure of the graph $G$,
   evaluating the edge weight function $w$,
   and maintaining internal data structures.}
\label{fig:sp-intro}
\end{figure}

\section{The Shortest Path Problem}

Graphs provide a powerful abstraction
capable of representing problems in a wide variety of domains
from computer networking to puzzle solving
to robotic motion planning.
%\ajnote{Way too vague and general -- disconnected from second sentence.
%Maybe mention ``standard problems'' on graphs.}
In particular,
many important problems can be captured
as \emph{shortest path problems} (Figure~\ref{fig:sp-intro}),
wherein a path $p^*$ of minimal length is sought
between two query vertices through a graph $G$
with respect to an edge weight function $w$.
%As such,
%a large number of algorithms have been proposed in the literature
%for solving shortest path problems efficiently.

Despite the expansive applicability of this single abstraction,
there exist a wide variety of algorithms in the literature
for solving the shortest path problem efficiently.
%\ajnote{You say ``despite'' but the second part seems to follow from
%the first.
%There are many solutions because there are many problems.}
This is because the measure of computational efficiency,
and therefore the correct choice of algorithm,
is inextricably tied to the underlying problem domain.

The computational costs incurred by an algorithm
can be broadly categorized into three sources
corresponding to the blocks in Figure~\ref{fig:sp-intro}.
One such source consists of queries on the structure
of the graph $G$ itself.
The most commonly discussed such operation,
\emph{expanding} a vertex (determining its successors),
is especially fundamental
%\ajnote{especially costly?}
when the graph is represented implicitly,
e.g. for domains with large graphs
such as the 15-puzzle or Rubik's cube.
It is with respect to vertex expansions
that A* \citep{hart1968astar} is optimally efficient.

A second source of computational cost consists of maintaining
ordered data structures inside the algorithm itself,
which is especially important for problems with large branching
factors.
For such domains,
approaches such as partial expansion \citep{yoshizumi2000peastar}
or iterative deepening \citep{korf1985idastar}
significantly reduce the number of vertices generated and stored
by either selectively filtering surplus vertices from the frontier,
or by not storing the frontier at all.

The third source of computational cost arises not from reasoning
over the structure of $G$,
but instead from evaluating the edge weight function $w$
(i.e. we treat discovering an out-edge and determining its weight
separately).
Consider for example the problem of articulated robotic motion planning
using roadmap methods \citep{kavrakietal1996prm}.
While these graphs are often quite small
(fewer than $10^5$ vertices),
determining the weight of each edge requires performing many
collision and distance computations for the complex geometry
of the robot and environment,
resulting in planning times of multiple seconds to find a path.

As described in Chapter~\ref{chap:roadmaps},
we consider problem domains in which evaluating the edge weight
function $w$ dominates algorithm running time.
In this chapter,
we investigate the following research question:
\begin{quote}
{\normalsize
How can we minimize the number of edges we need to evaluate
to answer shortest-path queries?
}
\end{quote}

We make three primary contributions.
First,
inspired by lazy collision checking techniques from 
robotic motion planning \citep{bohlin2000lazyprm},
we formulate a class of shortest-path algorithms 
that is well-suited to problem domains with expensive edge evaluations.
Second,
we show that several existing algorithms in the literature
can be expressed as special cases of this algorithm.
Third,
we show that the extensibility afforded by the algorithm allows for
novel edge evaluation strategies,
which can outperform existing algorithms
over a set of example problems.

\section{Lazy Shortest Path Algorithm}

We describe a lazy approach to finding short paths
which is well-suited to domains with
expensive edge evaluations.

\subsection{Problem Definition}

A path $p$ in a graph $G = (V,E)$
is composed of a sequence of adjacent edges 
connecting two endpoint vertices.
Given an edge weight function
$w : E \rightarrow [0,+\infty]$,
the length of the path with respect to $w$ is then:
\begin{equation}
   \mbox{len}(p, w) = \sum_{e \in p} w(e).
   \label{eqn:lazysp:len-definition}
\end{equation}
Given a single-pair planning query
$u: (v_{\ms{start}}, \, v_{\ms{goal}})$
inducing a set of satisfying paths $P_u$,
the \emph{shortest-path problem} is:
\begin{equation}
   p^* = \argmin_{p \, \in \, P_u} \mbox{len}(p, w).
   \label{eqn:objective}
\end{equation}

A shortest-path algorithm computes a satisfying solution $p^*$
given $(G, u, w)$.
Many such algorithms have been proposed
to efficiently accommodate a wide array of underlying problem domains.
%(outlined in Section~\ref{sec:discussion}).
The well-known principle of best-first search (BFS)
is commonly employed to select vertices for expansion
so as to minimize such expansions while guaranteeing optimality.
Since we seek to minimize edge evaluations,
we apply BFS to the question of selecting candidate paths in
$G$ for evaluation.
The resulting algorithm, Lazy Shortest Path (LazySP),
is presented in Algorithm~\ref{alg:lazy-outline},
and can be applied to graphs defined implicitly or explicitly.

%\cdnote{To Sidd: You might not like this paragraph -- but I feel like
%going directly from the problem definition to the algorithm doesn't
%connect to the rationale strongly enough without it.
%What do you think?}

\subsection{The Algorithm}

\begin{algorithm}[t]
\caption{Lazy Shortest Path (LazySP)}
\label{alg:lazy-outline}
\begin{algorithmic}[1]
\Function {\textsc{LazyShortestPath}}{$G, u, w, w_{\ms{est}}$}
\State $E_{\ms{eval}} \leftarrow \emptyset$ %\Comment Initialize evaluated edges to empty
\State $w_{\ms{lazy}}(e) \leftarrow w_{\ms{est}}(e) \quad \forall e \in E$ %\Comment Initialize lazy edge weights to estimate (inexpensive)
\Loop
   \State $p_{\ms{candidate}} \leftarrow
      \mbox{\sc ShortestPath}(G, u, w_{\ms{lazy}})$ %\Comment Compute the shortest path with lazy edge weights
      \label{line:lazy-outline-shortestpath}
   \If {$p_{\ms{candidate}} \subseteq E_{\ms{eval}}$} %\Comment If all edges on path have already been evaluated,
      \State \Return $p_{\ms{candidate}}$ %\Comment path returned is provably optimal
   \EndIf
   \State $E_{\ms{selected}} \leftarrow  \mbox{\sc Selector}(G, p_{\ms{candidate}})$ %\Comment Select edges on path to process
   \label{line:lazy-outline-chooseedges} 
   \For {$e \in E_{\ms{selected}} \setminus E_{\ms{eval}}$} %\Comment For all unevaluated selected edges
      \State $w_{\ms{lazy}}(e) \leftarrow w(e)$ \Comment Evaluate (expensive)
      \State $E_{\ms{eval}} \leftarrow E_{\ms{eval}} \cup e$ %\Comment Add to evaluated edge set
   \EndFor
\EndLoop
\EndFunction
\end{algorithmic}
\end{algorithm}

We track evaluated edges with the set $E_{\ms{eval}}$.
We are given an estimator function $w_{\ms{est}}$ of the true edge weight $w$.
This estimator is inexpensive to compute
(e.g. edge length or even $0$).
We then define a \emph{lazy} weight function $w_{\ms{lazy}}$
which returns the
true weight of an evaluated edge and otherwise
uses the inexpensive estimator $w_{\ms{est}}$.

At each iteration of the search,
the algorithm uses $w_{\ms{lazy}}$ to compute a candidate path
$p_{\ms{candidate}}$
by calling an existing solver \textsc{ShortestPath}
(note that this invocation requires no evaluations of $w$).
Once a candidate path has been found,
it is returned if it is fully evaluated.
Otherwise,
an \emph{edge selector} is employed which selects
graph edge(s) for evaluation.
The true weights of these edges are then evaluated
(incurring the requisite computational cost),
and the algorithm repeats.

%\subsection{Theoretical Properties}

LazySP is complete and optimal:
\marginnote{Proof of all theorems are available
in Appendix~\ref{sec:appendix-proofs}.}

\begin{theorem}[Completeness of LazySP]
If the graph $G$ is finite,
\textsc{ShortestPath} is complete,
and the set $E_{\ms{selected}}$
returned by \textsc{Selector}
returns at least one unevaluated edge on $p_{\ms{candidate}}$,
then \textsc{LazyShortestPath} is complete.
\label{thm:lazy-completeness}
\end{theorem}

\begin{theorem}[Optimality of LazySP]
If $w_{\ms{est}}$ is chosen such that
$w_{\ms{est}}(e) \leq \epsilon \, w(e)$ for some parameter
$\epsilon \geq 1$ and
\textsc{LazyShortestPath} terminates
with some path $p_{\ms{ret}}$,
then $\mbox{len}(p_{\ms{ret}}, w) \leq \epsilon \, \ell^*$
with $\ell^*$ the length of an optimal path.
\label{thm:lazy-optimality}
\end{theorem}

The optimality of LazySP depends on the admissibility of
$w_{\ms{est}}$
in the same way that the optimality of A* depends on
the admissibility of its goal heuristic $h$.
Theorem~\ref{thm:lazy-optimality} establishes the general
bounded suboptimality of LazySP
w.r.t. the inflation parameter $\epsilon$.
While our theoretical results (e.g. equivalences)
hold for any choice of $\epsilon$,
for clarity our examples and experimental results
focus on cases with $\epsilon = 1$.

\subsection{The Edge Selector: Key to Efficiency}

\begin{algorithm}[t]
\caption{Various Simple LazySP Edge Selectors}
\begin{algorithmic}[1]
\Function {\textsc{SelectExpand}}{$G, p_{\ms{candidate}}$}
   \State $e_{\ms{first}} \leftarrow$ first unevaluated $e \in p_{\ms{candidate}}$
   \State $v_{\ms{frontier}} \leftarrow G.\mbox{source}(e_{\ms{first}})$
   \State $E_{\ms{selected}} \leftarrow G.\mbox{out\_edges}(v_{\ms{frontier}})$
   \State \Return $E_{\ms{selected}}$
\EndFunction
\vspace{0.02in}
\Function {\textsc{SelectForward}}{$G, p_{\ms{candidate}}$}
   \State \Return $\{ \mbox{first unevaluated } e \in p_{\ms{candidate}} \}$
\EndFunction
\vspace{0.02in}
\Function {\textsc{SelectReverse}}{$G, p_{\ms{candidate}}$}
   \State \Return $\{ \mbox{last unevaluated } e \in p_{\ms{candidate}} \}$
\EndFunction
\vspace{0.02in}
\Function {\textsc{SelectAlternate}}{$G, p_{\ms{candidate}}$}
   \If {LazySP iteration number is odd}
      \State \Return $\{ \mbox{first unevaluated } e \in p_{\ms{candidate}} \}$
   \Else
      \State \Return $\{ \mbox{last unevaluated } e \in p_{\ms{candidate}} \}$
   \EndIf
\EndFunction
\vspace{0.02in}
\Function {\textsc{SelectBisection}}{$G, p_{\ms{candidate}}$}
   \State \Return $\left\{ \begin{array}{ll}
      \mbox{unevaluated } e \in p_{\ms{candidate}} \\
      \mbox{furthest from nearest evaluated edge}
      \end{array} \right\}$
\EndFunction
\end{algorithmic}
\label{alg:simple-selectors}
\end{algorithm}

%The algorithm purposely leaves undecided
%the edge evaluation selector codified in
%\textsc{Selector} (line~\ref{line:lazy-outline-chooseedges}),
%and therefore describes a class of algorithms
%differentiated by the choice of this selector.
%This paper discusses particular choices.

The LazySP algorithm exhibits a rough similarity to optimal
replanning algorithms such as
D* \citep{stentz1994dstar,stentz1995focusseddstar}
which plan a sequence of shortest paths for a mobile robot
as new edge weights are discovered during its traverse.
D* treats edge changes
passively as an aspect of the problem setting
(e.g. a sensor with limited range).

The key difference is that our problem setting treats 
edge evaluations as an active choice that can be exploited.
While any choice of edge selector that meets the conditions above
will lead to an algorithm that is complete and optimal,
its \emph{efficiency} is dictated by the choice of this
selector.
This motivates the theoretical and empirical investigation of different
edge selectors in this chapter.

\textbf{Simple selectors.}
We codify five common strategies in
Algorithm~\ref{alg:simple-selectors}.
The Expand selector captures the edge weights that are evaluated
during a conventional vertex expansion.
The selector identifies the first unevaluated edge
$e_{\ms{first}}$ on the candidate path,
and considers the source vertex of this edge a \emph{frontier} vertex.
It then selects all out-edges of this frontier vertex
for evaluation.
The Forward and Reverse selectors select the first and last
unevaluated edge on the candidate path, respectively
(note that Forward returns a subset of Expand).

The Alternate selector simply alternates between Forward
and Reverse on each iteration.
This can be motivated by both bidirectional search algorithms
as well as motion planning algorithms such as
RRT-Connect \citep{kuffner2000rrtconnect}
which tend to perform well w.r.t. state evaluations.

The Bisection selector
chooses among those unevaluated edges
the one furthest from an evaluated edge on the candidate path.
This selector is roughly analogous to the collision checking strategy
employed by the Lazy PRM \citep{bohlin2000lazyprm}
as applied to our problem on abstract graphs.

In the following section,
we demonstrate that instances of LazySP using simple selectors
yield equivalent results to existing vertex algorithms.
We then discuss two more sophisticated
selectors motivated by weight function sampling
and statistical mechanics.

% prob-box2d00-halton-roots16
% fwd:34 partall:22 rev:24 fwdexpand:77
% bisect:25 worlddist:22 alt:23 partsimple:22
\begin{figure*}[t!]%
   \!\!%
   \subfloat[Expand(77)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-fwdexpand-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-fwdexpand-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-fwdexpand-path-bars}};
      \end{tikzpicture}%
   }%
   \!\!%
   \subfloat[Forward(34)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-fwd-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-fwd-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-fwd-path-bars}};
      \end{tikzpicture}%
   }%
   \!\!%
   \subfloat[Reverse(24)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-rev-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-rev-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-rev-path-bars}};
      \end{tikzpicture}%
   }%
   \!\!%
   \subfloat[Alternate(23)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-alt-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-alt-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-alt-path-bars}};
      \end{tikzpicture}%
   }%
   \!\!%
   \subfloat[Bisect(25)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-bisect-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-bisect-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-bisect-path-bars}};
      \end{tikzpicture}%
   }%
   \!\!%
   \subfloat[WeightSamp(22)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-worlddist1000-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-worlddist1000-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-worlddist1000-path-bars}};
      \end{tikzpicture}%
   }%
   \!\!%
   \subfloat[Partition(22)]{%
      \centering%
      \begin{tikzpicture}
         \node at (0,-0.0) {\includegraphics{build/lazysp-example-1/alg-partall-after5}};
         \node at (0,-2.5) {\includegraphics{build/lazysp-example-1/alg-partall-end}};
         \node at (0,-4.8) {\includegraphics{build/lazysp-example-1/alg-partall-path-bars}};
      \end{tikzpicture}%
   }%
   \caption[Snapshots of the LazySP algorithm using each edge selector
      discussed in this chapter on the same obstacle roadmap graph problem,
      with start and goal.
      At top, the algorithms after evaluating five edges
      (evaluated edges labeled as valid or invalid).
      At middle, the final set of evaluated edges.
      At bottom, for each unique path considered from left to right,
      the number of edges on the path that are
      already evaluated, evaluated and valid, evaluated and invalid,
      and unevaluated.
      The total number of edges evaluated is noted in brackets.
      Note that the scale on the Expand plot has been adjusted
      because the selector evaluates many edges not on the candidate
      path at each iteration.
   ]{Snapshots of the LazySP algorithm using each edge selector
      discussed in this chapter on the same obstacle roadmap graph problem,
      with start (\protect\tikz[baseline=-0.5ex]{\protect\node[circle,fill=blue,inner sep=1pt]{};})
      and goal (\protect\tikz[baseline=-0.5ex]{\protect\node[circle,fill=green,inner sep=1pt]{};}).
      At top, the algorithms after evaluating five edges
      (evaluated edges labeled as
      \protect\tikz{\protect\draw[very thick] (0,0) -- (0.15,0.15);}  valid
      or \protect\tikz{\protect\draw[very thick,red] (0,0) -- (0.15,0.15);} invalid).
      At middle, the final set of evaluated edges.
      At bottom, for each unique path considered from left to right,
      the number of edges on the path that are
      \protect\tikz{\protect\node[fill=green!40!white,draw=black]{};}\;already evaluated,
      \protect\tikz{\protect\node[fill=green!70!black,draw=black]{};}\;evaluated and valid,
      \protect\tikz{\protect\node[fill=red!70!black,draw=black]{};}\;evaluated and invalid,
      and \protect\tikz{\protect\node[fill=black!10!white,draw=black]{};}\;unevaluated.
      The total number of edges evaluated is noted in brackets.
      Note that the scale on the Expand plot has been adjusted
      because the selector evaluates many edges not on the candidate
      path at each iteration.
      }
   \label{fig:snapshots}
\end{figure*}

\section{Edge Equivalence to A* Variants}

In the previous section,
we introduced LazySP as the path-selection analogue
to BFS vertex-selection algorithms.
In this section,
we make this analogy more precise.
In particular,
we show that LazySP-Expand
%(that is, LazySP with the Expand selector)
%\ajnote{name consistency}
is edge-equivalent to a variant of A*
(and Weighted A*),
and that LazySP-Forward is edge-equivalent to a variant of
Lazy Weighted A*
(see Table~\ref{table:equivalences}).
%\ajnote{Say ``as described below''?}
It is important to be specific about the conditions under which
these equivalences arise,
which we detail here.

\begin{table}
   \centering
   {\small%
   \begin{tabular}{lll}
      \toprule
      LazySP & Existing & \\
      Selector & Algorithm & Result \\
      \midrule
      Expand & (Weighted) A* & Edge-equivalent \\
      & & (Theorems \ref{thm:astar-equiv-from-lazy},
                 \ref{thm:astar-equiv-to-lazy}) \\
      \addlinespace[0.3em]
      Forward & Lazy Weighted A* & Edge-equivalent \\
      & & (Theorems \ref{thm:lwastar-equiv-from-lazy},
                 \ref{thm:lwastar-equiv-to-lazy}) \\
      \addlinespace[0.3em]
      Alternate & Bidirectional Heuristic & Conjectured \\
      & Front-to-Front Algorithm & \\
      \bottomrule
   \end{tabular}%
   }%
   \caption{LazySP equivalence results.
      The A*, LWA*, and BHFFA algorithms use reopening and the dynamic
      $h_{\ms{lazy}}$ heuristic (\ref{eqn:h_lazy}).}
   \label{table:equivalences}
\end{table}

\textbf{Edge equivalence.}
We say that two algorithms are \emph{edge-equivalent} if they
evaluate the same edges in the same order.
We consider an algorithm to have evaluated an edge
the first time the edge's true weight is requested.

\textbf{Arbitrary tiebreaking.}
For some graphs,
an algorithm may have multiple allowable choices at each iteration
(e.g. LazySP with multiple shortest candidate paths,
or A* with multiple vertices in OPEN with lowest $f$-value).
We will say that algorithm A is equivalent to algorithm B
if for any choice available to A,
there exists an allowable choice available to B
such that the same edge(s) are evaluated by each.

\textbf{A* with reopening.}
We show equivalence to variants of A* and Lazy Weighted A*
that do not use a CLOSED list to prevent
vertices from being visited more than once.
%\ssnote{Why this comparison?! Not explained.}.
%Note that if the heuristic used is $h(v) = \epsilon h_c(v)$
%with $h_c(v)$ consistent,
%a CLOSED list could potentially reduce edge evaluations
%while still guaranteeing $\epsilon$-suboptimality.

\begin{figure}
   \centering
   \begin{tikzpicture}
      \tikzset{>=latex} % arrow heads
      %\draw[step=1cm,black!10,very thin] (0,0) grid (8,4);
      \node[draw,circle,inner sep=1pt,fill=black!20] (S) at (1,1) {S};
      \node[draw,circle,inner sep=1pt] (X) at (2,2.7) {X};
      \node[draw,circle,inner sep=1pt,fill=black!20] (Y) at (3,1) {Y};
      \node[draw,circle,inner sep=1pt] (G) at (5,1) {G};
      \draw[->] (S) -- (Y) node [midway,fill=white] {1,1};
      \draw[->] (Y) -- (G) node [midway,fill=white] {1,3};
      \draw[->] (S) -- (X) node [midway,fill=white] {1,1};
      \draw[->,densely dotted] (X) -- (Y) node [midway,fill=white] {1,?};
      \node[anchor=west] at (2.3,2.9) {$h_{\ms{est}} = 2$};
      \node[anchor=west] at (2.3,2.5) {$h_{\ms{lazy}} = 4$};
      
      % for legend
      %\node at (7,2.9) {unevaluated:};
      %\draw[->,dashed] (6,2.5) -- (8,2.5)
      %   node [midway,fill=white] {($w_{est}$)\,?};
      %\node at (7,1.9) {evaluated:};
      %\draw[->] (6,1.5) -- (8,1.5)
      %   node [midway,fill=white] {($w_{est}$)\,$w$};
      \draw[->,densely dotted] (4.5,2.9) -- (6.5,2.9)
         node[midway,yshift=0.03cm,fill=white,inner sep=1pt,font=\small] {unevaled};
      \draw[->] (4.5,2.55) -- (6.5,2.55)
         node[midway,yshift=0.03cm,fill=white,inner sep=1pt,font=\small] {evaled};
      \node at (5.5,2.2) {$w_{\ms{est}},w$};
      \draw[black!10] (4.25,1.95) rectangle (6.75,3.2);
   \end{tikzpicture}
   \caption{A* comparison between
      the static goal heuristic $h_{\ms{est}}$ (\ref{eqn:h_est})
      and the dynamic goal heuristic $h_{\ms{lazy}}$ (\ref{eqn:h_lazy})
      on a simple graph from start S to goal G.
      The values of both the edge weight estimate $w_{\ms{est}}$
      and the true edge weight $w$ (for evaluated edges) are shown.
      Using either goal heuristic,
      the A* algorithm first expands vertices S and Y,
      evaluating three edges in total and leaving X and G on OPEN.
      After finding that edge YG has $w=3$,
      the dynamic heuristic value $h_{\ms{lazy}}(\mbox{X})$
      is updated from 2 to 4.
      While the A* using the static $h_{\ms{est}}$ would next expand X,
      the A* using the dynamic $h_{\ms{lazy}}$ would next expand G
      and terminate, having never evaluated edge XY.}
   \label{fig:updating-heuristic}
\end{figure}

\textbf{A* with a dynamic heuristic.}
In order to apply A* and Lazy Weighted A* to our problem,
we need a goal heuristic over vertices.
The most simple may be
\begin{equation}
   h_{\ms{est}}(v) = \min_{p : v \rightarrow v_g} \mbox{len}(p, w_{\ms{est}}).
   \label{eqn:h_est}
\end{equation}
Note that the value of this heuristic could be computed as a
pre-processing step using Dijkstra's algorithm \citep{dijkstra1959anote}
before iterations begin.
However,
in order for the equivalences to hold,
we require the use of the lazy heuristic
\begin{equation}
   h_{\ms{lazy}}(v) = \min_{p : v \rightarrow v_g} \mbox{len}(p, w_{\ms{lazy}}).
   \label{eqn:h_lazy}
\end{equation}
This heuristic is dynamic in that it depends on $w_{\ms{lazy}}$
which changes as edges are evaluated.
Therefore,
heuristic values must be recomputed for all affected vertices on OPEN
after each iteration.
%An illustrative example is shown in
%Figure~\ref{fig:updating-heuristic}.
%(We discuss efficient implementation of $h_{\ms{lazy}}$ as it relates
%to the D* family of algorithms in Section~\ref{sec:discussion}.)

\subsection{Equivalence to A*}

We show that the LazySP-Expand algorithm
is edge-equivalent to a variant of the A*
shortest-path algorithm.
%We consider the variant of A* which allows a vertex $v$ to be reopened
%if its stored cost $g[v]$ is improved.
We make use of two invariants that are maintained during the
progression of A*.
\marginnote{Proof of all invariants are available
in Appendix~\ref{sec:appendix-proofs}.}
\begin{invariant}
If $v$ is discovered by A* and $v'$ is undiscovered,
with $v'$ a successor of $v$,
then $v$ is on OPEN.%
\label{inv:astar-cundisc-popen}%
\end{invariant}
\begin{invariant}
If $v$ and $v'$ are discovered by A*,
with $v'$ a successor of $v$,
and $g[v] + w(v,v') < g[v']$,
then $v$ is on OPEN.%
\label{inv:astar-wless-popen}%
\end{invariant}
When we say a vertex is \emph{discovered},
we mean that it is either on OPEN or CLOSED.
Note that Invariant \ref{inv:astar-wless-popen} holds
because we allow vertices to be reopened;
without reopening (and with an inconsistent heuristic),
later finding a cheaper path to $v$ (and not reopening $v'$)
would invalidate the invariant.

We will use the goal heuristic $h_{\ms{lazy}}$ from (\ref{eqn:h_lazy}).
Note that if an admissible edge weight estimator $\hat{w}$ exists
(that is, $\hat{w} \leq w$),
then our A* can approximate the Weighted A* algorithm
\citep{pohl1970weightedastar}
with parameter $\epsilon$
by using $w_{\ms{est}} = \epsilon \, \hat{w}$,
and the suboptimality bound from
Theorem~\ref{thm:lazy-optimality} holds.

\begin{figure}[t]
   \centering
   \begin{tikzpicture}
      %\draw[step=1cm,gray,very thin] (0,0) grid (8,3);
      
      \draw[fill=black!05] (2.1,1.5) ellipse (0.4cm and 0.5cm);
      \draw[fill=black!05] (5.9,1.5) ellipse (0.4cm and 0.5cm);
      \draw[fill=black!05] (4,1) ellipse (0.4cm and 0.4cm);
      
      \node[align=center] at (0.75,1.5) {$P_{\ms{candidate}}$\\(LazySP)};
      \node[align=center] at (7.25,1.5) {$S_{\ms{candidate}}$\\(A*)};
      \node[align=center] at (4,1.75) {$V_{\ms{frontier}}$};
      
      \node[fill=black,circle,inner sep=1pt] (p1) at (2.15,1.7) {};
      \node[fill=black,circle,inner sep=1pt] (p2) at (2.05,1.3) {};
      
      \node[fill=black,circle,inner sep=1pt] (s1) at (5.95,1.8) {};
      \node[fill=black,circle,inner sep=1pt] (s2) at (5.85,1.3) {};
      
      \node[fill=black,circle,inner sep=1pt] (v1) at (4.1,0.9) {};
      
      \draw[->] (p1) -- (v1);
      \draw[->] (p2) -- (v1);
      
      \draw[->] (s1) -- (v1);
      \draw[->] (s2) -- (v1);
      
   \end{tikzpicture}
   \caption{Illustration of the equivalence
      between A* and LazySP-Expand.
      After evaluating the same set of edges,
      the next edges to be evaluated by each algorithm
      can both be expressed as a surjective mapping onto
      a common set of unexpanded
      frontier vertices.
      }
   \label{fig:astar-equiv-mapping}
\end{figure}

\textbf{Equivalence.}
In order to show edge-equivalence,
we consider the case where both algorithms
are beginning a new iteration
having so far evaluated the same set of edges.

LazySP-Expand has some set $P_{\ms{candidate}}$ of allowable
candidate paths minimizing $\mbox{len}(p,w_{\ms{lazy}})$;
the Expand selector will then identify a vertex on the chosen path
for expansion.

A* will iteratively select a set of vertices from OPEN to expand.
Because it is possible that a vertex is expanded multiple times
(and only the first expansion results in edge evaluations),
we group iterations of A* into \emph{sequences},
where each sequence $s$ consists of
(a) zero or more vertices from OPEN that have already been expanded,
followed by (b) one vertex from OPEN that is to be expanded
for the first time.

We show that both the set of allowable candidate paths $P_{\ms{candidate}}$
available to LazySP-Expand
and the set of allowable candidate vertex sequences $S_{\ms{candidate}}$
available to A*
map surjectively to the same set of unexpanded frontier vertices $V_{\ms{frontier}}$
as illustrated in Figure~\ref{fig:astar-equiv-mapping}.
This is described by way of
Theorems \ref{thm:astar-equiv-from-lazy}
and \ref{thm:astar-equiv-to-lazy} below.
\marginnote{Proof of all theorems are available
in Appendix~\ref{sec:appendix-proofs}.}

\begin{theorem}
If LazySP-Expand and A* have evaluated the same set of edges,
then for any candidate path $p_{\ms{candidate}}$ chosen by LazySP
yielding frontier vertex $v_{\ms{frontier}}$,
there exists an allowable A* sequence $s_{\ms{candidate}}$
which also yields $v_{\ms{frontier}}$.
\label{thm:astar-equiv-from-lazy}
\end{theorem}

\begin{theorem}
If LazySP-Expand and A* have evaluated the same set of edges,
then for any candidate sequence $s_{\ms{candidate}}$ chosen by A*
yielding frontier vertex $v_{\ms{frontier}}$,
there exists an allowable LazySP path $p_{\ms{candidate}}$
which also yields $v_{\ms{frontier}}$.
\label{thm:astar-equiv-to-lazy}
\end{theorem}

\subsection{Equivalence to Lazy Weighted A*}

%\begin{algorithm}
%   \caption{Forward Edge Evaluation Selector}
%   \begin{algorithmic}[1]
%   \Function {\textsc{SelectForward}}{$G, p_{\ms{candidate}}$}
%   \State $e_{\ms{first}} \leftarrow$ first unevaluated $e \in p_{\ms{candidate}}$
%   \State \Return $\{ e_{\ms{first}} \}$
%   \EndFunction
%   \end{algorithmic}
%   \label{alg:selectforward}
%\end{algorithm}

In a conventional vertex expansion algorithm,
determining a successor's cost is a function of both
the cost of the edge and the value of the heuristic.
If either of these components is expensive to evaluate,
an algorithm can defer its computation by maintaining the successor
on the frontier with an approximate cost until it is expanded.
The Fast Downward algorithm \citep{helmert2006fastdownward} is motivated
by expensive heuristic evaluations in planning,
whereas the Lazy Weighted A* (LWA*) algorithm \citep{cohen2014narms}
is motivated by expensive edge evaluations in robotics.

We show that the LazySP-Forward algorithm
is edge-equivalent to a variant of the Lazy Weighted A*
shortest-path algorithm.
For a given candidate path,
the Forward selector returns the first unevaluated edge.

\textbf{Variant of Lazy Weighted A*.}
We reproduce a variant of LWA* without a CLOSED list
in Algorithm~\ref{alg:lwastar}.
For the purposes of our analysis,
the reproduction differs from the original presentation,
and we detail those differences here.
With the exception of the lack of CLOSED,
the differences do not affect the behavior of the algorithm.

\begin{algorithm}[t]
\caption{Lazy Weighted A* (without CLOSED list)}
\label{alg:lwastar}
\begin{algorithmic}[1]
\Function {\textsc{LazyWeightedA*}}{$G, w, \hat{w}, h$}
\State $g[v_{\ms{start}}] \leftarrow 0$
\State $Q_v \leftarrow \{ v_{\ms{start}} \}$
   \Comment Key: $g[v] + h(v)$
   \label{line:lwastar-key-qvertices}
\State $Q_e \leftarrow \emptyset$
   \Comment Key: $g[v] + \hat{w}(v,v') + h(v')$
   \label{line:lwastar-key-qedges}
\While {$\min(Q_v.{\mbox{TopKey}}, Q_e.{\mbox{TopKey}}) < g[v_{\ms{goal}}]$}
   \If {$Q_v.{\mbox{TopKey}} \leq Q_e.{\mbox{TopKey}}$}
      \State $v \leftarrow Q_v.{\mbox{Pop}}()$
      \For {$v' \in G.\mbox{GetSuccessors}(v)$}
         \State $Q_e.\mbox{Insert}((v,v'))$
      \EndFor
   \Else
      \State $(v,v') \leftarrow Q_e.{\mbox{Pop}}()$
      \If {$g[v'] \leq g[v] + \hat{w}(v,v')$}
         \label{line:lwastar-test}
         \State {\bf continue}
      \EndIf
      \State $g_{\ms{new}} \leftarrow g[v] + w(v,v')$
         \Comment evaluate
      \If {$g_{\ms{new}} < g[v']$}
         \State $g[v'] = g_{\ms{new}}$
         \State $Q_v.\mbox{Insert}(v')$
      \EndIf
   \EndIf
\EndWhile
\EndFunction
\end{algorithmic}
\end{algorithm}

The most obvious difference is that we present the original OPEN list
as separate vertex ($Q_v$) and edge ($Q_e$) priority queues,
with sorting keys shown on lines \ref{line:lwastar-key-qvertices}
and \ref{line:lwastar-key-qedges}.
A vertex $v$ in the original OPEN with $trueCost(v) = true$
corresponds to a vertex $v$ in $Q_v$,
whereas a vertex $v'$ in the original OPEN
with $trueCost(v') = false$ (and parent $v$)
corresponds to an edge $(v,v')$ in $Q_e$.
Use of the edge queue obviates the need for
duplicate vertices on OPEN with different parents
and the $conf(v)$ test for identifying such duplicates.
This presentation also highlights the similarity between LWA*
and the inner loop of the Batch Informed Trees (BIT*) algorithm
\citep{gammell2015bitstar}.

The second difference is that the edge usefulness test
(line 12 of the original algorithm)
has been moved from before inserting into OPEN
to after being popped from OPEN,
but before being evaluated
(line~\ref{line:lwastar-test} of Algorithm~\ref{alg:lwastar}).
This change is partially in compensation for removing the CLOSED
list.
This adjustment
does not affect the edges evaluated.

We make use of an invariant that is maintained during the
progression of Lazy Weighted A*.
\marginnote{Proof of all invariants are available
in Appendix~\ref{sec:appendix-proofs}.}
\begin{invariant}
For all vertex pairs $v$ and $v'$,
with $v'$ a successor of $v$,
if $g[v] + \max(w(v,v'), \hat{w}(v,v')) < g[v']$,
then either vertex $v$ is on $Q_{v}$
or edge $(v,v')$ is on $Q_e$.%
\label{inv:lwastar}%
\end{invariant}
We will use $h(v) = h_{\ms{lazy}}(v)$ from (\ref{eqn:h_lazy})
and $\hat{w} = w_{\ms{lazy}}$.
Note that the use of these dynamic heuristics requires that the
$Q_v$ and $Q_e$ be resorted after every edge is evaluated.

\textbf{Equivalence.}
The equivalence follows similarly to that for A* above.
Given the same set of edges evaluated,
the set of allowable next evaluations is identical for each
algorithm.
\marginnote{Proof of all theorems are available
in Appendix~\ref{sec:appendix-proofs}.}

\begin{theorem}
If LazySP-Forward and LWA* have evaluated the same set of edges,
then for any allowable candidate path $p_{\ms{candidate}}$
chosen by LazySP yielding first unevaluated edge $e_{ab}$,
there exists an allowable LWA* sequence $s_{\ms{candidate}}$
which also yields $e_{ab}$.
\label{thm:lwastar-equiv-from-lazy}
\end{theorem}

\begin{theorem}
If LazySP-Forward and LWA* have evaluated the same set of edges,
then for any allowable sequence of vertices and edges $s_{\ms{candidate}}$
considered by LWA* yielding evaluated edge $e_{ab}$,
there exists an allowable LazySP candidate path $p_{\ms{candidate}}$
which also yields $e_{ab}$.
\label{thm:lwastar-equiv-to-lazy}
\end{theorem}

\subsection{Relation to Bidirectional Heuristic Search}

LazySP-Alternate chooses unevaluated edges from either
the beginning or the end of the candidate path at each iteration.
We conjecture that an alternating version of the Expand selector
is edge-equivalent to the
Bidirectional Heuristic Front-to-Front Algorithm
\citep{champeauxsint1977bhffa}
for appropriate lazy vertex pair heuristic,
and that LazySP-Alternate is edge-equivalent
to a bidirectional LWA*.

\begin{algorithm}[t]
   \caption{Maximum Edge Probability Selector
      \emph{(for WeightSamp and Partition path distributions)}}
   \begin{algorithmic}[1]
   \Function {\textsc{SelectMaxEdgeProb}}{$G, p_{\ms{candidate}}, \mathcal{D}_p$}
   \State $p(e) \leftarrow \Pr( \, e \in P \, )
      \mbox{ for } P \sim \mathcal{D}_p$
   \State $e_{\ms{max}} \leftarrow$ unevaluated $e \in p_{\ms{candidate}}$
      maximizing $p(e)$
   \State \Return $\{ e_{\ms{max}} \}$
   \EndFunction
   \end{algorithmic}
   \label{alg:selectmaxscore}
\end{algorithm}

\section{Novel Edge Selectors}

Because we are conducting a search over paths,
we are free to implement selectors which are not constrained to
evaluate edges in any particular order
(i.e. to maintain evaluated trees rooted at the start and goal
vertices).
In this section,
we describe a novel class of edge selectors which is designed
to reduce the total number of edges evaluated during the course
of the LazySP algorithm.
These selectors operate by maintaining a distribution over potential
paths at each iteration of the algorithm
(see Figure~\ref{fig:maxprob-selectors-overview}).
This path distribution induces a Bernoulli distribution for each
edge $e$ which indicates its probability $p(e)$ to lie on
the potential path;
at each iteration,
the selectors then choose the unevaluated edge that maximizes
this edge indicator probability (Algorithm~\ref{alg:selectmaxscore}).
The two selectors described in this section differ
with respect to how they maintain this distribution over potential paths.

% make -f e8_experiments/scripts/Makefile.lazysp-fig-dists
\begin{figure}[t]
   \centering
   \begin{tikzpicture}
      \tikzset{>=latex}
      
      \node[draw,minimum width=2.4cm,minimum height=3.0cm] (startbox) at (-3.0,0) {};
      \node[inner sep=0pt] at (-3.0,-0.35) {\includegraphics[scale=2.0]{build/lazysp-fig-dists/fig-sofar}};
      \node[align=center,font=\small,below] at (startbox.north) {known\\edges};
      
      \node[draw] (quesbox) at (-1.2,0) {?};
      
      \node[draw,minimum width=2.1cm,minimum height=3.0cm] (pathsbox) at (0.4,0) {};
      \node[inner sep=0pt] at (-0.05, 0.1) {\includegraphics[scale=0.8]{build/lazysp-fig-dists/fig-path-00}};
      \node[inner sep=0pt] at ( 0.85, 0.1) {\includegraphics[scale=0.8]{build/lazysp-fig-dists/fig-path-01}};
      \node[inner sep=0pt] at (-0.05,-0.8) {\includegraphics[scale=0.8]{build/lazysp-fig-dists/fig-path-02}};
      \node[inner sep=0pt] at ( 0.85,-0.8) {\includegraphics[scale=0.8]{build/lazysp-fig-dists/fig-path-03}};
      \node[align=center,font=\small,below] at (pathsbox.north) {path\\distribution};
      \node[align=center,font=\normalsize,above] at (pathsbox.south) {$\dots$};
      
      \node[draw,minimum width=2.4cm,minimum height=3.0cm] (goalbox) at (3.0,0) {};
      \node[inner sep=0pt] at (3.0,-0.35) {\includegraphics[scale=2.0]{build/lazysp-fig-dists/fig-dist-probs}};
      \node[align=center,font=\small,below] at (goalbox.north) {edge indicator\\distributions};
      
      \draw[->] (startbox) -- (quesbox);
      \draw[->] (quesbox) -- (pathsbox);
      \draw[->] (pathsbox) -- (goalbox);
      
   \end{tikzpicture}
   \caption{Illustration of maximum edge probability selectors.
      A distribution over paths
      (usually conditioned on the known edge evaluations)
      induces on each edge $e$ a Bernoulli distribution
      with parameter $p(e)$
      giving the probability that it belongs to the path.
      The selector chooses the edge with the largest such probability.}
   \label{fig:maxprob-selectors-overview}
\end{figure}

\subsection{Weight Function Sampling Selector}

The first selector, WeightSamp,
is motivated by the intuition that it is preferable to evaluate edges
that are most likely to lie on the true shortest path.
Therefore,
it computes its path distribution $\mathcal{D}_p$
by performing shortest path queries
on sampled edge weight functions drawn from a distribution
$\mathcal{D}_w$.
This edge weight distribution is conditioned on the the known weights
of all previously evaluated edges $E_{\ms{eval}}$:
\begin{equation}
   \mathcal{D}_p : \mbox{SP}(w)
   \mbox{ for } w \sim \mathcal{D}_w(E_{\ms{eval}})
   \label{eqn:weightsamp}.
\end{equation}

For example,
the distribution $\mathcal{D}_w$ might consist of
the edge weights induced by a model of the distribution of
environment obstacles
(Figure~\ref{fig:weightsamp}).
Since this obstacle distribution is conditioned on the results
of known edge evaluations,
we consider the subset of worlds which are consistent
with the edges we have evaluated so far.
However,
depending on the fidelity of this model,
solving the corresponding shortest path problem for a given
sampled obstacle arrangement might require as much computation as
solving the original problem,
since it requires computing the resulting edge weights.
In practice,
we can approximate $\mathcal{D}_w$
by assuming that each edge is independently distributed.

\begin{figure}[t]
   \centering
   \begin{tikzpicture}
      \tikzset{>=latex}
      
      \node[draw,minimum width=1.8cm,minimum height=2.6cm] (startbox) at (-4.4,0) {};
      \node[inner sep=0pt] at (-4.4,-0.35) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-sofar}};
      \node[align=center,font=\small,below] at (startbox.north) {known\\edges};
      
      \node[draw,minimum width=1.8cm,minimum height=6cm] (abox) at (-2.2,0) {};
      \node[inner sep=0pt] at (-2.2, 1.3) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-world-00}};
      \node[inner sep=0pt] at (-2.2,-0.3) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-world-01}};
      \node[inner sep=0pt] at (-2.2,-1.9) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-world-02}};
      \node[align=center,font=\small,below] at (abox.north) {obstacle\\distribution};
      \node[align=center,font=\normalsize,above] at (abox.south) {$\dots$};
      
      \node[draw,minimum width=1.8cm,minimum height=6cm] (bbox) at (0,0) {};
      \node[inner sep=0pt] at (0, 1.3) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-wfn-00}};
      \node[inner sep=0pt] at (0,-0.3) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-wfn-01}};
      \node[inner sep=0pt] at (0,-1.9) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-wfn-02}};
      \node[align=center,font=\small,below] at (bbox.north) {weight fn\\distribution};
      \node[align=center,font=\normalsize,above] at (bbox.south) {$\dots$};
      
      \node[draw,minimum width=1.8cm,minimum height=6cm] (cbox) at (2.2,0) {};
      \node[inner sep=0pt] at (2.2, 1.3) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-path-00}};
      \node[inner sep=0pt] at (2.2,-0.3) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-path-01}};
      \node[inner sep=0pt] at (2.2,-1.9) {\includegraphics[scale=1.5]{build/lazysp-fig-dists/fig-path-02}};
      \node[align=center,font=\small,below] at (cbox.north) {path\\distribution};
      \node[align=center,font=\normalsize,above] at (cbox.south) {$\dots$};
      
      \draw[->] (startbox) -- (abox);
      \draw[->] (abox) -- (bbox);
      \draw[->] (bbox) -- (cbox);
   \end{tikzpicture}
   \caption{The WeightSamp selector uses the path distribution induced by
      solving the shortest path problem on a distribution over possible
      edge weight functions $\mathcal{D}_w$.
      In this example, samples from $\mathcal{D}_w$ are computed by
      drawing samples from $\mathcal{D}_O$,
      the distribution of obstacles that are consistent with
      the known edge evaluations.}
   \label{fig:weightsamp}
\end{figure}

\subsection{Partition Function Selector}

While the WeightSamp selector captures the intuition that it is
preferable to focus edge evaluations in areas that are useful for
many potential paths,
the computational cost required to calculate it at each iteration
may render it intractable.
One candidate path distribution that is more efficient to compute
follows an exponential form:
\begin{equation}
   \mathcal{D}_p : f_P(p) \propto
   \exp( - \beta \, \mbox{len}(p, w_{\ms{lazy}}) ).
\end{equation}
In other words,
we consider all potential paths $P$
between the start and goal vertices,
with shorter paths assigned more exponentially probability
than longer ones
(with positive parameter $\beta$).
We call this the Partition selector
because this distribution is closely related to calculating
partition functions from statistical mechanics.
The corresponding partition function is:
\begin{equation}
   Z(P) = \sum_{p \in P}
      \exp( - \beta \, \mbox{len}(p, w_{\ms{lazy}}) ).
   \label{eqn:partitionfn}
\end{equation}
Note that the edge indicator probability
required in Algorithm~\ref{alg:selectmaxscore}
can then be written:
\begin{equation}
   p(e) = 1 - \frac{Z(P \setminus e)}{Z(P)}.
   \label{eqn:edge-ind-prob}
\end{equation}
Here, $P \setminus e$ denotes paths in $P$ that do not
contain edge $e$.

\begin{figure}
   \centering
   \subfloat[Initial $p(e)$ scores on a constant-weight
         grid with $\beta$: 50, 33, 28]{%
      \centering
      \includegraphics{build/lazysp-selscores/empty-50}
      \includegraphics{build/lazysp-selscores/empty-33}
      \includegraphics{build/lazysp-selscores/empty-28}
      \label{subfig:partition-empty}
   }
   
   \subfloat[Initial $p(e)$ scores with $\infty$-weight
         obstacles with $\beta$: 50, 33, 28]{%
      \centering
      \includegraphics{build/lazysp-selscores/gap-50}
      \includegraphics{build/lazysp-selscores/gap-33}
      \includegraphics{build/lazysp-selscores/gap-28}
      \label{subfig:partition-passage}
   }
   
   % $ rosrun e8_experiments lazysp-partall-figure.py
   % --probdir=prob-box2d08-halton-roots12
   % --snapshot-afteredges=0 --out-tikz=partall-figure-0.tex
   \subfloat[Initial $p(e)$ scores]{%
      \centering
      \;
      \includegraphics{build/lazysp-partall/partall-figure-0}
      \;
      \label{subfig:partition-example-initial}
   }
   % $ rosrun e8_experiments lazysp-partall-figure.py
   % --probdir=prob-box2d08-halton-roots12
   % --snapshot-afteredges=5 --out-tikz=partall-figure-5.tex
   \subfloat[Scores after five evaluations]{%
      \centering
      \;
      \includegraphics{build/lazysp-partall/partall-figure-5}
      \;
      \label{subfig:partition-example-after5}
   }
   \vspace{0.2cm}

   \caption{Examples of the Partition selector's
      $p(e)$ edge score function.
      %(\subref{subfig:partition-empty})
      With no known obstacles,
      a high $\beta$ assigns near-unity score to only edges on the
      shortest path;
      as $\beta$ decreases and more paths are considered,
      edges immediately adjacent to the roots score highest.
      %(\subref{subfig:partition-passage})
      Since all paths must pass
      through the narrow passage,
      edges within score highly.
      %(\subref{subfig:partition-example-initial})
      For a problem with two a-priori known obstacles (dark gray),
      the score first prioritizes evaluations between the two.
      %(\subref{subfig:partition-example-after5})
      Upon finding these edges are blocked,
      the next edges that are prioritized lie along the top of the world.}
   \label{ref:example-scores}
\end{figure}

It may appear advantageous to restrict $P$ to only
\emph{simple} paths,
since all optimal paths are simple.
Unfortunately,
an algorithm for computing (\ref{eqn:edge-ind-prob}) efficiently is not
currently known in this case.
However,
in the case that $P$ consists of all paths,
there does exist an efficient incremental calculation of
(\ref{eqn:partitionfn}) via a recursive formulation.

We use the notation $Z_{xy} = Z(P_{xy})$,
with $P_{xy}$ the set of paths from $x$ to $y$.
Suppose the values $Z_{xy}$ are known between
all pairs of vertices $x, y$ for a graph $G$.
(For a graph with no edges,
$Z_{xy}$ is 1 if $x = y$ and 0 otherwise.)
Consider a modified graph $G'$ with one additional edge $e_{ab}$
with weight $w_{ab}$.
All additional paths use the new edge $e_{ab}$ a non-zero
number of times;
the value $Z'_{xy}$ can be shown to be
\begin{equation}
   Z'_{xy} = Z_{xy} + \frac{Z_{xa} Z_{by}}{\exp(\beta w_{ab}) - Z_{ba}}
   \mbox{ if }
   \exp(\beta w_{ab}) > Z_{ba}.
\end{equation}
This form is derived from simplifying the induced geometric series;
note that if $\exp(\beta w_{ab})  \leq Z_{ba}$,
the value $Z'_{xy}$ is infinite.
One can also derive the inverse:
given values $Z'$,
calculate the values $Z$ if an edge were removed.
A derivation of this formulation is given in
Appendix~\ref{chap:appendix-partition}.

This incremental formulation of (\ref{eqn:partitionfn})
allows for the corresponding score $p(e)$ for edges
to be updated efficiently during each iteration of LazySP as
the $w_{\ms{lazy}}$ value for edges chosen for evaluation are updated.
In fact,
if the values $Z$ are stored in a square matrix,
the update for all pairs after an edge weight change consists of a single
vector outer product.

\section{Experiments}

We compared the seven edge selectors on three classes of shortest path
problems.
The average number of edges evaluated by each,
as well as timing results from our implementations,
are shown in Figure~\ref{fig:results}.
In each case,
the estimate was chosen so that $w_{\ms{est}} \leq w$,
so that all runs produced optimal paths.
The experimental results serve primarily to illustrate that
the A* and LWA* algorithms
(i.e. Expand and Forward)
are not optimally edge-efficient,
but they also expose differences in behavior and prompt
future research directions.
All experiments were conducted using an open-source
implementation.
Motion planning results were implemented using
OMPL \citep{sucan2012ompl}.

\textbf{Random partially-connected graphs.}
We tested on a set of 1000 randomly-generated undirected graphs
with $|V|=100$,
with each pair of vertices sharing an edge with probability 0.05.
Edges have an independent 0.5 probability of having infinite weight,
else the weight is uniformly distributed on $[1,2]$;
the estimated weight was unity for all edges.
For the WeightSamp selector,
we drew 1000 $w$ samples.
For the Partition selector, we used $\beta = 2$.

\textbf{Roadmap graphs on the unit square.}
We considered roadmap graphs formed via the first 100 points
of the $(2,3)$-Halton sequence on the unit square
with a connection radius of 0.15,
with 30 pairs of start and goal vertices chosen randomly.
The edge weight function was derived from 30 sampled obstacle fields
consisting of 10 randomly placed boxes
with dimensions uniform on $[0.1,0.3]$,
with each edge having infinite weight on collision,
and weight equal to its Euclidean length otherwise.
One of the resulting 900 example problems is shown in
Figure~\ref{fig:snapshots}.
For the WeightSamp selector,
we drew 1000 $w$ samples
with a na\"{\i}ve edge weight distribution in which
each edge had an independent 0.1 collision probability.
For the Partition selector, we used $\beta = 21$.

\textbf{Roadmap graphs for robot arm motion planning.}
We considered roadmap graphs in the configuration space
corresponding to 7-DOF right arm of the HERB home robot across three
motion planning problems inspired by a table clearing scenario
(see Figure~\ref{fig:herbbin0}).
The problems consisted of first moving from the robot's
home configuration to one of 7 feasible grasp configurations for a mug
(pictured),
second transferring the mug to one of 72 feasible configurations with
the mug above the blue bin,
and third returning to the home configuration.
Each problem was solved independently.
This common scenario spans various numbers of starts/goals
and allows a comparison w.r.t. difficulty at different problem
stages as discussed later.

For each problem,
50 random graphs were constructed by applying a random offset to
the 7D Halton sequence with $N = 1000$,
with additional vertices for each problem start and goal configuration.
We used an edge connection radius of 3 rad,
resulting $|E|$ ranging from 23404 to 28109.
Each edge took infinite weight on collision,
and weight equal to its Euclidean length otherwise.
For the WeightSamp selector,
we drew 1000 $w$ samples
with a na\"{\i}ve edge weight distribution in which
each edge had an independent 0.1 probability of collision.
For the Partition selector, we used $\beta = 3$.

\begin{figure*}
\centering
%\includegraphics[width=3cm]{figs/herbbin0.png}
\includegraphics[width=3.1cm]{figs/lazysp-herbarm/herbarm-roadmap.png}
\includegraphics[width=3.1cm]{figs/lazysp-herbarm/herbarm-path02.png}
%\includegraphics[width=3cm]{figs/lazysp-herbarm/herbarm-path11.png}
%\includegraphics[width=3cm]{figs/lazysp-herbarm/herbarm-path21.png}
\includegraphics[width=3.1cm]{figs/lazysp-herbarm/herbarm-path33.png}
\includegraphics[width=3.1cm]{figs/lazysp-herbarm/herbarm-path42.png}
\includegraphics[width=3.1cm]{figs/lazysp-herbarm/herbarm-path46.png}
\caption{Visualization of the first of three articulated motion
   planning problems in which the HERB robot must move its right arm
   from the start configuration (pictured)
   to any of seven grasp configurations for a mug.
   Shown is the progression of the Alternate selector on one of the
   randomly generated roadmaps;
   approximately 2\% of the 7D roadmap is shown in gray by projecting
   onto the space of end-effector positions.}
\label{fig:herbbin0}
\end{figure*}

\begin{figure}[t!]
   \centering
   \subfloat[
      Average number of edges evaluated for each problem class
         and selector.
         The minimum selector,
         along with any selector within one unit of its standard error,
         is shown in bold.
         The ArmPlan class is split into its three constituent problems.
         Online timing results are also shown,
         including the components from the invoking the selector
         and evaluating edges.
         \dag PartConn and UnitSquare involve trivial edge evaluation
         time.
         \ddag Timing for the Partition selector does not include
         pre-computation time.
         See Figure~\ref{fig:table-timing-results} for details.]
   {%
      \centering
      {\small%
      \setlength{\tabcolsep}{0.06cm}%
      \begin{tabular}{lrrrrrrr}
         \toprule
            & E\;\;\;\;
            & F\;\;\;\; & R\;\;\;\; & A\;\;\;\;
            & B\;\;\;\; & W\;\;\;\; & P\ddag\;\; \\
         \midrule
         \addlinespace[0.3em]
         PartConn &  87.10 & 35.86 & 34.84 & 22.23 & 44.81 & \textbf{20.66} & \textbf{20.39} \\
         \;\;\emph{online\dag (ms)} & \bf\emph{1.22} & \emph{1.96} & \emph{1.86} & \bf\emph{1.20} & \emph{2.41} & \emph{4807.19} & \emph{3.32} \\
         \;\;\;\;\emph{sel (ms)} & \emph{0.02} & \emph{0.01} & \emph{0.01} & \emph{0.01} & \emph{0.03} & \emph{4805.64} & \emph{2.07} \\
         \addlinespace[0.3em]
         UnitSquare &  69.21 & 27.29 & 27.69 & 17.82 & 32.62 & 15.58 & \textbf{14.08} \\
         \;\;\emph{online\dag (ms)} & \bf\emph{0.91} & \emph{1.47} & \emph{1.49} & \bf\emph{0.94} & \emph{1.71} & \emph{3864.95} & \emph{1.72} \\
         \;\;\;\;\emph{sel (ms)} & \emph{0.01} & \emph{0.01} & \emph{0.01} & \emph{0.01} & \emph{0.02} & \emph{3863.49} & \emph{0.87} \\
         \addlinespace[0.3em]
         ArmPlan(avg) & 949.05 & 63.62 & 74.94 & 55.48 & 68.01 & 56.93 & \textbf{48.07} \\
         \;\;\emph{online (s)} & \emph{269.82} & \bf\emph{5.90} & \emph{8.22} & \bf\emph{5.96} & \emph{7.34} & \emph{3402.21} & \bf\emph{5.80} \\
         \;\;\;\;\emph{sel (s)} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{3392.76} & \emph{1.54} \\
         \;\;\;\;\emph{eval (s)} & \emph{269.78} & \emph{5.87} & \emph{8.20} & \emph{5.94} & \emph{7.31} & \emph{9.39} & \emph{4.21} \\
         \addlinespace[0.3em]
         ArmPlan1 &  344.74 & \textbf{49.72} & 95.58 & 59.44 & 58.90 & 73.72 & \textbf{50.66} \\
         \;\;\emph{online (s)} & \emph{109.09} & \bf\emph{4.81} & \emph{14.81} & \emph{7.03} & \emph{7.91} & \emph{3375.35} & \emph{7.25} \\
         \;\;\;\;\emph{sel (s)} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{3358.82} & \emph{1.61} \\
         \;\;\;\;\emph{eval (s)} & \emph{109.07} & \emph{4.78} & \emph{14.77} & \emph{7.01} & \emph{7.88} & \emph{16.47} & \emph{5.59} \\
         \addlinespace[0.3em]
         ArmPlan2 &  657.02 & \textbf{62.24} & 98.54 & 69.96 & 75.88 & \textbf{66.24} & \textbf{62.16} \\
         \;\;\emph{online (s)} & \emph{166.19} & \bf\emph{3.27} & \emph{7.36} & \emph{5.95} & \emph{5.63} & \emph{4758.04} & \emph{5.99} \\
         \;\;\;\;\emph{sel (s)} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{4750.16} & \emph{2.03} \\
         \;\;\;\;\emph{eval (s)} & \emph{166.17} & \emph{3.26} & \emph{7.34} & \emph{5.93} & \emph{5.61} & \emph{7.82} & \emph{3.91} \\
         \addlinespace[0.3em]
         ArmPlan3 & 1845.38 & 78.90 & \textbf{30.70} & 37.04 & 69.26 & \textbf{30.82} & \textbf{31.38} \\
         \;\;\emph{online (s)} & \emph{534.16} & \emph{9.61} & \bf\emph{2.50} & \emph{4.91} & \emph{8.47} & \emph{2073.23} & \emph{4.17} \\
         \;\;\;\;\emph{sel (s)} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{0.00} & \emph{2069.29} & \emph{0.98} \\
         \;\;\;\;\emph{eval (s)} & \emph{534.10} & \emph{9.58} & \emph{2.48} & \emph{4.89} & \emph{8.44} & \emph{3.90} & \emph{3.15} \\
         \addlinespace[0.15em]
         \bottomrule
      \end{tabular}%
      }%
      \label{subfig:table-results}
   }
   
   \vspace{0.1in}
   
   \subfloat[PartConn]{%
      \centering
      \begin{tikzpicture}
      \begin{axis}[
         width=4.1cm,
         height=4.0cm,
         ybar,
         bar width=7,
         ymin=0,ymax=90,
         ytick pos=bottom,
         symbolic x coords={E, F, R, A, B, W, P},
         xtick=data,
         xtick pos=left,
         ymajorgrids,
         ymajorticks=false,
         ticklabel style={font=\small}
         ]
      \node[circle,fill=white,inner sep=1pt,text=black!40] at (axis cs:P,40) {\scriptsize 40};
      \node[circle,fill=white,inner sep=1pt,text=black!40] at (axis cs:P,60) {\scriptsize 60};
      \node[circle,fill=white,inner sep=1pt,text=black!40] at (axis cs:P,80) {\scriptsize 80};
      \addplot[color=black,fill=black!20,error bars/.cd,y dir=both,y explicit] coordinates {
         (E, 87.10) +- (2.39,2.39)
         (F, 35.86) +- (1.04,1.04)
         (R, 34.84) +- (1.04,1.04)
         (A, 22.23) +- (0.60,0.60)
         (B, 44.81) +- (1.11,1.11)
         (W, 20.66) +- (0.57,0.57)
         (P, 20.39) +- (0.56,0.56)
      };
      \end{axis}
      \end{tikzpicture}
   }
   \subfloat[UnitSquare]{%
      \centering
      \begin{tikzpicture}
      \begin{axis}[
         width=4.1cm,
         height=4.0cm,
         ybar,
         bar width=7,
         ymin=0,ymax=90,
         ytick pos=bottom,
         symbolic x coords={E, F, R, A, B, W, P},
         xtick=data,
         xtick pos=left,
         ymajorgrids,
         ymajorticks=false,
         ticklabel style={font=\small}
         ]
      \node[circle,fill=white,inner sep=1pt,text=black!40] at (axis cs:P,40) {\scriptsize 40};
      \node[circle,fill=white,inner sep=1pt,text=black!40] at (axis cs:P,60) {\scriptsize 60};
      \node[circle,fill=white,inner sep=1pt,text=black!40] at (axis cs:P,80) {\scriptsize 80};
      \addplot[color=black,fill=black!20,error bars/.cd,y dir=both,y explicit] coordinates {
         (E, 69.21) +- (2.55,2.55)
         (F, 27.29) +- (1.03,1.03)
         (R, 27.69) +- (1.02,1.02)
         (A, 17.82) +- (0.60,0.60)
         (B, 32.62) +- (0.72,0.72)
         (W, 15.58) +- (0.47,0.47)
         (P, 14.08) +- (0.46,0.46)
      };
      \end{axis}
      \end{tikzpicture}
   }
   \subfloat[ArmPlan]{%
      \centering
      \begin{tikzpicture}
      \begin{axis}[
         width=4.1cm,
         height=4.0cm,
         ybar,
         bar width=7,
         ymin=0,ymax=115,
         max space between ticks=10,
         ytick pos=bottom,
         symbolic x coords={E, F, R, A, B, W, P},
         xtick=data,
         xtick pos=left,
         ymajorgrids,
         ymajorticks=false,
         ticklabel style={font=\small}
         ]
      \node[circle,fill=white,inner sep=0pt,text=black!40] at (axis cs:P,60) {\scriptsize 60};
      \node[circle,fill=white,inner sep=0pt,text=black!40] at (axis cs:P,80) {\scriptsize 80};
      \node[circle,fill=white,inner sep=0pt,text=black!40] at (axis cs:P,100) {\scriptsize 100};
      \addplot[color=black,fill=black!20,error bars/.cd,y dir=both,y explicit] coordinates {
         (E, 115) +- (0,0) % 49.06 +- 61.63.46
         (F, 63.62) +- (4.15,4.15)
         (R, 74.94) +- (5.07,5.07)
         (A, 55.48) +- (2.95,2.95)
         (B, 68.01) +- (3.86,3.86)
         (W, 56.93) +- (3.37,3.37)
         (P, 48.07) +- (2.44,2.44)
      };
      \node[align=center,anchor=north,inner sep=0pt] at (axis cs:E,111) {\scriptsize $\uparrow$};
      \end{axis}
      \end{tikzpicture}
   }
   \caption{
      Experimental results for the three problem classes
      across each of the seven selectors,
      E:Expand, F:Forward, R:Reverse,
      A:Alternate, B:Bisection,
      W:WeightSamp, and P:Partition.
      In addition to the summary table (a),
      the plots (b-d) show summary statistics for
      each problem class.
      The means and standard errors in (b-c) are across the
      1000 and 900 problem instances, respectively.
      The means and standard errors in (d) are for
      the average across the three constituent problems
      for each of the 50 sampled roadmaps.
      A more detailed table of results is available
      in Appendix~\ref{sec:appendix-proofs}.}
   \label{fig:results}
\end{figure}

\section{Discussion}
%\label{sec:discussion}

The first observation that is evident from the experimental results
is that lazy evaluation
-- whether using Forward (LWA*) or one of the other selectors --
grossly outperforms Expand (A*).
The relative penalty that Expand incurs by evaluating all edges from
each expanded vertex is a function of the graph's branching factor.

Since the Forward and Reverse selectors are simply mirrors of each
other,
they exhibit similar performance
averaged across the PartConn and UnitSquare problem classes,
which are symmetric.
However,
this need not the case for a particular instance.
For example,
the start of ArmPlan1 and the goal of ArmPlan3 consist
of the arm's single home configuration in a relatively confined space.
As shown in the table in
Figure~\ref{fig:results}\subref{subfig:table-results},
it appears that the better selector on these problems attempts
to solve the more constrained side of the problem first.
While it may be difficult to determine a priori which part of the
problem will be the most constrained,
the simple Alternate selector's respectable performance
suggests that it may be a reasonable compromise.

The per-path plots at the bottom of Figure~\ref{fig:snapshots}
allow us to characterize the selectors' behavior.
For example,
Alternate often evaluates several edges on each path before finding
an obstacle.
Its early evaluations also tend to be useful later,
and it terminates after considering 10 paths on the illustrated problem.
In contrast, Bisection exhibits a fail-fast strategy,
quickly invalidating most paths after a single evaluation,
but needing 16 such paths (with very little reuse)
before it terminates.
In general, the Bisection selector did not outperform any of the
lazy selectors in terms of number of edges evaluated.
However,
it may be well suited to problem domains in which
evaluations that fail tend be less costly.

The novel selectors based on path distributions tend to minimize
edge evaluations on the problems we considered.
While the WeightSamp selector performs similarly to Partition on the
simpler problems,
it performs less well in the ArmPlan domain.
This may be because many more samples are needed to approximate
the requisite path distribution.

The path distribution selectors are motivated by focusing evaluation
effort in areas that are useful for many distinct candidate paths,
as illustrated in Figure~\ref{ref:example-scores}.
Note that in the absence of a priori knowledge,
the edges nearest to the start and goal tend to have the highest
$p(e)$ score,
since they are members of many potential paths.
Because it tends to focus evaluations in a similar way,
the Alternate selector may serve as a simple proxy for the
more complex selectors.

We note that
an optimal edge selector could be theoretically achieved by posing the
edge selection problem as a POMDP,
given a probabilistic model of the true costs.
While likely intractable in complex domains,
exploring this solution may yield useful approximations or insights.