Fast-Marching Methods for Curvature Penalized Shortest Paths

Abstract

We introduce numerical schemes for computing distances and shortest paths with respect to several planar paths models, featuring curvature penalization and data-driven velocity: the Dubins car, the Euler/Mumford elastica, and a two variants of the Reeds–Shepp car. For that purpose, we design monotone and causal discretizations of the associated Hamilton–Jacobi–Bellman PDEs, posed on the three-dimensional domain ${\mathbb R}^2 \times {\mathbb S}^1$. Our discretizations involve sparse, adaptive and anisotropic stencils on a cartesian grid, built using techniques from lattice geometry. A convergence proof is provided, in the setting of discontinuous viscosity solutions. The discretized problems are solvable in a single pass using a variant of the fast-marching algorithm. Numerical experiments illustrate the applications of our schemes in motion planning and image segmentation.

Appendices

Appendix A: Local Validation of the Numerical Scheme, via the Control Sets

We present a local validation of our PDE discretization procedure, by comparing the model control sets (4), see also Fig. 2, with some numerical counterparts.

Consider a compact and convex set $\mathcal{B}\subseteq {\mathbb E}= {\mathbb R}^3$, containing the origin, and the corresponding Hamiltonian $\mathcal{H}$. Contrary to the rest of this paper, the possible dependency of $\mathcal{B}$ and $\mathcal{H}$ on some underlying base point ${\mathbf p}\in {\mathbb M}$ is not considered, since the discussion is purely local. Let also H be an approximation of $\mathcal{H}$, for instance of the form (15). Consider the set

$$\begin{aligned} B := \{\dot{\mathbf p}\in {\mathbb E};\, \forall \hat{\mathbf p}\in {\mathbb E}^*,\, H(\hat{\mathbf p}) \le 1/2 \Rightarrow \langle \hat{\mathbf p}, \dot{\mathbf p}\rangle \le 1\}, \end{aligned}$$

(63)

and note that $B=\mathcal{B}$ if $H=\mathcal{H}$. We regard the closeness of B and $\mathcal{B}$, inspected visually, as a good witness of the closeness of H and $\mathcal{H}$.

In Fig. 13, left, is illustrated the case where $\mathcal{B}$ is an ellipsoid, with principal axes of length (1, 0.1, 0.1). In that case $\mathcal{H}$ is a quadratic function, and the discrete representation using Corollary 4.13 is exact. One therefore has $H=\mathcal{H}$, thus $B=\mathcal{B}$ as can be observed. This particular case is at the foundation of [36].

In Fig. 13, right, is illustrated the case where $\mathcal{B}= [0,\dot{\mathbf n}]$ is a segment, where $\dot{\mathbf n}\in {\mathbb E}$ is a unit vector, and therefore $\mathcal{H}(\hat{\mathbf p}) = \frac{1}{2} \langle \hat{\mathbf p},\dot{\mathbf n}\rangle _+^2$. The discretization is performed using Proposition 2.2 with $\varepsilon =0.1$, via the basis reduction techniques presented in Sect. 4. As can be observed, the vectors $\dot{\mathbf e}_k$, $1 \le k \le K$, are almost aligned with $\dot{\mathbf n}$, and the set B is close to the segment $\mathcal{B}$ in the Hausdorff distance, although slightly fatter.

Figure 14 is devoted to the models of interest in this paper. The parameters are $\xi =0.2$ [appearing in the curvature cost (2)], $\theta =\pi /3$ (the current orientation), and $\varepsilon =0.1$ (tolerance in Proposition 2.2). The offsets $\dot{\mathbf e}_k$, $1\le k \le K$, are illustrated in Fig. 4 page 7. Comparing with Fig. 2, we can confirm that the sets B are close to the corresponding ellipse,¹⁰ half ellipse, non-centered ellipse, and triangle $\mathcal{B}$, respectively. However the true control sets $\mathcal{B}$ are flat, with Haussdorff dimension 2, whereas their counterparts B are slightly fatter.

Fig. 13

Illustration of Appendix A. Left: Discretization stencil for a quadratic anisotropic Hamiltonian. The Hamiltonian and control set representation are exact. Right: Discretization stencil of Proposition 2.2, for a Hamiltonian $\mathcal{H}(\hat{\mathbf p}) = \frac{1}{2} \langle \hat{\mathbf p},\dot{\mathbf n}\rangle _+^2$. The approximate control set (63) is close to the segment $[0,\dot{\mathbf n}]$, but slightly fatter

Fig. 14

