Optimal Paths for Variants of the 2D and 3D Reeds–Shepp Car with Applications in Image Analysis

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 \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathbb {R}^d \times {\mathbb {S}}^{d-1}$$\end{document}Rd×Sd-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 \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathbb {R}^{d} \times \mathbb {S}^{d-1}$$\end{document}Rd×Sd-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 \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$d=2$$\end{document}d=2 and brain connectivity measures from diffusion-weighted MRI data for the case \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$d=3$$\end{document}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.


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 [49] 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 R 2 × (R/2πZ), equipped with the natural Euclidean metric but only among curves γ(t) = (x(t), y(t), θ(t)) subject to the constraint that ( ẋ(t), ẏ(t)) is proportional to (cos θ(t), sin θ(t)).Formulating the problem this way, it becomes one of the simplest examples of sub-Riemannian (SR) geometry: the Fig. 1 Top: A car can only move in its current orientation or change its current orientation.In other words, when the path γ(t) = (x(t), y(t), θ(t)) is considered as indicated in the left figure, the tangent γ(t) is restricted to the span of (cos θ(t), sin θ(t), 0) and (0, 0, 1), of which the green plane on the right is an example.Bottom: the meaning of shortest path between points in an image is determined by a combination of a cost computed from the data, the restriction above, and a curvature penalization.The path optimization problem is formulated on the position-orientation domain such as in the image on the right.The cost for moving through the orange parts is lower than elsewhere.
tangent vector γ(t) is constrained to remain in the span of (cos θ(t), sin θ(t), 0) and (0, 0, 1), see Fig. 1.The SR curve optimization problem and the properties of its geodesics in R 2 × S1 have been studied and applied in image analysis by [47,16,22,11,36,2], and in particular for modelling the Reeds-Shepp car in [43,10,51], whereas the latter presented a complete and optimal synthesis for the geometric control problem on R 2 × S 1 with uniform cost.Properties of SR geodesics in R d × 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 [44].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 R 2 ×S 1 .The projection on 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 con-sider 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.We fix the dimension d ∈ {2, 3} , and let M := R d ×S d−1 be the 2d − 1 dimensional manifold of positions and orientations.We use a Finsler metric on the tangent bundle of M, F : T (M) → [0, +∞] , of which specific properties are discussed later, to define a geometry on M. Any such Finsler metric F induces a measure of length Length F on the class of paths with Lipschitz regularity, defined as 1   Length F (γ) := 1 0 F(γ(t), γ(t)) dt, with the convention γ(t) := d dt γ(t).The path is said to be normalized w.r.t.F iff F(γ(t), γ(t)) = Length F (γ) for all t ∈ [0, 1].Any Lipschitz continuous path of finite length can be normalized by a suitable reparametrization.Finally, the quasi-distance d F : M × M → [0, +∞] is defined for all p, q ∈ M by with Γ := Lip([0, 1], M).Normalized minimizers of (1) are called minimizing geodesics from p to q w.r.t.F. For certain pairs (p, q) these minimizers may not be unique, and these points are often of interest, see for example [43,9] Definition 1 (Maxwell point) Let p S ∈ M be a fixed point source and γ ∈ Γ a geodesic connecting The black arrows indicate the begin and end condition in the plane, corresponding to the blue dots in R 2 × S. The paths in the lifted space are smooth, but vertical tangents appear in both cases.In the left figure, the projection of the path has two cusps, and the first and last part of the path is traversed backwards (the red parts).On the right, backward motion is not possible.Instead, according to our model, the shortest path is a concatenation of an in-place rotation (green), a SR geodesic, and again an in-place rotation.Bottom: corresponding control sets as defined in (7) for the allowed velocities at each position and orientation, with B F 0 on the left and B F + 0 on the right.
p S with q ∈ M, q = p S .Then q is a Maxwell point if there exists another extremal path γ ∈ Γ connecting p S and q, with Length F (γ) = Length F (γ).If q is the first point (distinct from p S ) on γ where such γ exists, then q is called the first Maxwell point.The curves γ, γ lose global optimality after the first Maxwell point.
Remark 1 (Terminology) We use the common terminology of 'Finsler metric' for F, although it is also called 'Finsler function', 'Finsler norm' or 'Finsler structure', and despite the fact that F is not a metric (distance) in the classical sense.The Finsler metric F induces the quasi-distance d F as defined in (1).If F(p, ṗ) = F(p, − ṗ) for all p ∈ M and tangent vectors ṗ ∈ T p (M), then d F is a true metric, satisfying d F (p, q) = d F (q, p) for all p, q ∈ M.However, to avoid confusion of the word metric, we will only refer to d F as a distance or quasi-distance.If the 'Finsler metric' F is induced by a metric tensor field G on Riemannian manifold (M, G) then one has F(p, ṗ) = G| p ( ṗ, ṗ).
Throughout the document, we use the words path and curve synonymously.When we consider the 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.

