\section{Introduction}


Multi-Goal Path Finding (MGPF) problems aim to find a least-cost path for a robot to travel from an origin ($s$) to a destination ($d$) such that the path visits each node in a given set of goals ($\bar{T}$) at least once. In the process of finding a least-cost path, MGPF algorithms also find an optimal sequence in which the goals must be visited. When the search space is discrete ($i.e.$, a finite graph), the cost of traveling between any two nodes is computed using an all-pairs shortest paths algorithm.
In this case, the MGPF encodes
a variant of the Steiner\footnote{Any node that is \emph{not} required to be visited is referred to as a \emph{Steiner node}. A path may choose to visit a Steiner node if it helps in either finding feasible solutions or reducing the cost of travel.} Traveling Salesman Problem (TSP) and is NP-Hard \cite{Kou1981}. In the general case, the search space is continuous and the least cost to travel between any two nodes is not known a-priori. This least-cost path computation between any two nodes in the presence of obstacles, in itself, is one of the most widely studied problems in robot motion planning \cite{kavraki_probabilistic_1996, lavalle}. We address the general case of MGPF as it naturally arises in active perception \cite{best2016multi, mcmahon2015autonomous}, surface inspection \cite{edelkamp2017surface} and
logistical applications~\cite{janovs2021multi,otto2018optimization,macharet2018survey}. MGPF is notoriously hard as it combines the challenges in Steiner TSP and the least-cost path computations in the presence of obstacles; hence, we are interested in finding approximate solutions for MGPF.

\begin{figure*}[t]
    \centering
    \includegraphics[width=\textwidth]{figures/Visualization/iteration_merged.png}
    \caption{The Steiner tree (thick-blue lines) computed using a IST\textsuperscript{*}, showing the advantage of pruning. The environment has 2 U-shaped obstacles with tiny openings. Numbered nodes indicate terminals to be connected. Ellipses indicate an edge's informed set, with color intensity related to sampling probability. Sampled points in free space are part of the roadmap (shown in grey). \textbf{Left}: without pruning, all non-MST edges (thin-red lines) are also sampled. \textbf{Middle}: with pruning, a subset of edges (dashed lines) may safely be removed without affecting the optimal MST. \textbf{Right}: after pruning, the roadmap is further densified around active edges that can lead to an optimal MST faster.}
    \label{fig:pruning}
    \vspace{-0.3cm}
\end{figure*}



 



Irrespective of whether the search space is discrete or continuous, Steiner trees spanning the origin, goals and the destination play a critical role in the development of approximation algorithms for MGPF. In the discrete case, doubling the edges in a suitable Steiner tree, and finding a feasible path in the resulting Eulerian graph leads to 2-approximation algorithms for MGPF \cite{Kou1981,Mehlhorn1988,chour2021s}. This approach doesn't readily extend to the continuous case because we do not a-priori know the travel cost between any two nodes in $T:=\{s,t\}\bigcup \bar{T}$. One can appeal to the well-known sampling-based methods \cite{karaman2011sampling,kavraki_probabilistic_1996,gammell2014informed,gammell2015batch} to estimate the costs between the nodes, but the following key questions remain: 1) How to sample the space so that the costs of the edges joining the nodes in $T$ can be estimated quickly so that we can get a desired Steiner tree?  2) Should we estimate the cost of all the edges or can we ignore some edges and focus our effort on edges we think will likely end up in the Steiner tree?

We call our approach {\it Informed Steiner Tree}$^*$ (IST$^*$).
IST$^*$ iteratively alternates between sampling points in the search space and pruning edges. Throughout its execution, a Steiner tree is maintained which is initially empty but eventually spans the nodes in $T$, possibly including a subset of sampled points. 
IST* relies on two key ideas. {\it First}, finding a Steiner tree spanning $T$ commonly involves finding a Minimum Spanning Tree (MST) in the metric completion\footnote{The metric completion here is a complete weighted graph on all the nodes in $T$ where the cost of an edge between a pair of nodes in $T$ is the minimum cost of a path between them.} of the nodes in $T$. For any two distinct terminals $u,v$, we maintain a lower bound and an upper bound\footnote{Upper bound is the cost of a feasible path from $u$ to $v$.} on the cost of the edge $(u,v)$. Using these bounds and cycle properties of an MST, we identify edges which will never be part of the MST.
This allows us to {\it only} sample regions corresponding to the edges that are likely to be part of the MST. We further bias our sampling by assigning a suitable probability distribution over the search space based on the bounds on the cost of the edges (See Fig. \ref{fig:pruning}). {\it Second}, as the algorithm progresses, a new set of points are added to the search graph in each iteration. Each new sample added may facilitate a lower-cost feasible path between terminals requiring us to frequently update the Steiner tree. To address this efficiently, we develop an {\it incremental} version of the Steiner tree algorithm while maintaining its properties. Since this incremental approach correctly finds a Steiner tree, as the number of sampled points tend the infinity, IST* provides an asymptotic 2-approximation guarantee for MGPF. 



We use the sampling procedure developed in {\it Informed RRT*} \cite{gammell2018informed} to choose points from selected regions in our approach. Informed sampling in synergy with pruning enables faster convergence to the optimal solution than uniform sampling. After describing IST$^*$ with its theoretical properties, we provide extensive computational results on high-dimensional problem instances.

\paragraph{Related Work:} The MGPF and several variants of it have been addressed in the literature. Here, we discuss the most relevant work in continuous domains (more details are presented in the appendix). Un-supervised learning approaches\footnote{Since, the code for these learning approaches were not available, we could not directly test them on the problem instances considered in this paper.}\cite{ faigl2011application,faigl2016self} using Self Organizing Maps (SOMs) have been used to solve MGPF. In \cite{faigl2016self}, SOM is combined with a rapidly exploring random graph algorithm to find feasible solutions for the MGPF. In \cite{devaurs2014multi}, a meta-heuristic similar to \textit{simulated annealing} is combined with multiple Rapidly-exploring Random Tree (RRT) expansions to solve MGPF. In \cite{vonasek2019space}, a method called \textit{Space Filling Forests} (SFF) is proposed to solve MGPF. Multiple trees are grown from the nodes in $T$ and unlike the approach in \cite{devaurs2014multi} where any two close enough (or neighboring) trees are merged into a single tree, in SFF, multiple virtual connections are allowed between two neighboring trees leading to multiple paths between any two nodes in $T$. Recently, in \cite{janovs2021multi}, a generalization of SFF (called SFF*) has been proposed to also include rewiring of the edges in the trees similar to RRT*. Computational results show that SFF* performed the best among several previously known solvers for the simulation environments considered in \cite{janovs2021multi}. 
We evaluated SFF* on the environments we considered and report our findings in the results section.

A generalization of MGPF was considered in  \cite{saha2006planning} where all the goals were partitioned into groups and the aim is to also visit one goal from each partition. They work under the assumption that a path between two nodes is computed at most once which may, in reality, be far from the optimal path. Their planner stops the instant it has found a feasible path connecting all the goals unlike our approach which keeps refining the paths.





























\section{Background and Preliminaries}\label{sec:background}

 Let $X\subseteq \mathbb{R}^n$ be the $n$-dimensional configuration spac
 of a robot, and let $X_{f}\subset X$ be the set of obstacle-free configurations. Let $\sigma:[0,1]\rightarrow X_{f}$ be a collision free, feasible path such that the path starts at the origin ($\sigma(0)=s$), visits each of the goals in $\bar{T}$ at least once and ends at the destination ($\sigma(1)=d$). Let $c(\sigma)\in \mathbb{R}_{\ge 0}$ denote the cost of the path $\sigma$. The objective of MGPF is to find a collision-free feasible path $\sigma$ such that $c(\sigma)$ is minimized. 
 
 Let $S \subseteq X_f$ be a set of sampled points (also referred to as nodes) from $X_f$. Nodes which are within a radius of $\rho$ of each other are treated as neighbors. Edges are formed between neighbors if a feasible path is found between them using a local planner (for example, straight-line connection). Let $V:=S\bigcup {T}$. We refer to all the nodes in $T$ as terminals\footnote{This is a commonly used term in the optimization literature.}. Let $G = (V, E)$ denote the undirected graph thus formed where $E$ denotes all the edges joining any pair of neighbors in $V$. Henceforth, we refer to $G$ as the roadmap. Given a graph $G$, we use $E(G)$ to refer to all the edges present in $G$.
 
 Let $e := (u, v)$ be an edge joining two distinct nodes $u, v$ in $G$. Edge $e$ represents a feasible path between $u$ and $v$ found by the local planner. The cost of a feasible path joining vertices $u$ and $v$ is denoted as $dist(e)$. Let $h(e)$ be a lower bound on the length of any feasible path between $u$ and $v$; typically, we set $h(e)$ to be the Euclidean distance between the nodes $u$ and $v$.
 
 One approach for solving the MGPF is to sample as many points uniformly as possible from the obstacle-free space and form a roadmap spanning the terminals and the sampled points. At the end of the search process,
 the recently developed $A^*$ based approach \cite{chour2021s} for discrete version of MGPF can be used to find a suitable Steiner tree. In this article, we consider this approach as the baseline against which IST$^*$ is compared. 



\iffalse
\subsection{Sampling-based planning}

The task of finding a suitable continuous curve in $X_f$ can be very challenging due to the complexity of high-dimensional spaces. Sampling-based algorithms work around this issue by constructing a roadmap, a graph data structure that captures the connectivity of $X_f$ through random sampling of $X$. In effect, these algorithms form a trade-off between solution quality and computational speed; as more samples are added to the roadmap, previous solution may be improved. Early algorithms such as RRT and PRM were however not asymptotically optimal. In order to guarantee optimality, results from Random Geometric Graph (RGG) theory was later leveraged.  

In RGG theory, a set of nodes are randomly sampled from an underlying space and an edge exists between two nodes if the distance between them is less than some connection radius  \cite{gilbert1961random}. In a seminal work \cite{karaman2011sampling}, the close association between RGG theory and roadmap construction was finally exploited. The results showed that asymptotically optimal solutions exist on a roadmap if the connection radius is proportional to $(\frac{\log n}{n})^\frac{1}{d}$, given that the straight-line distance between the two nodes are obstacle-free. This gave rise to a new family of algorithms based on PRM\textsuperscript{*} and RRT\textsuperscript{*}.

Roadmap construction can also be improved in two ways: using i) informed sampling, and/or ii) \textit{incremental} search techniques with auxiliary graph data structures. In the former, informed sampling can aid in cutting down the overall graph size by ensuring or increasing the likelihood of useful samples being added. In the latter, incremental techniques are used to update a known shortest path as the roadmap changes. This can save valuable computational effort by avoiding the regeneration of shortest path queries from scratch.

