Current Quintic Hermite Lateral Bridge augmentation is delaying the lateral convergence with replanning in nature

[Owen-Liuyuxuan]

Summary

The Quintic Hermite lateral bridge used in ego-state perturbation augmentation always assumes p''(0) = 0 — zero lateral second derivative at the current frame — regardless of the vehicle's actual steering / lateral acceleration.

That assumption is fine for a one-shot recovery plan. Under high-frequency replanning (the real Autoware / closed-loop setting), it systematically delays lateral convergence: each replan restarts with a locally flatter profile, so the executed trajectory stays farther from the lane than the original one-shot bridge intended.

This is not specific to one implementation. The same boundary conditions appear in:

Label in this issue	Where	Link
Current mainline (“old”)	tier4-main torch bridge	data_augmentation_bridge.py on tier4-main
Proposed fast core (“new”)	#276 numpy rewrite	PR #276 — Replace bridge augmentation core with fast numpy implementation

History bump / bridge-time randomization in #276 do not fix this — they decorrelate past history, but the future lateral profile still hard-codes the same p''(0) = 0.

---

Working principle (bridge augmentation)

Given a laterally perturbed ego pose at t = 0, we reconnect to the GT trajectory with a C2 quintic lateral offset profile p(s) along centerline arc length s, with merge distance L = s_merge:

Boundary	Intended meaning	Current code
p(0) = Δy	lateral offset at current	correct
p'(0) = tan(Δψ)	heading error at current	correct
p''(0) = 0	lateral 2nd derivative / steering onset	always forced to 0
p(L) = p'(L) = p''(L) = 0	merge back onto GT	by construction

So training examples teach the model: “from any (Δy, Δψ), recover as if lateral steering derivative is currently zero.”

Where it shows up in code

Mainline (tier4-main) — Hermite bases with no curvature state:

• lateral_offset_profile_* / derivative in data_augmentation_bridge.py @ tier4-main

PR #276 — same BCs via polynomial coeffs, with a2 = 0.0 hard-coded:

• solve_lateral_profile_coeffs / lateral_offset_profile in the new data_augmentation_bridge_core.py (#276)

---

Why replanning makes this harmful

In deployment the planner we roughly assume that at every 0.1 of the merging distance, the planner track the vehicle perfectly according to the trajectory. Idealized loop:

1. t = 0: offset (Δy₀, Δψ₀). Model predicts a recovery assuming p''(0) = 0.
2. Vehicle follows that plan for one tick and acquires some real lateral curvature / steering.
3. t = 0.1: new observation. The residual offset is (Δy₁, Δψ₁), but the model again plans as if p''(0) = 0, discarding the curvature already built up.
4. Repeat.

Each replan therefore re-flattens the beginning of the correction. The executed path is a concatenation of short “flat-start” segments, not the one-shot quintic. Heading correction is postponed and lateral error decays slower than the training bridge suggests.

one-shot bridge:     p(s) designed to hit 0 at s = L
                     (smooth, uses full horizon)

replanned every Δs:  at each tick, rebuild p with p''(0)=0
                     → executed path stays flatter
                     → residual lateral offset remains at s = L

---

Minimal demo (test.py)

A self-contained NumPy demo mirrors the production profile and simulates perfect actuation with replanning every step_size = 0.1 over s_merge = 1.0:

• Black dashed: one-shot plan → converges to 0 at s = 1.
• Blue faint: every newly replanned full profile (each starts flat at its own s = 0).
• Red: executed trajectory under replanning → still not zero when global s reaches 1.

Reproduction:

python test.py

Expected qualitative result: after 10 steps with s_merge = 1, global s = 1.0 but executed lateral offset remains clearly positive (demo prints the residual).

---

Impact on Diffusion-Planner training / closed loop

1. Distribution mismatch: augmentation futures are one-shot bridges with p''(0) = 0; closed-loop rollouts replan and execute flatter recoveries.
2. Biased recovery timing: the model is rewarded for “slow / postponed” heading adjustment relative to what a curvature-aware bridge would do.
3. Shared by mainline and Replace bridge augmentation core with fast numpy implementation #276: fixing only the numpy port (or only tier4-main) is not enough — both encode the same BCs.

---

References

• Demo: test.py (local repro of the figure above)
• Mainline bridge: https://github.com/tier4/Diffusion-Planner/blob/tier4-main/diffusion_planner/diffusion_planner/utils/data_augmentation_bridge.py
• Fast numpy rewrite PR: Replace bridge augmentation core with fast numpy implementation #276
• Branch tier4-main: https://github.com/tier4/Diffusion-Planner/tree/tier4-main