Geometry of the Reeds-Shepp model
We introduce the Finsler metric F 0 underlying the Reeds-Shepp car model, and the Finsler metric F + 0 corresponding to the variant without reverse gear.Let (p, ṗ) ∈ T (M) be a pair consisting of a point p ∈ M and a tangent vector ṗ ∈ T p (M) at this point.The physical and angular components of a point p ∈ M are denoted by x ∈ R d and n ∈ S d−1 , and this convention carries over to the tangent: We say that ẋ is proportional to n, that we write as ẋ ∝ n, iff there exists a λ ∈ R such that ẋ = λn.Define otherwise. (2) Here • denotes the norm and "•" the usual inner product on the Euclidean space R d .The functions C 1 and C 2 are assumed to be continuous on M, and uniformly bounded from below by a positive constant δ > 0. In applications, C 1 and 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 F to have the dimension [T ] of a travel time, then C −1 1 is a physical, (strictly) spatial velocity [Length][T ] −1 , and C −1 2 is an angular velocity [Rad][T ] −1 .For simplicity one often sets C 1 = ξC 2 , where ξ −1 > 0 is a unit of spatial length.The special case C 1 (p) = ξC 2 (p) = ξ for all p ∈ M is referred to as the uniform cost case.

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 p S ∈ M be an arbitrary source point, and let U be the associated distance function Then U is the unique viscosity solution [19,18] to the eikonal PDE: Here F * is the dual metric of F and dU is the differential of the distance map U .However, for these relations to hold, and for numerical discretization to be practical, F should be at least continuous 2 .We therefore propose in Section 2.3 for both F 0 and F + 0 an approximating metric, that we denote by F ε and F + ε , respectively, that are continuous and converge to F 0 and F + 0 2 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 {p S } as the target set.
as ε → 0. The approximating metrics correspond to a highly anisotropic Riemannian and Finslerian metric, rather than a sub-Riemannian or sub-Finslerian metric.The metric F is in line with previous approximations [16,8,52] for the case d = 2.
We design a monotone and causal discretization scheme for the 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 F. We provide constructions for the metrics F ε or F + ε of interest, based on the earlier works [40,39].

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 fastmarching solver over the numerical method in [8] in this setting was demonstrated by Sanguinetti et al. [52].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 gray-scale images are first lifted to an orientation score defined on the higher dimensional manifold R 2 × S 1 .There, the combination of the sub-Riemannian metric, the cost function derived from the orientation score, and the numerical fast-marching solver, provided a solid approach to accurately track vessels in challenging sets of images.
In the previous works [8] and [9] the clear advantage of sub-Riemannian geometrical models over isotropic Riemannian models on R 2 × S 1 has been shown with many experiments 3 .
In this work we will show similar benefits for our sub-Riemannian tracking in R 3 × S 2 .In general, regardless the choice of image dimension d ∈ {2, 3}, one has that our extension of the Hamilton-Jacobi framework from the conventional base manifold of position space only (i.e.R d ) to the base manifold of positions and orientations (i.e.R d × 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 R 3 × 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 Ω ⊂ R 3 × S 2 .Fig. 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 x is proportional to the data-value U (x, n) and the coloring is related to the orientation n ∈ S 2 .As such the dMRI data already provide a distribution on R 3 × S 2 and does not require an 'orientation score' as depicted in Fig. 1 and Fig. 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.[58,20] or references in [33].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.[29,34,32].One can also make use of the more general Finsler geodesic tracking to include directionality [37,38], and use high angular resolution data (HARDI), examples of which can be found in [54,5].Recently, a promising method has been proposed, based on geodesics on the full position and orientation space using a dataadaptive Riemannian metric [46].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.
Fig. 4 Top: An orientation score [23,31] provides a complete overview of how the image is decomposed out of local orientations.It is a method that enlarges the image domain from R d to R d × S d−1 (here d = 2).Bottom: Conventional geodesic wavefront propagation in images (in red) typically leaks at crossings, whereas wavefront propagation in orientation scores (in green) does not suffer from this complication.A minimum intensity projection over orientation gives optimal fronts in the image.The cost for moving through the orange parts is lower than elsewhere, and is computed from the orientation score, see e.g.[8].The 'leakage problem' is gone both for propagating symmetric sub-Riemannian spheres (left), and it is also gone for propagation of asymmetric Finsler spheres (right).

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 backwards 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 F ε and F + ε of the Finsler metrics F 0 and F + 0 , which make our numerical discretization possible.We show that both the corresponding distances converge to d F0 and d F + 0 as ε → 0 and the minimizing curves converge to the ones for ε = 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 F 0 , and that using 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 end point, 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 intrin-sic 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 [40].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 is 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. Outline In Section 2, we give a detailed overview of the theoretical results of the paper.The theorems 1, 3 and 4 are discussed and proven in Sections 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. We discuss the numerics briefly in Section 6. Section 7 contains all experimental results.Conclusion and discussion follow in Section 8.For an overview of notations, Appendix F may be helpful.

Main results
In this section, we state formally the mathematical results announced in Section 1.
On each tangent space, the metric should be 1-homogeneous, convex and quantitatively non-degenerate with a uniform constant δ > 0: for all p = (x, n) ∈ M, ṗ, ṗ0 , ṗ1 ∈ T p (M), and λ ≥ 0: A weak regularity property is required as well, see the next remark.The induced distance d F , defined in (1), obeys d F (p, q) = 0 iff p = q, and obeys the triangle inequality.However, unlike a regular distance, d F needs not be finite, or continuous, or symmetric in its arguments.Note that F 0 and 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 (M), but use a weaker condition instead.Following [13], we require that the sets are closed and vary continuously with respect to the point p ∈ M in the sense of the Hausdorff distance.
The sets B F (p) are illustrated in Fig. 2 for the models of interest.The condition implies that a shortest path exists from p to q ∈ M whenever d F (p, q) is finite, and is used to prove convergence results in Appendix A.
A common technique in optimal control theory is to reformulate the shortest path problem defining the distance d F (p, q) into a time optimal control problem.That is, for p ∈ [1, ∞] one has by Hölder's (in)equality, time re-parametrization, and by 1-homogeneity of F in its 2nd entry, that: where Γ T := Lip([0, T ], M), and with B F (p) as defined in (7).The latter reformulation is used in Appendix A 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 F = F 0 the geodesics are SR geodesics, where F 0 is obtained by the square root of quadratic form associated to a SR metric G 0 | p (•, •) = F 0 (p, •) 2 on a SR manifold (M, ∆, G 0 ), where ∆ ⊂ T (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,51], for d = 3 see [25].Finally, we note that for the uniform cost case (ξ −1 C 1 = 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 C 1 , C 2 .Therefore, only in the uniform cost case, for d = 2, 3, we shall use a reference point ('the origin') e ∈ R d ×S d−1 .To adhere to common conventions we use

Controllability of the Reeds-Shepp model
A model (M, d F ) is globally controllable if the distance d F takes finite values on M × 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 F = F 0 and F = F + 0 , given in (2) and (3).Local controllability is satisfied when d F satisfies a certain continuity requirement: if p → q ∈ (M, • ), with • denoting the standard (flat) Euclidean norm on M = R d × S d−1 , we must have d F (p, q) → 0. We prove in Theorem 1 that the metric space (M, d F0 ) is locally controllable, but the quasimetric space (M, d 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 [30,57,16,48]).
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-Rashevski theorem and Fillipov theorems [2].
If the cost C 2 = δ is constant on M, then this inequality is sharp: -The sub-Riemannian Reeds-Shepp model is locally controllable, since For a proof see Section 3.

A Continuous Approximation for the Reeds-Shepp geometry
We introduce approximations F ε and F + ε of the Finsler metrics F 0 and F + 0 , depending on a small parameter 0 < ε ≤ 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 ẋ ∝ n, and in addition, F + ε penalizes negativity of the scalar product ẋ • n, appearing in (2) and (3).For that purpose, we introduce some additional notation: for ẋ ∈ R d and These are respectively the norm of the orthogonal projection4 of ẋ onto the plane orthogonal to n, and the negative and positive parts of their scalar product.The two metrics F ε , F + ε : T (M) → R + are defined for each See Fig. 5 for a visualization of a level set of both metrics in R 2 ×S 1 .Note that F ε is a Riemannian metric on M (with the same smoothness as the cost functions C 2 , C 1 ), and that F + ε is neither Riemannian nor smooth due to the term ( ẋ • n) − .One clearly has the pointwise convergence F ε (p, ṗ) → F 0 (p, ṗ) as ε → 0, and likewise F + ε (p, ṗ) → F + 0 (p, ṗ).The use of F ε and F + ε 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 p, q ∈ M d Fε (p, q) → d F0 (p, q), Consider for each ε > 0 a minimizing path γ * ε from p to q, with respect to the metric F ε , parametrized at constant speed Assume that there is a unique shortest path γ * from p to q with respect to the sub-Riemannian distance d F0 (in other words q is not within the cut locus of p), parametrized at constant speed: The proof, presented in Appendix A 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 Fε → d F0 locally uniformly on M × M, and that the convergence Remark 3 If there exists a family of minimizing geodesics (γ * i ) i∈I from p to q with respect to F 0 (resp.F + 0 ), then one can show that for any sequence ε n → 0 one can find a subsequence and an index i Note that for vessel-tracking applications, cusps are not wanted (there is no reason why the entering angle should be the same as the departing angle), whereas keypoints are only desirable at bifurcations.
I.e. a cusp point is a point where the spatial control aligned with n(t 0 ) vanishes and switches sign locally.
Although this definition explains the notion of a cusp geometrically (as can be observed in Fig. 2 and Fig. 6), it contains a redundant part for the relevant case of interest: the second condition automatically follows when considering the SR geodesics in (M, d F0 ).The following lemma gives a characterization of a cusp point in terms of the distance function along a curve.
The proof can be found in Appendix D.
Definition 3 (Keypoint) A point x on the spatial projection of a geodesic γ( x and ṅ(t) = 0 for all t ∈ [t 0 , t 1 ], i.e., a point where an in-place rotation takes place.

Definition 4
We define the set R ⊂ M to be all endpoints that can be reached with a geodesic γ

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 P curve , see e.g.[22,24], in which the same positivity condition for ũ is imposed.Fig. 7 shows more precisely what this set looks like for d = 2 [22], in particular that it is contained in the half-space a • x ≥ 0, and for d = 3 [24].We extend these results with the following theorem.) departing from e = (0, 0, 0) and ending in p = (x, y, θ) has , where E(z, m) denotes the Elliptic integral of the second kind.Fig. 7 The set R of endpoints reachable from the origin e (recall ( 11)) via SR geodesics whose spatial projections do not exhibit cusps has been studied for the case d = 2 (left), and for the case d = 3 (right).For d = 2 it is contained in x ≥ 0 and for d = 3 it is contained in z ≥ 0. The boundary of this set contains of endpoints of geodesics departing at a cusp (in red) or of endpoints of geodesics ending in a cusp (in blue).If an endpoint (x, n) is placed outside R (e.g. the green points above) then following the approach in Theorem 4, depending on its initial spatial location it first connects to a blue point (x, n new ) via a spherical geodesic end then connects to the origin e via a SR geodesic.Then it has a keypoint at the endpoint.For other locations spatial locations (orange points), the geodesic has the keypoint in the origin, or even at both boundaries, cf.Fig. 8.

Remark 5
In case A, γ + is a minimizing geodesic in (M, d F0 ) as well.In case B, γ + departs from a cusp.In case C, γ + is a concatenation of a minimizing geodesic in (M, d F0 ) and an in-place rotation.For other endpoints (x, y, θ) for geodesics departing from e with 0 ≤ x < 2, other than the ones reported in C2 it is not immediately clear what happens, due to [22,Thm.9].Also points with x < 0 may have keypoints at the end as well.See Fig. 8 where various cases of minimizing geodesics in (M, d F + 0 ) are depicted.

Remark 6
It is also interesting to study the effect of ε ≥ 0 on the removal (or rather smoothing out in practice) of cusps on non-optimal geodesics in (M, d Fε ) and keypoints in (M, d F + ε ) when ε moves away from 0. See Fig. 9, where such non-optimal geodesics are obtained via Euler-Lagrange formalism (or equivalently by integration of the canonical equations in the Pontryagin Maximum principle).

The Eikonal PDE Formalism
As briefly discussed in Section 1.3, continuous metrics like F ε and F + ε for any ε > 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 F ∈ C 0 (T (M), R + ), and define the dual F * on the co-tangent bundle as follows: for all (p, p) ∈ T * (M) The distance map U = d F (p S , •) from a given source point p S ∈ M is the unique solution, in the sense of viscosity solutions, of the static Hamilton Jacobi equation: U (p S ) = 0, and for all p ∈ M Fig. 8 Shortest paths for d = 2 using the Finsler metrics F 0 (blue) and F + 0 (red), with point source p S = (0, 0, 0) and varying end conditions.Row A: p = (0, 0.8, πn/4).Row B: p = (0.8, 0.8, πn/4).Row C: p = (−0.8,0, πn/4).Here n = 1, . . ., 8, corresponding to the columns.When there are two minimizing geodesics, both are drawn.Circles around the begin or end point indicate in-place rotation of the red curve at that point.We see that whenever the blue geodesic has a cusp, the red geodesic has at least one in-place rotation (keypoint).This numerically supports our statements in Theorem 3 considering cusps and keypoints.For high accuracy we applied the relatively slow iterative PDE approach [8]  Fig. 9 Non-optimal geodesics in the special case C = 1 and d = 2.In particular in the case F 0 , keypoints do not appear in the interior of globally optimal curves, only at the end, cf.Theorem 1.We also observe that cusps disappear when ε > 0, cf.Theorem 3. The red curves are spatially very similar, but the part after the cusp/keypoint is traversed with a different orientation.Similar behavior can be observed for the blue curves.
Furthermore, if γ is a minimizing geodesic from p S to some p ∈ M, then it obeys the ordinary differential equation (ODE): for any t ∈ [0, 1] such that the differentiability of U and F * holds at the required points.The proof of the ODE ( 23) is for completeness derived in Proposition 4 of Appendix C, where we also discuss in Remark 14 the common alternative formalism based on the Hamiltonian.We denoted by d pF * the differential of the dual Finsler metric F * with respect to the second variable p, hence d pF * (p, p) ∈ T * * p (M) ∼ = T p (M) is indeed a tangent vector to M, for all (p, p) ∈ T * M.
In the rest of this section, we specialize ( 22) and ( 23) to the Finsler metrics F ε and F + ε .Our first result provides explicit expressions for the dual Finsler metrics (required for the eikonal equation).
Proposition 1 For any 0 < ε ≤ 1, the duals to the approximating Finsler metrics F ε and F + ε are: for all (p, p) ∈ T * (M), with p = (x, n) and p = (x, n) In order to relate the Finslerian HJB equation ( 22) and backtracking equation ( 23) to some more classical Riemannian counterparts, we introduce two Riemannian metric tensor fields on M. The first is defined as the polarization of the norm where ṗ = ( ẋ, ṅ), and then one can also rely on gradient fields p → G −1 p;ε dU (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 ∇ S d−1 the gradient operator on S d−1 with respect to the inner product induced by the embedding S d−1 ⊂ R d , and by ∇ R d the canonical gradient operator on R d .
Corollary 1 Let ε ≥ 0. Then the eikonal PDE (5) for the case (M, F ε ) takes the form The eikonal PDE (5) for the case (M, F + ε ) now takes the explicit form: for those p ∈ M + ∪ M − where U + is differentiable 6 .
The proof of Proposition 1 and Corollary 1 can be found in Section 5.
We finally specialize the geodesic ODE (23) to the models of interest.Note that for the model (M, d F + ε ), the backtracking switches between qualitatively distinct modes, respectively almost sub-Riemannian and almost purely angular, in the spirit of Theorem 3. Given ε > 0 and n ∈ S d−1 let D ε n denote the d × d symmetric positive definite matrix with eigenvalue 1 in the direction n, and eigenvalue ε 2 in the orthogonal directions : Theorem 4 (Backtracking) Let 0 < ε < For the approximation paths of the car without reverse gear we have, provided that with G p;ε ( ṗ, ṗ) given by ( 26), with disjoint Riemannian manifold splitting is equipped with metric tensor G ε , M − is equipped with metric tensor G ε and denotes the transition surface (surface of keypoints).

Remark 7
The general abstract formula (29) reflects that the backtracking in (M, F + ) is a combined gradient descent flow on the distance map U + on a splitting of M into two (symmetric) Riemannian manifolds.Its explicit form (likewise ( 28)) is Note that for the (less useful) isotropic case ε = 1, F 1 and F + 1 coincide and geodesics consist of straight lines x(•) in R d and great circles n(•) in 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 sub-gradient in the sub-differential ∂U (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 F0 (•, e)) 2 is smooth is open and dense in any compact subset of M, see [1,Thm. 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]  ).From this we see that the convergence holds pointwise but not uniformly (otherwise the limit distance d F + 0 was continuous).Nevertheless the shortest paths converge strongly as ε ↓ 0, and we see that the spatial velocity tends to 0 in (31) In the SR case ε = 0, the gradient flows themselves fit continuously and the interface ∂M ± is reached with ẋ • n = 0 (and ẋ = 0).Theorem 4 can be extended to the SR case: Corollary 2 (SR Backtracking) Let the cost C 1 , C 2 be smooth, let the source p S ∈ M and p = p S ∈ M be such that they can be connected by a unique smooth minimizer γ * ε in (M, F ε ) and γ * 0 in (M, F 0 ), such that γ * ε (t) is not a conjugate point for all t ∈ [0, 1] and all sufficiently small ε > 0, say ε < ε 0 , for some ε 0 > 0. Then defining assuming U 0 is differentiable at γ * 0 (t).In addition U 0 satisfies the SR eikonal equation: From Theorem 2 we have pointwise convergence U ε (p) → U 0 (p) and uniform convergence γ * ε → γ * 0 as ε ↓ 0.Moreover, as γ * ε and γ * 0 are solutions of the canonical ODEs of Pontryagin's Maximum Principle, the trajectories are continuously depending on ε > 0, and so are the derivatives γ * ε .As a result, we can apply the backtracking Theorem 4 for ε > 0 and take the limits: Furthermore, where we recall Corollary 1.Here due to our assumptions, U ε and U 0 are both differentiable at p. Note that the limit for the inverse metric G −1 p,ε as ε ↓ 0 exists, recall Cor. 1.Now that we stated our 4 main theoretical results we will prove them in the subsequent sections (and Appendix A).
Now the statements ( 12) and ( 13 tion assumption we get the required coercivity: To prove local controllability of the model (M, d F0 ), we apply the logarithmic approximation for weighted sub-coercive operators on Lie groups, cf.[57] applied to the Lie group SE(d) = R d SO(d), in which the space of positions and orientations is placed via a Lie group quotient SE(d)/({0} × SO(d − 1)).One obtains a sharp estimate 7 , where the weights of allowable (horizontal) vector fields is 1, whereas the remaining spatial vector fields orthogonal to n • ∇ R d get weight 2, as they follow by a single commutator of allowable vector fields, see e.g.[25,24].Relaxing all spatial weights to 2 and continuity of costs C 1 , C 2 , yields (14).
Remark 10 In view of the above one might expect that the point (x − µn, n) is reached by a geodesic that consists of a concatenation of 1. an in-place rotation by π, 2. a straight line, 3. an in-place rotation by π.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.) that are a concatenation 1. an in-place rotation from (x, n) to (x, R ± π 2 n), 2. a full U-curve, see [43], departing from and ending in a cusp from We have the limit lim Proof.By rotation and translation covariance, we can restrict ourselves to x = 0, n = a, and by the coplanarity result in [25, Cor.6, Thm.8], we only need to consider d = 2 and a = (1, 0) T .From Theorem 3 (that we prove later) we know that keypoints only occur at the endpoints of minimal paths.Since cuspless geodesics stay in the positive half-space set by their initial orientation, recall Remark 4, the optimal path from (0, a) to (−µa, a) starts with a rotation by at least a π 2 angle.For the same reason, it ends with a rotation by at least a π 2 angle.There exists a U-curve, starting and ending in a cusp, optimally connecting (0, R π 2 a) with (−µa, R − π 2 a) [22].Hence the concatenation of curves as described points 1.-3. is optimal, and has the same length as the alternative (with rotations in the opposite direction).For the total distance by such a curve we use8 [22, Cor.2]: with s max (1, µ) the first positive root of the denominator of the integrand.Letting µ ↓ 0 we get lim .As we will apply tools from previous works [22,11,10,51], we will make use of the following notations for expansion 9 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, θ) applied in this article.Note that for the case ε = 0 admissible smooth curves γ in (M, d F0 ) satisfy the horizontality constraint γ(t) Proof of the statements regarding cusps: -We can describe our curve optimization problem (1) using a Hamiltonian formalism, with Hamiltonian [43].By Pontryagin's Maximum Principle, geodesics adhere to the following Hamilton equations: For fixed initial momentum p(0), this uniquely determines a SR geodesic.Moreover, SR geodesics are contained within the (co-adjoint) orbits The parameter t in the system (34) is SR arc length, but by reparametrizing (possible as long as u 1 does not change sign) to spatial arc length parameter s, with ds dt = p1 , we get a partially linear system.Combining (34) and (35), we find orbits in the (hyperbolic) phase portrait induced by Hence |p 3 (s)| = 1 always has a solution for some finite (possibly negative) s, except when p2 (0) = p3 (0) = 0, in which case the solutions are straight lines.Preservation of the Hamiltonian then implies p1 (s) = u 1 (s) = ũ(s) = 0. We conclude that every SR geodesic (with unconstrained time t ∈ R) in (M, d F0 ) which is not a straight line admits a cusp.
-We now consider (M, d Fε ), ε > 0. To have a cusp, we need p1 (t) = p2 (t) = 0 for some t ∈ R. The co-adjoint orbit condition (35) then implies that p1 (t) = p2 (t) = 0 for all t, corresponding to a vertical geodesic that has purely angular momentum and no cusp.The same argument holds for (M, d F + ε ).In (M, d F + 0 ) we have the condition that u 1 ≥ 0, hence by definition it can never switch sign and all geodesics are cuspless.