Approximate control set (63) for the Reeds–Shepp reversible, Reeds–Shepp forward, Euler–Mumford and Dubins models. The shapes of the original control sets, see Fig. 2, are recognizable, elongated due to the model parameter choice $\xi =0.2$, and slightly fatter due to the effect of discretization, with relaxation parameter $\varepsilon =0.1$. Orientation $\theta = \pi /3$. Seed Fig. 3 for the corresponding stencils

Appendix B: Convexity of the Metric

We prove in this appendix that the metrics $\mathcal{F}^{\text {RS}+}_{\mathbf p}, \mathcal{F}^\text {EM}_{\mathbf p},\mathcal{F}^\text {D}_{\mathbf p}: {\mathbb E}\rightarrow [0,\infty ]$ are convex, for any fixed ${\mathbf p}\in \Omega$, due to their construction (3) and to the following two results.

Lemma B.1

Let $\mathcal{C}: {\mathbb R}\rightarrow [1,\infty ]$ be convex and lower semi-continuous, and let $f : ]0,\infty [ \times {\mathbb R}\rightarrow [0,\infty ]$ be defined by $f(n,t):= n \mathcal{C}(t/n)$.

Then, f is lower semi-continuous, 1-positively homogeneous, everywhere positive, and obeys the triangular inequality.

Proof

Lower semi-continuity, homogeneity and positivity are obvious. In addition for any $(n,t),(n',t') \in ]0,\infty [\times {\mathbb R}$, one obtains

$$\begin{aligned} \frac{f(n+n',t+t')}{n+n'}= & {} \mathcal{C}\left( \frac{t}{n} \frac{n}{n+n'} + \frac{t'}{n'} \frac{n'}{n+n}\right) \\\le & {} \frac{n}{n+n'} \mathcal{C}\left( \frac{t}{n}\right) \\&+ \frac{n'}{n+n'} \mathcal{C}\left( \frac{t'}{n'}\right) {=} \frac{f(n,t)+f(n',t')}{n+n'}. \end{aligned}$$

$\square$

Note that 1-positive homogeneity and the triangular inequality together imply convexity. We recall the notation ${\mathbb E}:= {\mathbb R}^2 \times {\mathbb R}$, used in the next result.

Corollary B.2

Let $\mathcal{C}: {\mathbb R}\rightarrow [1,\infty ]$ be convex, lower semi-continuous, and such that $l(\varepsilon ) := \lim \mathcal{C}(\varepsilon t)/t$ as $t\rightarrow \infty$ exists and belongs to $]0,\infty ]$, for each $\varepsilon \in \{-1,1\}$. Let $F : {\mathbb E}\rightarrow [0,\infty ]$ be defined as $F(\dot{\mathbf x}, \dot{\theta }) := \Vert \dot{\mathbf x}\Vert \mathcal{C}(\dot{\theta }/\Vert \dot{\mathbf x}\Vert )$ for each $(\dot{\mathbf x}, \dot{\theta }) \in {\mathbb E}$ such that $\dot{\mathbf x}= \Vert \dot{\mathbf x}\Vert {\mathbf n}$ and $\Vert \dot{\mathbf x}\Vert >0$, where ${\mathbf n}\in {\mathbb S}^{d-1}$ is a fixed direction. Let also $F(0,0) = 0$, $F(0,\dot{\theta }) = |\dot{\theta }| l(\dot{\theta }/|\dot{\theta }|)$ if $\dot{\theta }\ne 0$, and $F( \dot{\mathbf x}, \dot{\theta }) := +\,\infty$ otherwise. Then, F is lower semi-continuous and obeys

• (1-positive homogeneity$)\,F(\lambda \dot{\mathbf p}) = \lambda F(\dot{\mathbf p})$, for all $\lambda >0$ and all $\dot{\mathbf p}\in {\mathbb E}$.

• (Separation$)\,F(\dot{\mathbf p})=0$ iff $\dot{\mathbf p}= 0$, for all $\dot{\mathbf p}\in {\mathbb E}$.

• (Triangular inequality$)\,F(\dot{\mathbf p}+\dot{\mathbf q}) \le F(\dot{\mathbf p})+F(\dot{\mathbf q})$, for all $\dot{\mathbf p}, \dot{\mathbf q}\in {\mathbb E}$.

Proof

By the previous lemma, F obeys the announced properties on the convex set $(]0,\infty [ {\mathbf n}) \times {\mathbb R}$. These properties are also satisfied on the closure $([0,\infty [ {\mathbf n}) \times {\mathbb R}$ since, clearly, F is extended to it by its lower continuous envelope, and since the limits l(1) and $l(-1)$ are positive (for separation). Finally, the announced properties hold for the trivial extension of F to ${\mathbb R}^2 \times {\mathbb R}$ by $+\,\infty$ since the subset $([0,\infty [{\mathbf n}) \times {\mathbb R}$ is closed and convex. $\square$