Abstract
We present a PDE-based approach for finding optimal paths for the Reeds–Shepp car. In our model we minimize a (data-driven) functional involving both curvature and length penalization, with several generalizations. Our approach encompasses the two- and three-dimensional variants of this model, state-dependent costs, and moreover, the possibility of removing the reverse gear of the vehicle. We prove both global and local controllability results of the models. Via eikonal equations on the manifold \(\mathbb {R}^d \times {\mathbb {S}}^{d-1}\) we compute distance maps w.r.t. highly anisotropic Finsler metrics, which approximate the singular (quasi)-distances underlying the model. This is achieved using a fast-marching (FM) method, building on Mirebeau (Numer Math 126(3):515–557, 2013; SIAM J Numer Anal 52(4):1573–1599, 2014). The FM method is based on specific discretization stencils which are adapted to the preferred directions of the Finsler metric and obey a generalized acuteness property. The shortest paths can be found with a gradient descent method on the distance map, which we formalize in a theorem. We justify the use of our approximating metrics by proving convergence results. Our curve optimization model in \(\mathbb {R}^{d} \times \mathbb {S}^{d-1}\) with data-driven cost allows to extract complex tubular structures from medical images, e.g., crossings, and incomplete data due to occlusions or low contrast. Our work extends the results of Sanguinetti et al. (Progress in Pattern Recognition, Image Analysis, Computer Vision, and Applications LNCS 9423, 2015) on numerical sub-Riemannian eikonal equations and the Reeds–Shepp car to 3D, with comparisons to exact solutions by Duits et al. (J Dyn Control Syst 22(4):771–805, 2016). Numerical experiments show the high potential of our method in two applications: vessel tracking in retinal images for the case \(d=2\) and brain connectivity measures from diffusion-weighted MRI data for the case \(d=3\), extending the work of Bekkers et al. (SIAM J Imaging Sci 8(4):2740–2770, 2015). We demonstrate how the new model without reverse gear better handles bifurcations.
Similar content being viewed by others
Avoid common mistakes on your manuscript.
1 Introduction
Shortest paths in position and orientation space are central in this paper. Dubins describes in [21] the problem of finding shortest paths for a car in the plane between initial and final points and direction, with a penalization on the radius of curvature, for a car that has no reverse gear. Reeds and Shepp consider in [50] the same problem, but then for a car that does have the possibility for backward motion. In both papers, the focus lies on describing and proving the general shape of the optimal paths, without giving explicit solutions for the shortest paths.
This can be considered a curve optimization problem in the space \({\mathbb {R}}^2 \times (\mathbb {R}/2 \pi \mathbb {Z})\), equipped with the natural Euclidean metric but only among curves \(\gamma (t) = (x(t),y(t),\theta (t))\) subject to the constraint that \((\dot{x}(t), \dot{y}(t))\) is proportional to \((\cos \theta (t), \sin \theta (t))\). Formulating the problem this way, it becomes one of the simplest examples of sub-Riemannian (SR) geometry: the tangent vector \(\dot{\gamma }(t)\) is constrained to remain in the span of \((\cos \theta (t),\sin \theta (t),0)\) and (0, 0, 1), see Fig. 1. The SR curve optimization problem and the properties of its geodesics in \({\mathbb {R}}^2 \times \mathbb {S}^1\) have been studied and applied in image analysis by [2, 11, 16, 22, 37, 48], and in particular for modeling the Reeds–Shepp car in [10, 44, 52], whereas the latter presented a complete and optimal synthesis for the geometric control problem on \({\mathbb {R}}^{2}\times \mathbb {S}^{1}\) with uniform cost. Properties of SR geodesics in \({\mathbb {R}}^d \times \mathbb {S}^{d-1}\) with \(d=3\) have been studied in [25] and for general d in [24]. Apart from the Reeds–Shepp car problem, there are other examples relating optimal control theory and SR geometry, see for example the books by Agrachev and Sachkov [2] and Montgomery [45]. Applications in robotics and visual modeling of SR geometry and control theory can be found in, e.g., [56].
On the left in Fig. 2, we show an example of an optimal path between two points in \({\mathbb {R}}^2 \times \mathbb {S}^1\). The projection on \({\mathbb {R}}^2\) of this curve has two parts where the car moves in reverse (the red parts of the line), resulting in two cusps. From the perspective of image analysis applications this is undesirable and it is a valid question what the optimal paths are if cusps and reverse gear are not allowed. In this paper, similar to the difference between the Dubins car and the Reeds–Shepp car, we also consider this variant: it can be accounted for by requiring that the spatial propagation is forward. This variant falls outside the SR framework and requires asymmetric Finsler geometry instead.
Furthermore, we would like to extend the Finsler metric using two data-driven factors that can vary with position and orientation. This can be used to compute shortest paths for a car, where for example road conditions and obstacles are taken into account. In [8] it is shown this approach is useful for tracking vessels in retinal images. Likewise, the 3D variant of the problem provides a basis for algorithms for blood vessel detection in 3D magnetic resonance angiography (MRA) data or detection of shortest paths and quantification of structural connectivity in 5D diffusion-weighted magnetic resonance imaging (MRI) data of the brain.
1.1 A Distance Function and the Corresponding Shortest Paths on \(\mathbb {R}^d \times \mathbb {S}^{d-1}\)
We fix the dimension \(d\in \{2,3\}\), and let \({\mathbb {M}} := \mathbb {R}^d\times \mathbb {S}^{d-1}\) be the \(2d-1\)-dimensional manifold of positions and orientations. We use a Finsler metric on the tangent bundle of \({\mathbb {M}}\), \(\mathcal {F}: T({\mathbb {M}}) \rightarrow [0,+\infty ]\), of which specific properties are discussed later, to define a geometry on \({\mathbb {M}}\). Any such Finsler metric \(\mathcal {F}\) induces a measure of length \({{\mathrm{Length}}}_\mathcal {F}\) on the class of paths with Lipschitz regularity, defined asFootnote 1
with the convention \(\dot{\gamma }(t) := \frac{d}{dt} \gamma (t)\). The path is said to be normalized w.r.t. \(\mathcal {F}\) iff \(\mathcal {F}(\gamma (t), \dot{\gamma }(t)) = {{\mathrm{Length}}}_\mathcal {F}(\gamma )\) for all \(t \in [0,1]\). Any Lipschitz continuous path of finite length can be normalized by a suitable reparameterization. Finally, the quasi-distance \(d_\mathcal {F}: {\mathbb {M}} \times {\mathbb {M}} \rightarrow [0,+\infty ]\) is defined for all \({\mathbf {p}},{\mathbf {q}}\in {\mathbb {M}}\) by
with \(\varGamma :={{\mathrm{Lip}}}([0,1], {\mathbb {M}})\). Normalized minimizers of (1) are called minimizing geodesics from \({\mathbf {p}}\) to \({\mathbf {q}}\) w.r.t. \(\mathcal {F}\). For certain pairs \(({\mathbf {p}},{\mathbf {q}})\) these minimizers may not be unique, and these points are often of interest, see for example [9, 44].
Definition 1
(Maxwell point) Let \({\mathbf {p}}_S \in {\mathbb {M}}\) be a fixed point source and \(\gamma \in \varGamma \) a geodesic connecting \({\mathbf {p}}_S\) with \({\mathbf {q}}\in {\mathbb {M}}\), \({\mathbf {q}}\ne {\mathbf {p}}_S\). Then \({\mathbf {q}}\) is a Maxwell point if there exists another extremal path \(\tilde{\gamma } \in \varGamma \) connecting \({\mathbf {p}}_S\) and \({\mathbf {q}}\), with \({{\mathrm{Length}}}_\mathcal {F}(\gamma ) = {{\mathrm{Length}}}_\mathcal {F}(\tilde{\gamma })\). If \({\mathbf {q}}\) is the first point (distinct from \({\mathbf {p}}_S\)) on \(\gamma \) where such \(\tilde{\gamma }\) exists, then \({\mathbf {q}}\) is called the first Maxwell point. The curves \(\gamma , \tilde{\gamma }\) lose global optimality after the first Maxwell point.
Remark 1
(Terminology) We use the common terminology of ‘Finsler metric’ for \(\mathcal {F}\), although it is also called ‘Finsler function’, ‘Finsler norm’ or ‘Finsler structure’, and despite the fact that \(\mathcal {F}\) is not a metric (distance) in the classical sense. The Finsler metric \(\mathcal {F}\) induces the quasi-distance \(d_{\mathcal {F}}\) as defined in (1). If \(\mathcal {F}({\mathbf {p}},\dot{{\mathbf {p}}}) = \mathcal {F}({\mathbf {p}},-\dot{{\mathbf {p}}})\) for all \({\mathbf {p}}\in {\mathbb {M}}\) and tangent vectors \(\dot{{\mathbf {p}}} \in T_{{\mathbf {p}}}({\mathbb {M}})\), then \(d_{\mathcal {F}}\) is a true metric, satisfying \(d_{\mathcal {F}}({\mathbf {p}},{\mathbf {q}}) = d_{\mathcal {F}}({\mathbf {q}},{\mathbf {p}})\) for all \({\mathbf {p}},{\mathbf {q}}\in {\mathbb {M}}\). However, to avoid confusion of the word metric, we will only refer to \(d_{\mathcal {F}}\) as a distance or quasi-distance. If the ‘Finsler metric’ \(\mathcal {F}\) is induced by a metric tensor field \(\mathcal {G}\) on Riemannian manifold \(({\mathbb {M}},{\mathcal {G}})\), then one has \({{\mathcal {F}}}(\mathbf {p},\dot{\mathbf {p}})=\sqrt{\left. \mathcal {G}\right| _{\mathbf {p}}(\dot{\mathbf {p}},\dot{\mathbf {p}})}\).
Throughout the document, we use the words path and curve synonymously. When we consider formal curve optimization problem (1), we speak of geodesics for the stationary curves. Such stationary curves are locally minimizing. A global minimizer of (1) is referred to as minimizing geodesic or minimizer.
1.2 Geometry of the Reeds–Shepp Model
We introduce the Finsler metric \(\mathcal {F}_0\) underlying the Reeds–Shepp car model and the Finsler metric \(\mathcal {F}_0^+\) corresponding to the variant without reverse gear. Let \(({\mathbf {p}}, \dot{{\mathbf {p}}}) \in T({\mathbb {M}})\) be a pair consisting of a point \({\mathbf {p}}\in {\mathbb {M}}\) and a tangent vector \(\dot{{\mathbf {p}}} \in T_{\mathbf {p}}({\mathbb {M}})\) at this point. The physical and angular components of a point \({\mathbf {p}}\in {\mathbb {M}}\) are denoted by \({{\mathbf {x}}}\in \mathbb {R}^d\) and \(\mathbf {n}\in \mathbb {S}^{d-1}\), and this convention carries over to the tangent:
We say that \(\dot{{{\mathbf {x}}}}\) is proportional to \(\mathbf {n}\), that we write as \(\dot{{{\mathbf {x}}}} \propto \mathbf {n}\), iff there exists a \(\lambda \in \mathbb {R}\) such that \(\dot{{{\mathbf {x}}}} = \lambda \mathbf {n}\). Define
Here \(\Vert {\cdot } \Vert \) denotes the norm and ‘\(\cdot \)’ the usual inner product on the Euclidean space \({\mathbb {R}}^d\). The functions \(\mathcal {C}_{1}\) and \(\mathcal {C}_{2}\) are assumed to be continuous on \({\mathbb {M}}\) and uniformly bounded from below by a positive constant \(\delta >0\). In applications, \(\mathcal {C}_{1}\) and \(\mathcal {C}_{2}\) are chosen so as to favor paths which remain close to regions of interest, e.g., along blood vessels in retinal images, see Fig. 1. Note that their physical units are distinct: if one wishes \(d_\mathcal {F}\) to have the dimension [T] of a travel time, then \(\mathcal {C}_1^{-1}\) is a physical, (strictly) spatial velocity \([\mathrm {Length}][T]^{-1}\), and \( \mathcal {C}_2^{-1}\) is an angular velocity \([\mathrm {Rad}][T]^{-1}\). For simplicity one often sets \(\mathcal {C}_{1} = \xi \mathcal {C}_{2}\), where \(\xi ^{-1}>0\) is a unit of spatial length. The special case \(\mathcal {C}_1({\mathbf {p}}) = \xi \mathcal {C}_2({\mathbf {p}}) = \xi \) for all \({\mathbf {p}}\in {\mathbb {M}}\) is referred to as the uniform cost case.
1.3 The Eikonal Equation and the Fast-Marching Algorithm
We compute the distance map to a point source on a volume using the relation to eikonal equations. Let \({\mathbf {p}}_\mathrm{S}\in {\mathbb {M}}\) be an arbitrary source point, and let U be the associated distance function
Then U is the unique viscosity solution [18, 19] to the eikonal PDE:
Here \(\mathcal {F}^*\) is the dual metric of \(\mathcal {F}\) and \({\mathrm {d}}U\) is the differential of the distance map U. However, for these relations to hold, and for numerical discretization to be practical, \(\mathcal {F}\) should be at least continuous.Footnote 2 We therefore propose in Sect. 2.3 for both \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\) an approximating metric that we denote by \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\), respectively, that are continuous and converge to \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\) as \(\varepsilon \rightarrow 0\). The approximating metrics correspond to a highly anisotropic Riemannian and Finslerian metric, rather than a sub-Riemannian or sub-Finslerian metric. The metric \(\mathcal {F}_{\epsilon }\) is in line with previous approximations [8, 16, 53] for the case \(d=2\).
We design a monotone and causal discretization scheme for static Hamilton–Jacobi PDE (5), which allows to apply an efficient, single-pass fast-marching algorithm [59]. Let us emphasize that designing a causal discretization scheme for (5) is non-trivial, because its local connectivity needs to obey an acuteness property [55, 61] depending on the geometry defined by \(\mathcal {F}\). We provide constructions for the metrics \(\mathcal {F}_\varepsilon \) or \(\mathcal {F}_\varepsilon ^+\) of interest, based on the earlier works [40, 41].
1.4 Shortest Paths and Minimal Distances in Medical Images
The application of the Hamilton–Jacobi framework for finding shortest paths has been shown to be useful for vessel tracking in retinal images [8], see Fig. 3 (top, right). The computational advantage of the fast-marching solver over the numerical method in [8] in this setting was demonstrated by Sanguinetti et al. [53]. A related approach using fast marching with elastica functionals can be found in [14, 15]. The sub-Riemannian approach by Bekkers et al. [8] concerns the two-dimensional Reeds–Shepp car model with reverse gear, where 2D grayscale images are first lifted to an orientation score [23] defined on the higher-dimensional manifold \(\mathbb {R}^2 \times \mathbb {S}^1\). There, the combination of the sub-Riemannian metric, the cost function derived from the orientation score, and the numerical anisotropic fast-marching solver, provided a solid approach to accurately track vessels in challenging sets of images.
In the previous works [8, 9] the clear advantage of sub-Riemannian geometrical models over isotropic Riemannian models on \({\mathbb {R}}^{2}\times \mathbb {S}^{1}\) has been shown with many experiments.Footnote 3
In this work we will show similar benefits for our sub-Riemannian tracking in \({\mathbb {R}}^{3}\times \mathbb {S}^{2}\). In general, regardless of the choice of image dimension \(d \in \{2,3\}\), one has that our extension of the Hamilton–Jacobi framework from the conventional base manifold of position space only (i.e., \({\mathbb {R}}^d\)) to the base manifold of positions and orientations (i.e., \({\mathbb {R}}^d \times \mathbb {S}^{d\!-\!1}\)) generically deals with the ‘leakage problem’ where wavefronts leak at crossings in the conventional eikonal frameworks acting directly in the image domain. See Fig. 4 where our solution to the ‘leakage problem’ is illustrated for \(d=2\).
Regarding image analysis applications, we propose to use the same strategy of sub-Riemannian and Finslerian tracking above the extended base manifold \({\mathbb {R}}^{3} \times \mathbb {S}^{2}\) of positions and orientations for fiber tracking and structural connectivity in brain white matter in diffusion-weighted MRI data. For diffusion-weighted MRI images, a signal related to the amount of diffusion of water molecules is measured, which in the case of neuroimages is considered to reflect the structural connectivity in brain white matter. The images can in a natural way be considered to have domain \(\varOmega \subset \mathbb {R}^3 \times \mathbb {S}^2\). Figure 3 (bottom) illustrates such images. On the left we use a glyph visualization that shows a surface for each grid point, where the distance from the surface to the corresponding grid point \(\mathbf {x}\) is proportional to the data value \(U(\mathbf {x},\mathbf {n})\) and the coloring is related to the orientation \(\mathbf {n} \in \mathbb {S}^{2}\). As such the dMRI data already provide a distribution on \({\mathbb {R}}^{3}\times \mathbb {S}^2\) and do not require an ‘orientation score’ as depicted in Figs. 1 and 4.
A large number of tractography methods exist that are designed to estimate/approximate the fiber paths in the brain based on dMRI data. Most of these methods construct tracks that locally follow the structure of the data, see, e.g., [20, 58] or references in [34]. More related to our approach are geodesic methods that have the advantage that they minimize a functional, and thereby are less sensitive to noise and provide a certain measure of connectivity between regions. These methods can be based on diffusion tensors in combination with Riemannian geometry on position space, e.g., [30, 33, 35]. One can also make use of the more general Finsler geodesic tracking to include directionality [38, 39] and use high angular resolution data (HARDI), examples of which can be found in [5, 54]. Recently, a promising method has been proposed, based on geodesics on the full position and orientation space using a data-adaptive Riemannian metric [47]. We also work on this joint space of positions and orientations, but use either Riemannian or asymmetric Finsler metrics that are highly anisotropic that we solve by a numerical fast-marching method that is able to deal with this high anisotropy. We show on artificial datasets how our method can be employed to give shortest paths between two regions w.r.t the imposed Finsler metric and that these paths correctly follow the bundle structure.
1.5 Contributions and Outline
The extension to 3D of the Reeds–Shepp car model and the adaptation to model shortest paths for cars that cannot move backward are new and provide an interesting collection of new theoretical and practical results:
-
In Theorem 1 we show that the Reeds–Shepp model is globally and locally controllable, and that the Reeds–Shepp model without reverse gear is globally but not locally controllable. Hence, the distance map loses continuity.
-
We introduce regularizations \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) of the Finsler metrics \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\), which make our numerical discretization possible. We show that both the corresponding distances converge to \(d_{\mathcal {F}_0}\) and \(d_{\mathcal {F}_0^+}\) as \(\varepsilon \rightarrow 0\) and the minimizing curves converge to the ones for \(\varepsilon = 0\), see Theorem 2.
-
We present and prove for \(d =2\) and uniform cost a theorem that describes the occurrence of cusps for the sub-Riemannian model using \(\mathcal {F}_0\), and that using \(\mathcal {F}_0^+\) leads to geodesics that are a concatenation of purely angular motion, a sub-Riemannian geodesic without cusps and again a purely angular motion. We call the positions where in-place rotation (or purely angular motion) takes place keypoints. For uniform cost, we show that the only possible keypoints are the begin and endpoints, and for many end conditions we can describe how this happens. The precise theoretical statement and proof are found in Theorem 3.
-
Furthermore, we show in Theorem 4 how the geodesics can be obtained from the distance map, for a general Finsler metric, and in the more specific cases that we use in this paper. For our cases of interest, we show that backtracking of geodesics is either done via a single intrinsic gradient descent (for the models with reverse gear) or via two intrinsic gradient descents (for the model without reverse gear).
-
For our numerical experiments we make use of a fast-marching implementation, for \(d = 2\) introduced in [41]. In Section 6 we give a summary of the numerical approach for \(d = 3\), but a detailed discussion of the implementation and an evaluation of the accuracy of the method are beyond the scope of this paper and will follow in future work. For \(d = 2\), we show an extensive comparison between the models with and without reverse gear for uniform cost, to illustrate the useful principle of the keypoints and to show the qualitative difference between the two models. In examples with non-uniform cost, see for example the top row of Fig. 3, we show that the model places the keypoints optimally at corners/bifurcations in the data, where the in-place rotation forms a natural, automatic ‘re-initialization’ of the tracking.
For \(d = 3\), we give several examples to show the influence of the model parameters, in particular the cost parameter. The examples indicate that the method adequately deals with crossing or kissing structures.
Outline In Sect. 2, we give a detailed overview of the theoretical results of the paper. Theorems 1, 3 and 4 are discussed and proven in Sects. 3, 4 and 5, respectively. The reader who is primarily interested in the application of the methods may choose to skip these three sections. The proof of Theorem 2 is given in ‘Appendix A’ section. We discuss the numerics briefly in Sect. 6. Section 7 contains all experimental results. Conclusion and discussion follow in Sect. 8. For an overview of notations, ‘Appendix D’ section may be helpful.
2 Main Results
In this section, we state formally the mathematical results announced in Sect. 1. Some preliminaries regarding the distance function are introduced in the section below. Results regarding the exact Reeds–Shepp car models are gathered in Sect. 2.2. The description of the approximate models and the related convergence results appears in Sect. 2.3. Analysis of special interest points (cusps and keypoints) is done in Sect. 2.4. Results on the eikonal equation and subsequent backtracking of minimizing geodesics via intrinsic gradients are presented in Sect. 2.5.
2.1 Preliminaries on the (Quasi-)Distance Function and Underlying Geometry
Geometries on the manifold of states \({\mathbb {M}} = \mathbb {R}^d \times \mathbb {S}^{d-1}\) are defined by means of Finsler metrics which are functions \(\mathcal {F}: T({\mathbb {M}}) \rightarrow [0,+\infty ]\). On each tangent space, the metric should be 1-homogeneous, convex and quantitatively non-degenerate with a uniform constant \(\delta >0\): for all \({\mathbf {p}}=(\mathbf {x},\mathbf {n})\in {\mathbb {M}}\), \(\dot{{\mathbf {p}}}, \dot{{\mathbf {p}}}_0,\dot{{\mathbf {p}}}_1\in T_{\mathbf {p}}({\mathbb {M}})\) and \(\lambda \ge 0\):
A weak regularity property is required as well, see the next remark. The induced distance \(d_\mathcal {F}\), defined in (1), obeys \(d_\mathcal {F}({\mathbf {p}}, {\mathbf {q}}) = 0\) iff \({\mathbf {p}}= {\mathbf {q}}\) and obeys the triangle inequality. However, unlike a regular distance, \(d_\mathcal {F}\) needs not be finite, or continuous, or symmetric in its arguments. Note that \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\) as defined in (2) and (3), respectively, indeed satisfy the properties in (6).
Remark 2
In contrast to the more common definition of Finsler metrics, we will not assume the Finsler metric to be smooth on \(T(\mathbb {M}) \setminus (\mathbb {M} \times \{\mathbf {0}\})\) but use a weaker condition instead. Following [13], we require that the sets
are closed and vary continuously with respect to the point \({\mathbf {p}}\in {\mathbb {M}}\) in the sense of the Hausdorff distance. The sets \(\mathcal {B}_\mathcal {F}({\mathbf {p}})\) are illustrated in Fig. 2 for the models of interest. The condition implies that a shortest path exists from \({\mathbf {p}}\) to \({\mathbf {q}}\in {\mathbb {M}}\) whenever \(d_\mathcal {F}({\mathbf {p}}, {\mathbf {q}})\) is finite, and is used to prove convergence results in ‘Appendix A’ section.
A common technique in optimal control theory is to reformulate the shortest path problem defining the distance \(d_\mathcal {F}({\mathbf {p}}, {\mathbf {q}})\) into a time optimal control problem. That is, for \(p \in [1,\infty ]\) one has by Hölder’s (in)equality, time reparameterization, and by 1-homogeneity of \(\mathcal {F}\) in its 2nd entry, that:
where \(\varGamma _T :={{\mathrm{Lip}}}([0,T],{\mathbb {M}})\), and with \(\mathcal {B}_\mathcal {F}({\mathbf {p}})\) as defined in (7). The latter reformulation is used in ‘Appendix A’ section to prove convergence results via closedness of controllable paths and Arzela–Ascoli’s theorem, based on a general result originally applied to Euler elastica curves in [13].
In the special case \(\mathcal {F}=\mathcal {F}_{0}\) the geodesics are SR geodesics, where \(\mathcal {F}_0\) is obtained by the square root of quadratic form associated with a SR metric \(\left. \mathcal {G}_0\right| _{{\mathbf {p}}}(\cdot ,\cdot )=\mathcal {F}_{0}({\mathbf {p}},\cdot )^2\) on a SR manifold \((\mathbb {M}, \Delta , \mathcal {G}_0)\), where \(\Delta \subset T(\mathbb {M})\) is a strict subset of allowable tangent vectors that comes along with the horizontality constraint
that arises from (2). For details on the case \(d = 2\) see [11, 52], for \(d =3\) see [25].
Finally, we note that for the uniform cost case (\(\xi ^{-1}\mathcal {C}_{1}=\mathcal {C}_{2}=1\)), the problem is covariant with respect to rotations and translations. For the data-driven case, such covariance is only obtained when simultaneously rotating the data-driven cost factors \(\mathcal {C}_{1}, \mathcal {C}_{2}\). Therefore, only in the uniform cost case, for \(d=2,3\), we shall use a reference point (‘the origin’) \(\mathbf {e} \in {\mathbb {R}}^{d} \times \mathbb {S}^{d-1}\). To adhere to common conventions we use
2.2 Controllability of the Reeds–Shepp Model
A model \(({\mathbb {M}}, d_\mathcal {F})\) is globally controllable if the distance \(d_\mathcal {F}\) takes finite values on \({\mathbb {M}}\times {\mathbb {M}}\); in other words, a car can go from any place on the manifold to any other place in finite time. In Theorem 1 we show that this is indeed the case for \(\mathcal {F}= \mathcal {F}_0\) and \(\mathcal {F}= \mathcal {F}_0^+\), given in (2) and (3). Local controllability is satisfied when \(d_\mathcal {F}\) satisfies a certain continuity requirement: if \({\mathbf {p}}\rightarrow {\mathbf {q}}\in (\mathbb {M},\Vert {\cdot }\Vert )\), with \(\Vert {\cdot }\Vert \) denoting the standard (flat) Euclidean norm on \(\mathbb {M}={\mathbb {R}}^{d} \times \mathbb {S}^{d-1}\), we must have \(d_{\mathcal {F}}({\mathbf {p}},{\mathbf {q}}) \rightarrow 0\). We prove in Theorem 1 that the metric space \(({\mathbb {M}}, d_{\mathcal {F}_0})\) is locally controllable, but the quasi-metric space \(({\mathbb {M}},d_{\mathcal {F}_0^+})\) is not. Indeed the SR Reeds–Shepp car can achieve sideways motions by alternating the forward and reverse gear with slight direction changes, whereas the model without reverse gear lacks this possibility. For completeness, the theorem contains a standard (rough) estimate of the distance near the source (due to well-known estimates [16, 31, 49, 57]).
Furthermore, we prove existence of minimizers for the Reeds–Shepp model without reverse gear. Existence results of minimizers of the model with reverse gear (the SR model) already exist, by the Chow–Rashevsky theorem and Filippov theorems [2].
Theorem 1
((Local) controllability properties) Minimizers exist for both the classical Reeds–Shepp model and for the Reeds–Shepp model without reverse gear. Both models are globally controllable.
-
The Reeds–Shepp model without reverse gear is not locally controllable, since
$$\begin{aligned} \limsup _{{\mathbf {p}}' \rightarrow {\mathbf {p}}} d_{\mathcal {F}^+_0}({\mathbf {p}},{\mathbf {p}}')\ge 2 \pi \delta , \text { for all }{\mathbf {p}}\in \mathbb {M}. \end{aligned}$$(12)If the cost \(\mathcal {C}_{2}=\delta \) is constant on \({\mathbb {M}}\), then this inequality is sharp:
$$\begin{aligned} \limsup _{{\mathbf {p}}' \rightarrow {\mathbf {p}}} d_{\mathcal {F}^+_0}({\mathbf {p}},{\mathbf {p}}') = \lim _{\mu \downarrow 0} d_{\mathcal {F}_0^+} (({{\mathbf {x}}},\mathbf {n}),\, ({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n})) = 2 \pi \delta . \end{aligned}$$(13) -
The sub-Riemannian Reeds–Shepp model is locally controllable, since
$$\begin{aligned} d_{\mathcal {F}_0}({\mathbf {p}},{\mathbf {p}}')&= \mathcal {O}\left( \mathcal {C}_{2}({\mathbf {p}}) \Vert \mathbf {n}-\mathbf {n}'\Vert + \sqrt{\mathcal {C}_{2}({\mathbf {p}})\mathcal {C}_{1}({\mathbf {p}})\Vert {{\mathbf {x}}}-{{\mathbf {x}}}'\Vert }\right) \ \nonumber \\&\text { as } {\mathbf {p}}'=({{\mathbf {x}}}',\mathbf {n}') \rightarrow {\mathbf {p}}=({{\mathbf {x}}},\mathbf {n}). \end{aligned}$$(14)
For a proof see Sect. 3.
2.3 A Continuous Approximation for the Reeds–Shepp Geometry
We introduce approximations \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) of the Finsler metrics \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\), depending on a small parameter \(0< \varepsilon \le 1\), which are continuous and in particular take only finite values. This is a prerequisite for our numerical methods. Both approximations penalize the deviation from the constraints of collinearity \(\dot{{{\mathbf {x}}}} \propto \mathbf {n}\), and in addition, \(\mathcal {F}_\varepsilon ^+\) penalizes negativity of the scalar product \(\dot{{{\mathbf {x}}}} \cdot \mathbf {n}\), appearing in (2) and (3). For that purpose, we introduce some additional notation: for \(\dot{{{\mathbf {x}}}} \in \mathbb {R}^d\) and \(\mathbf {n}\in \mathbb {S}^{d-1}\) we define
These are, respectively, the norm of the orthogonal projectionFootnote 4 of \(\dot{{{\mathbf {x}}}}\) onto the plane orthogonal to \(\mathbf {n}\) and the negative and positive parts of their scalar product. The two metrics \(\mathcal {F}_\varepsilon ,\mathcal {F}_\varepsilon ^+ : T({\mathbb {M}}) \rightarrow \mathbb {R}_+\) are defined for each \(0<\varepsilon \le 1\), as follows: for \(({\mathbf {p}}, \dot{{\mathbf {p}}}) \in T({\mathbb {M}})\) with components \({\mathbf {p}}= ({{\mathbf {x}}},\mathbf {n})\) and \(\dot{{\mathbf {p}}} = (\dot{{{\mathbf {x}}}},\dot{\mathbf {n}})\) we define
See Fig. 5 for a visualization of a level set of both metrics in \({\mathbb {R}}^2 \times \mathbb {S}^1\). Note that \(\mathcal {F}_\varepsilon \) is a Riemannian metric on \({\mathbb {M}}\) (with the same smoothness as the cost functions \(\mathcal {C}_{2}, \mathcal {C}_{1}\)), and that \(\mathcal {F}_\varepsilon ^+\) is neither Riemannian nor smooth due to the term \((\dot{{{\mathbf {x}}}} \cdot \mathbf {n})_-\). One clearly has the pointwise convergence \(\mathcal {F}_\varepsilon ({\mathbf {p}}, \dot{{\mathbf {p}}}) \rightarrow \mathcal {F}_0({\mathbf {p}}, \dot{{\mathbf {p}}})\) as \(\varepsilon \rightarrow 0\), and likewise \(\mathcal {F}^+_\varepsilon ({\mathbf {p}}, \dot{{\mathbf {p}}}) \rightarrow \mathcal {F}^+_0({\mathbf {p}}, \dot{{\mathbf {p}}})\). The use of \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) is further justified by the following convergence result.
Theorem 2
(Convergence of the approximative models to the exact models) One has the pointwise convergence: for any \({\mathbf {p}},{\mathbf {q}}\in {\mathbb {M}}\)
Consider for each \(\varepsilon >0\) a minimizing path \(\gamma _\varepsilon ^*\) from \({\mathbf {p}}\) to \({\mathbf {q}}\), with respect to the metric \(\mathcal {F}_\varepsilon \), parameterized at constant speed
Assume that there is a unique shortest path \(\gamma ^*\) from \({\mathbf {p}}\) to \({\mathbf {q}}\) with respect to the sub-Riemannian distance \(d_{\mathcal {F}_{0}}\) (in other words \({\mathbf {q}}\) is not within the cut locus of \({\mathbf {p}}\)), parameterized at constant speed:
Then \(\gamma _{\varepsilon }^* \rightarrow \gamma ^*\) as \(\varepsilon \rightarrow 0\), uniformly on [0, 1]. Likewise replacing \(\mathcal {F}_{\varepsilon }\) with \(\mathcal {F}_{\varepsilon }^+\) for all \(\varepsilon \ge 0\).
The proof, presented in ‘Appendix A’ section, is based on a general result originally applied to the Euler elastica curves in [13]. Combining Theorem 2 with the local controllability properties established in Theorem 1, one obtains that \(d_{\mathcal {F}_\varepsilon } \rightarrow d_{\mathcal {F}_0}\) locally uniformly on \({\mathbb {M}} \times {\mathbb {M}}\), and that the convergence \(d_{\mathcal {F}_\varepsilon ^+} \rightarrow d_{\mathcal {F}_0^+}\) is only pointwise.
Remark 3
If there exists a family of minimizing geodesics \((\gamma _i^*)_{i \in I}\) from \({\mathbf {p}}\) to \({\mathbf {q}}\) with respect to \(\mathcal {F}_0\) (resp. \(\mathcal {F}_0^+\)), then one can show that for any sequence \(\varepsilon _n \rightarrow 0\) one can find a subsequence and an index \(i \in I\) such that \(\gamma _{\varepsilon _{\varphi (n)}}^* \rightarrow \gamma _i^*\) uniformly as \(n\rightarrow \infty \).
2.4 Points of Interest in Spatial Projections of Geodesics for the Uniform Cost Case: Cusps Versus Keypoints
Next we provide a theorem that tells us in each of the models/metric spaces \((\mathbb {M},d_{\mathcal {F}_{0}})\), \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) and \((\mathbb {M},d_{\mathcal {F}_{0}^+})\), \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^+})\), with \(\mathcal {C}_1=\mathcal {C}_2=1\) and \(d=2\) where cusps occur in spatial projections of geodesics or where keypoints with in-place rotations take place. Recall Fig. 2 for a geometric illustration of the specific behavior of the path at such points. In Theorem 3, we provide an analysis of the occurrence of these points for the uniform cost case.
Note that for vessel tracking (or fiber tracking) applications, cusps are not wanted as they are unnatural for vessels (or fibers), whereas keypoints are only desirable at bifurcations of vessels. In the data-driven case, the practical advantage of the forward-only model resulting in keypoints instead of cusps can indeed be observed (see, e.g., Figs. 13, 14).
Definition 2
(Cusp) A cusp point \(\mathbf {x}(t_0)\) on a spatial projection of a (SR) geodesic \( t \mapsto (\mathbf {x}(t),\mathbf {n}(t))\) in \(\mathbb {M}\) is a point where
That is, a cusp point is a point where the spatial control aligned with \(\mathbf {n}(t_0)\) vanishes and switches sign locally.
Although this definition explains the notion of a cusp geometrically (as can be observed in Figs. 2, 6), it contains a redundant part for the relevant case of interest: the second condition automatically follows when considering the SR geodesics in \((\mathbb {M}, d_{\mathcal {F}_{0}})\). The following lemma gives a characterization of a cusp point in terms of the distance function along a curve.
Lemma 1
Consider a SR geodesic \(\gamma =(\mathbf {x},\mathbf {n}) : [0,1] \rightarrow (\mathbb {M}, d_{\mathcal {F}_{0}})\), parameterized at constant speed, and which physical position \({{\mathbf {x}}}(\cdot )\) is not identically constant. Denote \({\mathbf {p}}_S := \gamma (0)\) and \(U(\cdot ):= d_{\mathcal {F}_{0}}({\mathbf {p}}_{S},\cdot )\). Let \(t_0\in (0,1)\) be such that U is differentiable at \(\gamma (t_0) =(\mathbf {x}(t_0),\mathbf {n}(t_0))\). Then
The proof can be found in ‘Appendix C’ section.
Definition 3
(Keypoint) A point \(\tilde{{{\mathbf {x}}}}\) on the spatial projection of a geodesic \(\gamma (\cdot ) = ({{\mathbf {x}}}(\cdot ),\mathbf {n}(\cdot ))\) in \({\mathbb {M}}\) is a keypoint of \(\gamma \) if there exist \(t_0 < t_1\), such that \({{\mathbf {x}}}(t) = \tilde{{{\mathbf {x}}}}\) and \(\dot{\mathbf {n}}(t) \ne 0\) for all \(t \in [t_0,t_1]\), i.e., a point where an in-place rotation takes place.
Definition 4
We define the set to be all endpoints that can be reached with a geodesic \(\gamma ^*:[0,1] \rightarrow \mathbb {M}\) in \((\mathbb {M},d_{\mathcal {F}_0})\) whose spatial control \(\tilde{u}(t)\) stays positive for all \(t \in [0,1]\).
Remark 4
The word ‘geodesic’ in this definition can (in the case \(d = 2\)) be replaced by ‘globally minimizing geodesic’ [11]. For a definition in terms of the exponential map of a geometrical control problem \(\mathbf {P}_{curve}\), see, e.g., [22, 24], in which the same positivity condition for \(\tilde{u}\) is imposed. Figure 7 shows more precisely what this set looks like for \(d = 2\) [22], in particular, that it is contained in the half-space \(\mathbf {a} \cdot \mathbf {x} \ge 0\), and for \(d = 3\) [24]. We extend these results with the following theorem.
Theorem 3
(Cusps and Keypoints) Let \(\varepsilon >0\), \(d=2\), \(\mathcal {C}_{1}=\mathcal {C}_{2}=1\). Then,
-
in \((\mathbb {M},d_{\mathcal {F}_{0}})\) cusps are present in spatial projections of almost every optimal SR geodesics when their times t are extended on the real line (until they lose optimality). The straight lines connecting specific boundary points \(\mathbf {p}=(\mathbf {x},\mathbf {n})\) and \(\mathbf {q}=(\mathbf {x}+ \lambda \mathbf {n},\mathbf {n})\) with \(\lambda \in {\mathbb {R}}\) are the only exceptions.
-
in \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^+})\) and \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) and \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) no cusps appear in spatial projections of geodesics.
Furthermore,
-
in \((\mathbb {M},d_{\mathcal {F}_{0}})\), \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) and \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^+})\) keypoints only occur with vertical geodesics (moving only angularly).
-
in \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) keypoints only occur at the endpoints of shortest paths.
A minimizing geodesic \(\gamma _{+}\) in \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) departing from \(\mathbf {e}=(0,0,0)\) and ending in \({\mathbf {p}}=(x,y,\theta )\) has
-
(A)
no keypoint if ,
-
(B)
a keypoint in (0, 0) if \(x<0\),
-
(C)
a keypoint only in (x, y) ifFootnote 5
-
(C1)
and \(x \ge 2\),
-
(C2)
and \(0 \le x < 2\) and
\(|y| \le -i x \, E \left( i \text {arcsinh}\left( \frac{x}{\sqrt{4-x^2}}\right) , \frac{x^2-4}{x^2} \right) \), where E(z, m) denotes the Elliptic integral of the second kind.
-
(C1)
Remark 5
In case A, \(\gamma _{+}\) is a minimizing geodesic in \((\mathbb {M},d_{\mathcal {F}_0})\) as well. In case B, \(\gamma _{+}\) departs from a cusp. In case C, \(\gamma ^+\) is a concatenation of a minimizing geodesic in \((\mathbb {M},d_{\mathcal {F}_0})\) and an in-place rotation. For other endpoints \((x,y,\theta )\) for geodesics departing from \(\mathbf {e}\) with \(0\le x < 2\), other than the ones reported in C2 it is not immediately clear what happens, due to [22, Theorem 9]. Also points with \(x<0\) may have keypoints at the end as well. See Fig. 8 where various cases of minimizing geodesics in \((\mathbb {M},d_{\mathcal {F}_0^+})\) are depicted.
Remark 6
See [27, Fig. 6] to see the smoothing effect of taking \(\varepsilon \) small but nonzero on the cusps of non-optimal geodesics in \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) and keypoints in \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^{+}})\).
2.5 The Eikonal PDE Formalism
As briefly discussed in Sect. 1.3, continuous metrics like \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) for any \(\varepsilon >0\) allow to use the standard theory of viscosity solutions of eikonal PDEs and thus to design provable and efficient numerical schemes for the computation of distance maps and minimizing geodesics. More precisely, consider a continuous Finsler metric \(\mathcal {F}\in C^0(T({\mathbb {M}}),{\mathbb {R}}^+)\), and define the dual \(\mathcal {F}^*\) on the co-tangent bundle as follows: for all \(({\mathbf {p}}, \hat{{\mathbf {p}}}) \in T^*({\mathbb {M}})\)
The distance map \(U = d_\mathcal {F}({\mathbf {p}}_\mathrm{S}, \cdot )\) from a given source point \({\mathbf {p}}_\mathrm{S}\in {\mathbb {M}}\) is the unique solution, in the sense of viscosity solutions, of the static Hamilton Jacobi equation: \(U({\mathbf {p}}_\mathrm{S})=0\), and for all \(p \in {\mathbb {M}}\)
Furthermore, if \(\gamma \) is a minimizing geodesic from \({\mathbf {p}}_\mathrm{S}\) to some \({\mathbf {p}}\in {\mathbb {M}}\), then it obeys the ordinary differential equation (ODE):
for any \(t\in [0,1]\) such that the differentiability of U and \(\mathcal {F}^*\) holds at the required points. The proof of ODE (23) is for completeness derived in Proposition 4 of ‘Appendix B’ section, where we also discuss in Remark 14 the common alternative formalism based on the Hamiltonian. We denoted by \(\mathrm{d}_{\hat{{\mathbf {p}}}} \mathcal {F}^*\) the differential of the dual Finsler metric \(\mathcal {F}^*\) with respect to the second variable \(\hat{{\mathbf {p}}}\); hence, \(\mathrm{d}_{\hat{{\mathbf {p}}}} \mathcal {F}^*({\mathbf {p}}, \hat{{\mathbf {p}}}) \in T^{**}_{\mathbf {p}}({\mathbb {M}}) \cong T_{\mathbf {p}}({\mathbb {M}})\) is indeed a tangent vector to \({\mathbb {M}}\), for all \(({\mathbf {p}},\hat{{\mathbf {p}}})\in T^*{\mathbb {M}}\).
In the rest of this section, we specialize (22) and (23) to the Finsler metrics \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\). Our first result provides explicit expressions for the dual Finsler metrics (required for the eikonal equation).
Proposition 1
For any \(0<\varepsilon \le 1\), the duals to the approximating Finsler metrics \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) are: for all \(({\mathbf {p}},\hat{{\mathbf {p}}}) \in T^*({\mathbb {M}})\), with \({\mathbf {p}}= ({{\mathbf {x}}},\mathbf {n})\) and \(\hat{{\mathbf {p}}}= (\hat{{{\mathbf {x}}}}, \hat{\mathbf {n}})\)
In order to relate Finslerian HJB equation (22) and backtracking equation (23) to some more classical Riemannian counterparts, we introduce two Riemannian metric tensor fields on \({\mathbb {M}}\). The first is defined as the polarization of the norm \(\mathcal {F}_\varepsilon ({\mathbf {p}},\cdot )\)
where \(\dot{{\mathbf {p}}}=(\dot{\mathbf {x}},\dot{\mathbf {n}})\), and then one can also rely on gradient fields \({\mathbf {p}}\mapsto \mathcal {G}_{{\mathbf {p}}; \varepsilon }^{-1}\mathrm{d}U({\mathbf {p}})\) relative to this metric tensor. This has benefits if it comes to geometric understanding of the eikonal equation and its tracking. Even in the analysis of the non-symmetric case—where one does not have a single metric tensor—this notion plays a role, as we will see in the next main theorem. To this end, in the non-symmetric case, we shall rely on a second spatially isotropic metric tensor given by:
We denote by \(\nabla _{\mathbb {S}^{d-1}}\) the gradient operator on \(\mathbb {S}^{d-1}\) with respect to the inner product induced by the embedding \(\mathbb {S}^{d-1} \subset \mathbb {R}^d\) and by \(\nabla _{\mathbb {R}^d}\) the canonical gradient operator on \(\mathbb {R}^d\).
Corollary 1
Let \(\varepsilon \ge 0\). Then eikonal PDE (5) for the case \((\mathbb {M},\mathcal {F}_\varepsilon )\) takes the form
Eikonal PDE (5) for the case \((\mathbb {M},\mathcal {F}_\varepsilon ^+)\) now takes the explicit form:
for those \(\mathbf {p} \in \mathbb {M}_+ \cup \mathbb {M}_{-}\) where \(U^{+}\) is differentiable.Footnote 6
The proof of Proposition 1 and Corollary 1 can be found in Sect. 5.
We finally specialize geodesic ODE (23) to the models of interest. Note that for the model \(({\mathbb {M}},d_{\mathcal {F}_\varepsilon ^+})\), the backtracking switches between qualitatively distinct modes, respectively, almost sub-Riemannian and almost purely angular, in the spirit of Theorem 3. Given \(\varepsilon > 0\) and \(\mathbf {n}\in \mathbb {S}^{d-1}\) let \(D_\mathbf {n}^\varepsilon \) denote the \(d \times d\) symmetric positive definite matrix with eigenvalue 1 in the direction \(\mathbf {n}\) and eigenvalue \(\varepsilon ^2\) in the orthogonal directions :
Theorem 4
(Backtracking) Let \(0< \varepsilon <1\). Let \({\mathbf {p}}_\mathrm{S}\in {\mathbb {M}}\) be a source point. Let \(U({\mathbf {p}}):= d_{\mathcal {F}_{\varepsilon }}({\mathbf {p}},{\mathbf {p}}_s)\), \(U^+({\mathbf {p}}):=d_{\mathcal {F}_{\varepsilon }^+}({\mathbf {p}},{\mathbf {p}}_s)\) be distance maps from \({\mathbf {p}}_{s}\), w.r.t. the Finsler metric \(\mathcal {F}_{\varepsilon }\), and \(\mathcal {F}_{\varepsilon }^+\). Let \(\gamma , \gamma ^+:[0,1] \rightarrow \mathbb {M}\) be normalized geodesics of length L starting at \({\mathbf {p}}_s\) in \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) resp. \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^+})\). Let time \(t \in [0,1]\).
For the Riemannian approximation paths of the Reeds–Shepp car we have, provided that U is differentiable at \(\gamma (t)=(\mathbf {x}(t),\mathbf {n}(t))\), that
For the approximation paths of the car without reverse gear we have, provided that \(U^+\) is differentiable at \(\gamma ^+(t) = ({{\mathbf {x}}}^+(t),\mathbf {n}^+(t))\), that
with \(\widetilde{\mathcal {G}}_{{\mathbf {p}}; \varepsilon }(\dot{{\mathbf {p}}},\dot{{\mathbf {p}}})\) given by (26), with disjoint Riemannian manifold splitting \(\mathbb {M}=\mathbb {M}_{+} \cup \mathbb {M}_{-} \cup \partial \mathbb {M}_{\pm }\). Manifold \(\mathbb {M}_{+}\) is equipped with metric tensor \(\mathcal {G}_{\varepsilon }\), \(\mathbb {M}_{-}\) is equipped with metric tensor \(\widetilde{\mathcal {G}}_{\varepsilon }\) and
denotes the transition surface (surface of keypoints).
Remark 7
General abstract formula (29) reflects that the backtracking in \((\mathbb {M},\mathcal {F}^{+})\) is a combined gradient descent flow on the distance map \(U^+\) on a splitting of \(\mathbb {M}\) into two (symmetric) Riemannian manifolds. Its explicit form (likewise (28)) is
Note that for the (less useful) isotropic case \(\varepsilon =1\), \(\mathcal {F}_{1}\) and \(\mathcal {F}_{1}^+\) coincide and geodesics consist of straight lines \(\mathbf {x}(\cdot )\) in \({\mathbb {R}}^{d}\) and great circles \(\mathbf {n}(\cdot )\) in \(\mathbb {S}^{d}\) that do not influence each other.
Remark 8
In Theorem 4, we assumed distance maps U and \(U^+\) to be differentiable along the path, which is not always the case. In points where the distance map is not differentiable, one can take any subgradient in the subdifferential \(\partial U(\mathbf {p})\) in order to identify Maxwell points (and Maxwell strata). In particular, in SR geometry, the set of points where the squared distance function \((d_{\mathcal {F}_0}(\cdot ,e))^2\) is smooth is open and dense in any compact subset of \({\mathbb {M}}\), see [1, Theorem 11.15]. The points where it is non-smooth are rare and meaningful: they are either first Maxwell points, conjugate points or abnormal points. The last type does not appear here, because we have a 2-bracket generating distribution, see, e.g., [25, Remark 4] and [1, Chap. 20.5.1.]. At points in the closure of the first Maxwell set, two geodesically equidistant wavefronts collide for the first time, see for example [8, Fig.3, Theorem 3.2] for the case \(d=2\) and \(\mathcal {C}=\mathcal {C}_{1}=\mathcal {C}_{2}=1\). See also Fig. 8, where for some end conditions 2 optimally backtracked geodesics end with the same length in such a first Maxwell point. The conjugate points are points where local optimality is lost, for a precise definition see, e.g., [1, Definition 8.43].
Remark 9
Recall the convergence result from Theorem 2, and the non-local-controllability for the model \((\mathbb {M},d_{\mathcal {F}_{0}^+})\). From this we see that the convergence holds pointwise but not uniformly. (Otherwise the limit distance \(d_{\mathcal {F}_0^+}\) was continuous.) Nevertheless the shortest paths converge strongly as \(\varepsilon \downarrow 0\), and we see that the spatial velocity tends to 0 in (31) if \(\varepsilon \downarrow 0\) if \(\gamma _{\varepsilon }^*(t) \in \mathbb {M}_{-}\). In the SR case \(\varepsilon =0\), the gradient flows themselves fit continuously and the interface \(\partial {\mathbb {M}}_{\pm }\) is reached with \(\dot{{{\mathbf {x}}}} \cdot \mathbf {n}= 0\) (and \(\dot{{{\mathbf {x}}}} = 0\)).
Theorem 4 can be extended to the SR case:
Corollary 2
(SR backtracking) Let the cost \(\mathcal {C}_1, \mathcal {C}_2\) be smooth, let the source \({\mathbf {p}}_S \in {\mathbb {M}}\) and \({\mathbf {p}}\ne {\mathbf {p}}_S \in {\mathbb {M}}\) be such that they can be connected by a unique smooth minimizer \(\gamma _\varepsilon ^*\) in \(({\mathbb {M}},\mathcal {F}_\varepsilon )\) and \(\gamma _0^*\) in \(({\mathbb {M}}, \mathcal {F}_0)\), such that \(\gamma _\varepsilon ^*(t)\) is not a conjugate point for all \(t \in [0,1]\) and all sufficiently small \(\varepsilon >0\), say \(\varepsilon < \varepsilon _0\), for some \(\varepsilon _0 > 0\). Then defining \(U_0: {\mathbf {q}}\in {\mathbb {M}} \mapsto d_{\mathcal {F}_\varepsilon }({\mathbf {p}}_s,{\mathbf {q}})\) one has
assuming \(U_0\) is differentiable at \(\gamma _0^*(t)\). In addition \(U_0\) satisfies the SR eikonal equation:
Proof
From our assumptions on \({\mathbf {p}}\) and \(\gamma _\varepsilon ^*(t)\) for \(\varepsilon < \varepsilon _0\), we have, recall Remark 8, that \((U_\varepsilon (\cdot ))^2\) is differentiable at \(\gamma _\varepsilon ^*(t)\) for all \(0 \le t \le 1\) and \(0 \le \varepsilon < \varepsilon _0\). This implies that \(U_\varepsilon \) is differentiable at \(\{\gamma _\varepsilon ^*(t) \; | \; 0 < t \le 1 \}\), for all \(0<\varepsilon < \varepsilon _0\).
From Theorem 2 we have pointwise convergence \(U_\varepsilon ({\mathbf {p}})\rightarrow U_0({\mathbf {p}})\) and uniform convergence \(\gamma _\varepsilon ^* \rightarrow \gamma _0^*\) as \(\varepsilon \downarrow 0\). Moreover, as \(\gamma _\varepsilon ^*\) and \(\gamma _0^*\) are solutions of the canonical ODEs of Pontryagin’s maximum principle, the trajectories are continuously depending on \(\varepsilon >0\) and so are the derivatives \(\dot{\gamma }_\varepsilon ^*\). As a result, we can apply backtracking Theorem 4 for \(\varepsilon > 0\) and take the limits:
Furthermore,
where we recall Corollary 1. Here due to our assumptions, \(U_\varepsilon \) and \(U_0\) are both differentiable at \({\mathbf {p}}\). Note that the limit for the inverse metric \({\mathcal {G}}_{{\mathbf {p}},\varepsilon }^{-1}\) as \(\varepsilon \downarrow 0\) exists, recall Corollary 1. \(\square \)
Now that we stated our 4 main theoretical results we will prove them in the subsequent sections (and ‘Appendix A’ section).
3 Controllability Properties: Proof of Theorem 1 and Maxwell Points in \((\mathbb {M},d_{\mathcal {F}_0^+})\)
(Global controllability) The two considered Reeds–Shepp models \((\mathbb {M},d_{\mathcal {F}_0})\) and \((\mathbb {M},d_{\mathcal {F}_0^+})\) are globally controllable, in the sense that the distances \(d_{\mathcal {F}_0}\) and \(d_{\mathcal {F}_0^+}\) take finite values on \({\mathbb {M}}\times {\mathbb {M}}\). This easily follows from the observation that any path \({{\mathbf {x}}}: [0,1] \rightarrow \mathbb {R}^d\), which time derivative \(\dot{{{\mathbf {x}}}} := \frac{\mathrm{d}{{\mathbf {x}}}}{\mathrm{d}t}\) is Lipschitz and non-vanishing, can be lifted into a path \(\gamma : [0,1]\rightarrow {\mathbb {M}}\) of finite length w.r.t. \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\), defined by \(\gamma (t):=({{\mathbf {x}}}(t),\dot{{{\mathbf {x}}}}(t)/\Vert \dot{{{\mathbf {x}}}}(t)\Vert )\) for all \(t\in [0,1]\). The fact that the infimum in (1) is actually a minimum for \(\mathcal {F}=\mathcal {F}_{0}^+\) follows by Corollary 3 in ‘Appendix A’ section and (9), and the fact that the quasi-distances take finite values.
(Local controllability) In order to show that the model \(({\mathbb {M}},d_{\mathcal {F}_0^+})\) is not locally controllable, we need the following lemma.
Lemma 2
Let \(\mathbf {n}:[0,\pi ] \rightarrow \mathbb {S}^{d-1}\) be strictly 1-Lipschitz. Then \(\int _{0}^{\pi } \mathbf {n}(0) \cdot \mathbf {n}(t)\, \mathrm{d}t>0\). Let \(\mathbf {n}:{\mathbb {R}}\rightarrow \mathbb {S}^{d-1}\) be strictly 1-Lipschitz and \(2\pi \)-periodic. Then all points \(\mathbf {n}(t)\) lay in a common strict hemisphere. In particular \(\mathbf {0} \notin \text {Hull}\{\mathbf {n}(t)\;|\; t \in [0,2\pi ] \}\).
Proof
The Lipschitzness assumption implies \(\mathbf {n}(0) \cdot \mathbf {n}(t) > \cos (t)\) for all \(t \in (0,\pi ]\) so \(\int _{0}^{\pi } \mathbf {n}(0) \cdot \mathbf {n}(t)\, \mathrm{d}t>0\).
Let \(\mathbf {n}:{\mathbb {R}}\rightarrow \mathbb {S}^{d-1}\) be strictly 1-Lipschitz and \(2\pi \)-periodic. Set \(\mathbf {M}:= \int _{0}^{2\pi }\mathbf {n}(t)\, \mathrm{d}t\). Then for any \(t_0 \in [0,2\pi ]\) one has by the two assumptions
so for all \(t_0\), we have \(\mathbf {n}(t_0) \in \{\mathbf {n} \in \mathbb {S}^{d-1} \;|\; \mathbf {n} \cdot \mathbf {M} >0\}\). \(\square \)
Now statements (12) and (13) on the non-local-controllability of \(({\mathbb {M}}, d_{\mathcal {F}^+_0})\) are shown in two steps.
Step 1: we show in the case of a constant cost function \(\mathcal {C}_{2}=\delta \) one has \(\limsup \limits _{{\mathbf {p}}' \rightarrow {\mathbf {p}}} d_{\mathcal {F}^+_0}({\mathbf {p}},{\mathbf {p}}') \le 2\pi \delta \), for any \({\mathbf {p}}\in {\mathbb {M}}\). Indeed, one can design an admissible curve in \((\mathbb {M}, \mathcal {F}_{0}^+)\) as the concatenation of an in-place rotation, a straight line and an in-place rotation. The length of the straight line is \(\mathcal {O}(\Vert {\mathbf {p}}'-{\mathbf {p}}\Vert )\) and vanishes when \({\mathbf {p}}' \rightarrow {\mathbf {p}}\), and the in-place rotations each have maximum cost \(\pi \delta \).
Step 2: we prove the lower bound \(\lim \limits _{\mu \downarrow 0} d_{\mathcal {F}_0^+}(({{\mathbf {x}}},\mathbf {n}),({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n})) \ge 2 \pi \delta \), for any \(({{\mathbf {x}}},\mathbf {n})\in {\mathbb {M}}\). This and the above-established upper bound implies the required result. As \(\mathcal {C}_{1}, \mathcal {C}_{2} \ge \delta \), we can restrict ourselves to the case of uniform cost \(\mathcal {C}_{1}=\mathcal {C}_{2}=\delta =1\) and just show equality (13), as estimate (12) follows by scaling with \(\delta \).
Consider a Lipschitz regular path \(\gamma (t) = ({{\mathbf {x}}}(t), \mathbf {n}(t))\), with \(\dot{{{\mathbf {x}}}} \propto \mathbf {n}\text { and } \dot{{{\mathbf {x}}}} \cdot \mathbf {n}\ge 0\), from \(({{\mathbf {x}}},\mathbf {n})\) to \(({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n})\). Then
so \(\mathbf {0} \in {{\mathrm{Hull}}}\{\mathbf {n}(t);\, 0 \le t \le 1\}\). Let \(\mathbf {m}:[0,1] \rightarrow \mathbb {S}^{d-1}\) be a constant speed parameterization of \(\mathbf {n}\). Let \(\tilde{\mathbf {m}}: {\mathbb {R}}\rightarrow \mathbb {S}^{d-1}\) be defined by \(\tilde{\mathbf {m}}(2\pi t)=\mathbf {m}(t)\) for all \(t \in [0,2\pi ]\) and extended by \(2\pi \)-periodicity. If \(\tilde{\mathbf {m}}(\cdot )\) were strictly 1-Lipschitz, then by Lemma 2 we would get \(\mathbf {0} \notin \text {Hull}\{\tilde{\mathbf {m}}(t)\;|\; t \in [0,2\pi ] \}=\text {Hull}\{\mathbf {n}(t)\;|\; t \in [0,1]\}\) and a contradiction. Hence there exists a \(t_0 \in {\mathbb {R}}\) such that \(\Vert \dot{\tilde{\mathbf {m}}}(t_0)\Vert \ge 1\) and via the constant speed parameterization assumption we get the required coercivity:
To prove local controllability of the model \(({\mathbb {M}}, d_{\mathcal {F}_0})\), we apply the logarithmic approximation for weighted subcoercive operators on Lie groups, cf. [57] applied to the Lie group \(SE(d)={\mathbb {R}}^{d} \rtimes SO(d)\), in which the space of positions and orientations is placed via a Lie group quotient \(SE(d)/(\{0\} \times SO(d-1))\). One obtains a sharp estimate,Footnote 7 where the weights of allowable (horizontal) vector fields are 1, whereas the remaining spatial vector fields orthogonal to \(\mathbf {n} \cdot \nabla _{{\mathbb {R}}^d}\) get weight 2, as they follow by a single commutator of allowable vector fields, see, e.g., [24, 25]. Relaxing all spatial weights to 2 and continuity of costs \(\mathcal {C}_{1}, \mathcal {C}_{2}\), yields (14). \(\square \)
Remark 10
In view of the above one might expect that the point \((\mathbf {x} - \mu \mathbf {n}, \mathbf {n})\) is reached by a geodesic that consists of a concatenation of 1. an in-place rotation by \(\pi \), 2. a straight line, 3. an in-place rotation by \(\pi \). However, this is not the case as can be observed in the very lower left corner in Fig. 8, where the two minimizing red curves show a very different behavior. This is explained by the next lemma.
Lemma 3
Let \(\mu >0\), and \(\mathcal {C}_{1}=\mathcal {C}_{2}=\delta \). Let \({{\mathbf {R}}}_\theta \) denote the (counter-clockwise) rotation matrix about the origin by angle \(\theta \). The endpoint \(({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n})\) for each \(\mu \ge 0\) is a Maxwell point w.r.t. \(({{\mathbf {x}}},\mathbf {n})\), since there are two minimizing geodesics in \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) that are a concatenation
-
1.
an in-place rotation from \(({{\mathbf {x}}},\mathbf {n})\) to \(({{\mathbf {x}}},{{\mathbf {R}}}_{\pm \frac{\pi }{2}}\mathbf {n})\),
-
2.
a full U-curve, see [44], departing from and ending in a cusp from \(({{\mathbf {x}}},{{\mathbf {R}}}_{\pm \frac{\pi }{2}}\mathbf {n})\) to \(({{\mathbf {x}}}- \mu \mathbf {n},{{\mathbf {R}}}_{\mp \frac{\pi }{2}}\mathbf {n})\),
-
3.
an in-place rotation from \(({{\mathbf {x}}}- \mu \mathbf {n},{{\mathbf {R}}}_{\mp \frac{\pi }{2}}\mathbf {n})\) to \(({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n})\).
We have the limit \(\lim \limits _{\mu \downarrow 0} d_{\mathcal {F}_{0}^+}(({{\mathbf {x}}},\mathbf {n}),({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n}))=2\pi \delta \).
Proof
See [27]. \(\square \)
Remark 11
Consider the case \(d=2\), \(\mathcal {C}_{1}=\mathcal {C}_{2}=\delta \), and source point \(\mathbf {p}_S=(\mathbf {x},\mathbf {n})=\mathbf {e}=(0,0,\theta =0)\). The endpoints \(({{\mathbf {x}}}-\mu \mathbf {n},\mathbf {n})=(-\mu ,0,0)\), with \(\mu >0\) sufficiently small, are 1st Maxwell points in \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) where geodesically equidistant wavefronts departing from the source point collide for the first time, see Fig. 9C. The distance mapping \(d_{\mathcal {F}_{0}}^{+}({\mathbf {p}}_{S}, \cdot )\) is not continuous, but the asymmetric distance spheres
are connected and compact, and they collide at \(R=2\pi \) in such a way that the origin \({\mathbf {p}}_{s}\) becomes an interior point in the asymmetric balls of radius \(R> 2\pi \).
4 Cusps and Keypoints: Proof of Theorem 3
In this section we provide a proof of Theorem 3 on the occurrence of cusps and keypoints. For the uniform cost case \(\mathcal {C}_{1}=\mathcal {C}_{2}=1\) for \(d=2\), our curve optimization problem (1) \((\mathbb {M},d_{\mathcal {F}_{0}})\) in consideration boils down to a standard left-invariant curve optimization in the roto-translation group \(SE(2)={\mathbb {R}}^{2} \rtimes SO(2)\). As we will apply tools from previous works [10, 11, 22, 52], we will make use of the following notations for expansionFootnote 8 of velocity and momentum in the left-invariant (co)-frame:
where the indexing of the left-invariant frame is different here, in order to stick to the ordering \((x,y,\theta )\) applied in this article. Note that for the case \(\varepsilon =0\) admissible smooth curves \(\gamma \) in \((\mathbb {M}, d_{\mathcal {F}_{0}})\) satisfy the horizontality constraint \(\dot{\gamma }(t) \in \text {Span}\{\left. \mathcal {A}_{1}\right| _{\gamma (t)},\left. \mathcal {A}_{3}\right| _{\gamma (t)}\}\).
Proof of the statements regarding cusps
-
We can describe our curve optimization problem (1) using a Hamiltonian formalism, with Hamiltonian \(H(\hat{{\mathbf {p}}})= \frac{1}{2}\left( \hat{p}_{1}^2 + \hat{p}_{3}^2 \right) = \frac{1}{2}\) [44]. By Pontryagin’s maximum principle, geodesics adhere to the following Hamilton equations:
$$\begin{aligned} \left\{ \begin{array}{l} \dot{p}_{1}=u^1= \hat{p}_{1}, \\ \dot{p}_{2}=u^2= 0, \\ \dot{p}_{3}=u^3=\hat{p}_{3}, \end{array} \right. , \ \left\{ \begin{array}{l} \frac{d \hat{p}_{1}}{dt}= \hat{p}_{2} \hat{p}_{3}, \\ \frac{d \hat{p}_{2}}{dt}= -\hat{p}_{1} \hat{p}_{3}, \\ \frac{d \hat{p}_{3}}{dt}= -\hat{p}_{1} \hat{p}_{2}. \end{array} \right. \end{aligned}$$(34)
For fixed initial momentum \(\hat{{\mathbf {p}}}(0)\), this uniquely determines a SR geodesic. Moreover, SR geodesics are contained within the (co-adjoint) orbits
The parameter t in system (34) is SR arc length, but by reparameterizing (possible as long as \(u^1\) does not change sign) to spatial arc length parameter s, with \(\frac{ds}{dt} = \hat{p}_1\), we get a partially linear system. Combining (34) and (35), we find orbits in the (hyperbolic) phase portrait induced by
Hence \(|\hat{p}_3(s)| = 1\) always has a solution for some finite (possibly negative) s, except when \(\hat{p}^{2}(0)=\hat{p}_{3}(0)=0\), in which case the solutions are straight lines. Preservation of the Hamiltonian then implies \(\hat{p}_1(s) = u^1(s) = \tilde{u}(s)=0\). We conclude that every SR geodesic (with unconstrained time \(t \in {\mathbb {R}}\)) in \((\mathbb {M},d_{\mathcal {F}_0})\) which is not a straight line admits a cusp.
-
We now consider \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\), \(\varepsilon > 0\). To have a cusp, we need \(\hat{p}_1(t) = \hat{p}_2(t) = 0\) for some \(t \in {\mathbb {R}}\). Co-adjoint orbit condition (35) then implies that \(\hat{p}_1(t) = \hat{p}_2(t) = 0\) for all t, corresponding to a vertical geodesic that has purely angular momentum and no cusp. The same argument holds for \(({\mathbb {M}},d_{\mathcal {F}_\varepsilon ^+})\). In \(({\mathbb {M}},d_{\mathcal {F}_0^+})\) we have the condition that \(u^1 \ge 0\); hence, by definition it can never switch sign and all geodesics are cuspless.
Proof of the statements regarding keypoints
-
For the cases \(({\mathbb {M}},d_{\mathcal {F}_\varepsilon })\) and \(({\mathbb {M}},d_{\mathcal {F}_\varepsilon ^+})\) with \(\varepsilon > 0\) we can use the same line of arguments as above. Also here both spatial controls have to vanish, resulting in vertical geodesics. The spatial projection of such curves is a single keypoint. For \(({\mathbb {M}},d_{\mathcal {F}_0})\) we rely on the result that SR geodesics are analytical, and therefore if the control \(u^1(t) = 0\) for some open time interval \((t_0,t_1)\), then \(u^1(t) = 0\) for all \(t \in {\mathbb {R}}\), again corresponding to purely angular motion.
-
Geodesics in \((\mathbb {M},d_{\mathcal {F}_0^+})\) can have keypoints only at the boundaries. Suppose a geodesic \(\gamma :[0,1] \rightarrow \mathbb {M}\) in \((\mathbb {M},d_{\mathcal {F}_0^+})\) has an internal keypoint, with a corner of angle \(\delta >0\), at internal time \(T_{1} \in (0,1)\). Then one can create a local shortcut with a straight line segment connecting two sufficiently close points before and after the corner with two in-place rotations whose angles add up to \(\delta \). With a suitable mollifier this shortcut can be approximated by a curve in \(\varGamma \). For details see similar arguments in [11].
Next we explain the cases (A), (B) and (C), where we fix initial point \(\gamma (0)=\mathbf {e}=(0,0,0)\).
-
(A)
Suppose that the endpoint and \(x \ge 0\). Then \(\mathbf {p}\) can already be reached by a geodesic in \((\mathbb {M},d_{\mathcal {F}_0})\), and the positivity constraint (i.e., no reverse gear), which can only increase length, becomes obsolete.
-
(B)
Now suppose the endpoint \(\mathbf {p}=(x,y,\theta )\) lays in the half-space \(x<0\). Then by the half-space property of geodesics in \((\mathbb {M},d_{\mathcal {F}_0})\), cf. [22, Theorem 7], the geodesic in \((\mathbb {M},d_{\mathcal {F}_0^+})\) must have a keypoint. By the preceding keypoints can only be located at the boundaries. If it takes place at the endpoint only, then still the constraint \(x<0\) is not satisfied; thereby, it must take place at the origin.
-
(C)
In those cases the endpoint \(\mathbf {p}\) lays outside the connected cone of reachable angles, which are by [22, Theorem 9] bounded (for those endpoints) by geodesics ending in a cusp (so not endpoints of geodesics starting at a cusp). So for those points, minimizing geodesics will first move by an in-place rotation (along a spherical geodesic) until it hits the cusp surface , after which it is traced back to the origin by a regular geodesic with strictly positive spatial control inside the volume .
5 Eikonal Equations and Backtracking: Proof of Proposition 1, Corollary 1 and Theorem 4
First we shall prove Proposition 1, regarding the duals of \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\), and Corollary 1, providing explicit expressions for the corresponding eikonal equations. To this end we need a basic lemma on computing dual norms on \({\mathbb {R}}^{n}\), where later we will set \(n = 2d - 1 = \mathrm {dim}({\mathbb {M}})\).
Lemma 4
Let \(\mathbf {w} \in {\mathbb {R}}^{n}\) and let \(M \in {\mathbb {R}}^{n\times n}\) be symmetric, positive definite. Define the norm \(F_{M, \mathbf {w}}:{\mathbb {R}}^{n} \rightarrow {\mathbb {R}}^+\) by
Then its dual norm \(F_{M, \mathbf {w}}^*: ({\mathbb {R}}^{n})^* \rightarrow {\mathbb {R}}^+\) equals
with \(\hat{M}=(M + \mathbf {w} \otimes \mathbf {w})^{-1}\) and \(\hat{\mathbf {w}}=\frac{M^{-1}\mathbf {w}}{\sqrt{1+ (\mathbf {w}, M^{-1}\mathbf {w})}}\).
Proof
For \(n=1\) the result is readily verified, and for \(\mathbf {w}=\mathbf {0}\) the result is classical. We next turn to the special case \(M= {{\mathrm{Id}}}\), and \({\mathbf {w}}= (w_1, \mathbf {0}_{\mathbb {R}^{n-1}})\) is zero except maybe for its first coordinate \(w_1\). Thus for any \({\mathbf {v}}= (v_1,{\mathbf {v}}_2) \in \mathbb {R}^n = \mathbb {R}\times \mathbb {R}^{n-1}\) one has the splitting
Using the compatibility of norm duality with such splittings, and the special cases \(n=1\) and \({\mathbf {w}}=0\) mentioned above, we obtain
which is exactly of form (36). The general case for arbitrary \({\mathbf {w}}\) and symmetric positive definite M follows from affine invariance. Indeed let A be an invertible \(n \times n\) matrix, and let \(M' = A^{\mathrm T}M A\) and \({\mathbf {w}}' = A^{\mathrm T}{\mathbf {w}}\). Let \(F = F_{M,{\mathbf {w}}}\) and \(F'=F_{M',{\mathbf {w}}'}\), so that \(F'({\mathbf {v}})=F(A{\mathbf {v}})\) for all \({\mathbf {v}}\in \mathbb {R}^n\). Let \(F^*\), \(\hat{M}\), \(\hat{\mathbf {w}}\) and \(F'^*\), \(\hat{M}'\), \(\hat{\mathbf {w}}'\), be, respectively, the dual norms and the matrices defined by the explicit formulas above. Then denoting \(B := (A^{\mathrm T})^{-1}\) one has by the definition of dual norms that \(F'^*(\hat{{\mathbf {v}}}) = F^*(B \hat{{\mathbf {v}}})\) for all \(\hat{{\mathbf {v}}}\in \mathbb {R}^n\) and by the explicit formulas \(\hat{M}' = B^{\mathrm T}\hat{M} B\), \({\mathbf {w}}' = B^{\mathrm T}{\mathbf {w}}\). Thus, \(F^*=F^*_{M, {\mathbf {w}}}\) holds if and only if \(F'^* = F^*_{M', {\mathbf {w}}'}\). Since for any \(M, {\mathbf {w}}\), there exists a linear change of variables A such that \(M'={{\mathrm{Id}}}\) and \({\mathbf {w}}'\) is zero except maybe for its first coordinate, the proof is complete. \(\square \)
Now Proposition 1 follows from Lemma 4 by writing out the dual norm, using for each \({\mathbf {p}}\in {\mathbb {M}}\):
with \(D_\mathbf {n}^\varepsilon \) as in (27). Corollary 1 then follows by setting the momentum covector \(\hat{{\mathbf {p}}}=\mathrm{d}U({\mathbf {p}})\) equal to the derivative of the value function evaluated at \({\mathbf {p}}\).
Now that we have derived the eikonal equations, we obtain backtracking Theorem 4 by Proposition 4 in ‘Appendix B’ section, which shows us that level sets of solutions of the eikonal equations are geodesically equidistant surfaces and that geodesics are found by an intrinsic gradient descent.
However, to obtain the explicit backtracking formulas we differentiate the Hamiltonian, rather than the dual metric, which is equivalent thanks to (57) (in Remark 14 in ‘Appendix B’ section). We focus below on the model \(({\mathbb {M}},d_{\mathcal {F}_\varepsilon ^+})\) without reverse gear, since the other case is similar. Let \({\mathbf {p}}\in {\mathbb {M}}\), let \(F := \mathcal {F}_\varepsilon ^+({\mathbf {p}}, \cdot )\), and let \(\hat{{\mathbf {p}}}= (\hat{{{\mathbf {x}}}}, \hat{\mathbf {n}}) \in T^*_{\mathbf {p}}({\mathbb {M}})\). Then differentiating w.r.t. \(\hat{\mathbf {n}}\) we obtain
where \(\Vert {\cdot }\Vert \) is the Riemannian metric induced by the embedding \(\mathbb {S}^{d-1} \subset \mathbb {R}^d\). Differentiating w.r.t. \(\hat{{{\mathbf {x}}}}\) we obtain
Announced result (31), which is equivalent to its more concise abstract form (28), follows by choosing \(\hat{{{\mathbf {x}}}}:= \nabla _{\mathbb {R}^d} U(\gamma (t))\) and \(\hat{\mathbf {n}}:= \nabla _{\mathbb {S}^{d-1}} U(\gamma (t))\) and a basic re-scaling \([0,L]\in t \mapsto t/L \in [0,1]\). \(\square \)
Remark 12
The computation of the dual norms can be simplified by expressing velocity (entering the Finsler metric) and momentum (entering the dual metric) in a (left-invariant) local, orthogonal, moving frame of reference, attached to the point \({\mathbf {p}}= ({{\mathbf {x}}},\mathbf {n}) \in {\mathbb {M}}\):
where a moving frame of reference is chosen such that
inducing a corresponding dual frame \(\{\left. \omega ^{i}\right| _{\mathbf {p}}\}\) via
w.r.t. the left-invariant frame the matrices \(D_\mathbf {n}^\varepsilon \), \(M_{\mathbf {p}}\) as in (38) and \(\hat{M_{\mathbf {p}}}\) all become diagonal matrices, and the dual can be computed straightforwardly. Furthermore, in this formulation we can see from the expression for the dual \((\mathcal {F}_0^+)^*\), i.e., in the limit \(\varepsilon \downarrow 0\), that the positive spatial control \(u^d\) constraint results in a positive momentum \(\hat{p}_d\) constraint:
Therefore the eikonal equation in the positive control model \(({\mathbb {M}}, d_{\mathcal {F}_0^+})\) is simply given by
6 Discretization of the Eikonal PDEs
6.1 Causal Operators and the Fast-Marching Algorithm
The fast-marching algorithm is an efficient numerical method [59] for numerically solving static first-order Hamilton–Jacobi–Bellman (or simply eikonal) PDE (5) which characterizes the distance map U to a fixed source point \({\mathbf {p}}_\mathrm{S}\). Fast marching is tightly connected with Dijkstra’s algorithm on graphs, and in particular it shares the \(\mathcal {O}(K N \ln N)\) complexity, where \(N = \#(X)\) is the cardinality of the discrete domain \(X \subset {\mathbb {M}}\), \(X\ni {\mathbf {p}}_\mathrm{S}\), and K is the average number of neighbors for each point. Both fast-marching and Dijkstra’s algorithms can be regarded as specialized solvers of nonlinear fixed point systems of equations \(\varLambda u = u\), where the unknown \(u\in \mathbb {R}^X\) is a discrete map representing the front arrival times, which rely on the a-priori assumption that the operator \(\varLambda :\mathbb {R}^X \rightarrow \mathbb {R}^X\) is causal (and monotone, but this second assumption is not discussed here). Causality informally means that the estimated front arrival time \(\varLambda u({\mathbf {p}})\) at a point \({\mathbf {p}}\in X\) depends on the given arrival times \(u({\mathbf {q}})\), \({\mathbf {q}}\in X\), prior to \(\varLambda u({\mathbf {p}})\), but not on the simultaneous or the future ones. Formally, one requires that for any \(u,v\in \mathbb {R}^X\), \(t\in \mathbb {R}\):
and \(v^{<t}\), \((\varLambda u)^{\le t}\) and \((\varLambda v)^{\le t}\) are defined similarly.
A semi-Lagrangian scheme We implemented two discretizations of eikonal equation (5) which benefit from the causality property. The first one is a semi-Lagrangian scheme, inspired by Bellman’s optimality principle which informally states that any subpolicy of an optimal policy is an optimal policy. Formally, let \(\mathcal {F}\) be a Finsler metric, and let \(U := d_\mathcal {F}(\cdot , {\mathbf {p}}_\mathrm{S})\) be defined as the distance to a given source point \({\mathbf {p}}_\mathrm{S}\). Then for any \({\mathbf {p}}\in {\mathbb {M}}\) and any neighborhood V of \({\mathbf {p}}\) not containing \({\mathbf {p}}_\mathrm{S}\) one has the property
In the spirit of [55, 59] we discretize (45) by introducing for each interior \({\mathbf {p}}\in X {\setminus }\{{\mathbf {p}}_\mathrm{S}\}\) a small polygonal neighborhood \(V({\mathbf {p}})\), which vertices belong to the discrete point set X. The nonlinear operator \(\varLambda \) is defined as
where \(\varXi = \{ \xi \in \mathbb {R}^n_+; \, \sum _{i=1}^n \xi _i=1\}\). In other words, the boundary point \({\mathbf {q}}\in \partial V({\mathbf {p}})\) in (45) is represented in (46) by the barycentric sum \({\mathbf {q}}= \sum _{i=1}^n \xi _i {\mathbf {q}}_i\), the distance \(d_\mathcal {F}({\mathbf {p}}, {\mathbf {q}})\) is approximated with the norm \(\mathcal {F}({\mathbf {p}}, {\mathbf {q}}-{\mathbf {p}})\), and the value \(U({\mathbf {q}})\) is approximated with the interpolation \(\sum _{i=1}^n \xi _i u({\mathbf {q}}_i)\).
We refer to [55, 61] for proofs of convergence and for the following essential property: operator (46) obeys causality property (44) iff the chosen stencil \(V({\mathbf {p}})\) obeys the following generalized acuteness property: for any \({\mathbf {q}}, {\mathbf {q}}'\) in a common facet of \(V({\mathbf {p}})\), one has
For the construction of such stencils \(V({\mathbf {p}})\), \({\mathbf {p}}\in X\), we rely on the previous works [40, 41] and on the following observation: the metrics \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) associated with the Reeds–Shepp car models can be decomposed as
which allows to build the stencils \(V({\mathbf {p}})\) for \(\mathcal {F}\) by combining, as discussed in [41, p. 9], some lower-dimensional stencils \(V_1({\mathbf {p}})\) and \(V_2({\mathbf {p}})\) built independently for the spatial \({{\mathbf {x}}}\in \mathbb {R}^d\) and spherical \(\mathbf {n}\in \mathbb {S}^{d-1}\) variables.
We discretize \(\mathbb {S}^1\) uniformly, with the standard choice of stencil. We discretize \(\mathbb {S}^2\) by refining uniformly the faces of an icosahedron and projecting their vertices onto the sphere (as performed by the Mathematica\(^{\textregistered }\) Geodesate function). The resulting triangulation only features acute interior angles, in the classical Euclidean sense, and thus provides adequate stencils since in our applications \(\mathcal {F}_2({\mathbf {p}}, \dot{\mathbf {n}}) = \mathcal {C}_2({\mathbf {p}}) \Vert \dot{\mathbf {n}}\Vert \) is proportional to the Euclidean norm, see Fig. 10. We typically use 60 discretization points for \(\mathbb {S}^1\) and from 200 to 2000 points for \(\mathbb {S}^2\).
We discretize \(\mathbb {R}^d\) using the Cartesian grid \(h \mathbb {Z}^d\), where \(h >0\) is the discretization scale. The norm \(\mathcal {F}_{\varepsilon ,1}({\mathbf {p}}, \dot{{{\mathbf {x}}}}) = \mathcal {C}_1({\mathbf {p}}) \sqrt{\dot{{{\mathbf {x}}}}^T (D_\mathbf {n}^{\varepsilon })^{-1} \dot{{{\mathbf {x}}}}}\), recall the notation in (47), that is induced by the approximate Finsler metric \(\mathcal {F}_\varepsilon \) on the physical variables in \(\mathbb {R}^d\), is of Riemannian type and is strongly anisotropic. In dimension \(d \le 3\), this is the adequate setting for the adaptive stencils of [41], built using discrete geometry tools known as lattice basis reduction. The norm \(\mathcal {F}_{\varepsilon ,1}^+({\mathbf {p}}, \dot{{{\mathbf {x}}}}) = \mathcal {C}_1({\mathbf {p}}) \sqrt{\dot{{{\mathbf {x}}}}^T (D_\mathbf {n}^{\varepsilon })^{-1} \dot{{{\mathbf {x}}}}+ (\varepsilon ^{-2}-1)(\mathbf {n},{{\mathbf {x}}})_-^2}\) induced by \(\mathcal {F}_\varepsilon ^+\) on \(\mathbb {R}^d\) is Finslerian (i.e., non-Riemannian) and strongly anisotropic. In dimension \(d=2\), this is the adequate setting for the adaptive stencils of [40], built using an arithmetic object known as the Stern–Brocot tree.
Direct approximation of the Hamiltonian A new approach, not semi-Lagrangian, had to be developed for the Finsler metric \(\mathcal {F}_\varepsilon ^+\) in dimension \(d=3\) due to our failure to construct viable (i.e., with a reasonably small number of reasonably small vertices) stencils obeying the generalized acuteness property in this case, see Fig. 11. For manuscript size reasons, we only describe it informally and postpone proofs of convergence for future work.
Let \(\mathbf {n}\in \mathbb {S}^2\) and let \(\varepsilon >0\) be fixed. Then one can find nonnegative weights and integral vectors \((\rho _i,{\mathbf {w}}_i) \in (\mathbb {R}_+\times \mathbb {Z}^3)^6\), such that for all \({\mathbf {v}}\in \mathbb {R}^3\)
A simple and efficient construction of \((\rho _i, {\mathbf {w}}_i)_{i=1}^6\), relying on the concept of obtuse superbase of a lattice, is in [29] described and used to discretize anisotropic diffusion PDEs. One may furthermore assume that \((\mathbf {n}, {\mathbf {w}}_i) \ge 0\) for all \(1 \le i \le 6\), up to replacing \({\mathbf {w}}_i\) with its opposite. Then
up to, respectively, an \(\mathcal {O}(\varepsilon ^2)\Vert {\mathbf {v}}\Vert ^2\) and \(\mathcal {O}(\varepsilon ^2+h)\) error. Following [51], we design a similar upwind discretization of the angular part of the metric
where \(\delta _\theta U({\mathbf {p}})\), and likewise \(\delta _\varphi U({\mathbf {p}})\), is defined as
We denoted by \(\mathbf {n}(\theta , \varphi ) := (\sin \theta \cos \varphi , \sin \theta \sin \varphi , \cos \theta )\) the parameterization of \(\mathbb {S}^2\) by Euler angles \((\theta , \varphi ) \in [0,\pi ] \times [0, 2 \pi ]\). Combining (49) and (50), one obtains an approximation of \(\mathcal {F}_0^{+*}({\mathbf {p}}, \mathrm{d}U({\mathbf {p}}))^2\), within \(\mathcal {O}(\varepsilon ^2+r(\varepsilon )h)\) error for smooth U, denoted . We denoted by \(r(\varepsilon ) := \max _{i=1}^6 |{\mathbf {w}}_i|\) the norm of the largest offset appearing in (48), since these clearly depend on \(\varepsilon \). Importantly, only depends on positive parts of finite differences \((U({\mathbf {p}})-U({\mathbf {q}}))_+\); hence, the system can be solved using the fast-marching algorithm, as shown in [51]. The convergence analysis of this discretization, as the grid scale h and tolerance \(\varepsilon \) tend to zero suitably, is postponed for future work, see [42, 43].
Note that this approach could also be applied in dimension \(d=2\), and to the symmetric model \(({\mathbb {M}}, d_{\mathcal {F}_{\varepsilon }})\) featuring a reverse gear. We present only a single assessment of the numerical performance of our method, see Fig. 12. We compare numerically obtained shortest paths with exact SR geodesics for a small number of endpoints that correspond to various types of curves. For fair end conditions (a, b, c) the numerical curves are close to the exact curves. For very challenging end conditions inducing torsion (d) or extreme curvature (e) the curves are further from the exact SR geodesics. An extensive evaluation of the performance of the numerics is left for future work.
7 Applications
To show the potential of anisotropic fast marching for path tracing in 2D and 3D (medical) images we performed experiments on each of the datasets in Fig. 3:
-
a 2D toy example using a map of Centre Pompidou,
-
a 2D retinal image,
-
two synthetic diffusion-weighted magnetic resonance imaging (dMRI) datasets, with different bundle configurations.
We use the 2D datasets to point out the difference in results when using the metric \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) and to explain the role of the keypoints when using \(\mathcal {F}_\varepsilon ^+\) that occur instead of (possibly unwanted) cusps.
On the synthetic dMRI datasets we present the first application of our methods to this type of data. We present how a cost function can be extracted from the data, and how this leads to correct tracking of bundles, similar to the 2D case. The benefits of anisotropic metrics compared to isotropic metrics are demonstrated by performing backtracking for various model parameter variations.
The experiments were performed using an anisotropic FM implementation written in C++, for \(d = 2\) described in [41]. Implementation details for \(d = 3\) will be described in future work. Mathematica 11.0 (Wolfram Research, Inc., Champaign, IL) was used for further data analysis, applying Wolfram LibraryLink (Wolfram Research, Inc., Champaign, IL) to interface with the FM library.
7.1 Applications in 2D
7.1.1 Shortest Path to the Exit in Centre Pompidou
To illustrate the difference between the models with and without reverse gear and to show the role of the keypoints for non-uniform cost, we use a map of Centre Pompidou as a 2D image, see Fig. 13. The walls (in black) have infinite cost, everywhere else the cost is 1. We place endpoints (black dots) in various places of the museum and look for the shortest path from those points to one of the two exits, regardless of the end orientation. Since there are now two exits, say at \({\mathbf {p}}_0\) and \({\mathbf {p}}_1\), the distance \(U({\mathbf {p}})\) of any point \({\mathbf {p}}\in {\mathbb {M}}\) to one of the exits is given by
We use a resolution of \(N_x \times N_y \times N_o = 706 \times 441 \times 60\). The cost in this example is only dependent on position, but constant in the orientation. Moreover, we use \(\mathcal {C}_1 = \mathcal {C}_2\) and \(\varepsilon = 1/10\).
On the left of Fig. 13 we see optimal paths (in blue) obtained using the Finsler metric \(\mathcal {F}= \mathcal {F}_\varepsilon \). The fast-marching algorithm successfully connects all endpoints to one of the exits. Some of the geodesics have cusps, indicated with white points, resulting in backward motion on (a part of) the curve. The colors show the distance \(U_{\mathcal {F}_\varepsilon }\) as above, at each position minimized over the orientations.
On the right, the optimal paths using the asymmetric Finsler metric \(\mathcal {F}= \mathcal {F}_\varepsilon ^+\) are shown in red. The curves no longer exhibit cusps, but have in-place rotations (white dots) instead. These keypoints occur in this example on corners of walls. (Due to the fact that \(\varepsilon \) is small but nonzero, there can still be small sideways motion.) The shortest paths for this model are successions of sub-Riemannian geodesics and of in-place rotations, which can be regarded as reinitializations of the former: the orientation is adapted until an orientation is found from which the path can continue in an optimal sub-Riemannian way.
We stress that the fast-marching algorithm has no special treatment for keypoints, which are only detected in a postprocessing step. We observe that keypoints are automatically positioned at positions where it makes sense to have an in-place rotation. Small differences in the distance maps between \(U_{\mathcal {F}_\varepsilon }\) left and \(U_{\mathcal {F}_\varepsilon ^+}\) right can be observed: the constrained model usually has a slightly higher cost right around corners.
7.1.2 Vessel Tracking in Retinal Images
Another application is vessel tracking in retinal images, for which the model with reverse gear and the fast-marching algorithm have shown to be useful in [8, 53]. Although the algorithm works fast and led to successful vessel segmentation in many cases, in some cases, in particular bifurcations of vessels, cusps occur. Figure 14 shows one such example on the left. The image has resolution \(N_x \times N_y \times N_o = 121 \times 114 \times 64\). The cost is constructed as in [8]: the image is first lifted using cake wavelets [23], resulting in an image on \(\mathbb {R}^2 \times \mathbb {S}^1\). For the lifting and for the computation of the cost function from the lifted image, we rely on their parameter settings. We use \(\mathcal {C}_1 = \xi \mathcal {C}_2\), with \(\xi = 0.02\) (top) and \(\xi = 0.04\), and \(\varepsilon = 0.1\). The orientations of the end conditions A, B and C (white arrows) are chosen tangent to the vessel, where we considered both the forward case and the backward case. The vessel with end condition C is particularly challenging, since it comes across a bifurcation. For the tracking of this vessel, we indicated the orientation with yellow arrows.
The unconstrained model \(({\mathbb {M}},d_{\mathcal {F}_\varepsilon })\), corresponding to the blue tracks on the left half of Fig. 14, gives a correct vessel tracking for the forward end conditions of A and B, for both values of \(\xi \). This is obviously the better choice than the backward cases. However, for end condition C, neither the forward or backward with neither values of \(\xi \) gives a vessel tracking without cusps. On the other hand, if we use the constrained model \(({\mathbb {M}},d_{\mathcal {F}^+_\varepsilon })\), we obtain an in-place rotation or keypoint in the neighborhood of the bifurcation. Typically a higher value of \(\xi \) brings these points closer to the bifurcation. Taking the backward end conditions in combination with this model, we see in some cases that end locations are first passed by the vessel tracking algorithm, until it reaches a point where in-place rotation is cheaper, and then returns to the end position.
7.2 Application to Diffusion-Weighted MRI Data
DW-MRI is a magnetic resonance technique for noninvasive measurement of water diffusion in fibrous tissues [46]. In the brain, diffusion is less constrained parallel to white matter fibers (or axons) than perpendicular to them, allowing us to infer the paths of these fibers. The diffusion measurements are distributions \((\mathbf {y}, \mathbf {n}) \mapsto U(\mathbf {y},\mathbf {n})\) within the manifold \({\mathbb {M}}\) for \(d=3\). From these measurements a fiber orientation distribution (FOD) can be created, yielding a probability of finding a fiber at a certain position and orientation [60].
Backtracking is performed through forward Euler integration of the backtracking PDE involving the intrinsic gradient, following Theorem 4 and Eqs. (28) and (31). The spatial derivative was implemented as a first-order Gaussian derivative. The angular derivatives are implemented by a first-order spherical harmonic derivative. The latter has the key advantage that in a spherical harmonic basis exact analytic computations can be done. Here, one must rely on two-fold recursions in [28, Lemma 2 & 4], so that the poles due to a standard Euler angle parameterization of \(\mathbb {S}^{2}\) do not appear in exact recursions of Legendre polynomials!
If data-driven factors \(\mathcal {C}_{1}\) and \(\mathcal {C}_{2}\) come in a spherical sampling or if one wants to work in a spherical sampling (e.g., higher-order tessellation of the icosahedron) in a fast-marching method, then one can easily perform the pseudo-inverse of the discrete inverse spherical harmonic transform, where one typically keeps the number of spherical harmonics very close to the number of spherical sampling points, so that maximum accuracy order is maintained for computing angular derivatives in the intrinsic gradient descent of Theorem 4.
7.2.1 Construction of the Cost Function
The synthetic dMRI data are created by generating/simulating a fiber orientation density (FOD) of a desired structure. There are sophisticated methods for this, e.g., [12, 17], but evaluation on phantom data constructed with these tools is left for future work. Here we use a basic but practical method on two simple configurations of bundles in \(\mathbb {R}^3\), the ones on the bottom row in Fig. 3. In each voxel inside a bundle, we place a spherical \(\delta \)-distribution, with the peak in the orientation of the bundle. We convolve each \(\delta \)-distribution with an FOD kernel that was extracted from real dMRI data and is related to the dMRI signal measured in a voxel with just a single orientation of fibers. Spherical rotation of the FOD kernel is done in the spherical harmonics domain by use of the Wigner D-matrix to prevent interpolation issues. We compose from all distributions an FOD function \(W: {\mathbb {M}}\rightarrow {\mathbb {R}}^+\). This function evaluates to high values in positions/orientations that are inside and aligned with the bundle structure.
We use the FOD W to define the cost function \(\frac{1}{1+\sigma } \le \mathcal {C}\le 1\) via
where \(\sigma \ge 0\), \(p \in \mathbb {N}\), with \(\Vert {\cdot } \Vert _\infty \) the sup-norm and \(W_+({\mathbf {p}}) = {\text {max}}\{0,W({\mathbf {p}})\}\). The cost function \(\mathcal {C}\) induces the following spatial and angular cost functions \((\mathcal {C}_1, \mathcal {C}_2)\):
The implementation of non-uniform cost is comparable to the application of vessel tracking in retinal images in \(d=2\) by Bekkers et al. [8].
7.2.2 Influence of Model Parameters
The first synthetic dataset consists of a curved and a straight bundle (tube), which cross at two locations as shown in Fig. 15. The experiments using metric \(\mathcal {F}_\varepsilon \) demonstrate the effect of the model parameters on the geodesic backtraced from the bottom-left to the seed location at the bottom-right of the curved bundle. A distance map is computed for parameter configuration A (Fig. 15, right) in which suitable values are used for the data term \(\sigma \), and the fast-marching parameters \(\xi \) and \(\varepsilon \). Furthermore, fixed values are used for data sharpening \(p=3\), spatial smoothing \(\sigma s=0.5\), forward Euler integration step size \(\delta t=0.04\) and a gridscale of 1. By use of these parameters the global minimizing geodesic (Fig. 15A, left) is shown to take the longer, curved route. In parameter configuration B the data term \(\sigma \) is lowered, which creates a geodesic that is primarily steered by internal curve-dependent costs and is shown to take the shortcut route (Fig. 15B). Setting \(\varepsilon =1\) in configuration C leads to a Riemannian case where the geodesic resembles a piecewise linear curve. In configuration D the relative cost of spatial movement relative to angular movement is high, leading to geodesics with shortcuts.
We conclude that configuration \(\mathbf A \) with a relatively strong data term, large bending stiffness (\(\xi ^{-1}=10\)) and a nearly SR geometry (\(\varepsilon =0.1\)) avoids unwanted shortcuts.
7.2.3 Positive Control Constraint
For the application of FM in dMRI data it is desirable that the resulting geodesic is not overly sensitive to the boundary conditions, i.e., the placement and orientation of the geodesic tip. Furthermore, since neural fibers do not form cusps, these are undesirable in the backtracking results. In Fig. 16 the backtracking results are shown for the cases without reverse gear \(\mathcal {F}_\varepsilon ^+\) (top) and the model with reverse gear \(\mathcal {F}_\varepsilon \) (bottom). The distance map for \(\mathcal {F}_\varepsilon ^+\) was computed by the iterative method implementing the forward Reeds–Shepp car, while for \(\mathcal {F}_\varepsilon \) the FM method was used.
We conclude that without the positive control constraint, small changes in tip orientation cause large variations in the traced geodesic in the metric space \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\), whereas the traced geodesic in the quasi-metric space \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^+})\) is both more stable and more reasonable.
7.2.4 Robustness to Neighboring Structures
A pitfall of methods that provide globally minimizing curves using a data term is that dominant structures in the data attract many of the curves, much like the highway usually has the preference for cars rather than local roads. This phenomenon is to a certain extent unwanted in our applications, and we illustrate with the following example that it can be circumvented using a sub-Riemannian instead of Riemannian metric. We use the dataset as introduced in Fig. 3. It consists of one bundle that has torsion (green) and that crosses with another bundle (blue), and a third bundle (red) that is parallel with the first in one part. The cost in these bundles is constructed in the same way as above, but now the cost in the red bundle is twice as low as in the other bundles. A small part of the data is visualized on the left of Fig. 17. These data are used to construct the cost function as explained above.
The resolution of the data is \(N_x \times N_y \times N_z \times N_o = 32 \times 32 \times 32 \times 162\). Again we use \(\mathcal {C}_1 = \xi \mathcal {C}_2 = \mathcal {C}\), with \(\xi = 0.1\). To have comparable parameters as in the previous experiment, despite increasing the amplitude in one of the bundles by a factor 2, we choose to construct the cost using parameter \(p = 3\), and \(\sigma = 3 \cdot 2^p = 24\). From various positions inside the green, blue and red bundle, the shortest paths to the end of the bundles computed by the FM algorithm nicely follow the shape of the actual bundles, when we choose \(\varepsilon =.1\) small, corresponding to an almost SR geodesic. This is precisely what prevents the geodesic in the green bundle to drift into the (much cheaper) red bundle. We show on the right in Fig. 17 that choosing \(\varepsilon = 1\), corresponding to having an isotropic Riemannian metric, this unwanted behavior can easily occur.
We conclude that the SR geodesics in \(({\mathbb {M}}= {\mathbb {R}}^3 \times \mathbb {S}^2, d_{\mathcal {F}_\varepsilon })\) with \(\varepsilon \ll 1\) are less attracted to parallel, dominant structures than isotropic Riemannian geodesics.
8 Conclusion and Discussion
We have extended the existing methodology for modeling and solving the problem of finding optimal paths for a Reeds–Shepp car to 3D and to a case without reverse gear. We have shown that the use of the constrained model leads to more meaningful shortest paths in some cases and that the extension to 3D has opened up the possibility for tractography in dMRI data.
Instead of using a hard constraint on the curvature as in the original paper by Reeds and Shepp [50], we used symmetric and asymmetric Finsler metrics. We have introduced these metrics, \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\), for \(d = 2, 3\), such that they allow for curves that have a spatial displacement proportional to the orientation, with a positive proportionality constant in the case of \(\mathcal {F}_0^+\).
We have captured theoretically some of the nature of the distance maps and geodesics following from the new constrained model. We have shown in Theorem 1 that both models are globally controllable, but only the unconstrained model is also locally controllable.
The sub-Riemannian and sub-Finslerian nature is difficult to capture numerically. To this end, we introduced approximating Finsler metrics \(\mathcal {F}_\varepsilon \) and \(\mathcal {F}_\varepsilon ^+\) that do allow for numerical approaches. We have shown in Theorem 2 that as \(\varepsilon \rightarrow 0\), the distance map converges pointwise and the geodesics converge uniformly, implying that for sufficiently small \(\varepsilon \) we indeed have a reasonable approximation of the \(\varepsilon = 0\) case.
We have analyzed cusps in the metric space \((\mathbb {M},d_{\mathcal {F}_{0}})\) and keypoints in the quasi-metric space \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) which occur on the interface surface \(\partial \mathbb {M}_{\pm }\) given by (30). The analysis, for uniform costs, is summarized in Theorem 3. We have shown that cusps are absent in \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) for \(\varepsilon >0\) and that keypoints in \((\mathbb {M},d_{\mathcal {F}_{0}^+})\) occur only on the boundary, and we provided analysis on how this happens. In Theorem 4 we have shown how minimizing geodesics in \(({\mathbb {M}}, d_{\mathcal {F}_\varepsilon })\) and \(({\mathbb {M}}, d_{\mathcal {F}^+_\varepsilon })\) can be obtained from the distance maps with an intrinsic gradient descent method.
To obtain solutions for the distance maps and optimal paths, we used a fast-marching method. By formulating an equivalent problem to the minimization problem for optimal paths in the form of an eikonal equation, the FM method can be used using specific discretization schemes. We briefly compared the numerical solutions using \(\mathcal {F}_\varepsilon \) with \(\varepsilon \ll 1\) with the exact sub-Riemannian geodesics in SE(2) with uniform cost, which showed sufficient accuracy for not too extreme begin and end conditions.
To show the use of our method in image analysis, we have tested it on two 2D problems and two 3D problems. All four experiments confirm that the combination of the eikonal PDE formulation, the fast-marching method and the construction of the non-uniform cost from the images, results in geodesics that follow the desired paths. From the experiment on an image of Centre Pompidou, with constant, finite cost everywhere except for the walls, it followed that instead of having cusps when using the Finsler metric \(\mathcal {F}_\varepsilon \), we get keypoints (in-place rotations) when using \(\mathcal {F}_\varepsilon ^+\). These keypoints turn out to be located on logical places in the image. On the 2D retinal image we showed that the Finsler metric \(\mathcal {F}_\varepsilon ^+\) gives a new tool for tackling vessel tracking through bifurcations. We see that keypoints appear close to the bifurcation, leading to paths that more correctly follow the data.
The basic experiments on 3D show advantages of the model \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) with \(0 < \varepsilon \ll 1\) over the model \((\mathbb {M},d_{\mathcal {F}_{1}})\) in the sense that the minimizing geodesics better follow the curvilinear structure and deal with crossings and nearby parallel bundles (even if torsion is present). Furthermore, we have shown the advantage of model \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }^+})\) with \(0 < \varepsilon \ll 1\), compared to \((\mathbb {M},d_{\mathcal {F}_{\varepsilon }})\) in terms of stability, with keypoints instead of cusps.
The strong performance of the Reeds–Shepp car model in 2D vessel tracking and positive first results on artificial dMRI data, encourages us to pursue a more quantitative assessment of the performance in both 3D vessel tracking problems and in actual dMRI data. Such 3D vessel tracking problems are encountered in, for example magnetic resonance angiography. In future work we will elaborate on the implementation and evaluation of the fast-marching and the iterative PDE implementation of [27, Appendix B]. Furthermore, we aim to integrate locally adaptive frames [26] into the Finsler metrics \(\mathcal {F}_{\varepsilon }\), \(\mathcal {F}_{\varepsilon }^+\), for a more adaptive vessel/fiber tracking.
Notes
In contrast to previous works [8, 11, 22, 25, 36] we parameterize such that the time integration stays on [0, 1], and \(t>0\) is not a priori reserved (unless explicitly stated otherwise) for arc length parameterization (which satisfies \(\mathcal {F}_{{\varvec{\gamma }}(t)}(\dot{{\varvec{\gamma }}}(t))=1\)).
From a theoretical standpoint, one may rely on the notion of discontinuous viscosity solution [7]. But this concept is outside of the scope of this paper, and in addition, it forbids the use of a singleton \(\{{\mathbf {p}}_S\}\) as the target set.
For vessel tracking experiments that show the benefit of the sub-Riemannian approach \(({\mathbb {R}}^{2}\times \mathbb {S}^{1},d_{\mathcal {F}_0})\) in [8] see: http://epubs.siam.org/doi/suppl/10.1137/15M1018460.
The quantity \(\Vert \dot{{{\mathbf {x}}}} \wedge \mathbf {n}\Vert \) is also the norm of the wedge product of \(\dot{{{\mathbf {x}}}}\) and \(\mathbf {n}\), but defining it this way would require introducing some algebra which is not needed in the rest of this paper.
Here denotes the complement of the closure of and \(E(z,m)=\int _{0}^z \sqrt{1- m \sin ^2v}\, \mathrm{d}v\).
On \(\partial M_{\pm }\) distance function \(U^+\) is not differentiable.
For specific sharp estimates for \(d=3\), in the context of heat-kernels estimation, see [49, ch.5.1].
Note that we use upper indices for the control’s (velocity components) as they are contravariant.
References
Agrachev, A.A., Barilari, D., Boscain, U.: Introduction to Riemannian and sub-Riemannian geometry (2016). https://webusers.imj-prg.fr/~davide.barilari/2016-11-21-ABB.pdf
Agrachev, A.A., Sachkov, Y.L.: Control Theory from the Geometric Viewpoint. Encyclopaedia of Mathematical Sciences, vol. 87. Springer, Berlin (2004)
Arzela, C.: Sulle funzioni di linee. Mem. Accad. Sci. lst. Bologna Cl. Sci. Fis. Mat. 5(5), 55–74 (1895)
Ascoli, G.: Le curve limiti di una varieta data di curve. Atti della R. Accad. Dei Lincei Memorie della Cl. Sci. Fis. Mat. Nat. 18(3), 521–586 (1883)
Astola, L., Florack, L.M.J.: Finsler geometry on higher order tensor fields and applications to high angular resolution diffusion imaging. Int. J. Comput. Vis. 92(3), 325–336 (2011)
Bao, D., Chern, S.-S., Shen, Z.: An Introduction to Riemann–Finsler Geometry. Graduate Texts in Mathematics. Springer, New York (2000)
Bardi, M., Capuzzo-Dolcetta, I.: Optimal Control and Viscosity Solutions of Hamilton–Jacobi–Bellman Equations. Birkhäuser, Basel (1997)
Bekkers, E., Duits, R., Mashtakov, A., Sanguinetti, G.: A PDE approach to data-driven sub-Riemannian geodesics in SE(2). SIAM J. Imaging Sci. 8(4), 2740–2770 (2015)
Bekkers, E.J., Duits, R., Mashtakov, A., Sachkov, Y.: Vessel tracking via sub-Riemannian geodesics on the projective line bundle. Geometric Science of Information, Volume 10589 in Lecture Notes in Computer Science, November (2017)
Boscain, U., Charlot, G., Rossi, F.: Existence of planar curves minimizing length and curvature. Proc. Steklov Inst. Math. 270(1), 43–56 (2010)
Boscain, U., Duits, R., Rossi, F., Sachkov, Y.: Curve cuspless reconstruction via sub-Riemannian geometry. ESAIM Control Optim. Calc. Var. 20(3), 748–770 (2014)
Caruyer, E., Daducci, A., Descoteaux, M., Houde, J.-C., Thiran, J.-P., Verma, R.: Phantomas: a flexible software library to simulate diffusion MR phantoms. In: ISMRM, Milan (May 2014)
Chen, D.: New minimal paths models for tubular structure extraction and image segmentation. Ph.D. Thesis, University Paris-Dauphine (2016)
Chen, D., Mirebeau, J.-M., Cohen, L.D.: Vessel tree extraction using radius-lifted keypoints searching scheme and anisotropic fast marching method. J. Algorithms Comput. Technol. 10(4), 224–234 (2016)
Chen, D., Mirebeau, J.-M., Cohen, L.D.: Global minimum for a Finsler elastica minimal path approach. Int. J. Comput. Vis. 122, 1–26 (2016)
Citti, G., Sarti, A.: A cortical based model of perceptual completion in the roto-translation space. J. Math. Imaging Vis. 24(3), 307–326 (2006)
Close, T.G., Tournier, J.-D., Calamante, F., Johnston, L.A., Mareels, I., Connelly, A.: A software tool to generate simulated white matter structures for the assessment of fibre-tracking algorithms. NeuroImage 47(4), 1288–1300 (2009)
Crandall, M.G., Ishii, H., Lions, P.-L.: User’s guide to viscosity solutions of second order partial differential equations. Bull. Am. Math. Soc. 27(1), 1–67 (1992)
Crandall, M.G., Lions, P.-L.: Viscosity solutions of Hamilton–Jacobi equations. Trans. Am. Math. Soc. 277(1), 1–42 (1983)
Descoteaux, M., Deriche, R., Knosche, T.R., Anwander, A.: Deterministic and probabilistic tractography based on complex fibre orientation distributions. IEEE Trans. Med. Imaging 28(2), 269–286 (2009)
Dubins, L.E.: On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Am. J. Math. 79(3), 497–516 (1957)
Duits, R., Boscain, U., Rossi, F., Sachkov, Y.: Association fields via cuspless sub-Riemannian geodesics in SE(2). J. Math. Imaging Vis. 49(2), 384–417 (2013)
Duits, R., Felsberg, M., Granlund, G., ter Haar Romeny, B.: Image analysis and reconstruction using a wavelet transform constructed from a reducible representation of the Euclidean motion group. Int. J. Comput. Vis. 72(1), 79–102 (2006)
Duits, R., Ghosh, A., Dela Haije, T., Sachkov, Y.: Cuspless sub-Riemannian geodesics within the Euclidean motion group SE(d). In: Neuromathematics of Vision, Lecture Notes in Morphogenesis, pp. 173–215 (2014). https://doi.org/10.1007/978-3-642-34444-2_5
Duits, R., Ghosh, A., Dela Haije, T.C.J., Mashtakov, A.: On sub-Riemannian geodesics in SE(3) whose spatial projections do not have cusps. J. Dyn. Control Syst. 22(4), 771–805 (2016)
Duits, R., Janssen, M.H.J., Hannink, J., Sanguinetti, G.R.: Locally adaptive frames in the roto-translation group and their applications in medical imaging. J. Math. Imaging Vis. 56(3), 367–402 (2016)
Duits, R., Meesters, S.P.L., Mirebeau, J.-M., Portegies, J.M.: Optimal paths for variants of the 2D and 3D Reeds–Shepp car with applications in image analysis (Dec 2016). arXiv:1612.06137 [math]
Eshagh, M.: Alternative expressions for gravity gradients in local north-oriented frame and tensor spherical harmonics. Acta Geophys. 58(2), 215–243 (2010)
Fehrenbach, J., Mirebeau, J.-M.: Sparse non-negative stencils for anisotropic diffusion. J. Math. Imaging Vis. 49, 1–25 (2013)
Fletcher, P.T., Joshi, S.: Riemannian geometry for the statistical analysis of diffusion tensor data. Sig. Process. 87(2), 250–262 (2007)
Gromov, M.: Carnot–Carathéodory spaces seen from within. In: Sub-Riemannian Geometry, Number 144 in Progress in Mathematics, pp. 79–323. Birkhäuser, Basel (1996). https://doi.org/10.1007/978-3-0348-9210-0_2
Janssen, M.H.J., Janssen, A.J.E.M., Bekkers, E.J., Olivan Bescos, J., Duits, R.: Design and processing of invertible orientation scores of 3D images for enhancement of complex vasculature. Invited Submission to JMIV (Selected Paper at SSVM 2017) (July 2017). arXiv:1707.02191 [cs.CV]
Jbabdi, S., Bellec, P., Toro, R., Daunizeau, J., Pélégrini-Issac, M., Benali, H.: Accurate anisotropic fast marching for diffusion-based geodesic tractography. Int. J. Biomed. Imaging 2008, 2 (2008)
Jbabdi, S., Johansen-Berg, H.: Tractography: where do we go from here? Brain Connect. 1(3), 169–183 (2011)
Lenglet, C., Prados, E., Pons, J., Deriche, R., Faugeras, O.: Brain connectivity mapping using Riemannian geometry, control theory, and PDEs. SIAM J. Imaging Sci. 2(2), 285–322 (2009)
Mashtakov, A., Duits, R., Sachkov, Y., Bekkers, E.J., Beschastnyi, I.: Tracking of lines in spherical images via sub-Riemannian geodesics in SO(3). J. Math. Imaging Vis. 58, 1–26 (2017)
Mashtakov, A.P., Ardentov, A.A., Sachkov, Y.L.: Parallel algorithm and software for image inpainting via sub-Riemannian minimizers on the group of rototranslations. Numer. Math. Theory Methods Appl. 6(01), 95–115 (2013)
Melonakos, J., Mohan, V., Niethammer, M., Smith, K., Kubicki, M., Tannenbaum, A.: Finsler tractography for white matter connectivity analysis of the Cingulum bundle. Med. Image Comput. Comput. Assist. Interv. 10(01), 36–43 (2007)
Melonakos, J., Pichon, E., Angenent, S., Tannenbaum, A.: Finsler active contours. IEEE Trans. Pattern Anal. Mach. Intell. 30(3), 412–423 (2008)
Mirebeau, J.-M.: Efficient fast marching with Finsler metrics. Numer. Math. 126(3), 515–557 (2013)
Mirebeau, J.-M.: Anisotropic fast-marching on cartesian grids using lattice basis reduction. SIAM J. Numer. Anal. 52(4), 1573–1599 (2014)
Mirebeau, J.-M.: Anisotropic fast marching on cartesian grids using Voronoi’s reduction of quadratic forms. Preprint Available on HAL (2017). https://hal.archives-ouvertes.fr/hal-01507334
Mirebeau, J.-M.: Fast marching methods for curvature penalized shortest paths. Preprint Available on HAL (2017) https://hal.archives-ouvertes.fr/hal-01538482. Accepted for publication in the same JMIV-Special issue “Differential Geometry and Orientation Analysis”
Moiseev, I., Sachkov, Y.L.: Maxwell strata in sub-Riemannian problem on the group of motions of a plane. ESAIM Control Optim. Calc. Var. 16(02), 380–399 (2010)
Montgomery, R.: A Tour of Sub-Riemannian Geometries, Their Geodesics and Applications, Volume 91 of Mathematical Surveys and Monographs (2002)
Mori, S.: Introduction to Diffusion Tensor Imaging. Elsevier Science, Amsterdam (2007)
Péchaud, M., Descoteaux, M., Keriven, R.: Brain connectivity using geodesics in HARDI. Med. Image Comput. Comput. Assist. Interv. 12(Pt 2), 482–489 (2009)
Petitot, J.: The neurogeometry of pinwheels as a sub-Riemannian contact structure. J. Physiol. Paris 97(2–3), 265–309 (2003)
Portegies, J.M., Duits, R.: New exact and numerical solutions of the (convection–)diffusion kernels on SE(3). J. Differ. Geom. Appl. 53, 182–219 (2017)
Reeds, J.A., Shepp, L.A.: Optimal paths for a car that goes both forwards and backwards. Pac. J. Math. 145(2), 367–393 (1990)
Rouy, E., Tourin, A.: A viscosity solutions approach to shape-from-shading. SIAM J. Numer. Anal. 29(3), 867–884 (1992)
Sachkov, Y.L.: Cut locus and optimal synthesis in the sub-Riemannian problem on the group of motions of a plane. ESAIM Control Optim. Calc. Var. 17(2), 293–321 (2011)
Sanguinetti, G.R., Bekkers, E.J., Duits, R., Janssen, M.H.J., Mashtakov, A., Mirebeau, J.-M.: Sub-Riemannian fast marching in SE(2). In: Progress in Pattern Recognition, Image Analysis, Computer Vision, and Applications, Number 9423 in Lecture Notes in Computer Science, pp. 366–374. Springer International Publishing, Berlin (2015). https://doi.org/10.1007/978-3-319-25751-8_44
Sepasian, N.: Multi-valued geodesic tractography for diffusion weighted imaging. Ph.D. Thesis, Eindhoven University of Technology, Department of BME (2011)
Sethian, J.A., Vladimirsky, A.: Ordered upwind methods for static Hamilton–Jacobi equations. PNAS 98(20), 11069–11074 (2001)
Stefani, G., Boscain, U., Gauthier, J., Sarychev, A., Sigalotti, M.: Geometric Control Theory and Sub-Riemannian Geometry. Springer INdAM Series (2014)
ter Elst, A.F.M., Robinson, D.W.: Weighted subcoercive operators on lie groups. J. Funct. Anal. 157, 88–163 (1998)
Tournier, J.-D., Calamante, F., Connelly, A.: MRtrix: diffusion tractography in crossing fiber regions. Int. J. Imaging Syst. Technol. 22(1), 53–66 (2012)
Tsitsiklis, J.N.: Efficient algorithms for globally optimal trajectories. IEEE Trans. Autom. Control 40(9), 1528–1538 (1995)
Tuch, D.S.: Q-ball imaging. Magn. Reson. Med. 52(6), 1358–1372 (2004)
Vladimirsky, A.: Static PDEs for time-dependent control problems. Interfaces Free Bound. 8, 281–300 (2006)
Acknowledgements
The authors gratefully acknowledge Dr. G. R. Sanguinetti for fruitful discussion and ideas leading up to this article. We thank E. J. Bekkers for his assistance with and suggestions for Fig. 14.
Author information
Authors and Affiliations
Corresponding author
Additional information
The research leading to the results of this article has received funding from the European Research Council under the European Communitys 7th Framework Programme (FP7/20072014)/ERC Grant Agreement No. 335555 (Lie Analysis). This work was partly funded by ANR Grant NS-LBR. ANR-13-JS01-0003-01.
Appendices
A Well-Posedness and Convergence of the Reeds–Shepp Models
We introduce in ‘Appendix A.1’ section some general elements of control theory, which are specialized in ‘Appendix A.2’ section to the Reeds–Shepp models and their approximations.
1.1 A.1 Closedness of Controllable Paths
In this section, we introduce the notion of an admissible path \(\gamma \) with respect to some controls \(\mathfrak {B}\). We state in Theorem 5 a closedness result, slightly generalizing the one from [13], from which we deduce in Corollaries 3 and 4 an existence and a convergence result for a minimum time optimal control problem. The first ingredient of this approach is the notion of Hausdorff distance on a metric space.
Definition 5
Given a metric space \(\mathbb {E}\), we let \({{\mathcal K}}(\mathbb {E})\) be the collection of non-empty compact subsets of \(\mathbb {E}\). The distance function \(d_A : \mathbb {E}\rightarrow \mathbb {R}_+\) and the Hausdorff distance \(\mathcal {H}(A,B)\), where \(A,B \in {{\mathcal K}}(\mathbb {E})\), are defined, respectively, by
In the following, we fix a closed set \(\mathbb {X}\), contained in an Euclidean vector space \(\mathbb {E}\), or in a complete Riemannian manifold \({\mathbb {M}}\). In the applications considered in this paper, \(\mathbb {X}\) is of the form \(\mathbb {X}_0\times \mathbb {S}^{d-1}\), where \(\mathbb {X}_0 \subset \mathbb {R}^d\) is some image domain, see Fig. 14, or the set of accessible points in a map (which excludes the walls), see Fig. 13. The embedding space can be the vector space \(\mathbb {E}= \mathbb {R}^d \times \mathbb {R}^d\), which is an acceptable but rather extrinsic point of view, or the Riemannian manifold \({\mathbb {M}} = \mathbb {R}^d \times \mathbb {S}^{d-1}\), equipped with the metric \(\mathcal {G}_\varepsilon \) for some arbitrary but fixed \(\varepsilon >0\), see (25).
We equip the collection of all Lipschitz paths \(\varGamma := {{\mathrm{Lip}}}([0,1], \mathbb {X})\) with the topology of uniform convergence. We will make use of Ascoli’s lemma [3, 4], which states that any uniformly bounded and equicontinuous sequence of paths admits a converging subsequence. In our case the paths are Lipschitz with a common Lipschitz constant.
Definition 6
Given a normed vector space V, we denote by \(\mathfrak {C}(V) \subset {{\mathcal K}}(V)\) the collection of non-empty compact subsets of V, which are convex and contained in the unit ball.
Remark 13
The restriction to convex subsets is essential. For a uniformly converging sequence of Lipschitz functions \(\gamma _n: [0,1] \rightarrow {\mathbb {M}}\) with limit \(\gamma _*\), with \(\dot{\gamma }_n(t) \in K\) for a.e. \(t \in [0,1]\) and K a compact set, we can deduce that \(\dot{\gamma }_* \in \mathrm {Hull}(K)\), for a.e. \(t \in [0,1]\). The convexity then guarantees that \(\dot{\gamma }_* \in K = \mathrm {Hull}(K)\).
Definition 7
A family of controls \(\mathcal {B}\) on the set \(\mathbb {X}\) is an element of the set \(\mathfrak {B}\) defined by
-
If \(\mathbb {X}\subset \mathbb {E}\) an Euclidean vector space, then \(\mathfrak {B}:= C^0(\mathbb {X},\mathfrak {C}(\mathbb {E}))\).
-
If \(\mathbb {X}\subset {\mathbb {M}}\) a Riemannian manifold, then \(\mathfrak {B}:= \{\mathcal {B}\in C^0(\mathbb {X}, {{\mathcal K}}(T{\mathbb {M}})) \; | \; \forall {\mathbf {p}}\in \mathbb {X},\, \mathcal {B}({\mathbf {p}})\in \mathfrak {C}(T_{\mathbf {p}}{\mathbb {M}})\}\).
In both cases, \(\mathfrak {B}\) is equipped with the topology of locally uniform convergence.
Definition 8
A path \(\gamma \) is \(T\mathcal {B}\)-admissible, where \(\gamma \in \varGamma \), \(T\in \mathbb {R}_+\) and \(\mathcal {B}\in \mathfrak {B}\), iff for almost every \(t \in [0,1]\)
We denoted \(T B := \{T {\mathbf {v}}\; | \; {\mathbf {v}}\in B\}\), where \(T \in \mathbb {R}_+\) and B is a subset of a vector space. Note the potential conflict of notation with the tangent space \(T {\mathbb {M}}\) to the embedding manifold \({\mathbb {M}}\), which should be clear from context. If a path \(\gamma \) is \(T\mathcal {B}\)-admissible for some controls \(\mathcal {B}\in \mathfrak {B}\), then it must be T-Lipschitz. The following result slightly extends, for our convenience, Corollary A.5 in [13].
Theorem 5
The set \(\{(\gamma ,T,\mathcal {B})\in \varGamma \times \mathbb {R}_+\times \mathfrak {B}\; | \; \gamma \hbox { is } T\mathcal {B}-\hbox { admissible }\}\) is closed.
Proof
Let \((\gamma _n, T_n,\mathcal {B}_n)\) be sequences of paths, times and controls converging to \((\gamma _\infty , T_\infty , \mathcal {B}_\infty )\), and such that \(\gamma _n\) is \(T_n\mathcal {B}_n\)-admissible for all \(n \ge 0\). Since the paths \(\gamma _n\) are converging as \(n \rightarrow \infty \), they lay in a common compact subset \(\mathbb {X}'\) of the closed domain \(\mathbb {X}\), recall Remark 13. As a result, the restricted controls \(\mathcal {B}_n' := (\left. \mathcal {B}_n\right| _{\mathbb {X}'})\) are uniformly converging as \(n \rightarrow \infty \). In the case where \(\mathbb {X}\subset \mathbb {E}\) a Euclidean space, applying Corollary A.5 in [13] to the sequence \((\gamma _n, T_n\mathcal {B}'_n)\) we obtain that \(\gamma _\infty \) is \(T_\infty \mathcal {B}_\infty \)-admissible as announced.
In the case where \(\mathbb {X}\subset {\mathbb {M}}\) a Riemannian manifold, an additional proof ingredient is required. Let \({\mathbb {M}}'\) be an open neighborhood of \(\mathbb {X}'\) with compact closure in \({\mathbb {M}}\), and let \(\mathcal {I}: {\mathbb {M}}' \rightarrow \mathbb {E}\) be an embedding (i.e., an injective immersion) with bounded distortion of the manifold \({\mathbb {M}}'\) into a Euclidean space \(\mathbb {E}\) of sufficiently high dimension, which by Whitney’s embedding theorem is known to exist. Define the set \(\mathbb {X}'' := \mathcal {I}(\mathbb {X}')\), the paths \(\gamma ''_n := \mathcal {I}\circ \gamma _n\), and controls \(\mathcal {B}''_n(\mathcal {I}({\mathbf {p}})) := \mathrm{d}\mathcal {I}({\mathbf {p}}, \mathcal {B}_n({\mathbf {p}}) )\) for all \({\mathbf {p}}\in \mathbb {X}'\) and \(n\in \mathbb {N}\cup \{\infty \}\). Applying again Corollary A.5 in [13] we obtain that \(\gamma ''_\infty \) is \(T_\infty \mathcal {B}''_\infty \) admissible and hence that \(\gamma _\infty \) is \(T_\infty \mathcal {B}_\infty \)-admissible as announced. \(\square \)
In line with identity (9), we rely on the following definition where we rescale the time interval to [0, 1].
Definition 9
For any \(\mathcal {B}\in \mathfrak {B}\), \({\mathbf {p}}, {\mathbf {q}}\in \mathbb {X}\), we let
Corollary 3
If \(\mathcal {B}\in \mathfrak {B}\), \({\mathbf {p}},{\mathbf {q}}\in \mathbb {X}\) are such that \(T_\mathcal {B}({\mathbf {p}}, {\mathbf {q}}) < \infty \), then inf. (52) is attained.
Proof
Let \(T := T_\mathcal {B}({\mathbf {p}}, {\mathbf {q}})\), and for each \(0 < \varepsilon \le 1\) let \(\gamma _\varepsilon \) be a \((T+\varepsilon )\mathcal {B}\)-admissible path from \({\mathbf {p}}\) to \({\mathbf {q}}\), which is thus \((T+1)\)-Lipschitz. By Arzela–Ascoli’s lemma [3, 4] there exists a converging sequence of paths \(\gamma _{\varepsilon _n}\rightarrow \gamma _0\) as \(n \rightarrow \infty \). The limit path \(\gamma _0\) is \(T\mathcal {B}\)-admissible by Theorem 5, and the result follows. \(\square \)
Corollary 4
For all \(\varepsilon \in [0,1]\) let \(\mathcal {B}_\varepsilon \in \mathfrak {B}\). Assume that \(\mathcal {B}_\varepsilon \rightarrow \mathcal {B}_0\) as \(\varepsilon \rightarrow 0\), and that \(\mathcal {B}_\varepsilon ({\mathbf {p}}) \supset \mathcal {B}_0({\mathbf {p}})\) for all \(\varepsilon \ge 0\), \({\mathbf {p}}\in \mathbb {X}\). Then
Let \(T_\varepsilon := T_{\mathcal {B}_\varepsilon }({\mathbf {p}},{\mathbf {q}})\) for each \(\varepsilon \ge 0\). Assume in addition that there exists a unique \(T_0\mathcal {B}_0\)-admissible path \(\gamma _0\) from \({\mathbf {p}}\) to \({\mathbf {q}}\), and for each \(\varepsilon >0\) denote by \(\gamma _\varepsilon \) an arbitrary path from \({\mathbf {p}}\) to \({\mathbf {q}}\) which is \((\varepsilon +T_\varepsilon )\mathcal {B}_\varepsilon \) admissible. Then \(\gamma _\varepsilon \rightarrow \gamma _0\) as \(\varepsilon \rightarrow 0\).
Proof
The inclusion \(\mathcal {B}_\varepsilon ({\mathbf {p}}) \subset \mathcal {B}_0({\mathbf {p}})\), \(\forall {\mathbf {p}}\in {\mathbb {M}}\), implies the inequality \(T_\varepsilon \le T_0\), for all \(\varepsilon \ge 0\). Denoting \(T_* := \limsup T_\varepsilon \) as \(\varepsilon \rightarrow 0\), we thus observe that \(T_* \le T_0\). For the reverse inequality \(T_* \ge T_0\), we apply Arzela–Ascoli lemma to the family of paths \((\gamma _\varepsilon )_{0 < \varepsilon \le 1}\) which are \((T_0+1)\)-Lipschitz by construction, and obtain a converging subsequence of paths \(\gamma _{\varepsilon _n} \rightarrow \gamma _*\). Theorem 5 implies the admissibility of \(\gamma _*\) with respect to the controls \(T_* \mathcal {B}_0\). Thus \(T_*\ge T_0\) but since \(T_* \le T_0\), we must have \(T_*=T_0\), and \(\gamma _* = \gamma _0\) by the uniqueness assumption. The result follows. \(\square \)
More generally, if infimum (52) is realized by a family \((\gamma _i)_{i \in I}\) of paths, then for any sequence \(\varepsilon _n\rightarrow 0\) one can find a subsequence such that \(\gamma _{\varepsilon _{\varphi (n)}} \rightarrow \gamma _i\) as \(n \rightarrow \infty \) for some \(i \in I\).
1.2 A.2 Specialization to the Reeds–Shepp Models
We begin this section by recalling, and slightly generalizing, the notion of Finsler metric introduced in Sect. 2.2. We then prove that the Reeds–Shepp metrics \(\mathcal {F}_0\) and \(\mathcal {F}_0^+\) are indeed Finsler metrics in this sense.
Definition 10
A metric on a complete Riemannian manifold \({\mathbb {M}}\) is a map \(\mathcal {F}: T{\mathbb {M}} \rightarrow [0,+\infty ]\). With respect to the second variable, it must be 1-homogeneous, convex and bounded below by \(\delta \Vert {\cdot }\Vert \), where \(\delta \) is a positive constant. In terms of regularity, the sets \(\mathcal {B}_\mathcal {F}({\mathbf {p}}) := \{\dot{{\mathbf {p}}} \in T_{\mathbf {p}}{\mathbb {M}} \; | \; \mathcal {F}({\mathbf {p}}, \dot{{\mathbf {p}}}) \le 1\}\) must be closed and depend continuously on \({\mathbf {p}}\in {\mathbb {M}}\) with respect to the Hausdorff distance on \(T {\mathbb {M}}\).
The next proposition is due to (9).
Proposition 2
With the notations of Definition 10, the sets \({\mathbf {p}}\in {\mathbb {M}} \mapsto \mathcal {B}_\mathcal {F}({\mathbf {p}})\) form a family of controls on \(({\mathbb {M}}, \delta \Vert {\cdot }\Vert )\). In addition for all \({\mathbf {p}},{\mathbf {q}}\in {\mathbb {M}}\)
Proposition 3
The Reeds–Shepp metrics \((\mathcal {F}_\varepsilon )_{0\le \varepsilon \le 1}\) and \((\mathcal {F}_\varepsilon ^+)_{0\le \varepsilon \le 1}\) are indeed metrics in the sense of Definition 10, for any \(\varepsilon \in [0,1]\). The associated controls \(\mathcal {B}_\varepsilon := \mathcal {B}_{\mathcal {F}_{\varepsilon }}\), \(\mathcal {B}_\varepsilon ^+:= \mathcal {B}_{\mathcal {F}_{\varepsilon }^+}\) depend continuously on the parameter \(\varepsilon \in [0,1]\), and satisfy the inclusions \(\mathcal {B}_\varepsilon ({\mathbf {p}}) \subset \mathcal {B}_{\varepsilon '}({\mathbf {p}})\) and \(\mathcal {B}_\varepsilon ^+({\mathbf {p}}) \subset \mathcal {B}_{\varepsilon '}^+({\mathbf {p}})\) for any \({\mathbf {p}}\in {\mathbb {M}}\) and \(0 \le \varepsilon \le \varepsilon ' \le 1\).
Proposition 3 allows to apply the results of ‘Appendix A.1’ section to the Reeds–Shepp metrics. Theorem 2 then directly follows from Corollary 4. The only remaining non-trivial claim in Proposition 3 is the continuity of the controls on \({\mathbb {M}}\), recall Definition 7, and their convergence \(\mathcal {B}_\varepsilon \rightarrow \mathcal {B}_0\) as \(\varepsilon \rightarrow 0\), as required in Corollary 4. These two properties are implied by the continuity on \([0,1]\times {\mathbb {M}}\), that we next prove, of the following maps
with defined in Definition 6 and equipped with the Hausdorff distance.
Lemma 5
Let B be a compact subset of a metric space \(\mathbb {E}\), and let \(\varphi \in C^0(B,\mathbb {E})\). Then
This basic lemma, stated without proof, is used in the next lemma to obtain an explicit estimate of the Hausdorff distance between the controls sets of the Reeds–Shepp models.
Lemma 6
Let \(\mathbf {n}_1,\mathbf {n}_2\in \mathbb {S}^{d-1}\), let \(a_1,a_2,b_1,b_2 \ge 1\) and let \(\varepsilon _1,\varepsilon _2\in [0,1]\). For each \(i\in \{1,2\}\), let \(B_i\) be the collection of all \((\dot{{{\mathbf {x}}}},\dot{\mathbf {n}})\in \mathbb {R}^d\times \mathbb {R}^d\) obeying
The same estimate holds for the sets \(B_i^+\), \(i \in \{1,2\}\), defined by the inequalities
Proof
It suffices to establish announced estimate (54) when the tuples \((a_i,b_i,\mathbf {n}_i,\varepsilon _i)\), \(i \in \{1,2\}\), differ by a single element of the four, and then to use the subadditivity of the Hausdorff distance. In each case we apply Lemma 5 to a well-chosen surjective map \(\varphi : B_1 \rightarrow B_2\) (resp. \(\varphi ^+ : B_1^+ \rightarrow B_2^+\)).
-
Case \(a_1 \ne a_2\). Assume w.l.o.g. that \(a_1 < \infty \), and observe that for all \((\dot{{{\mathbf {x}}}},\dot{\mathbf {n}})\in B_1\) one has \(a_1\Vert \dot{{{\mathbf {x}}}}\Vert \le 1\), hence \(\Vert a_1 \dot{{{\mathbf {x}}}} / a_2 -\dot{{{\mathbf {x}}}}\Vert \le |a_1^{-1}-a_2^{-1}|\). Choose \(\varphi (\dot{{{\mathbf {x}}}}, \dot{\mathbf {n}}) := (a_1 \dot{{{\mathbf {x}}}} / a_2, \dot{\mathbf {n}})\).
-
Case \(b_1\ne b_2\). As above, with \(\varphi (\dot{{{\mathbf {x}}}}, \dot{\mathbf {n}}) := (\dot{{{\mathbf {x}}}}, b_1 \dot{\mathbf {n}} / b_2)\), yielding upper bound \(|b_{1}^{-1}-b_{2}^{-1}|\).
-
Case \(\mathbf {n}_1\ne \mathbf {n}_2\). Let R be the rotation of \(\mathbb {R}^d\) which maps \(\mathbf {n}_1\) onto \(\mathbf {n}_2\), in such a way that it maps the space orthogonal to the plane \(\mathrm{Span}(\mathbf {n}_1,\mathbf {n}_2)\) onto itself. A simple calculation yields \(\Vert R-{{\mathrm{Id}}}\Vert = 2 \sin [ \frac{1}{2} \cos ^{-1}(\mathbf {n}_1 \cdot \mathbf {n}_2)] = \sqrt{2(1-\mathbf {n}_1\cdot \mathbf {n}_2)}\). The result follows by choosing \(\varphi (\dot{{{\mathbf {x}}}}, \dot{\mathbf {n}}) := (R \dot{{{\mathbf {x}}}}, R \dot{\mathbf {n}})\), so that \(\Vert \varphi (\dot{{{\mathbf {x}}}}, \dot{\mathbf {n}}) - (\dot{{{\mathbf {x}}}}, \dot{\mathbf {n}})\Vert \le \Vert R-{{\mathrm{Id}}}\Vert \sqrt{\Vert \dot{\mathbf {n}}\Vert ^2+\Vert \dot{{{\mathbf {x}}}}\Vert ^2} \le \Vert R-{{\mathrm{Id}}}\Vert \) for all \((\dot{{{\mathbf {x}}}},\dot{\mathbf {n}}) \in B_1\) as announced.
-
Case \(\varepsilon _1 \ne \varepsilon _2\). Assume w.l.o.g. that \(\varepsilon _1 > 0\), and consider the orthogonal projections
$$\begin{aligned} P_1(\dot{{{\mathbf {x}}}})&:= (\dot{{{\mathbf {x}}}} \cdot \mathbf {n}_1) \mathbf {n}_1&P_1^\perp (\dot{{{\mathbf {x}}}})&:= (\text {Id}-P_1)(\dot{{{\mathbf {x}}}}). \end{aligned}$$Note that \(P_1^\perp (\dot{{{\mathbf {x}}}}) \le \varepsilon _1\) if \((\dot{{{\mathbf {x}}}},\dot{\mathbf {n}}) \in B_1\), and that \(\Vert \dot{{{\mathbf {x}}}}\Vert \le \varepsilon _1\) if \((\dot{{{\mathbf {x}}}},\dot{\mathbf {n}}) \in B_1^+\) and \(\dot{{{\mathbf {x}}}} \cdot \mathbf {n}_1 \le 0\). The result follows by choosing
$$\begin{aligned} \varphi (\dot{{{\mathbf {x}}}},\dot{\mathbf {n}})&:= \left( P_1(\dot{{{\mathbf {x}}}})+ \frac{\varepsilon _2}{\varepsilon _1} P_1^\perp (\dot{{{\mathbf {x}}}}), \dot{\mathbf {n}} \right) , \\ \varphi ^+(\dot{{{\mathbf {x}}}},\dot{\mathbf {n}})&:= {\left\{ \begin{array}{ll} \varphi (\dot{{{\mathbf {x}}}},\dot{\mathbf {n}}) &{} \text {if } \dot{{{\mathbf {x}}}} \cdot \mathbf {n}_1 \ge 0,\\ ( \frac{\varepsilon _2}{\varepsilon _1} \dot{{{\mathbf {x}}}},\dot{\mathbf {n}}) &{} \text {otherwise.} \end{array}\right. } \end{aligned}$$
\(\square \)
Proof of Proposition 3
Since working with Hausdorff distances on the abstract tangent bundle \(T {\mathbb {M}}\) is not very practical, we make use of the canonical embedding \(\mathcal {I}: \mathbb {R}^d \times \mathbb {S}^{d-1} \rightarrow \mathbb {R}^d \times \mathbb {R}^d\) of the manifold \({\mathbb {M}}\) into the Euclidean vector space \(\mathbb {R}^{2d}\) given by \((\mathbf {x},\mathbf {n}) \mapsto (\mathbf {x},\mathbf {n})\), which has bounded distortion. It suffices to prove the continuity of the image of the control sets \((\varepsilon ,{\mathbf {p}}) \rightarrow \mathrm{d}\mathcal {I}({\mathbf {p}},\mathcal {B}_{\mathcal {F}_\varepsilon }({\mathbf {p}}))\) (resp. likewise with \(\mathcal {F}_\varepsilon ^+\)) by the tangent maps to this embedding, which follows by Lemma 6. Indeed the lemma shows that
and it includes the spherical constraint via the velocity constraint \(\dot{\mathbf {n}} \cdot \mathbf {n}_i = \frac{d}{dt}(\mathbf {n}(t) \cdot \mathbf {n}(t))|_{t = 0} = 0\) for a smooth curve \(\gamma (t) = ({{\mathbf {x}}}(t),\mathbf {n}(t))\) passing through \(\gamma (0) = ({{\mathbf {x}}}_i,\mathbf {n}_i)\). \(\square \)
B Backtracking of Geodesics in \((\mathbb {M},d_{\mathcal {F}})\)
This section is devoted to a generic ingredient in the proof of Theorem 4, regarding backtracking of Geodesics in the (quasi)-metric space \((\mathbb {M},d_{\mathcal {F}})\) in general. Although these results are standard in Finsler geometry, we aim to provide a concise overview.
Lemma 7
Let F be an asymmetric norm on a vector space \(\mathbb {E}\), and assume that \(F^*\) is differentiable at \(\hat{{\mathbf {p}}}\in \mathbb {E}^*\). Then
Proof
The 1st claim follows by differentiation of \(F^*\)
The 2nd claim is Euler’s formula for homogeneous functions. \(\square \)
Proposition 4
Let \({\mathbf {p}}_\mathrm{S},{\mathbf {p}}_\mathrm{T}\in {\mathbb {M}}\), let \(\gamma \) be a minimizing geodesic from \({\mathbf {p}}_\mathrm{S}\) to \({\mathbf {p}}_\mathrm{T}\) w.r.t. a continuous metric \(\mathcal {F}\) and let \(t \in [0,1]\). Assume that the distance map U from \({\mathbf {p}}_\mathrm{S}\) is differentiable at \(\gamma (t)\), and that the dual metric \(\mathcal {F}^*\) is differentiable w.r.t. the second variable at \((\gamma (t), \mathrm{d}U(\gamma (t)))\). Then \(\gamma \) is differentiable at time t and with \(L := d_\mathcal {F}({\mathbf {p}}_\mathrm{S}, {\mathbf {p}}_\mathrm{T})\)
Proof
The path \(\gamma \) has constant speed L, and \(t\mapsto U(\gamma (t))\) increases linearly from 0 to L on it. Let \(t \in [0,1]\) be as in the statement of the proposition, and let
for some sequence \(\varepsilon _n\rightarrow 0\). Then
For typographic simplicity let us denote \({\mathbf {p}}:= \gamma (t)\), \(\dot{{\mathbf {p}}} := \dot{\varGamma }(t)\), \(F = \mathcal {F}({\mathbf {p}}, \cdot )\) and \(F^* := \mathcal {F}^*({\mathbf {p}},\cdot )\). By Lemma 7 and eikonal equation (5), the vector \(\dot{{\mathbf {q}}} = \mathrm{d}F^*( \mathrm{d}U( {\mathbf {p}}))\) obeys
Note that the duality-bracket/norm inequality is saturated by \(\langle \mathrm{d}U({\mathbf {p}}),\dot{{\mathbf {q}}}\rangle = 1 =F^*(\mathrm{d}U({\mathbf {p}}))F(\dot{{\mathbf {q}}})\), and that the assumed differentiability of the dual norm \(F^*\) at the point \(\hat{{\mathbf {p}}}= \mathrm{d}U({\mathbf {p}})\) implies the strict convexity of the primal norm F (up to 1-homogeneity) at the point \(\mathrm{d}F^*(\hat{{\mathbf {p}}}) = \dot{{\mathbf {q}}}\). Hence \(\dot{{\mathbf {q}}}\) is the unique solution to the system ‘\(F^*(\dot{{\mathbf {q}}})=1\) and \(\langle \mathrm{d}U({\mathbf {p}}), \dot{{\mathbf {q}}}\rangle =1\),’ and therefore \(\dot{\varGamma } = L \dot{{\mathbf {q}}}\). This implies the differentiability of \(\gamma \) at time t and announced equality (55). \(\square \)
Remark 14
(Lagrangians and Hamiltonians) Given an arbitrary Finsler metric \(\mathcal {F}\) on \({\mathbb {M}}\), its half-square \(\mathfrak {L}:= \frac{1}{2} \mathcal {F}^2 : T({\mathbb {M}}) \rightarrow [0,+\infty ]\) is usually called the Lagrangian. Shortest path problem (1) can be reformulated in terms of the Lagrangian, thanks to the Cauchy–Schwartz’s inequality which gives
A path \(\gamma \) is a minimizer of (56) iff it is simultaneously normalized and a minimizer of (1). The Hamiltonian \(\mathfrak {H}\) is the Legendre–Fenchel transform of its Lagrangian \(\mathfrak {L}\) w.r.t. the second variable; hence, \(\mathfrak {H}= \frac{1}{2} (\mathcal {F}^*)^2\) (for details see [6, ch.14.8]). The eikonal equation can thus be rephrased in terms of the Hamiltonian:
The Hamiltonian can also be used to reformulate the backtracking ODE of geodesics, thanks to the following identity which follows from the eikonal equation: for any \({\mathbf {p}}\in {\mathbb {M}}\)
In geometric control theory this Hamiltonian is often referred to the ‘fixed time Hamiltonian of the action functional,’ cf. [2, 8, 52], and is typically used [44] in the Pontryagin maximum principle [2] for (sub-)Riemannian geodesics.
C Characterization of Cusps: Proof of Lemma 1
Consider Lemma 1. The structure of this lemma is \(a \Leftrightarrow b \Leftrightarrow c\). The implication \(a \Rightarrow b\) is trivial. The equivalence \(b \Leftrightarrow c\) follows by Theorems 2 and 4. The implication \(b \Rightarrow a\) remains.
Suppose the d-th spatial control aligned with \(\mathbf {n}(t_0)\), recall (19), vanishes: \(\tilde{u}(t_0)=0\). Now we show by contradiction that in this case \(\dot{\tilde{u}}(t_0) \ne 0\). Suppose \(\tilde{u}(t_0)=\dot{\tilde{u}}(t_0)=0\).
Then by application of the PMP (Pontryagin’s maximum principle), similar to [8, Appendix A], [24]) and coercivity/invertibility of the SR-metric tensor \(\left. \mathcal {G}_{0}\right| _{\gamma (t_0)}\), recall (25), constrained to the horizontal part of the tangent space \(\left. \Delta \right| _{\gamma (t)}=\{({\mathbf {p}}_0=(\mathbf {x}_0,\mathbf {n}_{0}), \dot{{\mathbf {p}}}_0=(\dot{\mathbf {x}}_0,\dot{\mathbf {n}}_{0})) \in T(\mathbb {M}) \; |\; \mathbf {n}_{0} \equiv \dot{{{\mathbf {x}}}}_0\}\), that the (analytic) spatial control variable \(\tilde{u}= \mathcal {C}_{1}^{-2}\tilde{\lambda }\) vanishes for all times. (For \(d=2\) this is directly deduced from the pendulum phase portrait [44] in momentum space.) This leaves only purely angular momentum and motion, contradicting \(\dot{\mathbf {x}}(\cdot ) \ne \mathbf {0}\) in Lemma 1.
Next we verify \( \tilde{u}(t_0)=\dot{\tilde{u}}(t_0)=0 \Rightarrow \dot{\tilde{\lambda }}(t_0)=0 =\tilde{\lambda }(t_0)\). By the chain rule for differentiation (applied to the d-th spatial momentum component \(\tilde{\lambda }(t)= \langle \lambda (t), (\mathbf {n}(t), \mathbf {0})\rangle \)):
We deduce from PMP’s Hamiltonian equations (cf. [24]) that
D Table of Notations
See Table 1.
Rights and permissions
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
About this article
Cite this article
Duits, R., Meesters, S.P.L., Mirebeau, JM. et al. Optimal Paths for Variants of the 2D and 3D Reeds–Shepp Car with Applications in Image Analysis. J Math Imaging Vis 60, 816–848 (2018). https://doi.org/10.1007/s10851-018-0795-z
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10851-018-0795-z