\subsection{Steiner trees}
Existing 2-approximation algorithms for MGPF involve the following 3-step procedure \cite{kou1981fast}: i) construct a suitable Steiner Tree spanning the goal locations with cost that is at most the optimal path cost of MGPF, ii) using said Steiner tree to construct an Eulerian graph, and iii) finding a path from the Eulerian graph. A Steiner tree is a connected subgraph of edges that span a subset of relevant nodes $T\subseteq V$ called terminals. In this work, terminals and goals are used interchangeably. Finding a minimum cost Steiner tree is NP-hard \cite{Karp72}, thus an alternative approach is to find a Minimum Spanning Tree (MST) in the metric completion of the terminal nodes, where each edge in the MST is the least cost path in the graph. We aim to find an MST in the metric completion of the terminal nodes. 

\begin{lemma}\label{thm:merged}
The MST in the metric completion of the terminal nodes provides a 2-approximation algorithm for MGPF, i.e. $cost(MST) \leq 2 cost(OPT) $
\end{lemma}

\fi





\section{Informed Steiner Trees}


\begin{algorithm}[t!]
\caption{{\fontfamily{pcr}\selectfont IST\textsuperscript{*}}}
\SetAlgoLined
\DontPrintSemicolon
\label{alg:IST-star}
\textbf{Input}: \\
$X_f\subset X$ \tcp{Collision free space}
$T\subset X_f$ \tcp{Discrete set of terminals}
$h(e)$ for any $e=(u,v), u,v\in X_f$ \tcp{Heuristic cost between terminals $u$ and $v$}
$n_{s}$ \tcp{Samples per batch}
$n_b$ \tcp{Number of batches}
\textbf{Output}: \\
$S_T$ \tcp{Steiner tree spanning the terminals in $T$}
\textbf{Initialization}:\\
$V \leftarrow T$ \tcp{Add terminals to the roadmap} 
$\mathcal{N}_V(u) := \{v: dist(v,u)\leq \rho, v\neq u, v\in V\}$ for any $u\in V$ \tcp{nearest neighbors of $u$ in $V$} 
$E \leftarrow \{(u,v): v\in \mathcal{N}_V(u), u \in V\}$ \\
$cost(e)=dist(u,v)$ for any $e=(u,v) \in E$\\
$G\leftarrow (V,E)$  \\

$\mathcal{A} = \{(u,v):u,v\in T, u\neq v\}$ \tcp{Set of active edges joining terminals}
$cost_T(u,v)=\infty$ for any $(u,v)\in \mathcal{A}$ \\
$Prob(e) = \frac{h(e)}{\sum_{e^\prime\in \mathcal{A}} h(e^\prime)}$ for all $e\in \mathcal{A}$ \tcp{Initial probabilities using lower bounds} \label{line:alg:IST-star:lower_bound}
$S_T \leftarrow \emptyset$\\

\tcp{\bf Note: $G, \bar{G}, cost, cost_T$ are global variables.}
\vspace{.2cm}
\textbf{Main loop}:\\
\For{$iteration =1,\ldots, n_b$}
{    \vspace{.1cm}
  
    $S' \leftarrow AddSamples(Prob,\mathcal{A}, cost_T, n_s)$\\

\vspace{.1cm}
   
    $ Ripple(S')$  \\
\vspace{.1cm}

  
    $S_T \leftarrow UpdateTree(S_T,\mathcal{A})$\\

\vspace{.1cm}
    
    $\mathcal{A} \leftarrow PruneEdges(S_T,\mathcal{A})$\\
\vspace{.1cm}    
 
        $Prob \leftarrow UpdateProbability(Prob,\mathcal{A},S_T,h)$
}
\Return $S_T$ 
\end{algorithm}


An overview of IST\textsuperscript{*} is presented in Algorithm \ref{alg:IST-star}. At any iteration, {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} maintains three graphs to enable the search process: 
\begin{enumerate}
\item {\it Roadmap graph:} $G=(V,E)$
includes all the terminals and the sampled points, and the edges between its neighboring nodes. Two nodes $u, v$ are neighboring if $dist(u, v) \le \rho$, where $\rho$ is the connection radius. We provide a discussion on the role (and implementation) of $\rho$ in the Appendix. Initially, $G$ includes only the terminals and the edges between any pair of neighboring terminals.

\item {\it Shortest-path graph:} This graph is denoted as $\bar{G}:=(V,\bar{E})$ and is a subgraph of the roadmap. It is a forest which contains a tree corresponding to each terminal and maintains all the shortest paths from a terminal to each sampled point in its component. The shortest-path graph is used to find feasible paths between terminals. Initially, $\bar{G}$ is an empty forest ($i.e.$, contains no edges).
\item {\it Terminal graph:} This graph is denoted as $G_T=(T,\mathcal{A})$ and  consists of only the terminals and any edges connecting them. Any edge in $G_T$ corresponds to a path between two terminals in the roadmap. Therefore, any spanning tree $S_T$ in the terminal graph corresponds to a feasible Steiner tree in $G$. It is used in determining which regions to sample
and in finding feasible Steiner trees for MGPF. $\mathcal{A}$ contains the set of all the edges that can possibly be part of $S_T$, and hence are currently actively explored when sampling. Initially, $\mathcal{A}$ consists of all the edges that join any pair of terminals in $T$. As the algorithm progresses, edges in $\mathcal{A}$ may be pruned. Also, the cost of an edge connecting any pair of distinct terminals $u,v$ in $G_T$ is denoted as $cost_T(u,v)$. Initially, $cost_T(u,v)$ is set to $\infty$ for all $u,v\in T$ and $S_T$ is empty. 
\end{enumerate}


In each iteration of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}, the algorithm (i) samples new points in $X_f$ (line 21 of Algorithm \ref{alg:IST-star}),  (ii) updates $S_T$ based on a new incremental Steiner tree algorithm called {\fontfamily{pcr}\selectfont Ripple} (lines 22--23 of Algorithm  \ref{alg:IST-star}), (iii) prunes edges from $\mathcal{A}$ (line 24 of Algorithm \ref{alg:IST-star}), and (iv) updates the probability distribution (line 25 of Algorithm \ref{alg:IST-star}). Each of these key steps are discussed in the following subsections. 


\begin{algorithm}[tb]
\caption{ {\fontfamily{pcr}\selectfont AddSamples}$(Prob,\mathcal{A},cost_T,n_s)$}
\SetAlgoLined
\label{alg:sampling}
$S'\leftarrow\emptyset$\\
\For{$1,\ldots, n_s$}
{
   
    Choose $(u,v)\in \mathcal{A}$ using $Prob$\\
    \label{line:alg:sampling:c_start}
        $x_{rand} \leftarrow \texttt{Sample}(u, v, cost_T(u,v))$\\
       
       
    \label{line:alg:sampling:c_end}
    $S' \leftarrow S' \cup \{x_{rand}\}$
}
\Return $S'$

\end{algorithm}


