VehicleMapping HD AV Spatial Data

Extracting Road Geometry from OpenDRIVE Records

The reference line is where every other OpenDRIVE quantity is measured from, so an error in evaluating it is an error in the whole road. The schema defines four primitives with exact closed forms, and the two ways to get this wrong are both common: integrating numerically when a closed form exists, and treating the spiral as an arc because the Fresnel integrals look intimidating.

This task evaluates all four correctly for the pipeline in the OpenDRIVE schema breakdown, and asserts the continuity that a correct evaluation produces for free.

What each primitive contributes, expressed as its curvature against arc length:

Curvature Contributed by Each OpenDRIVE Primitive A single curvature-versus-arc-length trace made of a flat zero section, a linear ramp, a flat non-zero section, a second ramp and a cubic, with each section labelled by its record type. <line><spiral> <arc><spiral> <paramPoly3> κ = 0κ linear κ constantκ linear κ cubic 01/R the trace is continuous at every dashed join — a step there is an authoring error, not a modelling choice

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (special.fresnel).
  • Input: parsed <geometry> records carrying s, x, y, hdg, length and one typed child.
  • Upstream stage: streaming XML parsing, as in how to parse OpenDRIVE XML with Python.
  • Output: position and heading at any station, plus a continuity assertion across the record chain.

Step-by-Step #

1. Dispatch on the record's typed child #

python
def evaluate(record, ds: "np.ndarray"):
    """Position and heading at offsets `ds` within one geometry record."""
    kind = record.kind
    if kind == "line":
        return eval_line(record, ds)
    if kind == "arc":
        return eval_arc(record, ds)
    if kind == "spiral":
        if abs(record.curv_end - record.curv_start) < 1e-12:
            return eval_arc(record._as_arc(), ds)     # degenerate spiral
        return eval_spiral(record, ds)
    if kind in ("poly3", "paramPoly3"):
        return eval_param_poly3(record, ds)
    raise ValueError(f"unknown geometry record: {kind!r}")

Handling the degenerate spiral by dispatching to the arc evaluator, rather than by guarding inside the spiral code, keeps the Fresnel path free of special cases. Expected output: arrays of x, y and heading.

2. Line and arc in closed form #

python
import numpy as np

def eval_line(r, ds):
    x = r.x + ds * np.cos(r.hdg)
    y = r.y + ds * np.sin(r.hdg)
    return x, y, np.full_like(ds, r.hdg)


def eval_arc(r, ds):
    k = r.curvature
    hdg = r.hdg + k * ds
    x = r.x + (np.sin(hdg) - np.sin(r.hdg)) / k
    y = r.y - (np.cos(hdg) - np.cos(r.hdg)) / k
    return x, y, hdg

Both are exact at any station, so evaluating a 400 m arc at its endpoint costs the same and is as accurate as evaluating it at 0.1 m intervals.

3. The spiral through Fresnel integrals #

python
from scipy.special import fresnel

def eval_spiral(r, ds):
    """Clothoid with curvature ramping linearly from curv_start to curv_end."""
    c_dot = (r.curv_end - r.curv_start) / r.length          # curvature rate
    a = np.sqrt(np.pi / abs(c_dot))                          # Fresnel scale
    sign = np.sign(c_dot)

    s0 = r.curv_start / c_dot                                # virtual origin offset
    S1, C1 = fresnel((s0 + ds) / a)
    S0, C0 = fresnel(np.full_like(ds, s0) / a)

    dx, dy = a * (C1 - C0), sign * a * (S1 - S0)
    rot = r.hdg - sign * (s0 ** 2) * abs(c_dot) / 2.0
    x = r.x + dx * np.cos(rot) - dy * np.sin(rot)
    y = r.y + dx * np.sin(rot) + dy * np.cos(rot)
    hdg = r.hdg + r.curv_start * ds + 0.5 * c_dot * ds ** 2
    return x, y, hdg

Key parameters: c_dot is the curvature rate the record implies rather than a field it carries; s0 is the offset from the clothoid's own zero-curvature origin, which is what lets a record start at a non-zero curvature. The heading expression is exact and independent of the Fresnel path, which makes it a useful cross-check on the position.

4. Assert continuity across every join #

python
def assert_chain_continuous(records, pos_tol=1e-3, hdg_tol=1e-3) -> None:
    for a, b in zip(records, records[1:]):
        xa, ya, ha = (v[-1] for v in evaluate(a, np.array([a.length])))
        d = float(np.hypot(xa - b.x, ya - b.y))
        dh = float(abs(np.arctan2(np.sin(ha - b.hdg), np.cos(ha - b.hdg))))
        assert d <= pos_tol, f"gap of {d*1000:.2f} mm at s={b.s:.3f}"
        assert dh <= hdg_tol, f"heading step of {dh*1000:.2f} mrad at s={b.s:.3f}"

