VehicleMapping HD AV Spatial Data

Smoothing Centerlines with Quadratic Programming

A medial axis is geometrically correct and kinematically useless: it inherits every wobble of the boundary that produced it, so its curvature swings far beyond anything a vehicle would drive. Smoothing it is unavoidable. What is avoidable is smoothing it by an amount nobody chose.

This task poses the smoothing as a constrained quadratic program for centerline generation algorithms: minimise curvature energy, subject to each vertex staying inside a box sized from the accuracy budget. The budget becomes a constraint rather than a consequence.

What the constraint buys, drawn against the two unconstrained alternatives:

Three Smoothings and the Deviation Each Costs A noisy input with a tolerance corridor, overlaid with a moving average, a spline fit and a constrained QP result, each annotated with its maximum deviation. input medial axis ±0.04 m corridor moving average — 0.11 m at the bend spline fit — 0.07 m constrained QP — 0.04 m, by construction the QP result is not the smoothest curve — it is the smoothest curve that stays inside the budget

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (sparse, optimize), or osqp for large problems.
  • Input: an arc-length-resampled centerline and the stage's deviation budget.
  • Upstream stage: medial-axis extraction, as in generating centerlines with a Voronoi medial axis.
  • Output: a smoothed centerline with a certified maximum deviation.

Step-by-Step #

1. Build the second-difference objective #

python
import numpy as np
import scipy.sparse as sp

def second_difference(n: int) -> sp.csr_matrix:
    """D such that (D x) is the second difference of x."""
    main = sp.diags([1.0, -2.0, 1.0], [0, 1, 2], shape=(n - 2, n))
    return main.tocsr()


def objective(n: int) -> sp.csr_matrix:
    D = second_difference(n)
    return (D.T @ D).tocsc()          # P in ½ xᵀ P x

The problem separates in x and y — the objective couples neighbouring vertices but not the two coordinates — so it can be solved as two independent one-dimensional programs, which is a large saving at map scale.

2. Bound each vertex to a box #

python
def bounds(original: np.ndarray, budget_m: float):
    """Box constraints keeping every vertex within `budget_m` of its input."""
    return original - budget_m, original + budget_m

A box rather than a disc is a deliberate approximation: it is separable, which keeps the problem a QP with simple bounds rather than a second-order cone program, and it over-constrains by at most a factor of √2 in the diagonal direction. Where that matters, shrink the budget by √2 rather than changing the formulation.

3. Pin the endpoints #

python
def pin_endpoints(lo: np.ndarray, hi: np.ndarray, original: np.ndarray):
    lo[0] = hi[0] = original[0]
    lo[-1] = hi[-1] = original[-1]
    return lo, hi

Unpinned endpoints are the commonest cause of a smoothed lane that no longer meets its successor: the smoother pulls the ends inward, and a 3 cm gap at every lane join becomes a connectivity failure in detecting dangling lanes and connectivity gaps.

4. Solve and certify #

python
import osqp

def smooth_axis(xy: np.ndarray, budget_m: float = 0.04) -> np.ndarray:
    n = len(xy)
    P = objective(n)
    out = np.empty_like(xy)
    for k in (0, 1):
        lo, hi = pin_endpoints(*bounds(xy[:, k], budget_m), xy[:, k])
        prob = osqp.OSQP()
        prob.setup(P=P, q=np.zeros(n), A=sp.eye(n, format="csc"),
                   l=lo, u=hi, verbose=False, eps_abs=1e-6, eps_rel=1e-6)
        res = prob.solve()
        if res.info.status != "solved":
            raise RuntimeError(f"QP did not solve: {res.info.status}")
        out[:, k] = res.x
    return out

Key parameters: eps_abs/eps_rel at 1e-6 keep the solver's own tolerance an order of magnitude below the metre-scale budget, so a "solved" result really is inside the box. Raising the solver tolerance to speed things up silently loosens the accuracy guarantee.

Where the smoother spends the budget, and where it spends nothing:

Per-Vertex Deviation: Where the Budget Is Actually Spent A deviation trace along the centerline showing near-zero use on straights and the constraint being active through the noisy bend. 0.04 m bound constraint active 00.04 m straightnoisy bend straight a smoother that moved every vertex by a fixed amount would spend the budget on the straights, where it buys nothing

Verification & Acceptance Criteria #

python
def assert_smoothing(before, after, budget_m=0.04, k_max=None) -> None:
    dev = np.linalg.norm(after - before, axis=1)
    assert dev.max() <= budget_m + 1e-6, f"deviation {dev.max():.4f} m over budget"
    assert np.allclose(after[0], before[0]) and np.allclose(after[-1], before[-1]), \
        "endpoints moved — successors will no longer meet"

    e_before = np.sum(np.diff(before, 2, axis=0) ** 2)
    e_after = np.sum(np.diff(after, 2, axis=0) ** 2)
    assert e_after < e_before, "smoothing did not reduce curvature energy"
    if k_max is not None:
        assert peak_curvature(after) <= k_max, "peak curvature still over limit"

Acceptance gate: maximum deviation ≤ the budget; endpoints unmoved; curvature energy strictly reduced; and, where a kinematic limit applies, peak curvature inside it — which the QP does not guarantee and must therefore be checked separately.

What the QP gives and what it does not, which decides which checks belong downstream:

What the Quadratic Program Guarantees Four rows pairing a desired property with whether the quadratic program guarantees it and where it is checked. what the solver promises, and what it does not max deviation ≤ budget the accuracy constraint the box constraint, by construction guaranteed endpoints unmoved so successors still meet pinned bounds on the first and last vertex guaranteed total curvature energy falls the smoothness objective it is what the solver minimises guaranteed peak curvature ≤ limit the kinematic constraint low total energy admits one sharp corner check separately the last row is why a peak-curvature assertion belongs after the solve rather than inside it

Common Errors & Fixes #

The smoothed lane no longer meets its successor. Endpoints were not pinned. Pin them; do not widen the successor tolerance to compensate.

Deviation exceeds the budget by a few millimetres. The solver's tolerance is comparable to the budget. Tighten eps_abs and eps_rel.

Peak curvature is still too high after smoothing. Energy minimisation does not bound the maximum. Either raise the budget — deliberately, in the pipeline's allocation — or add an explicit curvature constraint, which makes the problem harder but not intractable.

The solve is slow on long lanes. The problem was posed in two dimensions jointly. Solve x and y separately; the objective does not couple them.

The result is barely different from the input. The budget is smaller than the input noise, which is the correct behaviour: the smoother cannot buy smoothness it has no budget for. Re-examine the allocation rather than the solver.

FAQ #

Why a constrained QP rather than a spline fit or a moving average? #

Because both alternatives trade accuracy for smoothness silently. A moving average shifts every vertex by an amount nobody chose, and a spline fit's smoothing parameter has no units a map budget can be expressed in. A QP makes the trade explicit: the objective is smoothness, and the accuracy budget is a hard constraint, so the solver returns the smoothest curve that is still inside the tolerance rather than a curve whose error you discover afterwards.

What does the second-difference objective actually minimise? #

The sum of squared second differences between consecutive vertices, which at constant spacing is proportional to discrete curvature energy. Minimising it pulls each vertex toward the average of its neighbours, so the result is the curve with the least total bending that still satisfies the constraints. It is not the same as minimising maximum curvature — a curve can have low total energy and one sharp corner — so a peak-curvature check still belongs downstream.

How is the deviation bound chosen? #

From the accuracy budget the section allocates to centerline generation, not from what looks smooth. If the pipeline's lane-level budget gives this stage 0.04 metres, that is the box half-width, and the solver will use all of it where the input is noisy and none of it where the input is already smooth. Setting it larger to get a prettier curve is spending a budget that later stages are relying on.

Up one level: Centerline Generation Algorithms — the stage this smoother completes.