\begin{algorithm}[tb] 
\caption{{\fontfamily{pcr}\selectfont Ripple}$(S')$} \label{alg:ripple}
\SetAlgoLined 
 \tcp{For any $u\in V$, let $r_u$ denote the closest terminal to $u$ in $G$. If $u$ is a terminal, then $r_u:= u$. The \textit{parent} of any terminal is itself} 
\While{$S'\neq \emptyset$}
{
    \label{line:alg:ripple:outer_loop}
    $s \leftarrow S'.pop()$ \\
    $V \leftarrow V \cup \{s\}$ \\
    $E\leftarrow E \cup \{(s,u): u\in \mathcal{N}_V(s)\}$ \\
    $parent(s), r_s\leftarrow NULL$ \\
    $n^* = \arg\min\{g(n) + cost(s,n): n\in \mathcal{N}_V (s), r_{n}\neq NULL\}$\\
    \If{$n^* \neq NULL $}
    {
        $\bar{E} \leftarrow \bar{E} \cup \{(s,n^*)\}$\\
        $g(s) \leftarrow g(n^*) + c(s,n^*)$\\
        $r_s \leftarrow r_{n^*}$\\
        $parent(s) \leftarrow n^*$\\    
    
        $\mathcal{Q}.insert(s,g(s))$ \tcp{Priority Queue}

        \While{$\mathcal{Q}\neq \emptyset$}
        {
            \label{line:alg:ripple:inner_loop_start}
           
           
           
            $u^* \leftarrow \mathcal{Q}.extractMin()$\\
           
            \ForEach{$n\in N_{V}(u^*)$}
            {
                \uIf{$g(u^*)+cost(u^*,n) < g(n)$}
                {
                   
                    $g(n) \leftarrow g(u^*)+cost(u^*,n)$\\
                    $\mathcal{Q}.insert(n,g(n))$\\
                    $r_n\leftarrow r_{u^*}$\\
                    $\bar{E} \leftarrow \bar{E}\setminus \{(n, parent(n))\cup \{(n, u^*)\}\}$\\
                    $parent(n) \leftarrow u^*$\\
                    \label{line:alg:ripple:inner_loop_end}
                }
                \ElseIf{$r_n\neq r_{u^*}$}
                {\label{line:alg:ripple:feasible}
                $d \leftarrow g(n) + cost(n,u^*) + g(u^*)$
                \uIf{$d < cost_T(r_n, r_{u^*})$}
    {
        $cost_T(r_n, r_{u^*})\leftarrow d$\\
    }
    
                }
            }
        }
   
}
}
\Return
\end{algorithm}
\begin{algorithm}[tb]
\caption{{\fontfamily{pcr}\selectfont UpdateTree}$(S_T, \mathcal{A})$}
\SetAlgoLined
\label{alg:update_tree}
\tcp{ Notation: $\Omega((u,v), S_T)$ is the cycle induced by adding edge $(u,v)$ to $S_T$} 
\uIf{$S_T$ is empty}
    {
     $S_T$ = Kruskal($cost_T$) \\
     \Return $S_T$
    } 
\vspace{.2cm}
\For{$(u,v) \in \mathcal{A}: (u,v)\not\in S_T$}
{  
    $P :=\{(u',v') \in \Omega((u,v), S_T)\setminus\{u,v\}\}$ \\
    $pathCost \leftarrow \sum\{cost_T(u', v'): (u', v') \in P\}$ \\
    $(u^*,v^*) = \arg\max \{cost_T(u',v'): (u',v') \in P\}\}$\\
    \uIf{$cost_T(u^*,v^*) > cost_T(u,v)$}
    {
     $S_T \leftarrow S_T \setminus \{ (u^*, v^*) \}\cup\{(u, v)\}$  
    }
    \vspace{.2cm}
    \uElseIf{$pathCost < cost_T(u, v)$}
    {
        $cost_T(u, v) \leftarrow pathCost$ \tcp{Update non-MST edge with a cheaper path via MST} 
    }
}
\Return $S_T$
\end{algorithm}


\begin{algorithm}[tb]
\caption{{\fontfamily{pcr}\selectfont PruneEdges}$(S_T, \mathcal{A})$}
\SetAlgoLined
\label{alg:PruneEdges}
\For{$(u,v) \in \mathcal{A}: (u,v)\not\in S_T$}
{  
    $(u^*,v^*) = \arg\max \{cost_T(u',v'): (u',v') \in \Omega((u,v), S_T) \setminus\{u,v\}\}$\\
    \uIf{$h(u,v)>cost_T(u^*,v^*)$}
    {   $\mathcal{A}\leftarrow \mathcal{A}\setminus \{(u,v)\}$
    } 
}

\Return $\mathcal{A}$
\end{algorithm}

\begin{algorithm}[tb]
\caption{{\fontfamily{pcr}\selectfont UpdateProbability}$(Prob,\mathcal{A},S_T,h)$}
\SetAlgoLined
\label{alg:update-probability-dist}

 \uIf{ $S_T$ is empty}
 { \Return $Prob$
 }
 \vspace{0.2cm}
 \tcp{Reset probability distribution}
 $Prob(e):=0$ for all $e\in \{(u,v): u,v\in T, u\neq v\}$ \\
$gap_1(e) := cost_T(u,v) - h(u,v), \forall (u,v)\in S_T$\\
$gap_2(e) := cost_T(u,v) - \max\{c(e'): e'\in \Omega(e,S_T)\setminus\{e\}\}, \forall e\in \mathcal{A}\setminus S_T$\\
    \vspace{0.2cm}
    \tcp{Partition active edges}
    $\mathcal{A}_{mst} = \{e \in S_T: gap_1(e)>0\}$\\
    $\mathcal{A}_{non-mst} = \{e \in \mathcal{A}\setminus S_T: gap_2(e)>0\}$\\
   
   
   
    \ForEach{$e\in \mathcal{A}_{mst}$}
    {
        $Prob(e)\leftarrow \frac{|\mathcal{A}_{mst}|}{ |\mathcal{A}_{mst}| + |\mathcal{A}_{non-mst}| }\cdot\frac{gap_1(e)}{\sum_{e'\in gap_1(e')}}$
    }
   
    \ForEach{$e \in \mathcal{A}_{non-mst}$}
    {
        $Prob(e)\leftarrow \frac{|\mathcal{A}_{non-mst}|}{ |\mathcal{A}_{mst}| + |\mathcal{A}_{non-mst}| }\cdot\frac{gap_2(e)}{\sum_{e'\in gap_2(e')}}$
    }
\Return $Prob$
\end{algorithm}

\paragraph{Sampling:}
The sampling procedure (Algorithm \ref{alg:sampling}) uses the routine \texttt{Sample} developed in Informed RRT* \cite[Algorithm 2]{gammell2014informed}. In this procedure, a fixed number of samples $n_s$ is added to $S'$ from $X_f$ in each iteration. First, an edge $e := (u, v)$ is drawn from the probability distribution over $\mathcal{A}$ (line 3 in Algorithm \ref{alg:sampling}). Initially, until a feasible $S_T$ is obtained, the probability distribution is computed using the heuristic lower bounds (line \ref{line:alg:IST-star:lower_bound} in Algorithm \ref{alg:IST-star}).
Once a random sample $x_{rand}$ is obtained for the edge $(u, v)$ using \texttt{Sample}, it is added to $S'$. 

\paragraph{Incremental Steiner tree algorithm:}
This procedure is accomplished by first finding lower-cost, feasible paths between the terminals through {\fontfamily{pcr}\selectfont Ripple} (Algorithm \ref{alg:ripple}), and then updating the spanning tree $S_T$. In {\fontfamily{pcr}\selectfont Ripple}, each sample is processed individually until $S'$ becomes empty. Adding a new point $s$ to the roadmap  
(consequently to $\bar{G}$) can facilitate new paths between terminals.
If $s$ has a neighbor that is connected to a terminal (or is itself a terminal), then $s$ and each of its neighbors are expanded in a {\it Dijkstra}-like fashion until the priority queue $\mathcal{Q}$ is empty (lines \ref{line:alg:ripple:inner_loop_start}--\ref{line:alg:ripple:inner_loop_end} of Algorithm \ref{alg:ripple}). 
The variable $g(u)$ keeps track of the shortest path from $u$ to its closest terminal which is stored in $r_u$.
The key part of the {\fontfamily{pcr}\selectfont Ripple} algorithm lies in finding new feasible paths between the terminals. 
This happens (line \ref{line:alg:ripple:feasible} of Algorithm \ref{alg:ripple}) during the expansion of a node $u^*$ to its neighbor $n$ when $g(u^*) + cost(u^*,n) \ge g(n)$ and $r_{u^*}\neq r_u$, $i.e.$, $n$ is closer to its root $r_{n}$ than to $r_{u^*}$. In this case, a feasible path from $r_{u^*}$ to $r_n$ has been discovered through the nodes $u^*$ and $n$. If the cost of this feasible path is lower than $cost_T(r_{u^*},r_{n})$, then $cost_T(r_{u^*},r_n)$ is updated accordingly.





 \
 After $cost_T(u,v)$ for any pair of terminals $(u,v)$ is updated, it is relatively straightforward to check if any edge not in $S_T$ should become part of $S_T$. First, we check if $S_T$ is empty or not. If $S_T$ is empty, then we just use the Kruskal's algorithm \cite{kruskal1956shortest} to find a spanning tree (if it exists) for $T$ (line 2 of Algorithm \ref{alg:update_tree}). 
If $S_T$ is not empty, we appeal to the following well-known cycle property of minimum spanning trees  to see if we should include $(u,v)$ in $S_T$. 
\begin{property}
\label{prop:prune}
Suppose that $S_T$ is a MST in $G_T$ and when a new edge $(u,v)\notin S_T$ is added to $G_T$, it forms a cycle $C = \Omega((u,v), S_T)$. Consider an edge $(u^*,v^*)\in C$ other than $(u,v)$ that has the maximum of all the edge costs in $C$. If $cost_T(u^*,v^*)>cost_T(u,v)$, then $S_T \setminus \{(u^*,v^*)\} \cup \{(u,v)\}$ is an MST of the updated $G_T$.
\end{property}

Thus, in our algorithm, if $cost_T(u^*,v^*)>cost_T(u,v)$, then edge $(u^*,v^*)$ is deleted from $S_T$ and edge $(u,v)$ is added to $S_T$ (lines 9 of Algorithm \ref{alg:update_tree}).

\paragraph{Pruning edges from $\mathcal{A}$:}

The pruning procedure (Algorithm \ref{alg:PruneEdges}) is similar to what we discussed in the previous tree update procedure except that we now appeal to the bounds on the cost of the edges. Consider an edge $(u^*,v^*)\in \Omega((u,v), S_T)$ other than $(u,v)$ that has the maximum of all the edge costs in $C$. If $cost_T(u^*,v^*)<h(u,v)$, and since $h(u,v)\leq cost_T(u,v)$, we have $ cost_T(u^*,v^*)< cost_T(u,v)$. Then, again applying the cycle property, edge $(u,v)$ is pruned from $\mathcal{A}$ and is never considered by the algorithm henceforth (line 4 of Algorithm \ref{alg:PruneEdges}).

\paragraph{Update probability distributions:}
Once a feasible $S_T$ is found, the probability distribution is updated (Algorithm \ref{alg:update-probability-dist}) in each iteration to reflect the changes in the costs and $\mathcal{A}$. We want to sample the region around an edge more if the uncertainty around the cost of the edge is higher. This is based on our assumption that the more we learn about the cost of an edge, the better we can decide on its inclusion in $S_T$. If an edge $(u,v)$ already belongs to $S_T$, we assign a sampling probability for $(u,v)$ proportional to the difference between the edge cost and its lower bound. On the other hand, if an edge $(u,v)$ does not belong to $S_T$, we first aim to include it in $S_T$ and later hope to drive its cost to its lower bound. Thus, we assign a sampling probability for $(u,v)$ proportional to the difference between the edge cost and the cost of its next largest cost edge in $\Omega((u,v), S_T)$. 




\subsection{Theoretical Properties}
At any iteration of the algorithm, the algorithm maintains a Steiner tree $S_T$ which is formed by expanding the paths of a MST on the terminal graph $G_T$. For any given state of the graph $G$ that contains the terminals $T$ along with the sampled points so far, we argue that the tree returned by {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} is such an MST.
\begin{theorem}
{\fontfamily{pcr}\selectfont IST\textsuperscript{*}} will return an MST over the shortest paths between terminals in $G$.
\end{theorem}
We show this theorem by arguing that the edge pruning from the active set $\cal A$ of inter-terminal edges is correct and that the sampling procedure with updated probabilities coupled with the {\fontfamily{pcr}\selectfont Ripple} update will eventually find all relevant shortest paths. The former claim follows from the cycle property as discussed in the pruning step.
To prove the latter claim, we use the following key lemma, that is proved in the Appendix.
\begin{lemma}
\label{lem:voronoi}
Suppose a sample point $s \in S'$ is processed by {\fontfamily{pcr}\selectfont Ripple} so that $r_s$ is a terminal $t$, then in $G$, $t$ is (one of) the closest terminal to $s$ among all terminals $T$.
\end{lemma}
In this way, {\fontfamily{pcr}\selectfont Ripple} maintains every sample point in the correct so-called `Voronoi' partition of the terminals (i.e. assigning it to its closest terminal). 
From this lemma, we see that the edges between terminals in $G_T$ whose costs are updated by {\fontfamily{pcr}\selectfont Ripple} (line 24, Algorithm 3) are precisely those where an edge between a node $u^*$ and its neighbor $n$ such that $r_{u^*} \neq r_n$ discovers a new cheaper path between $r_{u^*}$ and $r_n$. Thus, {\fontfamily{pcr}\selectfont Ripple} is able to find all shortest paths between pairs of terminals that are witnessed by a pair of neighboring boundary
\footnote{A node $u$ is said to be a \emph{boundary} node if not all its neighbors have root $r_u$.} 
We can now use a result of
\cite{Mehlhorn1988} (Lemma in Section 2.2) that shows that this subgraph $G_T$ of the subset of shortest paths between terminals in $T$ that are witnessed by adjacent boundary nodes is sufficient to reconstruct an MST of $G$.
This shows that $S_T$ constructed as a MST of $G_T$ is indeed an MST of the metric completion of $T$ using all the edges in $G$ as claimed.

Since the {\fontfamily{pcr}\selectfont UpdateTree} method in {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} always maintains a MST of the sampled graph $G$, in the limit with more sampling, the actual shortest paths corresponding to the true final MST will be updated to their correct lengths with diminishing error, and this MST will be output by {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}. Since the MST is a 2-approximation to the optimal Steiner tree in any graph and consequently, the MGPF problem \cite{Kou1981}, we get the following result. 
\begin{corollary}
{\fontfamily{pcr}\selectfont IST\textsuperscript{*}} outputs a 2-approximation to the MGPF problem asymptotically. 
\end{corollary}


\iffalse************
In the Appendix, we argue that the Ripple update procedure for processing new sample points correctly maintains all the edges in the {\kenny{Gabriel graph}} Delaunay triangulation of the terminals asymptotically.

\begin{definition}
(Voronoi diagram). Given a set of points (generators) $P\in X_f$ and a metric $d:X_f\times X_f \rightarrow \mathbb{R}^+$. A Voronoi cell is a convex polyhedra associated with a point, i.e. $p_k\in P$ is $R_k=\{x\in X_f: d(x,p_k)\leq d(x,p_j)~\forall j\not = k\}$. The set of all such cells is a Voronoi diagram (VD).
\end{definition}

\begin{definition}
(Delanuay Triangulation). Assume we have at least 3 terminals, else MGPF reduces to a single-source single goal shortest path problem. For any $e \in G_T$, $e\in DT$ if there is an edge in VD that is a perpendicular bisector of $e$.  
\end{definition}

\begin{definition}
(Gabriel Graph). For any $e=(u,v)\in G_T$, $e \in GG$, if $e$ intersects its Voronoi edge given by neighboring cells $V_u, V_v$.  
\end{definition}

\begin{lemma}
Under uniform sampling, the individual shortest path trees in $\bar G$ belong to a distinct Voronoi cell in the limit of samples added.
\end{lemma}

During initialization of $IST^*$, $\bar G$ only contains terminal nodes, which trivially satisfies the claim. Now consider the inclusion of a batch of samples. \textit{Ripple} will process each sample individually, by adding a new edge to $\bar G$ (Line 8, Algorithm 3) whenever possible. This edge will be connected to one of the trees in $\bar G$. Since each tree has equal probability (absent sampling bias) of being connected to a sample, the trees will grow by roughly the same rate. Additionally, the rewiring procedure ensures any node in $\bar G$ has the cheapest possible g-value upon termination of \textit{ripple}. These facts imply the existence of a VD in the limit of samples added. 

\begin{lemma}
Under uniform sampling the Gabriel graph will be constructed by finding all shortest paths between any two neighboring Voronoi cells, in the limit of samples added.
\end{lemma}

Feasible paths are detected by the \textit{ripple} procedure if junction nodes belonging to two different trees are detected (Line 22, Algorithm 3). These junction nodes can only be detected when the paths traced back from the junction nodes to their respective roots can no longer be rewired (relaxed) any further. 

To show that any feasible path found by ripple between two Voronoi cells is indeed the shortest path, we will appeal to bellman's equation. Prior to the addition of any samples, bellman's optimality principle is trivially satisfied. But consider a sequence of arbitrary sample additions. ... 


The claim then naturally follows. The set of all such paths form a Gabriel graph $GG$ for which it is a known that $GG\subset DT$.

\begin{theorem}
{\fontfamily{pcr}\selectfont IST\textsuperscript{*}} will return an MST over the shortest paths between terminals if informed sampling is used with the sampling distribution described in Algorithm \ref{alg:update-probability-dist}.
\end{theorem}
We show this theorem by arguing that the edge pruning from the active set $\cal A$ of inter-terminal edges is correct and that the sampling procedure with updated probabilities coupled with the Ripple update will eventually find all relevant shortest paths. The former claim is straightforward since the pruning uses a lower bound on inter-terminal distance (the Euclidean distance) whose value exceeds the largest edge in the current MST cycle it closes.

To prove the latter claim, we first observe that in Euclidean spaces, the MST is always a subgraph of the {\kenny{Gabriel graph (GG)}} Delaunay triangulation of the terminals. Then we observe that asymptotically under uniform sampling of $X_f$, Ripple will find all paths belonging to {\kenny{GG}} this triangulation. Since the MST-edge pruning method is accurate this will result in the method outputting the MST in the limit.
Note that since we de-emphasize only regions of edges that are never in a MST, the eventual consequence of the updated sampling procedure is only to ignore regions corresponding to non-MST edges. Hence the updated sampling processes the other relevant regions at least as much as they are considered under uniform sampling of $X_f$, but saves in using the samples efficiently.
In the Appendix, we argue that the Ripple update procedure for processing new sample points correctly maintains all the edges in the {\kenny{Gabriel graph}} Delaunay triangulation of the terminals asymptotically.






{\color{red} This section needs to be completed}
\begin{theorem}
For each new sample added, Ripple expands nodes with gcost that are monotonically nondecreasing over time until termination if and only if a node is never expanded more than once.
\end{theorem}

\begin{definition}
Let $\Delta(T)$ be defined as the Delaunay triangulation over a set of terminals.
\end{definition}

\begin{lemma}
(Uniqueness and Existence of DT). Given a set of points $P$ in $\mathbb{R}^n$, a unique Delaunay triangulation $D(P)$ exists if and only if no $n + 1$ points belong to the same hyperplane and no $n + 2$ points belong to the same hypersphere (WIP lemma).
\end{lemma}

\begin{proposition}
Asymptotically under uniform sampling of $X_f$, Ripple will find all paths belonging to $\Delta(T)$.
\end{proposition}

\begin{proposition}
For each new sample added, if a path belonging to $\Delta(T)$ (at most infinity) can be improved, Ripple will do so.
\end{proposition}

\begin{figure}
  \centering
  \includegraphics[width=\linewidth]{figures/paths.png}
  \caption{An instance of a roadmap shown as a forest of trees with $r_1, r_2, r_3$ denoting the terminals. Green edges connect leaves (labelled with $f$) of different trees. The optimal path from $r_1$ to $r_3$ is shown via the purple dotted line whereas the (suboptimal) path found by ripple is shown via the dashed grey line.}
  \label{fig:paths}
  \Description{}
\end{figure}


\begin{theorem}
For each new sample added, Ripple will correctly maintain a consistent forest of shortest path trees.
\end{theorem}

\begin{definition}
An edge that is considered \textit{pruned} will have null probability of being sampled and does not belong to $S_T$.
\end{definition}

\begin{proposition}
Any arbitrary order in which we scan candidate edges for updating $S_T$ will lead to the same MST.
\end{proposition}


\begin{theorem}
(Pruning Condition). Given an arbitrary tree $S_T'$ spanning $T$ with finite cost, any edge $e\in E(G_T)\setminus S_T'$ is pruned if and only if the edge heuristic cost exceeds the max cost edge in an induced cycle.
\end{theorem}

\begin{corollary}
Once an edge of $G_T$ is \textit{pruned} it will never become a part of $S_T$ in the future. 
\end{corollary}

\begin{theorem}
{\fontfamily{pcr}\selectfont IST\textsuperscript{*}} will return an MST over the shortest paths between terminals if informed sampling is used with the discussed probability distribution in Algorithm \ref{alg:update-probability-dist}
\end{theorem}

Sufficient but not necessary for obtaining an MST as other suitable probability distributions may exist

\f




\section{Results}

\begin{table*}
        \centering
        \begin{tabular
        {cM{0.3\linewidth}M{0.3\linewidth}M{0.3\linewidth}}
           \toprule
            \emph{Env.} & 10 Terminals & 30 Terminals & 50 Terminals \\
            \midrule
            \makecell{\texttt{CO} \\ $\mathbb{R}^4$} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/CenterObstacleR4/CenterObstacleR4_10_450s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/CenterObstacleR4/CenterObstacleR4_30_900s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/CenterObstacleR4/CenterObstacleR4_50_1350s_costDistributionOverTime.png} \\
   
   \makecell{\texttt{CO} \\ $\mathbb{R}^8$} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/CenterObstacleR8/CenterObstacleR8_10_900s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/CenterObstacleR8/CenterObstacleR8_30_1800s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/CenterObstacleR8/CenterObstacleR8_50_2700s_costDistributionOverTime.png} \\
   
   \makecell{\texttt{UH} \\ $\mathbb{R}^4$} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/UniformHyperRectanglesR4/UniformHyperRectanglesR4_10_600s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/UniformHyperRectanglesR4/UniformHyperRectanglesR4_30_1200s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/UniformHyperRectanglesR4/UniformHyperRectanglesR4_50_1800s_costDistributionOverTime.png} \\
   
   \makecell{\texttt{UH} \\ $\mathbb{R}^8$} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/UniformHyperRectanglesR8/UniformHyperRectanglesR8_10_1200s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/UniformHyperRectanglesR8/UniformHyperRectanglesR8_30_2400s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/UniformHyperRectanglesR8/UniformHyperRectanglesR8_50_3600s_costDistributionOverTime.png} \\
   
         
   \makecell{H\\O\\M\\E} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/Home/Home_env_10_600s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/Home/Home_env_30_1200s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/Home/Home_env_50_1800s_costDistributionOverTime.png} \\
   
   \makecell{A\\B\\S\\T\\R\\A\\C\\T} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/Abstract/Abstract_env_10_600s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/Abstract/Abstract_env_30_600s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstBaseline/Abstract/Abstract_env_50_600s_costDistributionOverTime.png} \\
   
            \bottomrule
            
        \end{tabular}
        \caption{Comparison of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} against the Baseline across \nolinebreak 6 different environments -- real vector spaces over uniform hyper rectangles (\texttt{UH}) and center obstacles \texttt{CO}, and $SE(3)$ environments HOME and ABSTRACT -- over 10/30/50 terminals. The thin solid line represents the mean solution cost with the shaded region being the 99\% confidence interval about this mean. }
        
        
        \label{tbl:IST}
    \end{table*}
    
\begin{table*}
        \centering
        \begin{tabular
        {cM{0.3\linewidth}M{0.3\linewidth}M{0.3\linewidth}}
           \toprule
            \emph{Env.} & 10 Terminals & 30 Terminals & 50 Terminals \\
            \midrule
         
   \makecell{H\\O\\M\\E} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstSstar/Home/Home_env_10_600s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstSstar/Home/Home_env_30_1200s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstSstar/Home/Home_env_50_1800s_costDistributionOverTime.png} \\
   
  
   \makecell{\texttt{UH} \\ $\mathbb{R}^8$} & 
  \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstSstar/UniformHyperRectanglesR8/UniformHyperRectanglesR8_10_1200s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstSstar/UniformHyperRectanglesR8/UniformHyperRectanglesR8_30_2400s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=34mm]{figures/ResultsAgainstSstar/UniformHyperRectanglesR8/UniformHyperRectanglesR8_50_3600s_costDistributionOverTime.png} \\
   
   
            \bottomrule
            
        \end{tabular}
        \caption{Comparison of the performance of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} when {\fontfamily{pcr}\selectfont Ripple} is used against S* on an $SE(3)$ instance ($Home$) and a high-dimensional environment (\texttt{UH} $\mathbb{R}^8$). The dark line represents the mean solution cost with the thick region being the 99\% confidence interval about the mean.}
        
        
        \label{tbl:Ripple}
    \end{table*}
    
To evaluate the performance of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}, we compared it with a baseline as described in the Background and Preliminaries section. Specifically for the baseline, we densified a roadmap given by PRM\textsuperscript{*} \cite{karaman2011sampling} for a fixed amount of time via uniform random sampling after which S\textsuperscript{*} (S\textsuperscript{*}-BS variant was used) was run on the resulting roadmap to obtain an MST-based Steiner tree \cite{chour2021s}. It was ensured that the combined time spent in growing the roadmap and running  S\textsuperscript{*} exhausted the input time limit.



The planners ({\fontfamily{pcr}\selectfont IST\textsuperscript{*}} and the Baseline) were implemented in Python 3.7 using OMPL \cite{sucan2012the-open-motion-planning-library} v1.5.2 on a desktop computer running Ubuntu 20.04, with 32 GB of RAM and an Intel i7-8700k processor. Further implementation details are available in the Appendix. 
Both the planners output the cost of the Steiner tree found over time. Each planner was ran 50 times to obtain a distribution of solution costs. The mean solution cost with a 99\% confidence interval is calculated from 50 trials and shown in Table \ref{tbl:IST}.
 

\begin{table}[h]
\caption{Time (in seconds) given to planners as input in different problem instances.}
\[\begin{array}{|c|c|c|c|}
\hline
\tikz{\node[below left, inner sep=1pt] (def) {\small{Environment}};%
      \node[above right,inner sep=1pt] (abc) {Terminals};%
      \draw (def.north west|-abc.north west) -- (def.south east-|abc.south east);}
 & 10 & 30 & 50 \\
\hline
\texttt{CO}\ \ \mathbb{R}^4 & 450s & 900s & 1350s\\
\hline 
\texttt{CO}\ \ \mathbb{R}^8 & 900s & 1800s & 2700s\\
\hline 
\texttt{UH}\ \ \mathbb{R}^4 & 600s & 1200s & 1800s\\
\hline 
\texttt{UH}\ \ \mathbb{R}^8 & 1200s & 2400s & 3600s\\
\hline 
HOME & 600s & 1200s & 1800s\\
\hline 
ABSTRACT & 600s & 600s & 600s \\
\hline 
\end{array}\]
\label{tbl:computationalTime}
\end{table}

Due to the absence of a standard suite of instances for benchmarking planners for MGPF, we use the environments commonly considered in motion planning and extend them to our setting by randomly generating valid goal configurations. Specifically, we test our planners in high-dimensional state spaces, and on rigid body motion planning instances available in the OMPL.app GUI. A problem instance is uniquely identified by an environment (with its specific robot model), number of terminals, and the computational time given for planning. For each environment, a varying number of terminals ($|T| = 10, 30, 50$) was generated. The choice for the size of the problem instances was motivated by unmanned vehicle applications \cite{oberlin}. The time specified in problem instances was based on the number of terminals and the difficulty of planning in that environment (Table \ref{tbl:computationalTime}). We now describe the testing environments and the corresponding results.

\subsection{Real-vector space problems}

{\fontfamily{pcr}\selectfont IST\textsuperscript{*}} and the Baseline were tested on two simulated problems in a unit hypercube with distinct obstacle configurations in $\mathbb{R}^4$ and $\mathbb{R}^8$ (Fig. \ref{fig:obstacleMap}). The collision detection resolution was set to $10^{-4}$ to make evaluating edge costs computationally expensive. 
\begin{figure}[h]
  \centering
  \includegraphics[width=\linewidth]{figures/abstractProblems.jpg}
  \caption{A 2-dimensional illustration of the problems in real-vector spaces. The configuration space for both is a unit hypercube, $i.e.$, each state space was bounded to the interval [0, 1], illustrated by the black bounding box. Solid regions represent invalid states. In Fig.\ref{fig:obstacleMap}(b), the hypercube obstacles are axis-aligned (specifically, 10 on each axis) and uniformly spread with $w=0.075, d = 0.025$.}
  \label{fig:obstacleMap}
 
 \vspace{.4cm}
\end{figure}



{\it Center Obstacle (CH):} This problem consisted of a big hypercube at the center (with volume $(0.9)^n$ in $\mathbb{R}^n$) (Fig. \ref{fig:obstacleMap}a).
In this environment, {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} started on par with the Baseline but significantly improved while the Baseline remained stuck in a sub-optimal solution.

{\it Uniform Hypercubes (UH):} This domain was filled with a regular pattern of axis-aligned hypercubes (identically distributed with uniform gaps)
(Fig.  \ref{fig:obstacleMap}b). For a real vector space of dimension $n$, this environment contains $10^n$ obstacles with a total volume of $0.75^n$. In this environment, we see a dominant performance of IST* over the Baseline with up to $30\%$ better solution cost.

\subsection{$\mathbf{SE(3)}$ problems}
As the environment models in OMPL App are designed for single source and destination motion planning (with no goals), many of them have disconnected regions making them unsuitable for benchmarking multi-goal path planning under random generation of goal points. Thus, we used the $Home$ and $Abstract$ environment which not only admit one connected $SE(3)$ configuration space but also offer several homotopy classes of solution paths.

$Home$ and $Abstract$ environments are related to the classic Piano Mover's problem, admitting several narrow passages. $Home$ offers the highest scope for optimization among all the environments considered, with the final solution often being $50\%$ better than the initial solution. In $Home$, Baseline performs competitively to {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}, and sometimes also finds better solutions initially. However, {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} converges to a better solution asymptotically in all the problem instances of $Home$. 
In $Abstract$, both Baseline and IST* find a good solution quickly, and as a result, the scope for optimization is not much. Still, IST* is able to improve very quickly, dominates throughout, and reaches the final solution cost of Baseline $\approx 6$ times faster. 









\subsection{Impact of Ripple}

In each iteration of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}, a batch of samples\footnote{Note that the batch size is a hyperparameter and remains fixed throughout the execution.
} is added to the roadmap $G$. As {\fontfamily{pcr}\selectfont Ripple} processes each sample incrementally and keeps rewiring a rooted shortest-path tree until convergence, it may seem better to simply add all the samples at once and then run S* on the updated roadmap to get $S_T$. However, the roadmap grows significantly over time, making S* computationally expensive for repeated executions. Whereas, {\fontfamily{pcr}\selectfont Ripple} only has to process a small fraction of the roadmap for a fixed number of samples per batch.

We benchmarked {\fontfamily{pcr}\selectfont Ripple} with S* to investigate its effect on the performance of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}. A pseudorandom generator was
used for generating the \emph{same} sample points for both. The problem instances considered for this comparison were same as before (Table \ref{tbl:computationalTime}). However, due to space constraints, we only show the results (in Table \ref{tbl:Ripple}) for just $2$ environments depicting extreme scenarios\footnote{The plot on rest of the problem instances with further discussion is available in the Appendix.}. In \texttt{UH}\   $\mathbb{R}^8$, both S* and {\fontfamily{pcr}\selectfont Ripple} perform similarly, with {\fontfamily{pcr}\selectfont Ripple} being marginally better in the instance with $50$ terminals. In $Home$, {\fontfamily{pcr}\selectfont Ripple} not only finds a better final solution, but is also $2-3$x faster than S* for the same solution cost consistently. In all other problem instances as well, these 2 scenarios were observed throughout: either {\fontfamily{pcr}\selectfont Ripple}'s performance was very similar to S* or it was significantly faster while converging to a better solution. In conclusion, while both S* and {\fontfamily{pcr}\selectfont Ripple} can lead to the optimal MST over $G_T$, {\fontfamily{pcr}\selectfont Ripple} dominates S* across all instances considered. 

\subsection{Comparison with SFF\textsuperscript{*}}

SFF\textsuperscript{*} \cite{janovs2021multi} is an algorithm that builds multiple RRTs to find distances between terminals and is designed to terminate when all target locations have been connected to a single component. The performance of SFF\textsuperscript{*} is not shown in the experimental plots for the following reasons: 1) The available implementation of SFF\textsuperscript{*}\footnote{\url{https://github.com/ctu-mrs/space_filling_forest_star}} only supports $SE(3)$ (and its subspace), and thus cannot be evaluated on environments like $\mathbb{R}^4$ and $\mathbb{R}^8$ presently. 2) We benchmarked SFF\textsuperscript{*} on $Home$ and $Abstract$ but we encountered long termination times\footnote{Due to the randomized nature of SFF\textsuperscript{*}, termination time can vary significantly; SFF\textsuperscript{*} terminated in approximately 30-45 minutes for the $Abstract$ environment with 10 terminals. However, for the $SE(3)$ problem instances, SFF\textsuperscript{*} did not terminate after an hour. Whereas, {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} was always able to find an initial solution in the first $5$ minutes in all the problem instances considered.}. 




\subsection{Comparing Path Costs}


While {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}'s primary output is a Steiner tree, we also evaluated the cost of the feasible path obtainable from {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} for MGPF. Overall, it was seen to perform better than the Baseline in path computations too, an example of which is shown on two challenging problem instances in Fig. \ref{fig:pathCosts}.

\begin{figure}[h]
  \centering
  \includegraphics[width=\linewidth]{figures/TSP/Combined_UH_CO_TSP_plot.png}
  \caption{Comparisons of the costs of the feasible MGPF solution found by the planners in the high-dimensional real environments with 50 terminals (over 50 runs).} 
 
  \label{fig:pathCosts}
\end{figure}

\vspace{-0.3cm}

\section{Conclusion}
Inspired by the recent advancements in sampling-based methods for shortest path problems for single source and destination in continuous space
and multi-goal path finding in discrete space, we provide a unifying framework to promote a novel line of research in MGPF.
For the first time, to the best of the authors' knowledge, the ideas of informed sampling have been fruitfully extended to the setting of planning for multiple goals. Our approach is decoupled in the sense that any further advancements in PRM* or informed sampling can be directly used to update the respective components of our proposed framework. 



In the future, we plan to explore obtaining effective lower bounds on the shortest path cost between two terminals instead of using the Euclidean distance. Including heuristics in {\fontfamily{pcr}\selectfont Ripple}
also seems to be a  promising line of research. 

\section{Acknowledgement}

We would like to thank Jaraslov Janoš for his assistance in clearing our queries and helping us benchmark SFF* properly. 

\section{Appendix}

\subsection{More Details on the Prior Work}



Self-organizing maps (SOM) have been used to solve MGPF \cite{fort1988solving, faigl2011application}. SOM is a two-layer neural network and is a iterative procedure that provides a nonlinear mapping between a higher dimensional space (set of goals) to a lower dimensional one (goal ordering). SOM also relies on an underlying distance metric, which can be Euclidean \cite{fort1988solving}, visibility graph-based \cite{faigl2011application,faigl2013visiting}, or sampling-based graph approximation \cite{faigl2016self, vanvek2014multi}. However, in comparison to our asymptotic 2-approximation guarantee, there is no guarantee on the quality of solutions obtained using SOMs. 
 
In {\cite{faigl2011application,faigl2013visiting}, a point robot moves in an environment represented as a polygonal domain, and a convex polygon partition is used for workspace decomposition. Such assumptions may not be straightforward to generalize, for example, such a decomposition is not possible even in common robotic environments like SE(3); thus, making them incomparable to {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} in the environments considered.}

Multi-goal motion planning has also been explored in application to data collection. \cite{faigl2014unifying} consider this problem in planar environment with neighborhood and prize on the goals. The vehicle is required to visit only the neighborhood of each goal and all the goal (regions) are not given the same priority. This makes their problem significantly different from the one we have addressed. Similarly, \cite{mcmahon2021dynamic} have considered rewards on goals but also assume the fact that both goals and rewards are unknown to the operating vehicle in the beginning and become known only when a goal comes under its sensing radius. 

\cite{faigl2019fast} consider the generalized traveling salesman problem with neighborhoods (GTSPN) \emph{specifically for 3D environments}, where an individual neighborhood may consist of multiple regions (thus, forming a neighborhood set), and the problem is to determine a shortest multi-goal path to visit at least one region of each neighborhood. They propose two heuristics for solving the GTSPN with neighborhoods defined as polyhedra and ellipsoids to quickly find a feasible solution. The heuristics exploit properties of the 3D instances with convex regions making their approach inapplicable for comparison with ours. Further, they mention that an extension of their proposed approach for high-dimensional problems is a subject of their future work, which precisely forms the motivation for our work. 

Other papers have addressed variations of multi-goal motion planning taking into account additional constraints like Dubins vehicle \citep{faigl2020fast}, energy-aware planning with recharging stations \citep{warsame2020energy}, availability of physics-based game engine to model dynamics \citep{edelkamp2014multi} which are outside our scope. 

There is also an extensive literature on efficiently constructing minimal-cost Euclidean Steiner Tree spanning a set of vertices on a plane avoiding obstacles, having applications in VLSI designs and ascent assembly engineering \cite{zuavoianu2018multi, lin2008obstacle, decroos2015solving}. However, they assume the underlying (roadmap) graph whose nodes act as Steiner points to be given up-front as input rather than constructing and refining it over time to reduce the path cost.


Finally, we would like to emphasize that the major difference in our proposed approach with respect to prior work is to adapt informed sampling (with a pruning method based on the current bounds on the edge lengths) to the setting of multiple goals enabling faster convergence to an asymptotically 2-approximate solution.


\subsection{Key Lemma on Ripple}
We will prove Lemma 2
here by induction.
At the beginning of the algorithm, $G$ consists only of the terminals $t' \in T$ for which $r_{t'} = t'$ so the claim is trivially true.
When a new sample point $s$ is processed, it is assigned to its neighbor $n$ with the smallest value of $g(n) + cost(s,n)$ (line 6 of Algorithm 3) via which it traces a shortest path to its closest terminal (by the definition of the $g$-values of its neighbors).
The subsequent ripple of updates via the priority queue update the value of $g(n)$ and the closest terminals for neighbors $n$ who have just discovered their shortest path to a terminal via the currently processed $u^*$, and they are entered into a priority queue for further expansion. In this way, if a new shorter path via the newly added sample node $s$ arises in $G$, the  {\fontfamily{pcr}\selectfont Ripple} update discovers and updates this information correctly hence maintaining the inductive invariant of the lemma that $r_u$ is (one of) the closest terminals to $u$ among all terminals $T$ for every node $u \in G$.


\iffalse*********************************
\ravi{We'll need to prove that "the Ripple update procedure for processing new sample points correctly maintains all the edges in the Delaunay triangulation of the terminals asymptotically."}

\begin{figure*}[h]
  \centering
  \includegraphics[width=0.99\linewidth]{figures/rippleUpdatedProof.png}
  \caption{The optimal path $p^*$ from terminal $r_1$ to $r_k$ which may not be discovered by Ripple. Here, $p^*$ passes through nodes in $k$ different terminal's (shortest path) trees.  Nodes with the same color indicate that all of them have the same root. Root of different trees is indicated by $r_i$ which is also a terminal.}
  \label{fig:rippleProof}
\end{figure*}

Notation: $G$ is the roadmap graph and $\mathcal{G}$ is a complete graph on the set of terminals on which an MST algorithm is run to get the Steiner Tree $S_T$. 

\theorem{Upon termination of \texttt{Ripple}, the MST constructed from $\mathcal{G}$ is optimal.}

\definition[Boundary Node]{A node $u$ with root $r_u$ is said to be a \emph{boundary} node if not all neighbors of $u$ have root $r_u$ i.e., $\exists\  v \in \mathcal{N}_V(u)$ \mid\  $r_v \neq r_u$. Ripple tries to update shortest path between terminals $t_i, t_j$ if their exist adjacent boundary nodes in $G$ with roots $t_i$ and $t_j$ respectively. \label{def:rippleUpdate}}
\newline \newline 
Let $p$ be a suboptimal path found by Ripple between some terminal $r_i$ and $r_j$. Further, let's assume that the optimal path $p^*$ from $r_i$ to $r_j$ is part of the MST of $\mathcal{G}$ (otherwise, it doesn't matter whether Ripple is able to find $p^*$ as it would not end up being a part of $S_T$).

\begin{lemma}
If  $p^*$ is not discovered by Ripple, it has to pass through the nodes of at least \textbf{three} different terminals' (shortest path) tree. 
\end{lemma}
\begin{proof}
Let us assume the contrary, ie., the optimal path only visits nodes belonging to just two trees (particularly, the two such trees would be the source and destination terminal's shortest path tree). Thus, $p^*$ reduces to concatenating a path from the source terminal to one of its boundary node and another path from the destination terminal to one of its boundary node such that both these boundary nodes are neighbors. However, such a case would be noticed in Ripple leading it to discover the optimal path $p^*$. 
\end{proof}

Now, we formally characterize $p^*$. Let $p^*$ visit the nodes of $k\ (\ge 3)$ different terminals' tree. Let $r_1$ denote the source terminal and $r_k$ denote the destination terminal. Also, let the order of the roots of the intermediate nodes (whose root is neither $r_1$ nor $r_k$) be $r_2, r_3, \ldots, r_{k-1}$. Thus, $p^*$ would be of the form 
\begin{align*}
    \langle r_1, \ldots, f^\prime_1, f_2, \ldots, f^\prime_2, f_3, \ldots, f_{k-1}, \ldots, f^\prime_{k-1}, f_k, \ldots, r_k \rangle
\end{align*} 
where $f_i, f^\prime_i$ are the first and the last nodes \footnote{Note that it is possible to have $f_i = f^\prime_i$ which would imply $p^*$ goes through only 1 node belonging to tree $i$. It is also possible that $r_j = f^\prime_j$ or $r_j = f_j$ for some $j \in \{1, \ldtos, k\}$.} of $p^*$ (respectively) with root $r_i$. Thus, $f, f^\prime_i$ are boundary nodes. We provide a visualization of $p^*$, root nodes and boundary nodes in Fig. \ref{fig:rippleProof}. 
\newline \newline 
Let $d(x, y)$ denote the cost of the optimal path from node $x$ to node $y$ in $G$.
Thus, for any $x, y, z \in G$, we have 
\begin{align*}
    d(x, y) \le d(x, z) + d(z, y) \quad (Traingle\  Inquality)  \label{eqn:triangle} \tag{1}
\end{align*}
Let $y_i = d(r_i, f^\prime_i)$, $i \in \{1, \ldots, k-1\}$ represent the path cost from the boundary node $f^\prime_i$ to its root. Similarly, let $x_i = d(r_i, f_i)$, $i \in \{2, \ldots, k\}$ represent the path cost from the boundary node $f_i$ to its root. Also, let $z_i = d(f_i, f^\prime_i)$, $i \in \{2, \ldots, k-1\}$ . Further, let $\delta_i$ denote the cost of the edge connecting the two neighboring boundary nodes $f^\prime_i, f_{i+1}$ which marks the switch of $p^*$ from nodes with root $i$ to the subsequent nodes having root $i+1$. We can develop a relation between the quantities defined above using the triangle inequality: 
\begin{align*}
    d(r_i, f_{i+1}) &\le d(r_i, f^\prime_i) + d(f^\prime_i, f_{i+1}) = y_i + \delta_
    \\
    d(r_{i+1}, f^\prime_i) &\le d(f^\prime_i, f_{i+1}) + d(f_{i+1}, r_{i+1}) = \delta_i + x_{i+1}
\end{align*}
As nodes $f_i, f^\prime_i$ have root $i$, thus their g-cost are optimal from terminal $i$. Therefore, 
\begin{align*}
    d(r_i, f^\prime_i) &\le d(r_j, f^\prime_i) \qquad \forall\  j \in \{1, \ldots, |\mathcal{T}|\} \\
    d(r_i, f^\prime_i) &\le d(r_{i+1}, f^\prime_i), \quad i \in \{1, \ldots, k-1\} \nonumber \\
    \Rightarrow y_i &\le x_{i+1} + \delta_i \label{eqn:y_iGcost} \tag{2}
\end{align*}
Similarly,  
\begin{align*}
    d(r_{i+1}, f_{i+1}) &\le d(r_j, f_{i+1}) \qquad \forall\  j \in \{1, \ldots, |\mathcal{T}|\} \\
    d(r_{i+1}, f_{i+1}) &\le d(r_i, f_{i+1}), \quad i \in \{1, \ldots, k-1\} \nonumber \\
    x_{i+1} &\le y_i + \delta_i \label{eqn:x_iGcost} \tag{3}
\end{align*}
Further, nodes with the same root ie., $f_i, f^\prime_i, r_i$ obey the following inequalities due to the \emph{triangle inequality:}
\begin{align*}
    d(f_i, f^\prime_i) &\le d(f_i, r_i) + d(r_i, f^\prime_i) \Rightarrow z_i \le x_i + y_i  \label{eqn:z_iTriangle} \tag{4} \\
    d(r_i, f_i) &\le d(r_i, f^\prime_i) + d(f^\prime_i, f_i) \Rightarrow x_i \le y_i + z_i  \label{eqn:x_iTriangle} \tag{5} \\
    d(r_i, f^\prime_i) &\le d(r_i, f_i) + d(f_i, f^\prime_i) \Rightarrow y_i \le x_i + z_i  \label{eqn:y_iTriangle} \tag{6} \\
\end{align*}
An illustration of this characterization of $p^*$ including all the inequalities established is shown in Figure \ref{fig:rippleProof}. Now, the cost of $p^*$ is
\begin{align*}
    c(p^*) &= d(r_1, r_k) = y_1 + \sum_{j=1}^{k-1} \delta_i + \sum_{j=2}^{k-1} z_i + x_k
\end{align*}
Now, let us consider the path between two consecutive terminals $r_i, r_{i+1}$ in $p^*$ via $f^\prime_i, f_{i+1}$. Let this path be denoted by $p_i$ and its cost be represented by $c(p_i)$.

\begin{align*}
    c(p_i) &= d(r_i, f^\prime_i) + d(f^\prime_i, f_{i+1}) + d(f_{i+1}, r_{i+1}) \\
    &= y_i + \delta_i + x_{i+1}, \quad 1 \le i < k \\
     c(p_i) &\ge d(r_i, r_{i+1})
\end{align*}

Note that $p_i$ is not necessarily the least cost path from $r_i$ to $r_{i+1}$. However, we claim that the cost of each such path between consecutive terminals in $p^*$ (i.e., $p_i,  \ \forall \ 1 \le i < k$) is not more than the optimal path cost from $r_i$ to $r_k$.

\begin{lemma}
$c(p_i) \le c(p^*) \quad \forall \ 1 \le i < k$ \label{lemma:eachPathShorter}
\end{lemma}

\begin{proof}
Let us assume $c(p_i) > c(p^*)$. Therefore, 
\begin{align*}
    c(p_i) &> c(p^*) \\
    y_i + \delta_i + x_{i+1} &> d(r_1, r_k) \\
    y_i + \delta_i + y_i - \delta_i &> d(r_1, r_k) \quad (\text{from } (\ref{eqn:y_iGcost})) \\
    \Rightarrow 2 y_i &> d(r_1, r_k) \\
\end{align*}
Now, we manipulate the RHS. First, we bring $y_i$ by from $y_1$ in the following way :
\begin{align*}
    d(r_1, r_k) &= y_1 + \sum_{j=1}^{k-1} \delta_j + \sum_{j=2}^{k-1} z_j + x_k \\
    &= y_1 + \delta_1 + \sum_{j=2}^{k-1} \delta_j + \sum_{j=2}^{k-1} z_j + x_k \\
    &\ge x_2 + \sum_{j=2}^{k-1} \delta_j + \sum_{j=2}^{k-1} z_j + x_k  \quad (\text{from } (\ref{eqn:x_iGcost})) \\
    &\ge x_2 + z_2 + \sum_{j=2}^{k-1} \delta_j + \sum_{j=3}^{k-1} z_j + x_k \\
    &\ge y_2 + \sum_{j=2}^{k-1} \delta_j + \sum_{j=3}^{k-1} z_j + x_k  \quad (\text{from } (\ref{eqn:y_iTriangle})) \\
    &\;\vdots \\
    &\ge y_i + \sum_{j=i}^{k-1} \delta_j + \sum_{j=i+1}^{k-1} z_j + x_k \\
\end{align*}
Next, we bring another $y_i$ using $x_k$ :
\begin{align*}
    d(r_1, r_k) &\ge y_i + \sum_{j=i}^{k-1} \delta_j + \sum_{j=i+1}^{k-1} z_j + x_k \\
    &\ge y_i + \sum_{j=i}^{k-1} \delta_j + \sum_{j=i+1}^{k-1} z_j + y_{k-1} - \delta_{k-1}  \quad (\text{from } (\ref{eqn:y_iGcost})) \\
    &\ge y_i + \sum_{j=i}^{k-2} \delta_j + \sum_{j=i+1}^{k-1} z_j + y_{k-1} \\
    &\ge y_i + \sum_{j=i}^{k-2} \delta_j + \sum_{j=i+1}^{k-2} z_j + z_{k-1} + y_{k-1} \\
    &\ge y_i + \sum_{j=i}^{k-2} \delta_j + \sum_{j=i+1}^{k-2} z_j + x_{k-1}  \quad (\text{from } (\ref{eqn:x_iTriangle})) \\
    &\;\vdots \\
    &\ge y_i + \sum_{j=i}^{i-1} \delta_j + \sum_{j=i+1}^{i} z_j + y_i \\
    &\ge 2y_i \\
\end{align*}
Plugging this into the original inequality, 
\begin{align*}
    \Rightarrow 2 y_i &> d(r_1, r_k) \ge 2y_i \\
\end{align*}
we come to a contradiction. Hence, 
\begin{align*}
    c(p_i) \le d(r_1, r_k) \quad \forall \ 1 \le i < k
\end{align*}
\end{proof}

Let $\mathcal{P} = \{p_i\}_{i=1}^{k-1}$ be the set of paths considered above. We have proved that each of the path in $\mathcal{P}$ has cost not more than the cost of the optimal path $p^*$ between $r_1$ and $r_k$. Recall that $p^*$ belongs to the MST of $\mathcal{G}$. Let us now remove $p^*$ from the MST of $\mathcal{G}$. This will break the MST into two connected components; let us call them $C_1, C_2$ and the resulting graph as $G^\prime$. Next, we iterate through the paths in $\mathcal{P}$ and check whether it introduces a cycle in $G^\prime$.

\lemma{There exists at least one path among $\mathcal{P}$ which connects $C_1$ to $C_2$ (ie., doesn't introduce a cycle in $G^\prime$). \label{lemma:RippleConnectivity}} 

\proof{Let us assume that no path in $\mathcal{P}$ connects $C_1$ to $C_2$. Notice that there does not exist a path from $r_1$ to $r_k$ in $G^\prime$ (otherwise, this would mean that before $p^*$ was removed, MST of $\mathcal{G}$ had a cycle which is not true). Now, let's add each of the paths in $\mathcal{P}$ to $G^\prime$. As $p_i$ begins at $r_i$ and ends at $r_{i+1}$ ($\forall \  1 \le i < k$) and we have inserted all $p_i$ to $G^\prime$, so a path beginning at $r_1$ and ending at $r_k$ must exist now. This implies that $G^\prime$ is now a single connected component. If none of the paths in $\mathcal{P}$ connect $C_1$ to $C_2$, then $G^\prime$ would remain at 2 connected components (even after inserting all paths in $\mathcal{P}$) and never fuse to 1 connected component. Hence, we have arrived at a contradiction. }
\newline \newline 
Let $p_j$ be one such path in $\mathcal{P}$ ($1 \le j < k$) which connects $C_1$ to $C_2$ \footnote{$p_j$ is chosen arbitrarily from $\mathcal{P}$ if there exists multiple paths in $\mathcal{P}$ connecting $C_1$ to $C_2$}. Recall that $p_j$ is the concatenation of the path from terminal $r_j$ to one of its boundary node $f^\prime_j$ (which has root $r_j)$ and the path from an adjacent boundary node $f_{j+1}$ to its root $r_{j+1}$. From Definition~\ref{def:rippleUpdate}, we know Ripple would discover $p_j$ as it is a \emph{direct} path from $r_j$ to $r_{j+1}$. As, $c(p_j) \le c(p^*)$ (from Lemma \ref{lemma:eachPathShorter}) so introducing $p_j$ and removing $p^*$ from the MST of $\mathcal{G}$ doesn't increase its cost. Also, we know from Lemma \ref{lemma:RippleConnectivity}, MST remains connected after removing $p^*$ from it and inserting $p_j$ in it. Thus, we have successfully replaced an optimal path $p^*$ in the MST (which may not be discovered by Ripple) by a path found by Ripple. Thus, we can repeatedly replace paths in the MST not found by Ripple with paths which are discovered by Ripple without increasing its cost. Hence, the MST formed from the paths found by Ripple is optimal. 


\fi 
    
\subsection{Connection Radius} 

The connection radius $\rho$ (used in determining the neighbors of a node) controls the sparseness of the roadmap graph ($G$). High values of $\rho$ will lead to a dense roadmap making graph-search algorithms (like  {\fontfamily{pcr}\selectfont Ripple}) computationally expensive while low values will make the roadmap sparse but may also cause it to be disconnected. As the module PRM* from OMPL was used as the underlying roadmap for both {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} and the Baseline, it adaptively limits the connections in $G$ as the number of samples increase by decreasing the radius $\rho$ (generally done to make the minimal number of connections required to ensure asymptotic optimality), as proposed in \cite{karaman2011sampling
\begin{align*}
     \rho(q) := \eta \Bigg( 2 \Big(1 + \frac{1}{d} \Big) \bigg( \frac{\lambda \big(X_f \big)}{\zeta_d} \bigg)
     \Big(\frac{\ log(q)}{q} \Big)
     \Bigg)^{\frac{1}{d}}
\end{align*}
where $q$ is the number of points sampled in $X_f$ (ie., $|V|$), $d$ is the dimension of $X$, $\eta > 1$ is a tuning parameter, and $\lambda(X_f)$ is Lebesgue measure of the obstacle-free space and $\zeta_d$ is the volume of the unit ball in the $d$-dimensional Euclidean space.

\f

\subsection{Pruning}

\subsubsection{Lower Bound on Edges:}
A critical phase in {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} is pruning edges of $G_T$ which cannot be part of the MST in future. Success of this phase relies heavily on how close are the lower bounds of these edges to their optimal cost. Past research has been focused mostly on finding lower bounds for special instances of the general motion planning problem \cite{canny1987new, lumelsky1987path}. A separate line of research recently has been focused on calculating the lower bound on \textbf{the past cost obtainable from the current set of samples}
\cite{salzman2015asymptotically}. However, these are not lower bound on the optimal path cost but only representative of the lower bound derived from the discrete approximation of the state space. Thus, due to lack of effective worst-case lower bound on the optimal path cost for the general motion planning problem, we use the Euclidean distance between two points
in $X_f$ as the heuristic estimate.

\subsubsection{Numerical Results:} In Table \ref{tbl:pruningStatistics}, we show the impact of the pruning condition we have developed. Even though we have a very simple heuristic estimate as the lower bound, we are able to prune a significant number of edges. With more terminals, it is likely that edges in the MST of $G_T$ will be a straight-segments in $X_f$ (hence, the optimal path is quite close to Euclidean distance). This is turn should lead to more pruning which we observe as a trend in Table \ref{tbl:pruningStatistics}.

\begin{table}[hbt!]
\caption{Average percentage of edges pruned from $G_T$ by {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} in each problem instance across 50 runs.}
\[\begin{array}{|c|c|c|c|}
\hline
\tikz{\node[below left, inner sep=1pt] (def) {\small{Environment}};%
      \node[above right,inner sep=1pt] (abc) {Terminals};%
      \draw (def.north west|-abc.north west) -- (def.south east-|abc.south east);}
 & 10 & 30 & 50 \\
\hline
\texttt{CO}\ \ \mathbb{R}^4 & 91\% & 98\% & 98\%\\
\hline 
\texttt{CO}\ \ \mathbb{R}^8 & 64\% & 84\% & 90\% \\
\hline 
\texttt{UH}\ \ \mathbb{R}^4 & 92\% & 97\% & 98\%\\
\hline 
\texttt{UH}\ \ \mathbb{R}^8 & 37\% & 79\% & 81\%\\
\hline 
HOME & 87\% & 91\% & 98\%\\
\hline 
ABSTRACT & 97\% & 98\% & 99\% \\
\hline 
\end{array}\]
\label{tbl:pruningStatistics}
\end{table}

While the pruning statistics may look appealing, it is possible to generate instances where no edges would be pruned. 

\paragraph{Worst Case:} 
Consider a problem instance with a star-shaped obstacle in the center with terminals at the concave openings between two sharp ends of the star obstacle. Further, the placement of terminals and the concave openings should be such that all terminals are near to each other while the optimal path is long and convoluted. For example, in Figure \ref{fig:WorstCaseStarProblem}, lower bound between every edge is less than the optimal cost between any two distinct terminals which means no edge will be pruned in $G_T$. 

\begin{figure}[h]
  \centering
  \includegraphics[width=0.99\linewidth]{figures/starFigure.png}
  \caption{A problem instance: Light brown star denotes the obstacle region. Small blue hollow circles represent the terminals. The dashed green line is the Euclidean distance between the terminals while the red lines are the optimal paths.}
  \label{fig:WorstCaseStarProblem}
\end{figure}





\subsection{Implementation Details}

While we mention that a fixed number of samples is added to the roadmap in the pseudocode of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}, the implementation could not exactly follow this as such an operation is not allowed in the available module for PRM*. However, OMPL does support growing the roadmap for a \emph{fixed amount of time}. Hence, we had a time-based implementation where in each iteration of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}, the roadmap was grown for the same amount of time. Consequently, the number of samples added per iteration was not fixed and varied depending upon the time spent in collision-checking for each new sample point. We let {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} and the Baseline run for all of the provided time. 

Both the planners used the implementation of PRM* from OMPL  (with default parameters).We also tried LazyPRM* from OMPL but it performed worse than PRM*, contrary to our expectations. However, our approach (subsequently, the codebase of {\fontfamily{pcr}\selectfont IST\textsuperscript{*}}) is modular such that LazyPRM* or any other implementation of the underlying roadmap can be used to interface with our planner. 


\subsection{Ripple : More Results and Discussion}

We show the comparison of  {\fontfamily{pcr}\selectfont Ripple} with S* on more environments in Table \ref{tbl:RippleAppendix}. In $Abstract$ with 10 terminals, we see  {\fontfamily{pcr}\selectfont Ripple} performing better throughout while in all other instances its performance is almost same as Baseline. In real-vector space instances, it was observed that size of the roadmap in the end was much smaller compared to instances like $Home$ or $Abstract$. As the size of roadmap was tiny, so we couldn't witness the benefits of  {\fontfamily{pcr}\selectfont Ripple}. We believe this was a consequence of our implementation in Python. 

Real environments like \texttt{CO} and \texttt{HR} (Fig. 
) were custom defined in Python so when the roadmap was grown for both {\fontfamily{pcr}\selectfont IST\textsuperscript{*}} and the Baseline, OMPL's planners in C++ had to interface with the collision checker of these environments in Python for each point sampled in the configuration space. This made the growth of PRM in these instances extremely slow due to the constant back and forth call between Python and C++. Thus, the size of the roadmap $G$ in these envrionments was pretty small compared to $Home$ or $Abstract$ which are defined in OMPL App itself. 

\begin{table*}[hbt!]
        \centering
        \begin{tabular
        {cM{0.3\linewidth}M{0.3\linewidth}M{0.3\linewidth}}
           \toprule
            \emph{Env.} & 10 Terminals & 30 Terminals & 50 Terminals \\
            \midrule
         
   \makecell{A\\B\\S\\T\\R\\A\\C\\T} & 
  \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/Abstract/Abstract_env_10_600s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/Abstract/Abstract_env_30_600s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/Abstract/Abstract_env_50_600s_costDistributionOverTime.png} \\
   
  
   \makecell{\texttt{UH} \\ $\mathbb{R}^4$} & 
  \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/UniformHyperRectanglesR4/UniformHyperRectanglesR4_10_600s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/UniformHyperRectanglesR4/UniformHyperRectanglesR4_30_1200s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/UniformHyperRectanglesR4/UniformHyperRectanglesR4_50_1800s_costDistributionOverTime.png} \\
   
   
   \makecell{\texttt{CO} \\ $\mathbb{R}^4$} & 
  \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/CenterObstacleR4/CenterObstacleR4_10_450s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/CenterObstacleR4/CenterObstacleR4_30_900s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/CenterObstacleR4/CenterObstacleR4_50_1350s_costDistributionOverTime.png} \\
   
   
   \makecell{\texttt{CO} \\ $\mathbb{R}^8$} & 
  \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/CenterObstacleR8/CenterObstacleR8_10_900s_costDistributionOverTime.png}
   & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/CenterObstacleR8/CenterObstacleR8_30_1800s_costDistributionOverTime.png} & \includegraphics[width=\linewidth, height=40mm]{figures/ResultsAgainstSstar/CenterObstacleR8/CenterObstacleR8_50_2700s_costDistributionOverTime.png} \\
   
            \bottomrule
            
        \end{tabular}
        \caption{Comparison of the performance of {IST\textsuperscript{*}} when {\fontfamily{pcr}\selectfont Ripple} is used against S* on environments not shown in the main paper. The dark line represents the mean solution cost with the thick region being the 99\% confidence interval about the mean.}
        
        
        \label{tbl:RippleAppendix}
    \end{table*}