Wrapping the heading difference through arctan2 is what keeps a join at ±π from reporting a 2π step. These tolerances are tight on purpose: in a well-formed file the joins are exact, so any measurable discrepancy is a defect worth surfacing rather than absorbing.

What each primitive costs to evaluate, and why only one of them needs a numerical step at all:

Evaluation Cost and Method per Primitive Four rows pairing a geometry primitive with its evaluation method, its cost and whether it is exact. evaluating one station of each primitive <line> constant heading two trigonometric calls exact <arc> constant curvature four trigonometric calls exact <spiral> curvature linear in s two Fresnel integrals exact <paramPoly3> cubic in the parameter polynomial plus arc-length inversion iterative three of the four have closed forms, so integrating them numerically trades accuracy away for nothing

Verification & Acceptance Criteria #

python
def assert_geometry_extraction(road, control) -> None:
    assert_chain_continuous(road.records)

    for s, expected in control.items():                  # surveyed reference points
        x, y, _ = road.at(s)
        assert np.hypot(x - expected[0], y - expected[1]) <= 0.02, \
            f"reference line off by more than 0.02 m at s={s}"

    total = sum(r.length for r in road.records)
    assert abs(road.records[-1].s + road.records[-1].length - total) < 1e-6, \
        "record stations do not tile the road length"

Acceptance gate: ≤1 mm positional and ≤1 mrad heading continuity at every join; reference-line position within 0.02 m of surveyed control; and record stations that tile the road length exactly, with no overlap and no gap.

What a join discrepancy of each size actually means, which is why the tolerance is a millimetre:

Reading a Reference-Line Join Discrepancy Four rows pairing a join-discrepancy magnitude with the specific defect it indicates. a discrepancy at the join is always a defect — the question is whose below 0.1 mm float64 rounding expected — a well-formed chain no action 0.1 mm – 1 mm error accumulating within a record a numerically integrated evaluator use the closed forms 1 mm – 1 cm a spiral evaluated from the wrong origin the virtual-origin offset was omitted fix the evaluator above 1 cm stations do not tile the length an authoring defect in the file reject the file loosening the tolerance to make a file pass moves a diagnosable defect into the permanent geometry

Common Errors & Fixes #

A small kink appears at every record boundary. Numerical integration is being used where a closed form exists, so error accumulates within each record. Replace with the closed forms above.

Spirals render as arcs. The evaluator ignored the curvature rate, or the file's start and end curvature were read as the same field. Check that curv_start and curv_end are distinct in the parsed record.

A division-by-zero in the spiral evaluator. A degenerate spiral with equal start and end curvature. Dispatch it to the arc evaluator rather than guarding inside the Fresnel path.

Headings are right and positions drift on long spirals. The virtual-origin offset s0 was omitted, so the record was evaluated as if it started at zero curvature. It rarely does.

Curvature computed downstream is noisy on the straights. The reference line is being resampled before differentiation at too fine a spacing, which amplifies float noise — the effect described in calculating road curvature with Python and Shapely. Take curvature from the record type instead; here it is known exactly.

FAQ #

Why not just sample every record numerically? #

Numerical integration accumulates error along the record, so the endpoint drifts from the position the next record assumes and the reference line develops a small gap at every join. Line, arc and spiral all have exact closed forms — the spiral's being the Fresnel integrals — so there is no accuracy to trade away. Reserve numerical evaluation for the parametric cubic, where it is genuinely needed to convert between the parameter and arc length.

How is a spiral evaluated in practice? #

Through the Fresnel sine and cosine integrals, which SciPy provides directly. The record gives a start curvature and an end curvature; their difference over the record length is the curvature rate, and that rate scales the Fresnel argument. The case that breaks naive implementations is a zero curvature rate, where the scaling divides by zero — that record is a plain arc and should be dispatched as one rather than special-cased inside the spiral evaluator.

What tolerance should the join continuity check use? #

Tight — a millimetre in position and a milliradian in heading. These joins are exact by construction in a well-formed file, so any measurable discrepancy is a defect in the file or in the evaluator, not a tolerance to accommodate. Loosening the check to make a map pass converts a detectable authoring error into a permanent kink that every downstream curvature computation will amplify.

Up one level: OpenDRIVE Schema Breakdown — the extraction pipeline this evaluator sits inside.