VehicleMapping HD AV Spatial Data

Fitting Clothoid Transitions to Lane Centerlines

A discrete centerline stores curvature as a noisy staircase, but the road it describes was engineered as clean clothoid transitions — spirals where curvature rises linearly with arc length as a driver turns the wheel at a constant rate. This task recovers that design: segment the centerline on curvature-rate breakpoints, fit a clothoid to each transition, and gate on lateral fit error so the result serializes directly as an OpenDRIVE spiral. It refines the raw curvature signal from calculating road curvature with Python Shapely into the parametric primitives the road curvature and superelevation mapping stage consumes.

The curvature-vs-arc-length signal is segmented into line, clothoid, and arc primitives, then fit and validated:

Clothoid Segmentation and Fit of a Curvature Profile A plot of curvature against arc length with three regions: a flat low line segment, a linearly rising clothoid segment, and a flat high arc segment, separated by breakpoint markers. Below, three boxes name the fitted primitives line, clothoid, and arc. An arrow leads to a gate checking lateral fit error under 0.05 metres. κ s line (κ=0) clothoid (κ′ const) arc (κ const) Line fit Clothoid fit Arc fit lateral fit error ≤ 0.05 m

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (optimize, special.fresnel), Shapely 2.0+.
  • Input: a projected, metric LineString centerline with per-vertex curvature already computed against cumulative arc length.
  • Upstream stage: curvature comes from the discrete differentiation covered in calculating road curvature with Python Shapely; geometry is in a projected CRS.
  • Output: an ordered list of primitives (line, clothoid, arc) with start curvature and curvature rate per segment, ready for OpenDRIVE serialization.

Step-by-Step #

1. Segment on curvature-rate breakpoints #

A clothoid has constant curvature rate κ' = dκ/ds; a line has κ=0, an arc has κ'=0. Detect breakpoints where κ' changes regime.

python
import numpy as np

def segment_by_curvature_rate(kappa: np.ndarray, s: np.ndarray, tol=2e-4):
    """Breakpoint indices where the curvature rate changes regime."""
    kprime = np.gradient(kappa, s)                # dκ/ds
    regime = np.where(np.abs(kprime) < tol, 0, np.sign(kprime))  # -1/0/+1
    breaks = np.where(np.diff(regime) != 0)[0] + 1
    return np.concatenate([[0], breaks, [len(s) - 1]])

Key parameter: tol is the curvature-rate threshold separating "constant" (arc/line) from "ramping" (clothoid); too small over-segments on noise, too large merges distinct transitions.

2. Fit a clothoid to each transition segment #

A clothoid is parameterized by start curvature κ0 and rate κ'. Fit them by least squares so the modelled curvature matches the samples.

python
def fit_clothoid(kappa_seg: np.ndarray, s_seg: np.ndarray):
    """Least-squares (κ0, κ') for κ(s) = κ0 + κ' * (s - s0)."""
    s0 = s_seg[0]
    A = np.column_stack([np.ones_like(s_seg), s_seg - s0])
    (k0, kprime), *_ = np.linalg.lstsq(A, kappa_seg, rcond=None)
    return float(k0), float(kprime)

Key parameters: κ0 is the entry curvature, κ' the constant rate; a near-zero κ' collapses the clothoid to an arc, a near-zero κ0 and κ' to a line.

3. Integrate the fitted primitive to recover geometry #

Recover the fitted centerline by integrating heading and position; a clothoid's coordinates come from the Fresnel integrals.

python
from scipy.special import fresnel

def clothoid_points(x0, y0, hdg0, k0, kprime, length, n=50):
    s = np.linspace(0, length, n)
    if abs(kprime) < 1e-9:                        # arc or line fallback
        theta = hdg0 + k0 * s
        x = x0 + np.cumsum(np.r_[0, np.diff(s)] * np.cos(theta))
        y = y0 + np.cumsum(np.r_[0, np.diff(s)] * np.sin(theta))
        return np.column_stack([x, y])
    # scale to standard clothoid, use Fresnel S/C, then rotate/translate
    a = np.sqrt(np.pi / abs(kprime))
    t = (k0 + kprime * s) / (np.sqrt(np.pi * abs(kprime)))
    S, C = fresnel(t)
    # (rotation/translation to (x0,y0,hdg0) omitted for brevity)
    return _place(C * a, S * a, x0, y0, hdg0, k0, kprime)

Key parameter: kprime selects the branch — the Fresnel path for a true clothoid, an arc/line integration when the rate is negligible.

Verification & Acceptance Criteria #

Integrate the whole fitted chain and compare to the source samples.

python
def assert_fit(fitted_xy: np.ndarray, source_xy: np.ndarray, tol=0.05):
    from scipy.spatial import cKDTree
    d, _ = cKDTree(source_xy).query(fitted_xy)
    assert d.max() <= tol, f"lateral fit error {d.max():.3f} m > {tol}"
    print(f"clothoid chain within {d.max()*100:.1f} cm of samples")

Acceptance gate: maximum lateral deviation ≤0.05 m; curvature continuous across every segment join (no step in κ0 versus the previous segment's exit curvature); and each fitted segment classified correctly as line, clothoid, or arc. A segment breaching the budget usually means the breakpoint detection merged two transitions — lower tol in step 1 and refit.

Common Errors & Fixes #

Curvature steps at segment joins. Segments were fit independently, so each segment's κ0 ignores the previous exit curvature. Constrain each segment's start curvature to the prior segment's end value, or fit the chain jointly.

Over-segmentation into dozens of tiny clothoids. tol is below the curvature noise floor. Smooth the curvature signal (Savitzky-Golay) before segmenting and raise tol until segments correspond to real transitions.

fresnel returns unexpected shapes for a straight segment. A line has κ'≈0 and the Fresnel scaling divides by near-zero. Branch to the arc/line integration whenever abs(kprime) < 1e-9.

Fit passes but OpenDRIVE rejects the spiral. OpenDRIVE <spiral> expects curvStart and curvEnd; ensure you export both, derived from κ0 and κ0 + κ'·length, and validate with OpenDRIVE XSD validation.

FAQ #

Why model roads as clothoids at all? #

Roads are designed with clothoid transitions — spirals where curvature changes linearly with arc length — because a driver turns the wheel at a roughly constant rate entering and leaving a curve. Representing a centerline as line, clothoid, and arc primitives matches how the road was engineered, is far more compact than a dense polyline, and is exactly the primitive set OpenDRIVE uses for its reference line, so a clothoid fit serializes directly.

How is a clothoid different from a spline fit? #

A spline minimizes some smoothness functional but has no physical meaning and no guarantee of linear curvature. A clothoid is defined by exactly that property: curvature equal to a start value plus a constant rate times arc length. Fitting clothoids therefore recovers the road's design parameters — entry curvature and curvature rate — rather than an arbitrary smooth interpolant, which is what makes the result meaningful for superelevation and comfort analysis.

What fit tolerance is acceptable? #

The fitted clothoid chain, integrated from its parameters, should stay within 0.05 m lateral of the source centerline samples, matching the map's positional budget. Curvature at segment joins must be continuous — no step — because a curvature discontinuity is both physically wrong and a comfort defect. If a segment cannot meet the lateral budget, it usually means the breakpoint segmentation merged two distinct transitions and needs a finer split.

Up one level: Road Curvature & Superelevation Mapping — the parent workflow that consumes fitted curvature to derive cross-slope and serialize road geometry.