Proof of the statements regarding keypoints:
-For the cases (M, d Fε ) and (M, d F + ε ) with ε > 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 (M, d F0 ) 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 ∈ R, again corresponding to purely angular motion.) has an internal keypoint, with a corner of angle δ > 0, at internal time T 1 ∈ (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 δ.With a suitable mollifier this shortcut can be approximated by a curve in Γ .For details see similar arguments in [11].
Next we explain the cases A), B) and C), where we fix initial point γ(0) = e = (0, 0, 0).A) Suppose that the endpoint p = (x, y, θ) ∈ R and x ≥ 0. Then p can already be reached by a geodesic in (M, d F0 ) and the positivity constraint (i.e.no reverse gear), which can only increase length, becomes obsolete.B) Now suppose the endpoint p = (x, y, θ) lays in the half-space x < 0. Then by the half-space property of geodesics in (M, d F0 ), cf.[22,Thm.7], the geodesic in (M, d 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 p lays outside the connected cone of reachable angles, which are by [22, Thm.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 ∂R, after which it is traced back to the origin by a regular geodesic with strictly positive spatial control inside the volume R.
5 Eikonal equations and backtracking: Proof of Prop. 1, Corr. 1 and Thm. 4 First we shall prove Proposition 1, regarding the duals of F ε and F + ε , and Corollary 1, providing explicit expressions for the corresponding eikonal equations.To this end we need a basic lemma on computing dual norms on R n , where later we will set n = 2d − 1 = dim(M).
Lemma 4 Let w ∈ R n and let M ∈ R n×n be symmetric, positive definite.Define the norm F M,w : R n → R + by Then its dual norm with M = (M + w ⊗ w) −1 and ŵ = .
Proof.For n = 1 the result is readily verified, and for w = 0 the result is classical.We next turn to the special case M = Id, and w = (w 1 , 0 R n−1 ) is zero except maybe for its first coordinate w 1 .Thus for any Using the compatibility of norm duality with such splittings, and the special cases n = 1 and w = 0 mentioned above, we obtain which is exactly of the form (36).The general case for arbitrary w and symmetric positive definite M follows from affine invariance.Indeed let A be an invertible n×n matrix, and let M = A T M A and w = A T w.Let F = F M,w and F = F M ,w , so that F (v) = F (Av) for all v ∈ R n .Let F * , M , ŵ, and F * , M , ŵ , be respectively the dual norms and the matrices defined by the explicit formulas above.Then denoting B := (A T ) −1 one has by the definition of dual norms that F * (v) = F * (B v) for all v ∈ R n , and by the explicit formulas M = B T M B, w = B T w.Thus, F * = F * M,w holds if and only if F * = F * M ,w .Since for any M, w, there exists a linear change of variables A such that M = Id and w is zero except maybe for its first coordinate, the proof is complete.Now Proposition 1 follows from Lemma 4 by writing out the dual norm, using for each p ∈ M: with D ε n as in (27).Corollary 1 then follows by setting the momentum covector p = dU (p) equal to the derivative of the value function evaluated at p. Now that we have derived the eikonal equations, we obtain the backtracking Theorem 4 by Proposition 4 in App.C, 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 (61) (in Remark 14 in App.C).We focus below on the model (M, d F + ε ) without reverse gear, since the other case is similar.Let p ∈ M, let F := F + ε (p, •), and let p = (x, n) ∈ T * p (M).Then differentiating w.r.t.n we obtain where • is the Riemannian metric induced by the embedding S d−1 ⊂ R d .Differentiating w.r.t.x we obtain The announced result (31), which is equivalent to its more concise abstract form (28), follows by choosing x := ∇ R d U (γ(t)) and n := ∇ S d−1 U (γ(t)) and a basic re-scaling [0, L] ∈ t → t/L ∈ [0, 1].

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 p = (x, n) ∈ M: where a moving frame of reference is chosen such that inducing a corresponding dual frame { ω i p } via W.r.t. the left-invariant frame the matrices D ε n , M p as in (38) and Mp all become diagonal matrices, and the dual can be computed straightforwardly.Furthermore, in this formulation we can see from the expression for the dual (F + 0 ) * , i.e. in the limit ε ↓ 0, that the positive spatial control u d constraint results in a positive momentum pd constraint: Therefore the eikonal equation in the positive control model (M, d F + 0 ) is simply given by 6 Discretization of the Eikonal PDEs

Causal operators and the fast marching algorithm
The fast marching algorithm is an efficient numerical method [59] for numerically solving the static first order Hamilton-Jacobi-Bellman (or simply eikonal) PDE (5) which characterizes the distance map U to a fixed source point p S .Fast marching is tightly connected with Dijkstra's algorithm on graphs, and in particular it shares the O(KN ln N ) complexity, where N = #(X) is the cardinality of the discrete domain X ⊂ M, X p 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 non-linear fixed point systems of equations Λu = u, where the unknown u ∈ R X is a discrete map representing the front arrival times, which rely on the a-priori assumption that the operator Λ : R X → R X is causal (and monotone, but this second assumption is not discussed here).Causality informally means that the estimated front arrival time Λu(p) at a point p ∈ X depends on the given arrival times u(q), q ∈ X, prior to Λu(p), but not on the simultaneous or the future ones.Formally, one requires that for any u, v ∈ R X , t ∈ R: and v <t , (Λu) ≤t and (Λv) ≤t are defined similarly.
A semi-Lagrangian scheme.We implemented two discretizations of the 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 sub-policy of an optimal policy is an optimal policy.Formally, let F be a Finsler metric, and let U := d F (•, p S ) be defined as the distance to a given source point p S .Then for any p ∈ M and any neighborhood V of p not containing p S one has the property In the spirit of [59,55] we discretize (45) by introducing for each interior p ∈ X \ {p S } a small polygonal neighborhood V (p), which vertices belong to the discrete point set X.The nonlinear operator Λ is defined as where In other words, the boundary point q ∈ ∂V (p) in ( 45) is represented in (46) by the barycentric sum q = n i=1 ξ i q i , the distance d F (p, q) is approximated with the norm F(p, q − p), and the value U (q) is approximated with the interpolation n i=1 ξ i u(q i ).We refer to [55,61] for proofs of convergence, and for the following essential property: the operator (46) obeys the causality property (44) iff the chosen stencil V (p) obeys the following generalized acuteness property: for any q, q in a common facet of V (p), one has For the construction of such stencils V (p), p ∈ X, we rely on the previous works [40,39] and on the following observation: the metrics F ε and F + ε associated to the Reeds-Shepp car models can be decomposed as which allows to build the stencils V (p) for F by combining, as discussed in [40, p. 9], some lower dimensional stencils V 1 (p) and V 2 (p) built independently for for the spatial x ∈ R d and spherical n ∈ S d−1 variables.We discretize S 1 uniformly, with the standard choice of stencil.We discretize S 2 by refining uniformly the faces of an icosahedron and projecting their vertices onto the sphere (as performed by the Mathematica R Geodesate function).The resulting triangulation only features acute interior angles, in the classical Euclidean sense, and thus provides adequate stencils since in our applications F 2 (p, ṅ) = C 2 (p) ṅ is proportional to the Euclidean norm, see Fig. 11.We typically use 60 discretization points for S 1 , and from 200 to 2000 points for S 2 .
We discretize R d using the Cartesian grid hZ d , where h > 0 is the discretization scale.The norm , recall the notation in (47), induced by the approximate Finsler metric F ε on the physical variables in R d , is of Riemannian type and strongly anisotropic.In dimension d ≤ 3, this is the adequate setting for the adaptive stencils of [40], built using discrete geometry tools known as lattice basis reduction.The norm e. non-Riemannian) and strongly anisotropic.In dimension d = 2, this is the adequate setting for the adaptive stencils of [39], 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 F + ε 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. 12.For manuscript size reasons, we only describe it informally, and postpone proofs of convergence for future work.
Let n ∈ S 2 and let ε > 0 be fixed.Then one can find non-negative weights and integral vectors (ρ i , A simple and efficient construction of (ρ i , w i ) 6 i=1 , relying on the concept of obtuse superbase of a lattice, is in [28] described and used to discretize anisotropic diffusion PDEs.One may furthermore assume that (n, w i ) ≥ 0 for all 1 ≤ i ≤ 6, up to replacing w i with its opposite.Then up to respectively an O(ε 2 ) v 2 and O(ε 2 + h) error.Following [50], we design a similar upwind discretization of the angular part of the metric where δ θ U (p), and likewise δ ϕ U (p), is defined as We denoted by n(θ, ϕ) := (sin θ cos ϕ, sin θ sin ϕ, cos θ) the parametrization of S 2 by Euler angles (θ, ϕ) ∈ [0, π] × [0, 2π].Combining ( 49) and ( 50), one obtains an approximation of F + * 0 (p, dU (p)) 2 , within O(ε 2 +r(ε)h) error for smooth U , denoted F ε U (p).We denoted by r(ε) := max 6 i=1 |w i | the norm of the largest offset appearing in (48), since these clearly depend on ε.Importantly, F ε U (p) only depends on positive parts of finite differences (U (p) − U (q)) + , hence the system F ε U (p) = 1 can be solved using the fast-marching algorithm, as shown in [50].The convergence analysis of this discretization, as the grid scale h and tolerance ε tend to zero suitably, is postponed for future work, see [41,42].
Note that this approach could also be applied in dimension d = 2, and to the symmetric model (M, d Fε ) featuring a reverse gear.We present only a single assessment of the numerical performance of our method, see Fig. 13.We compare numerically obtained shortest paths with exact SR geodesics for a small number of end points, 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.

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:   We use the 2D datasets to point out the difference in results when using the metric F ε and F + ε , and to ex-plain the role of the keypoints when using F + ε , 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 [40].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.

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  ε ) with the same boundary conditions.We recognize one end-condition case where on the left we get a cusp, whereas on the right we have a key-point (with in-place rotation) precisely at the bifurcation.
Pompidou as a 2D image, see Fig. 14.The walls (in black) have infinite cost, everywhere else the cost is 1.We place end points (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 p 0 and p 1 , the distance U (p) of any point p ∈ M to one of the exits is given by We use a resolution of N x ×N y ×N o = 706×441×60.The cost in this example is only dependent on position, but constant in the orientation.Moreover, we use C 1 = C 2 and ε = 1/10.
On the left of Fig. 14 we see optimal paths (in blue) obtained using the Finsler metric F = F ε .The fast marching algorithm successfully connects all end points 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 Fε as above, at each position minimized over the orientations.
On the right, the optimal paths using the asymmetric Finsler metric F = F + ε are shown in red.The curves no longer exhibit cusps, but have in-place rota-tions (white dots) instead.These keypoints occur in this example on corners of walls.(Due to the fact that ε 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 post-processing 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 Fε left and U F + ε right can be observed: the constrained model usually has a slightly higher cost right around corners.

Vessel Tracking in Retinal Images
Another application is vessel tracking in retinal images, for which the model with reverse gear and the fastmarching algorithm have shown to be useful in [8,52].Although the algorithm works fast and led to successful vessel segmentation in many cases, in some cases, in particular bifurcations of vessels, cusps occur.Fig. 15 shows one such example on the left.The image has resolution N x × N y × N o = 121 × 114 × 64.The cost is constructed as in [8]: the image is first lifted using cake wavelets [23], resulting in an image on R 2 × 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 C 1 = ξC 2 , with ξ = 0.02 (top) and ξ = 0.04, and ε = 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 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 (M, d Fε ), corresponding to the blue tracks on the left half of Fig. 15, gives a correct vessel tracking for the forward end conditions of A and B, for both values of ξ.This is obviously the better choice than the backward cases.However, for end condition C, neither the forward or backward with neither values of ξ gives a vessel tracking without cusps.On the other hand, if we use the constrained model (M, d F + ε ), we obtain an in-place rotation or keypoint in the neighborhood of the bifurcation.Typically a higher value of ξ 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.

Application to Diffusion-Weighted MRI Data
DW-MRI is a magnetic resonance technique for noninvasive measurement of water diffusion in fibrous tissues [45].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 (y, n) → U (y, n) within the manifold 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 Eq. ( 28) and Eq. ( 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 [27, Lemma 2 & 4], so that the poles due to a standard Euler angle parametrization of S 2 do not appear in exact recursions of Legendre polynomials!If data-driven factors C 1 and 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.

Construction of the Cost Function
The synthetic dMRI data is created by generating/simulating a Fiber Orientation Density (FOD) of a desired structure.There are sophisticated methods for this, e.g.[17,12], 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 R 3 , the ones on the bottom row in Fig. 3.In each voxel inside a bundle, we place a spherical δ-distribution, with the peak in the orientation of the bundle.We convolve each δ-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 : M → 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 where σ ≥ 0, p ∈ N, with • ∞ the sup-norm and W + (p) = max{0, W (p)}.The cost function C induces the following spatial and angular cost functions (C 1 , C 2 ): The implementation of nonuniform cost is comparable to the application of vessel tracking in retinal images in d = 2 by Bekkers et al. [8].

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. 16.The experiments using metric F ε demonstrate the effect of the model parameters on the geodesic back-traced 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. 16, right) in which suitable values are used for the data-term σ, and the fast-marching parameters ξ and ε.Furthermore, fixed values are used for data sharpening p = 3, spatial smoothing σs = 0.5, forward-Euler integration step size δt = 0.04, and a gridscale of 1.By use of these parameters the global minimizing geodesic (Fig. 16.A, left) is shown to take the longer, curved route.In parameter configuration B the dataterm σ is lowered, which creates a geodesic that is primarily steered by internal curve-dependent costs and is shown to take the shortcut route (Fig. 16.B).Setting ε = 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 move-ment relative to angular movement is high, leading to geodesics with shortcuts.We conclude that configuration A with a relatively strong data term, large bending stiffness (ξ −1 = 10), and a nearly SR geometry (ε = 0.1) avoids unwanted shortcuts.

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. 17 the backtracking results are shown for the cases without reverse gear F + ε (top) and the model with reverse gear F ε (bottom).The distance map for F + ε was computed by the iterative method implementing the forward Reeds-Shepp car, while for F ε 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 (M, d Fε ), whereas the traced geodesic in the quasimetric space (M, d F + ε ) is both more stable and more reasonable.

Robustness to neighboring structures
A pitfall of methods that provide globally minimizing curves using a dataterm 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), 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. 18.This data is used to construct the cost function as explained above.
The resolution of the data is N x × N y × N z × N o = 32 × 32 × 32 × 162.Again we use C 1 = ξC 2 = C, with ξ = 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 σ = 3 • 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 ε = .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. 18 that choosing ε = 1, corresponding to having an isotropic Riemannian metric, this unwanted behavior can easily occur.
We conclude that the SR geodesics in (M = R 3 × S 2 , d Fε ) with ε 1, are less attracted to parallel, dominant structures than isotropic Riemannian geodesics.

Conclusion and Discussion
We have extended the existing methodology for modelling 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 [49], we used symmetric and asymmetric Finsler metrics.We have introduced these metrics, F 0 and 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 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 Thm. 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 F ε and F + ε , that do allow for numerical approaches.We have shown in Thm. 2 that as ε → 0, the distance map converges pointwise and the geodesics converge uniformly, implying that for sufficiently small ε we indeed have a reasonable approximation of the ε = 0 case.
We have analyzed cusps in the metric space (M, d F0 ) and keypoints in the quasi-metric space (M, d F + 0 ) which occur on the interface surface ∂M ± given by (30).The analysis, for uniform costs, is summarized in Thm. 3. We have shown that cusps are absent in (M, d Fε ) for ε > 0, that keypoints in (M, d F + 0 ) occur only on the boundary, and we provided analysis on how this happens.In Thm. 4 we have shown how minimizing geodesics in (M, d Fε ) and (M, d F + ε ) can be obtained from the distance maps with an intrinsic gradient descent method.Fig. 18 Left: 3D configuration of bundles and a visualization of part of the synthetic dMRI data.Middle: backtracking of geodesics in (M, d F ε ) from several points inside the curves to end points of the bundle is successful when using ε = 0.1.Right: when using ε = 1, the dominant red bundle can cause the paths from the green bundle to deviate from the correct structure.
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 F ε with ε 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 de-sired 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 F ε , we get keypoints (inplace rotations) when using F + ε .These keypoints turn out to be located on logical places in the image.On the 2D retinal image we showed that the Finsler metric F + ε 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 (M, d Fε ) with 0 < ε 1 over the model (M, d F1 ) 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 (M, d F + ε ) with 0 < ε 1, compared to (M, d Fε ) 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 App.B. Furthermore, we aim to integrate locally adaptive frames [26] into the Finsler metrics F ε , F + ε , for a more adaptive vessel/fiber tracking.

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.A Well-posedness and convergence of the Reeds-Shepp models We introduce in §A.1 some general elements of control theory, which are specialized in §A.2 to the Reeds-Shepp models and their approximations.

A.1 Closedness of controllable paths
In this section, we introduce the notion of an admissible path γ with respect to some controls 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.In the following, we fix a closed set X, contained in an Euclidean vector space E, or in a complete Riemannian manifold M. In the applications considered in this paper, X is of the form X 0 × S d−1 , where X 0 ⊂ R d is some image domain, see Fig. 15, or the set of accessible points in a map (which excludes the walls), see Fig. 14.The embedding space can be the vector space E = R d × R d , which is an acceptable but rather extrinsic point of view, or the Riemannian manifold M = R d × S d−1 , equipped with the metric G ε for some arbitrary but fixed ε > 0, see (25).
We equip the collection of all Lipschitz paths Γ := Lip([0, 1], X) with the topology of uniform convergence.We will make use of Ascoli's lemma [4,3], which states that any uniformly bounded and equicontinuous sequence of paths admits a converging sub-sequence.In our case the paths are Lipschitz with a common Lipschitz constant.
Definition 6 Given a normed vector space V , we denote by C(V ) ⊂ K(V ) the collection of non-empty compact subsets of V , which are convex and contained in the unit ball.
Definition 7 A family of controls B on the set X is an element of the set B defined by -If X ⊂ E an Euclidean vector space, then In both cases, B is equipped with the topology of locally uniform convergence.We denoted T B := {T v v ∈ B}, where T ∈ R + and B is a subset of a vector space.Note the potential conflict of notation with the tangent space T M to the embedding manifold M, which should be clear from context.If a path γ is T B-admissible for some controls B ∈ B, then it must be T -Lipschitz.The following result slightly extends, for our convenience, Corollary A.5 in [13].
Proof.Let (γ n , T n , B n ) be sequences of paths, times and controls converging to (γ ∞ , T ∞ , B ∞ ), and such that γ n is T n B nadmissible for all n ≥ 0. Since the paths γ n are converging as n → ∞, they lay in a common compact subset X of the closed domain X, recall Remark 13.As a result, the restricted controls B n := ( B n | X ) are uniformly converging as n → ∞.
In the case where X ⊂ E a Euclidean space, applying Corollary A.5 in [13] to the sequence (γ n , T n B n ) we obtain that γ ∞ is T ∞ B ∞ -admissible as announced.
In the case where X ⊂ M a Riemannian manifold, an additional proof ingredient is required.Let M be an open neighborhood of X with compact closure in M, and let I : M → E be an embedding (i.e. an injective immersion) with bounded distortion of the manifold M into a Euclidean space E of sufficiently high dimension, which by Whitney's embedding theorem is known to exist.Define the set X := I(X ), the paths γ n := I • γ n , and controls B n (I(p)) := dI(p, B n (p)) for all p ∈ X and n ∈ N ∪ {∞}.Applying again Corollary A.5 in [13] In line with the identity (9), we rely on the following definition where we rescale the time interval to [0, 1].Definition 9 For any B ∈ B, p, q ∈ X, we let and γ is T B-admissible}. ( Corollary 3 If B ∈ B, p, q ∈ X are such that T B (p, q) < ∞, then the inf.(52) is attained.
Corollary 4 For all ε ∈ Let T ε := T B ε (p, q) for each ε ≥ 0. Assume in addition that there exists a unique T 0 B 0 -admissible path γ 0 from p to q, and for each ε > 0 denote by γ ε an arbitrary path from p to q which is (ε + T ε )B ε admissible.Then γ ε → γ 0 as ε → 0.
More generally, if the infimum ( 52) is realized by a family (γ i ) i∈I of paths, then for any sequence ε n → 0 one can find a subsequence such that γ ε ϕ(n) → γ i as n → ∞ for some i ∈ I.

A.2 Specialization to the Reeds-Shepp models
We begin this section by recalling, and slightly generalizing, the notion of Finsler metric introduced in §2.2.We then prove that the Reeds-Shepp metrics F 0 and F + 0 are indeed Finsler metrics in this sense.
with C(T p M) defined in Definition 6 and equipped with the Hausdorff distance.
Lemma 5 Let B be a compact subset of a metric space E, and let ϕ ∈ C 0 (B, 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.
The same estimate holds for the sets B + i , i ∈ {1, 2}, defined by the inequalities Proof.It suffices to establish the announced estimate (54) when the tuples (a i , b i , n i , ε i ), i ∈ {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 ϕ :   Note that the duality-bracket/norm inequality is saturated by dU (p), q = 1 = F * (dU (p))F ( q), and that the assumed differentiability of the dual norm F * at the point p = dU (p) implies the strict convexity of the primal norm F (up to 1homogeneity) at the point dF * (p) = q.Hence q is the unique solution to the system "F * ( q) = 1 and dU (p), q = 1", and therefore Γ = L q.This implies the differentiability of γ at time t, and the announced equality (59).
Remark 14 (Lagrangians and Hamiltonians) Given an arbitrary Finsler metric F on M, its half-square L := 1 2 F 2 : T (M) → [0, +∞] is usually called the Lagrangian.The shortest path problem (1) can be reformulated in terms of the Lagrangian, thanks to the Cauchy-Schwartz's inequality which gives d F (p, q) 2 = inf{ A path γ is a minimizer of (60) iff it is simultaneously normalized and a minimizer of (1).The Hamiltonian H is the Legendre-Fenchel transform of its Lagrangian L w.r.t. the second variable, hence H = 1 2 (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 p ∈ M In geometric control theory this Hamiltonian is often referred to the 'fixed time Hamiltonian of the action functional', cf.[2,8,51], and is typically used [43] in the Pontryagin maximum principle [2] for (sub-)Riemannian geodesics.Suppose the d-th spatial control aligned with n(t 0 ), recall (19), vanishes: ũ(t 0 ) = 0. Now we show by contradiction that in this case u(t 0 ) = 0. Suppose ũ(t 0 ) = u(t 0 ) = 0.

E On the Hamiltonian discretization
This appendix is devoted to the rigorous formulation and proof of (49).This particular result does not appear in the journal version of this paper, because it makes more sense within a complete convergence analysis for this discretization, to appear soon.
Then ∀v ∈ R d the positive part of the scalar product n • v can be approximated as follows Proof.We may assume that ρ i = 1, for all 1 ≤ i ≤ k, up to replacing w i with √ ρ i w i .Denote by w ⊥ i := w i − w i , n n the orthogonal projection of w i on the hyperplane orthogonal to n.Then by (62) The proof of ( 63) is split into two parts, depending on the sign of (n • v).If (n • v) ≤ 0, then (w i • v) ≤ (w ⊥ i • w) for all 1 ≤ i ≤ k, thus as announced 1≤i≤k In contrary if (n • v) ≥ 0, then the RHS of (63) is immediate, and in addition (w i • v)  Eq. ( 1), . . .
Finsler metric F defined on M, its dual F * : T * (M) → R the models with and without reverse gear F 0 , F + 0 , their approximations F ε , F + ε and their duals.

1. 1 A
distance function and the corresponding shortest paths on R d × S d−1

Fig. 2
Fig.2Top: Example of a shortest path with (left) and without (right) reverse gear in R 2 × S and its projection on R 2 .The black arrows indicate the begin and end condition in the plane, corresponding to the blue dots in R 2 × S. The paths in the lifted space are smooth, but vertical tangents appear in both cases.In the left figure, the projection of the path has two cusps, and the first and last part of the path is traversed backwards (the red parts).On the right, backward motion is not possible.Instead, according to our model, the shortest path is a concatenation of an in-place rotation (green), a SR geodesic, and again an in-place rotation.Bottom: corresponding control sets as defined in(7) for the allowed velocities at each position and orientation, with B F 0 on the left and B F +

Fig. 3
Fig. 3 Challenges and applications.Top row: the case d = 2, with a toy problem for finding the shortest way with or without reverse gear (blue and red, respectively) to the exit in Centre Pompidou (top left) and a vessel tracking problem in a retinal image.Bottom row: the case d = 3, connectivity in (simulated) dMRI data.Left: visualization of a dataset with two crossing bundles without torsion, with a glyph visualization of the data in R 3 × S 2 and a magnification of one such glyph, indicating two main fiber directions.Right: the spatial configuration in R 3 of bundles with torsion in an artificial dataset on R 3 × S 2 .

2. 4
Points of Interest in Spatial Projections of Geodesics for the Uniform Cost Case: Cusps vs. Keypoints Next we provide a theorem that tells us in each of the models/metric spaces (M, d F0 ), (M, d Fε ) and (M, d F + 0 ), (M, d F + ε ), with C 1 = C 2 = 1 and d = 2 where cusps occur in spatial projections of geodesics or where keypoints with in-place rotations take place.

Theorem 3 ()
Cusps and Keypoints) Let ε > 0, d = 2, C 1 = C 2 = 1.Then, in (M, d F0 ) 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 p = (x, n) and q = (x + λn, n) with λ ∈ R are the only exceptions. in (M, d F + ε ) and (M, d Fε ) and (M, d F + 0 ) no cusps appear in spatial projections of geodesics.Furthermore, in (M, d F0 ), (M, d Fε ) and (M, d F + ε ) keypoints only occur with vertical geodesics (moving only angularly). in (M, d F + 0 keypoints only occur at the endpoints of shortest paths.A minimizing geodesic γ + in (M, d F + 0 Fig.8Shortest paths for d = 2 using the Finsler metrics F 0 (blue) and F + 0 (red), with point source p S = (0, 0, 0) and varying end conditions.Row A: p = (0, 0.8, πn/4).Row B: p = (0.8, 0.8, πn/4).Row C: p = (−0.8,0, πn/4).Here n = 1, . . ., 8, corresponding to the columns.When there are two minimizing geodesics, both are drawn.Circles around the begin or end point indicate in-place rotation of the red curve at that point.We see that whenever the blue geodesic has a cusp, the red geodesic has at least one in-place rotation (keypoint).This numerically supports our statements in Theorem 3 considering cusps and keypoints.For high accuracy we applied the relatively slow iterative PDE approach[8] on a 101 × 101 × 64-grid in M to compute d F 0 (p, p S ) and d F + 0 (p, p S ), see App.B.

Remark 9
and [1, Ch. 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, Thm 3.2] for the case d = 2 and C = C 1 = C 2 = 1.See also Fig. 8, where for some end conditions 2 optimally back-tracked 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, Def.8.43].Recall the convergence result from Theorem 2, and the non-local-controllability for the model (M, d F + 0

Lemma 3
Let µ > 0, and C 1 = C 2 = δ.Let R θ denote the (counter-clockwise) rotation matrix about the origin by angle θ.The endpoint (x − µn, n) for each µ ≥ 0 is a Maxwell point w.r.t.(x, n), since there are two minimizing geodesics in (M, d F + 0

Remark 11
Consider the case d = 2, C 1 = C 2 = δ, and source point p S = (x, n) = e = (0, 0, θ = 0).The end-points (x − µn, n) = (−µ, 0, 0), with µ > 0 sufficiently small, are 1st Maxwell-points in (M, d F + 0 ) where geodesically equidistant wavefronts departing from the source point collide for the first time, see Fig. 10C.The distance mapping d + F0 (p S , •) is not continuous, but the asymmetric distance spheres S R := {p ∈ M | d + F0 (p S , p) = R} are connected and compact, and they collide at R = 2π in such a way that the origin p s becomes an interior point in the asymmetric balls of radius R > 2π. 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 C 1 = C 2 = 1 for d = 2, our curve-optimization problem (1) (M, d F0 ) in consideration, boils down to a standard left-invariant curve optimization in the rototranslation group SE(2) = R 2 SO(2)

Fig. 10
Fig. 10 The development of spheres centered around e = (0, 0, 0) with increasing radius R. A: the normal SR spheres on M given by {p ∈ M | d F 0 (p, e) = R} where the folds reflect the 1st Maxwell sets [8, 51].B: the SR spheres with identification of antipodal points given by {p ∈ M | min{ d F 0 (p, e), d F 0 (p + (0, 0, π), e) } = R} with additional folds (1st Maxwell sets) due to π-symmetry.C: the asymmetric Finsler norm spheres given by {p ∈ M | d F + 0 (p, e) = R} visualized from two perspectives with extra folds (1st Maxwell sets) at the back (−µ, 0, 0).The black dots indicate points with two folds.In the case of B, this is a Maxwell-point with 4 geodesics merging.In the case of C, this is just the origin itself reached from behind at R = 2π, recall Lemma 3.Although not depicted here, if the radius R > 2π the origin becomes an interior point of the corresponding ball.

Fig. 11
Fig. 11 Left: Stencil used for the metric F ε on R 2 × S 1 , ε = 0.1, obeying the generalized acuteness property required for the Bellman type discretization (46).See also the control sets in Fig. 2. Center: likewise with F + ε , ε = 0.1.Right: Coarse discretization of S 2 with 162 vertices, used in some experiments posed on R 3 × S 2 .Some acute stencils (in the classical Euclidean sense) shown in color.

Fig. 12
Fig.12Left: Slice in R 3 of the control sets(7) for F ε on R 3 × S 2 , ε = 0.2, for different orientations of n.Stencils obeying the generalized acuteness property required for Bellman type discretizations(46).Right: Slice in R 3 of the control sets for F + ε , ε = 0.2.Offsets used for the finite differences discretization(49), for four distinct orientations n.

Fig. 14
Fig. 14 Comparison between the shortest paths from end points (black) to one of the exits (green) in a model map of Centre Pompidou, for cars with (left, blue lines) and without (right, red lines) reverse gear.The yellow arrows indicate the orientation of the curve.The background colors show the distances at each position, minimized over the orientation.White points left indicate the cusps, white points right indicate the (automatically placed) keypoints where in-place rotations take place.

Fig. 15
Fig. 15 Left: SR geodesics (in blue) in (M, d F ε ) with given boundary conditions (both forward and backward).Right: SR geodesics (in red) in (M, d F +ε ) with the same boundary conditions.We recognize one end-condition case where on the left we get a cusp, whereas on the right we have a key-point (with in-place rotation) precisely at the bifurcation.

Fig. 16
Fig. 16 Comparison of the results of backtracking on a 2D plane in a synthetic dMRI dataset on M = R 3 × S 2 .In case A the default parameters for σ, ξ and ε are applied resulting in a global minimizing geodesic (left) and its corresponding distance map (right).Case B reflects the influence of the data-term σ.Case C reflects the isotropic Riemannian case.Case D reflects a high cost for moving spatially and results in curves that resemble a piecewise linear curve.The distance map is illustrated using a glyph visualization in which the size of the glyph corresponds to exp(−d F ε (p s , p e )/s) p where p s is the seed location, p e is a location on a glyph, and s and p are chosen based on visualization clarity.

Fig. 17
Fig. 17 Backtracking of minimizing geodesics of the model (M, d F + ε ) without reverse gear (top) and the model with reverse gear (M, d F ε ) (bottom) using the model parameters of configuration A (σ = 3.0, ξ = 0.1 and ε = 0.1) for various end conditions.

Definition 5
Given a metric space E, we let K(E) be the collection of non-empty compact subsets of E. The distance function d A : E → R + and the Hausdorff distance H(A, B), where A, B ∈ K(E), are defined respectively by d A (x) := inf y∈A d(x, y), H(A, B) := max{ sup x∈B d A (x), sup x∈A d B (x)}.

Definition 10 AProposition 2
metric on a complete Riemannian manifold M is a map F : T M → [0, +∞].With respect to the second variable, it must be 1-homogeneous, convex, and bounded below by δ • , where δ is a positive constant.In terms of regularity, the sets B F (p) := { ṗ ∈ T p M | F (p, ṗ) ≤ 1} must be closed and depend continuously on p ∈ M with respect to the Hausdorff distance on T M.The next proposition is due to(9).With the notations of Definition 10, the sets p ∈ M → B F (p) form a family of controls on (M, δ • ).In addition for all p, q ∈ M d F (p, q) = T B F (p, q).Proposition 3 The Reeds-Shepp metrics (F ε ) 0≤ε≤1 and (F + ε ) 0≤ε≤1 are indeed metrics in the sense of Definition 10, for any ε ∈ [0, 1].The associated controls B ε := B F ε , B + ε := B F + ε depend continuously on the parameter ε ∈ [0, 1], and satisfy the inclusions B ε (p) ⊂ B ε (p) and B + ε (p) ⊂ B + ε (p) for any p ∈ M and 0 ≤ ε ≤ ε ≤ 1. Proposition 3 allows to apply the results of §A.1 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 M, recall Definitions 7, and their convergence B ε → B 0 as ε → 0, as required in Corollary 4. These two properties are implied by the continuity on [0, 1] × M, that we next prove, of the following maps

D 1 Consider Lemma 1 .
Characterization of Cusps: Proof of Lemma The structure of this lemma is a ⇔ b ⇔ c.The implication a ⇒ b is trivial.The equivalence b ⇔ c follows by Theorems 4, 2. The implication b ⇒ a remains.
1≤i≤k |w i F Table of Notations