Spaces:
Configuration error
Configuration error
Commit ·
287b358
1
Parent(s): 3624d0b
feat(RCLane): archive parallel BEV projection modes
Browse files
experiments/bev_parallel_modes/README.md
ADDED
|
@@ -0,0 +1,41 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Archived parallel BEV modes
|
| 2 |
+
|
| 3 |
+
This directory preserves the two experimental BEV post-processing modes that
|
| 4 |
+
were evaluated before the native C++ runtime became the active path. The files
|
| 5 |
+
are isolated from the production Python/C++ pipeline: nothing here changes raw
|
| 6 |
+
model decoding or the default perspective-to-BEV projection.
|
| 7 |
+
|
| 8 |
+
Both modes alter only the cubic curves rendered/exported in BEV. The decoded
|
| 9 |
+
camera-view lanes remain the model's raw output.
|
| 10 |
+
|
| 11 |
+
## Mode 1: parallel detected lanes, camera funnel only
|
| 12 |
+
|
| 13 |
+
This mode keeps only lane IDs detected in the current frame, rebuilds their BEV
|
| 14 |
+
cubics as offsets of one reference curve, and clips the result to the camera
|
| 15 |
+
funnel. It never synthesizes a missing lane.
|
| 16 |
+
|
| 17 |
+
```bash
|
| 18 |
+
.venv/bin/python -u experiments/bev_parallel_modes/test_video_bev_parallel_legacy.py \
|
| 19 |
+
--model exports/rclane_b0_e19.onnx \
|
| 20 |
+
--video raw_Town04_Opt_20260714_093110.mp4 \
|
| 21 |
+
--output runs/bev_parallel_visible.mp4 \
|
| 22 |
+
--provider tensorrt \
|
| 23 |
+
--always-parallel-repair
|
| 24 |
+
```
|
| 25 |
+
|
| 26 |
+
## Mode 2: always complete four parallel lanes
|
| 27 |
+
|
| 28 |
+
This mode always exports P0-P3. Missing lane IDs are synthesized as parallel
|
| 29 |
+
offsets, and the BEV curves may extend outside the camera funnel.
|
| 30 |
+
|
| 31 |
+
```bash
|
| 32 |
+
.venv/bin/python -u experiments/bev_parallel_modes/test_video_bev_parallel_legacy.py \
|
| 33 |
+
--model exports/rclane_b0_e19.onnx \
|
| 34 |
+
--video raw_Town04_Opt_20260714_093110.mp4 \
|
| 35 |
+
--output runs/bev_parallel_four.mp4 \
|
| 36 |
+
--provider tensorrt \
|
| 37 |
+
--complete-four-parallel-lanes
|
| 38 |
+
```
|
| 39 |
+
|
| 40 |
+
Use `--max-frames N` for a short smoke test. This archived runner processes one
|
| 41 |
+
frame at a time and is intended for reproducibility, not deployment benchmarking.
|
experiments/bev_parallel_modes/bev.py
ADDED
|
@@ -0,0 +1,1015 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Ego-centric inverse-perspective mapping and cubic lane fitting.
|
| 2 |
+
|
| 3 |
+
The public coordinate convention is ADAS-style:
|
| 4 |
+
|
| 5 |
+
X: forward from the ego origin (metres)
|
| 6 |
+
Y: left of ego (metres)
|
| 7 |
+
|
| 8 |
+
Z is used only internally to intersect an image ray with the local road plane
|
| 9 |
+
``Z = 0``. The returned BEV lane points and cubic models are strictly 2-D.
|
| 10 |
+
"""
|
| 11 |
+
|
| 12 |
+
from dataclasses import dataclass
|
| 13 |
+
from functools import lru_cache
|
| 14 |
+
|
| 15 |
+
import numpy as np
|
| 16 |
+
|
| 17 |
+
|
| 18 |
+
@dataclass(frozen=True)
|
| 19 |
+
class CameraCalibration:
|
| 20 |
+
width: int = 1920
|
| 21 |
+
height: int = 1080
|
| 22 |
+
horizontal_fov_deg: float = 30.0
|
| 23 |
+
fx: float = 3582.768775
|
| 24 |
+
fy: float = 3582.768775
|
| 25 |
+
cx: float = 960.0
|
| 26 |
+
cy: float = 540.0
|
| 27 |
+
camera_to_vehicle: tuple = (
|
| 28 |
+
(0.997564, 0.0, 0.069756, 1.0),
|
| 29 |
+
(0.0, 1.0, 0.0, 0.0),
|
| 30 |
+
(-0.069756, 0.0, 0.997564, 1.8),
|
| 31 |
+
(0.0, 0.0, 0.0, 1.0),
|
| 32 |
+
)
|
| 33 |
+
|
| 34 |
+
@property
|
| 35 |
+
def intrinsic(self):
|
| 36 |
+
return np.array(
|
| 37 |
+
((self.fx, 0.0, self.cx),
|
| 38 |
+
(0.0, self.fy, self.cy),
|
| 39 |
+
(0.0, 0.0, 1.0)),
|
| 40 |
+
dtype=np.float64,
|
| 41 |
+
)
|
| 42 |
+
|
| 43 |
+
@property
|
| 44 |
+
def camera_to_vehicle_matrix(self):
|
| 45 |
+
return np.asarray(self.camera_to_vehicle, dtype=np.float64)
|
| 46 |
+
|
| 47 |
+
|
| 48 |
+
@dataclass(frozen=True)
|
| 49 |
+
class BevRange:
|
| 50 |
+
x_min: float = 0.0
|
| 51 |
+
x_max: float = 300.0
|
| 52 |
+
y_min: float = -85.0
|
| 53 |
+
y_max: float = 85.0
|
| 54 |
+
|
| 55 |
+
|
| 56 |
+
@lru_cache(maxsize=8)
|
| 57 |
+
def _projection_constants(calibration):
|
| 58 |
+
optical_to_ue = np.array(
|
| 59 |
+
((0.0, 0.0, 1.0),
|
| 60 |
+
(1.0, 0.0, 0.0),
|
| 61 |
+
(0.0, -1.0, 0.0)),
|
| 62 |
+
dtype=np.float64,
|
| 63 |
+
)
|
| 64 |
+
mount = calibration.camera_to_vehicle_matrix
|
| 65 |
+
return (
|
| 66 |
+
np.linalg.inv(calibration.intrinsic),
|
| 67 |
+
optical_to_ue,
|
| 68 |
+
mount[:3, 3].copy(),
|
| 69 |
+
mount[:3, :3].copy(),
|
| 70 |
+
)
|
| 71 |
+
|
| 72 |
+
|
| 73 |
+
def pixels_to_ground(pixels, calibration=CameraCalibration(),
|
| 74 |
+
bev_range=BevRange()):
|
| 75 |
+
"""Project raw-image pixels onto the local ground plane.
|
| 76 |
+
|
| 77 |
+
Args:
|
| 78 |
+
pixels: ``(N, 2)`` raw-image coordinates ``(u, v)``.
|
| 79 |
+
calibration: pinhole intrinsics and fixed camera-to-vehicle mount.
|
| 80 |
+
bev_range: accepted metric ROI.
|
| 81 |
+
|
| 82 |
+
Returns:
|
| 83 |
+
``(points_xy, valid_mask)``. ``points_xy`` contains only valid points in
|
| 84 |
+
ADAS coordinates ``(X forward, Y left)``. ``valid_mask`` indexes the
|
| 85 |
+
input array and is useful for carrying per-point scores through IPM.
|
| 86 |
+
"""
|
| 87 |
+
pixels = np.asarray(pixels, dtype=np.float64)
|
| 88 |
+
if pixels.size == 0:
|
| 89 |
+
return np.empty((0, 2), dtype=np.float64), np.zeros(0, dtype=bool)
|
| 90 |
+
if pixels.ndim != 2 or pixels.shape[1] != 2:
|
| 91 |
+
raise ValueError("pixels must have shape (N, 2)")
|
| 92 |
+
|
| 93 |
+
finite = np.isfinite(pixels).all(axis=1)
|
| 94 |
+
homogeneous = np.column_stack((pixels, np.ones(len(pixels))))
|
| 95 |
+
inverse_intrinsic, optical_to_ue, origin_vehicle, rotation = (
|
| 96 |
+
_projection_constants(calibration)
|
| 97 |
+
)
|
| 98 |
+
rays_optical = (inverse_intrinsic @ homogeneous.T).T
|
| 99 |
+
|
| 100 |
+
# CARLA/UE camera axes are X forward, Y right, Z up. OpenCV optical axes
|
| 101 |
+
# are x right, y down, z forward: optical -> UE = (z, x, -y).
|
| 102 |
+
rays_camera_ue = (optical_to_ue @ rays_optical.T).T
|
| 103 |
+
rays_vehicle = (rotation @ rays_camera_ue.T).T
|
| 104 |
+
dz = rays_vehicle[:, 2]
|
| 105 |
+
with np.errstate(divide="ignore", invalid="ignore"):
|
| 106 |
+
scale = -origin_vehicle[2] / dz
|
| 107 |
+
ground_vehicle = origin_vehicle + scale[:, None] * rays_vehicle
|
| 108 |
+
|
| 109 |
+
# CARLA Y points right; public BEV Y points left.
|
| 110 |
+
x_forward = ground_vehicle[:, 0]
|
| 111 |
+
y_left = -ground_vehicle[:, 1]
|
| 112 |
+
valid = (
|
| 113 |
+
finite
|
| 114 |
+
& np.isfinite(ground_vehicle).all(axis=1)
|
| 115 |
+
& (dz < -1e-8)
|
| 116 |
+
& (scale > 0.0)
|
| 117 |
+
& (x_forward >= bev_range.x_min)
|
| 118 |
+
& (x_forward <= bev_range.x_max)
|
| 119 |
+
& (y_left >= bev_range.y_min)
|
| 120 |
+
& (y_left <= bev_range.y_max)
|
| 121 |
+
)
|
| 122 |
+
return np.column_stack((x_forward[valid], y_left[valid])), valid
|
| 123 |
+
|
| 124 |
+
|
| 125 |
+
def ground_to_pixels(points_xy, calibration=CameraCalibration()):
|
| 126 |
+
"""Project ADAS ground points to raw pixels (used for calibration tests)."""
|
| 127 |
+
points_xy = np.asarray(points_xy, dtype=np.float64)
|
| 128 |
+
if points_xy.ndim != 2 or points_xy.shape[1] != 2:
|
| 129 |
+
raise ValueError("points_xy must have shape (N, 2)")
|
| 130 |
+
# ADAS Y-left -> CARLA Y-right.
|
| 131 |
+
points_vehicle = np.column_stack(
|
| 132 |
+
(points_xy[:, 0], -points_xy[:, 1], np.zeros(len(points_xy)))
|
| 133 |
+
)
|
| 134 |
+
mount = calibration.camera_to_vehicle_matrix
|
| 135 |
+
rotation_vehicle_to_camera = mount[:3, :3].T
|
| 136 |
+
points_camera_ue = (
|
| 137 |
+
rotation_vehicle_to_camera
|
| 138 |
+
@ (points_vehicle - mount[:3, 3]).T
|
| 139 |
+
).T
|
| 140 |
+
points_optical = np.column_stack(
|
| 141 |
+
(points_camera_ue[:, 1], -points_camera_ue[:, 2], points_camera_ue[:, 0])
|
| 142 |
+
)
|
| 143 |
+
projected = (calibration.intrinsic @ points_optical.T).T
|
| 144 |
+
with np.errstate(divide="ignore", invalid="ignore"):
|
| 145 |
+
return projected[:, :2] / projected[:, 2:3]
|
| 146 |
+
|
| 147 |
+
|
| 148 |
+
def model_lane_to_ground(lane, model_width=800, model_height=320,
|
| 149 |
+
calibration=CameraCalibration(),
|
| 150 |
+
bev_range=BevRange()):
|
| 151 |
+
"""Convert one decoded RCLane polyline into metric BEV points + scores."""
|
| 152 |
+
if len(lane.points) == 0:
|
| 153 |
+
return np.empty((0, 2)), np.empty(0)
|
| 154 |
+
lane_points = np.asarray(lane.points, dtype=np.float64)
|
| 155 |
+
raw_pixels = lane_points[:, :2].copy()
|
| 156 |
+
raw_pixels[:, 0] *= calibration.width / float(model_width)
|
| 157 |
+
raw_pixels[:, 1] *= calibration.height / float(model_height)
|
| 158 |
+
ground, valid = pixels_to_ground(raw_pixels, calibration, bev_range)
|
| 159 |
+
return ground, lane_points[valid, 2]
|
| 160 |
+
|
| 161 |
+
|
| 162 |
+
def _weighted_lstsq(design, targets, weights):
|
| 163 |
+
root_weight = np.sqrt(np.maximum(weights, 1e-8))
|
| 164 |
+
return np.linalg.lstsq(
|
| 165 |
+
design * root_weight[:, None], targets * root_weight, rcond=None
|
| 166 |
+
)[0]
|
| 167 |
+
|
| 168 |
+
|
| 169 |
+
def fit_cubic_lane(points_xy, point_scores=None, min_points=6,
|
| 170 |
+
huber_delta_m=0.30, iterations=5):
|
| 171 |
+
"""Robustly fit ``Y(X) = c0 + c1 X + c2 X^2 + c3 X^3``.
|
| 172 |
+
|
| 173 |
+
X is normalized internally for numerical stability out to 300 metres. The
|
| 174 |
+
returned coefficients are converted back to metric X and ordered
|
| 175 |
+
``[c0, c1, c2, c3]``.
|
| 176 |
+
"""
|
| 177 |
+
points = np.asarray(points_xy, dtype=np.float64)
|
| 178 |
+
if points.ndim != 2 or points.shape[1] != 2:
|
| 179 |
+
raise ValueError("points_xy must have shape (N, 2)")
|
| 180 |
+
finite = np.isfinite(points).all(axis=1)
|
| 181 |
+
points = points[finite]
|
| 182 |
+
if len(points) < min_points:
|
| 183 |
+
return None
|
| 184 |
+
order = np.argsort(points[:, 0])
|
| 185 |
+
x = points[order, 0]
|
| 186 |
+
y = points[order, 1]
|
| 187 |
+
if np.ptp(x) < 1.0:
|
| 188 |
+
return None
|
| 189 |
+
|
| 190 |
+
if point_scores is None:
|
| 191 |
+
base_weights = np.ones(len(x), dtype=np.float64)
|
| 192 |
+
else:
|
| 193 |
+
scores = np.asarray(point_scores, dtype=np.float64)[finite][order]
|
| 194 |
+
base_weights = np.clip(scores, 0.05, 1.0)
|
| 195 |
+
|
| 196 |
+
center = float((x.min() + x.max()) * 0.5)
|
| 197 |
+
scale = float(max((x.max() - x.min()) * 0.5, 1.0))
|
| 198 |
+
z = (x - center) / scale
|
| 199 |
+
design = np.column_stack((np.ones(len(z)), z, z ** 2, z ** 3))
|
| 200 |
+
weights = base_weights.copy()
|
| 201 |
+
coefficients_normalized = _weighted_lstsq(design, y, weights)
|
| 202 |
+
for _ in range(iterations):
|
| 203 |
+
residual = y - design @ coefficients_normalized
|
| 204 |
+
robust_weight = np.minimum(
|
| 205 |
+
1.0, huber_delta_m / np.maximum(np.abs(residual), 1e-8)
|
| 206 |
+
)
|
| 207 |
+
weights = base_weights * robust_weight
|
| 208 |
+
coefficients_normalized = _weighted_lstsq(design, y, weights)
|
| 209 |
+
|
| 210 |
+
residual = y - design @ coefficients_normalized
|
| 211 |
+
inliers = np.abs(residual) <= max(huber_delta_m, 2.5 * np.median(np.abs(residual)))
|
| 212 |
+
if np.count_nonzero(inliers) >= min_points:
|
| 213 |
+
coefficients_normalized = _weighted_lstsq(
|
| 214 |
+
design[inliers], y[inliers], base_weights[inliers]
|
| 215 |
+
)
|
| 216 |
+
else:
|
| 217 |
+
inliers = np.ones(len(x), dtype=bool)
|
| 218 |
+
|
| 219 |
+
# Compose p((X - center) / scale) and return ascending metric coefficients.
|
| 220 |
+
normalized_polynomial = np.polynomial.Polynomial(coefficients_normalized)
|
| 221 |
+
metric_argument = np.polynomial.Polynomial((-center / scale, 1.0 / scale))
|
| 222 |
+
metric_polynomial = normalized_polynomial(metric_argument)
|
| 223 |
+
coefficients = np.zeros(4, dtype=np.float64)
|
| 224 |
+
coefficients[:len(metric_polynomial.coef)] = metric_polynomial.coef
|
| 225 |
+
|
| 226 |
+
fitted = np.polynomial.polynomial.polyval(x[inliers], coefficients)
|
| 227 |
+
rmse = float(np.sqrt(np.mean((y[inliers] - fitted) ** 2)))
|
| 228 |
+
return {
|
| 229 |
+
"coefficients": coefficients,
|
| 230 |
+
"x_min": float(x[inliers].min()),
|
| 231 |
+
"x_max": float(x[inliers].max()),
|
| 232 |
+
"rmse": rmse,
|
| 233 |
+
"point_count": int(len(x)),
|
| 234 |
+
"inlier_count": int(np.count_nonzero(inliers)),
|
| 235 |
+
"inlier_ratio": float(np.mean(inliers)),
|
| 236 |
+
}
|
| 237 |
+
|
| 238 |
+
|
| 239 |
+
def evaluate_cubic(coefficients, x):
|
| 240 |
+
return np.polynomial.polynomial.polyval(
|
| 241 |
+
np.asarray(x, dtype=np.float64), np.asarray(coefficients, dtype=np.float64)
|
| 242 |
+
)
|
| 243 |
+
|
| 244 |
+
|
| 245 |
+
def evaluate_cubic_derivative(coefficients, x):
|
| 246 |
+
"""Evaluate dY/dX for an ascending-order cubic coefficient vector."""
|
| 247 |
+
coefficients = np.asarray(coefficients, dtype=np.float64)
|
| 248 |
+
derivative = np.array(
|
| 249 |
+
(coefficients[1], 2.0 * coefficients[2], 3.0 * coefficients[3]),
|
| 250 |
+
dtype=np.float64,
|
| 251 |
+
)
|
| 252 |
+
return np.polynomial.polynomial.polyval(
|
| 253 |
+
np.asarray(x, dtype=np.float64), derivative
|
| 254 |
+
)
|
| 255 |
+
|
| 256 |
+
|
| 257 |
+
def _copy_fit(fit):
|
| 258 |
+
copied = dict(fit)
|
| 259 |
+
copied["coefficients"] = np.asarray(
|
| 260 |
+
fit["coefficients"], dtype=np.float64
|
| 261 |
+
).copy()
|
| 262 |
+
return copied
|
| 263 |
+
|
| 264 |
+
|
| 265 |
+
def _shared_samples(left_fit, right_fit, sample_step_m):
|
| 266 |
+
x_min = max(float(left_fit["x_min"]), float(right_fit["x_min"]))
|
| 267 |
+
x_max = min(float(left_fit["x_max"]), float(right_fit["x_max"]))
|
| 268 |
+
if x_max - x_min < sample_step_m:
|
| 269 |
+
return np.empty(0, dtype=np.float64)
|
| 270 |
+
count = max(3, int(np.ceil((x_max - x_min) / sample_step_m)) + 1)
|
| 271 |
+
return np.linspace(x_min, x_max, count, dtype=np.float64)
|
| 272 |
+
|
| 273 |
+
|
| 274 |
+
def _normal_gap(left_fit, right_fit, x):
|
| 275 |
+
"""Approximate signed left-to-right distance normal to the mean curve."""
|
| 276 |
+
left_y = evaluate_cubic(left_fit["coefficients"], x)
|
| 277 |
+
right_y = evaluate_cubic(right_fit["coefficients"], x)
|
| 278 |
+
left_slope = evaluate_cubic_derivative(left_fit["coefficients"], x)
|
| 279 |
+
right_slope = evaluate_cubic_derivative(right_fit["coefficients"], x)
|
| 280 |
+
mean_slope = 0.5 * (left_slope + right_slope)
|
| 281 |
+
return (left_y - right_y) / np.sqrt(1.0 + mean_slope ** 2)
|
| 282 |
+
|
| 283 |
+
|
| 284 |
+
def _longest_true_run_m(mask, x):
|
| 285 |
+
longest = 0.0
|
| 286 |
+
start = None
|
| 287 |
+
for index, value in enumerate(mask):
|
| 288 |
+
if value and start is None:
|
| 289 |
+
start = index
|
| 290 |
+
if start is not None and (not value or index == len(mask) - 1):
|
| 291 |
+
end = index if value and index == len(mask) - 1 else index - 1
|
| 292 |
+
if end > start:
|
| 293 |
+
longest = max(longest, float(x[end] - x[start]))
|
| 294 |
+
start = None
|
| 295 |
+
return longest
|
| 296 |
+
|
| 297 |
+
|
| 298 |
+
def _estimate_lane_width(models, nominal_lane_width_m, sample_step_m):
|
| 299 |
+
"""Estimate one marking-to-marking width from healthy near-range gaps."""
|
| 300 |
+
candidates = []
|
| 301 |
+
for left, right in zip(models, models[1:]):
|
| 302 |
+
lane_steps = right["lane_index"] - left["lane_index"]
|
| 303 |
+
if lane_steps <= 0:
|
| 304 |
+
continue
|
| 305 |
+
x = _shared_samples(left["fit"], right["fit"], sample_step_m)
|
| 306 |
+
if len(x) < 3:
|
| 307 |
+
continue
|
| 308 |
+
# Prefer the closest 40%: this is where IPM is best conditioned and
|
| 309 |
+
# where decoded markings are least likely to have merged at a horizon.
|
| 310 |
+
near_count = max(3, int(np.ceil(len(x) * 0.40)))
|
| 311 |
+
per_lane_gap = _normal_gap(
|
| 312 |
+
left["fit"], right["fit"], x[:near_count]
|
| 313 |
+
) / lane_steps
|
| 314 |
+
plausible = per_lane_gap[
|
| 315 |
+
np.isfinite(per_lane_gap)
|
| 316 |
+
& (per_lane_gap >= 2.4)
|
| 317 |
+
& (per_lane_gap <= 5.0)
|
| 318 |
+
]
|
| 319 |
+
if len(plausible):
|
| 320 |
+
candidates.append(float(np.median(plausible)))
|
| 321 |
+
if not candidates:
|
| 322 |
+
return float(nominal_lane_width_m), "nominal"
|
| 323 |
+
estimated = float(np.median(candidates))
|
| 324 |
+
return float(np.clip(estimated, 2.6, 4.5)), "near_range_median"
|
| 325 |
+
|
| 326 |
+
|
| 327 |
+
def analyze_lane_topology(models, lane_width_m, trigger_gap_ratio=0.55,
|
| 328 |
+
minimum_gap_m=1.0, minimum_bad_run_m=4.0,
|
| 329 |
+
sample_step_m=0.5):
|
| 330 |
+
"""Inspect ordered BEV cubics for collapsed gaps or lane crossings."""
|
| 331 |
+
pair_reports = []
|
| 332 |
+
trigger_pairs = []
|
| 333 |
+
for left, right in zip(models, models[1:]):
|
| 334 |
+
lane_steps = right["lane_index"] - left["lane_index"]
|
| 335 |
+
if lane_steps <= 0:
|
| 336 |
+
continue
|
| 337 |
+
x = _shared_samples(left["fit"], right["fit"], sample_step_m)
|
| 338 |
+
if len(x) < 3:
|
| 339 |
+
continue
|
| 340 |
+
gap = _normal_gap(left["fit"], right["fit"], x)
|
| 341 |
+
expected_gap = lane_width_m * lane_steps
|
| 342 |
+
trigger_gap = max(minimum_gap_m, trigger_gap_ratio * expected_gap)
|
| 343 |
+
finite = np.isfinite(gap)
|
| 344 |
+
bad = finite & (gap < trigger_gap)
|
| 345 |
+
crossing = bool(np.any(finite & (gap <= 0.0)))
|
| 346 |
+
longest_bad_run = _longest_true_run_m(bad, x)
|
| 347 |
+
triggered = crossing or longest_bad_run >= minimum_bad_run_m
|
| 348 |
+
report = {
|
| 349 |
+
"left_lane": f"P{left['lane_index']}",
|
| 350 |
+
"right_lane": f"P{right['lane_index']}",
|
| 351 |
+
"lane_steps": int(lane_steps),
|
| 352 |
+
"shared_x_domain_m": [float(x[0]), float(x[-1])],
|
| 353 |
+
"expected_gap_m": float(expected_gap),
|
| 354 |
+
"trigger_gap_m": float(trigger_gap),
|
| 355 |
+
"minimum_gap_m": float(np.min(gap[finite])) if finite.any() else None,
|
| 356 |
+
"longest_bad_run_m": float(longest_bad_run),
|
| 357 |
+
"crossing": crossing,
|
| 358 |
+
"triggered": bool(triggered),
|
| 359 |
+
}
|
| 360 |
+
pair_reports.append(report)
|
| 361 |
+
if triggered:
|
| 362 |
+
trigger_pairs.append(
|
| 363 |
+
f"P{left['lane_index']}-P{right['lane_index']}"
|
| 364 |
+
)
|
| 365 |
+
return pair_reports, trigger_pairs
|
| 366 |
+
|
| 367 |
+
|
| 368 |
+
def _parallel_offset_fit(reference_fit, target_fit, offset_m,
|
| 369 |
+
sample_step_m=0.5):
|
| 370 |
+
"""Build a metric normal-offset curve and refit it as a cubic Y(X)."""
|
| 371 |
+
# Never extrapolate a reference polynomial beyond the metric interval in
|
| 372 |
+
# which it was fitted. High-order extrapolation was observed to turn one
|
| 373 |
+
# short reference lane into a physically impossible far-range curve.
|
| 374 |
+
x_min = max(float(reference_fit["x_min"]), float(target_fit["x_min"]))
|
| 375 |
+
x_max = min(float(reference_fit["x_max"]), float(target_fit["x_max"]))
|
| 376 |
+
span = x_max - x_min
|
| 377 |
+
if span < 1.0:
|
| 378 |
+
return None
|
| 379 |
+
count = max(16, int(np.ceil(span / sample_step_m)) + 1)
|
| 380 |
+
x = np.linspace(x_min, x_max, count)
|
| 381 |
+
y = evaluate_cubic(reference_fit["coefficients"], x)
|
| 382 |
+
slope = evaluate_cubic_derivative(reference_fit["coefficients"], x)
|
| 383 |
+
norm = np.sqrt(1.0 + slope ** 2)
|
| 384 |
+
offset_points = np.column_stack((
|
| 385 |
+
x - offset_m * slope / norm,
|
| 386 |
+
y + offset_m / norm,
|
| 387 |
+
))
|
| 388 |
+
return fit_cubic_lane(
|
| 389 |
+
offset_points, np.ones(len(offset_points)), min_points=6,
|
| 390 |
+
huber_delta_m=0.05, iterations=3,
|
| 391 |
+
)
|
| 392 |
+
|
| 393 |
+
|
| 394 |
+
def clip_cubic_fit_to_funnel(fit, camera_x_m=1.0,
|
| 395 |
+
horizontal_fov_deg=30.0, margin_m=0.10,
|
| 396 |
+
sample_step_m=0.5, minimum_span_m=1.0):
|
| 397 |
+
"""Restrict a cubic's declared domain to the visible camera funnel.
|
| 398 |
+
|
| 399 |
+
The polynomial coefficients are left unchanged. Only the longest
|
| 400 |
+
contiguous, physically visible X interval is exported, which makes the
|
| 401 |
+
``x_domain_m`` contract explicit and prevents a renderer from drawing an
|
| 402 |
+
otherwise valid cubic after it leaves the camera footprint.
|
| 403 |
+
"""
|
| 404 |
+
copied = _copy_fit(fit)
|
| 405 |
+
x_min = float(fit["x_min"])
|
| 406 |
+
x_max = float(fit["x_max"])
|
| 407 |
+
report = {
|
| 408 |
+
"original_x_domain_m": [x_min, x_max],
|
| 409 |
+
"x_domain_m": None,
|
| 410 |
+
"clipped": False,
|
| 411 |
+
"valid": False,
|
| 412 |
+
}
|
| 413 |
+
if x_max - x_min < minimum_span_m:
|
| 414 |
+
return None, report
|
| 415 |
+
count = max(3, int(np.ceil((x_max - x_min) / sample_step_m)) + 1)
|
| 416 |
+
x = np.linspace(x_min, x_max, count)
|
| 417 |
+
y = evaluate_cubic(fit["coefficients"], x)
|
| 418 |
+
half_width = np.maximum(0.0, x - camera_x_m) * np.tan(
|
| 419 |
+
np.deg2rad(horizontal_fov_deg * 0.5)
|
| 420 |
+
)
|
| 421 |
+
valid = (
|
| 422 |
+
np.isfinite(y)
|
| 423 |
+
& (x >= camera_x_m)
|
| 424 |
+
& (np.abs(y) <= half_width + margin_m)
|
| 425 |
+
)
|
| 426 |
+
|
| 427 |
+
runs = []
|
| 428 |
+
start = None
|
| 429 |
+
for index, value in enumerate(valid):
|
| 430 |
+
if value and start is None:
|
| 431 |
+
start = index
|
| 432 |
+
if start is not None and (not value or index == len(valid) - 1):
|
| 433 |
+
end = index if value and index == len(valid) - 1 else index - 1
|
| 434 |
+
if x[end] - x[start] >= minimum_span_m:
|
| 435 |
+
runs.append((start, end))
|
| 436 |
+
start = None
|
| 437 |
+
if not runs:
|
| 438 |
+
return None, report
|
| 439 |
+
start, end = max(runs, key=lambda run: x[run[1]] - x[run[0]])
|
| 440 |
+
copied["x_min"] = float(x[start])
|
| 441 |
+
copied["x_max"] = float(x[end])
|
| 442 |
+
report.update({
|
| 443 |
+
"x_domain_m": [copied["x_min"], copied["x_max"]],
|
| 444 |
+
"clipped": bool(
|
| 445 |
+
copied["x_min"] > x_min + 1e-6
|
| 446 |
+
or copied["x_max"] < x_max - 1e-6
|
| 447 |
+
),
|
| 448 |
+
"valid": True,
|
| 449 |
+
})
|
| 450 |
+
return copied, report
|
| 451 |
+
|
| 452 |
+
|
| 453 |
+
def repair_parallel_lane_fits(models, nominal_lane_width_m=3.5,
|
| 454 |
+
trigger_gap_ratio=0.55,
|
| 455 |
+
minimum_gap_m=1.0,
|
| 456 |
+
minimum_bad_run_m=4.0,
|
| 457 |
+
sample_step_m=0.5,
|
| 458 |
+
maximum_reference_extrapolation_m=2.0,
|
| 459 |
+
force_repair=False):
|
| 460 |
+
"""Repair overlapping BEV lanes using one confidence-selected reference.
|
| 461 |
+
|
| 462 |
+
``models`` is a sequence of dictionaries with ``lane_index``, ``score``
|
| 463 |
+
and a valid cubic ``fit``. If any adjacent pair collapses for a sustained
|
| 464 |
+
distance (or crosses at all), the highest-confidence cubic is retained and
|
| 465 |
+
all other detected markings are rebuilt as metric normal offsets. Setting
|
| 466 |
+
``force_repair`` applies that same constraint even when topology analysis
|
| 467 |
+
finds no collapse. The function returns replacement fits plus a
|
| 468 |
+
JSON-serializable audit report.
|
| 469 |
+
|
| 470 |
+
This is deliberately a BEV-only topology prior. It does not alter decoded
|
| 471 |
+
image-space polylines and does not back-project synthetic curves.
|
| 472 |
+
"""
|
| 473 |
+
ordered = sorted(
|
| 474 |
+
(model for model in models if model.get("fit") is not None),
|
| 475 |
+
key=lambda model: model["lane_index"],
|
| 476 |
+
)
|
| 477 |
+
output_fits = {
|
| 478 |
+
model["lane_index"]: _copy_fit(model["fit"]) for model in ordered
|
| 479 |
+
}
|
| 480 |
+
report = {
|
| 481 |
+
"applied": False,
|
| 482 |
+
"forced": bool(force_repair),
|
| 483 |
+
"activation": "always_parallel" if force_repair else "triggered",
|
| 484 |
+
"scope": "bev_only",
|
| 485 |
+
"reference_lane": None,
|
| 486 |
+
"reference_score": None,
|
| 487 |
+
"reference_selection": None,
|
| 488 |
+
"reference_domain_m": None,
|
| 489 |
+
"required_domain_m": None,
|
| 490 |
+
"lane_width_m": float(nominal_lane_width_m),
|
| 491 |
+
"lane_width_source": "nominal",
|
| 492 |
+
"trigger_pairs": [],
|
| 493 |
+
"pairs_before": [],
|
| 494 |
+
"pairs_after": [],
|
| 495 |
+
"method": None,
|
| 496 |
+
"offsets_m": {},
|
| 497 |
+
"validation_passed": True,
|
| 498 |
+
}
|
| 499 |
+
if len(ordered) < 2:
|
| 500 |
+
return output_fits, report
|
| 501 |
+
|
| 502 |
+
lane_width_m, width_source = _estimate_lane_width(
|
| 503 |
+
ordered, nominal_lane_width_m, sample_step_m
|
| 504 |
+
)
|
| 505 |
+
pairs_before, trigger_pairs = analyze_lane_topology(
|
| 506 |
+
ordered, lane_width_m, trigger_gap_ratio, minimum_gap_m,
|
| 507 |
+
minimum_bad_run_m, sample_step_m,
|
| 508 |
+
)
|
| 509 |
+
report.update({
|
| 510 |
+
"lane_width_m": float(lane_width_m),
|
| 511 |
+
"lane_width_source": width_source,
|
| 512 |
+
"trigger_pairs": trigger_pairs,
|
| 513 |
+
"pairs_before": pairs_before,
|
| 514 |
+
})
|
| 515 |
+
if not trigger_pairs and not force_repair:
|
| 516 |
+
report["pairs_after"] = pairs_before
|
| 517 |
+
return output_fits, report
|
| 518 |
+
|
| 519 |
+
required_x_min = min(float(model["fit"]["x_min"]) for model in ordered)
|
| 520 |
+
required_x_max = max(float(model["fit"]["x_max"]) for model in ordered)
|
| 521 |
+
|
| 522 |
+
def uncovered_distance(model):
|
| 523 |
+
fit = model["fit"]
|
| 524 |
+
return max(
|
| 525 |
+
max(0.0, float(fit["x_min"]) - required_x_min),
|
| 526 |
+
max(0.0, required_x_max - float(fit["x_max"])),
|
| 527 |
+
)
|
| 528 |
+
|
| 529 |
+
coverage_candidates = [
|
| 530 |
+
model for model in ordered
|
| 531 |
+
if uncovered_distance(model) <= maximum_reference_extrapolation_m
|
| 532 |
+
]
|
| 533 |
+
|
| 534 |
+
def shared_domain_support(model):
|
| 535 |
+
fit = model["fit"]
|
| 536 |
+
return sum(
|
| 537 |
+
max(
|
| 538 |
+
0.0,
|
| 539 |
+
min(float(fit["x_max"]), float(other["fit"]["x_max"]))
|
| 540 |
+
- max(float(fit["x_min"]), float(other["fit"]["x_min"])),
|
| 541 |
+
)
|
| 542 |
+
for other in ordered
|
| 543 |
+
if other is not model
|
| 544 |
+
)
|
| 545 |
+
|
| 546 |
+
if force_repair:
|
| 547 |
+
# Always-parallel mode must be able to derive every target from the
|
| 548 |
+
# reference on a real shared X interval. Union coverage alone can pick
|
| 549 |
+
# a long outer lane whose domain does not overlap a short inner lane.
|
| 550 |
+
reference = max(
|
| 551 |
+
ordered,
|
| 552 |
+
key=lambda model: (
|
| 553 |
+
shared_domain_support(model),
|
| 554 |
+
float(model["score"]),
|
| 555 |
+
float(model["fit"]["x_max"])
|
| 556 |
+
- float(model["fit"]["x_min"]),
|
| 557 |
+
-model["lane_index"],
|
| 558 |
+
),
|
| 559 |
+
)
|
| 560 |
+
reference_selection = "maximum_shared_domain_then_confidence"
|
| 561 |
+
elif coverage_candidates:
|
| 562 |
+
# Confidence remains the exact primary criterion, but only among lanes
|
| 563 |
+
# whose fitted domain can safely support the requested repair.
|
| 564 |
+
reference = max(
|
| 565 |
+
coverage_candidates,
|
| 566 |
+
key=lambda model: (float(model["score"]), -model["lane_index"]),
|
| 567 |
+
)
|
| 568 |
+
reference_selection = "highest_confidence_with_domain_coverage"
|
| 569 |
+
else:
|
| 570 |
+
# No lane spans the union. Prefer the least uncovered candidate and
|
| 571 |
+
# truncate every generated target to the actual shared domain below.
|
| 572 |
+
reference = min(
|
| 573 |
+
ordered,
|
| 574 |
+
key=lambda model: (
|
| 575 |
+
uncovered_distance(model),
|
| 576 |
+
-float(model["score"]),
|
| 577 |
+
model["lane_index"],
|
| 578 |
+
),
|
| 579 |
+
)
|
| 580 |
+
reference_selection = "best_domain_coverage_then_confidence"
|
| 581 |
+
reference_id = reference["lane_index"]
|
| 582 |
+
offsets = {
|
| 583 |
+
model["lane_index"]: float(
|
| 584 |
+
(reference_id - model["lane_index"]) * lane_width_m
|
| 585 |
+
)
|
| 586 |
+
for model in ordered
|
| 587 |
+
}
|
| 588 |
+
repaired = {}
|
| 589 |
+
normal_fit_ok = True
|
| 590 |
+
for model in ordered:
|
| 591 |
+
lane_id = model["lane_index"]
|
| 592 |
+
if lane_id == reference_id:
|
| 593 |
+
repaired[lane_id] = _copy_fit(reference["fit"])
|
| 594 |
+
continue
|
| 595 |
+
fit = _parallel_offset_fit(
|
| 596 |
+
reference["fit"], model["fit"], offsets[lane_id], sample_step_m
|
| 597 |
+
)
|
| 598 |
+
if fit is None:
|
| 599 |
+
normal_fit_ok = False
|
| 600 |
+
break
|
| 601 |
+
repaired[lane_id] = fit
|
| 602 |
+
|
| 603 |
+
method = "normal_offset_cubic"
|
| 604 |
+
passthrough_lane_ids = set()
|
| 605 |
+
if normal_fit_ok:
|
| 606 |
+
repaired_models = [
|
| 607 |
+
{**model, "fit": repaired[model["lane_index"]]}
|
| 608 |
+
for model in ordered
|
| 609 |
+
]
|
| 610 |
+
pairs_after, remaining_triggers = analyze_lane_topology(
|
| 611 |
+
repaired_models, lane_width_m, trigger_gap_ratio,
|
| 612 |
+
minimum_gap_m, minimum_bad_run_m, sample_step_m,
|
| 613 |
+
)
|
| 614 |
+
else:
|
| 615 |
+
remaining_triggers = ["normal_offset_fit_failed"]
|
| 616 |
+
pairs_after = []
|
| 617 |
+
|
| 618 |
+
# Independent cubic approximation of exact normal offsets can very rarely
|
| 619 |
+
# reintroduce a far-range crossing. A shared-shape vertical translation is
|
| 620 |
+
# the deterministic safety net: identical c1..c3 means crossings are
|
| 621 |
+
# mathematically impossible over every shared X domain.
|
| 622 |
+
if remaining_triggers:
|
| 623 |
+
method = "shared_shape_vertical_offset_fallback"
|
| 624 |
+
repaired = {}
|
| 625 |
+
for model in ordered:
|
| 626 |
+
lane_id = model["lane_index"]
|
| 627 |
+
fit = _copy_fit(model["fit"])
|
| 628 |
+
fit["x_min"] = max(
|
| 629 |
+
float(reference["fit"]["x_min"]), float(fit["x_min"])
|
| 630 |
+
)
|
| 631 |
+
fit["x_max"] = min(
|
| 632 |
+
float(reference["fit"]["x_max"]), float(fit["x_max"])
|
| 633 |
+
)
|
| 634 |
+
if fit["x_max"] - fit["x_min"] < 1.0:
|
| 635 |
+
# Never publish an inverted synthetic domain. With no shared
|
| 636 |
+
# support, retain the measured target fit; it cannot overlap
|
| 637 |
+
# the reference inside BEV because their X domains are disjoint.
|
| 638 |
+
repaired[lane_id] = _copy_fit(model["fit"])
|
| 639 |
+
passthrough_lane_ids.add(lane_id)
|
| 640 |
+
continue
|
| 641 |
+
fit["coefficients"] = np.asarray(
|
| 642 |
+
reference["fit"]["coefficients"], dtype=np.float64
|
| 643 |
+
).copy()
|
| 644 |
+
fit["coefficients"][0] += offsets[lane_id]
|
| 645 |
+
fit["rmse"] = 0.0
|
| 646 |
+
fit["repair_fit_rmse"] = 0.0
|
| 647 |
+
repaired[lane_id] = fit
|
| 648 |
+
repaired_models = [
|
| 649 |
+
{**model, "fit": repaired[model["lane_index"]]}
|
| 650 |
+
for model in ordered
|
| 651 |
+
]
|
| 652 |
+
pairs_after, remaining_triggers = analyze_lane_topology(
|
| 653 |
+
repaired_models, lane_width_m, trigger_gap_ratio,
|
| 654 |
+
minimum_gap_m, minimum_bad_run_m, sample_step_m,
|
| 655 |
+
)
|
| 656 |
+
|
| 657 |
+
validation_passed = not remaining_triggers and all(
|
| 658 |
+
pair["minimum_gap_m"] is None or pair["minimum_gap_m"] > 0.0
|
| 659 |
+
for pair in pairs_after
|
| 660 |
+
)
|
| 661 |
+
if not validation_passed:
|
| 662 |
+
# Never publish an unvalidated synthetic topology.
|
| 663 |
+
return output_fits, {
|
| 664 |
+
**report,
|
| 665 |
+
"reference_lane": f"P{reference_id}",
|
| 666 |
+
"reference_score": float(reference["score"]),
|
| 667 |
+
"reference_selection": reference_selection,
|
| 668 |
+
"reference_domain_m": [
|
| 669 |
+
float(reference["fit"]["x_min"]),
|
| 670 |
+
float(reference["fit"]["x_max"]),
|
| 671 |
+
],
|
| 672 |
+
"required_domain_m": [required_x_min, required_x_max],
|
| 673 |
+
"method": method,
|
| 674 |
+
"offsets_m": {f"P{k}": v for k, v in offsets.items()},
|
| 675 |
+
"pairs_after": pairs_after,
|
| 676 |
+
"validation_passed": False,
|
| 677 |
+
"failure": "repaired topology did not pass ordering validation",
|
| 678 |
+
}
|
| 679 |
+
|
| 680 |
+
for lane_id, fit in repaired.items():
|
| 681 |
+
if lane_id in passthrough_lane_ids:
|
| 682 |
+
continue
|
| 683 |
+
fit["parallel_repair"] = True
|
| 684 |
+
fit["parallel_reference_lane"] = reference_id
|
| 685 |
+
fit["parallel_offset_m"] = offsets[lane_id]
|
| 686 |
+
fit["parallel_repair_method"] = method
|
| 687 |
+
report.update({
|
| 688 |
+
"applied": True,
|
| 689 |
+
"reference_lane": f"P{reference_id}",
|
| 690 |
+
"reference_score": float(reference["score"]),
|
| 691 |
+
"reference_selection": reference_selection,
|
| 692 |
+
"reference_domain_m": [
|
| 693 |
+
float(reference["fit"]["x_min"]),
|
| 694 |
+
float(reference["fit"]["x_max"]),
|
| 695 |
+
],
|
| 696 |
+
"required_domain_m": [required_x_min, required_x_max],
|
| 697 |
+
"method": method,
|
| 698 |
+
"offsets_m": {f"P{k}": v for k, v in offsets.items()},
|
| 699 |
+
"passthrough_lanes": [
|
| 700 |
+
f"P{lane_id}" for lane_id in sorted(passthrough_lane_ids)
|
| 701 |
+
],
|
| 702 |
+
"pairs_after": pairs_after,
|
| 703 |
+
"validation_passed": True,
|
| 704 |
+
})
|
| 705 |
+
return repaired, report
|
| 706 |
+
|
| 707 |
+
|
| 708 |
+
def complete_four_parallel_lane_fits(models, nominal_lane_width_m=3.5,
|
| 709 |
+
sample_step_m=0.5):
|
| 710 |
+
"""Return a complete parallel P0/P1/P2/P3 BEV lane set.
|
| 711 |
+
|
| 712 |
+
Unlike :func:`repair_parallel_lane_fits`, this display/output mode does not
|
| 713 |
+
require all four markings to have been detected. It chooses the measured
|
| 714 |
+
fit with the greatest shared longitudinal support, retains it as the
|
| 715 |
+
reference, and constructs every lane index in ``[0, 3]`` as a metric normal
|
| 716 |
+
offset. Missing indices are explicitly reported as synthetic.
|
| 717 |
+
|
| 718 |
+
Camera-funnel clipping is intentionally not performed here. The caller may
|
| 719 |
+
display these completed geometric priors outside the current camera field
|
| 720 |
+
of view while keeping image-space detections untouched.
|
| 721 |
+
"""
|
| 722 |
+
ordered = sorted(
|
| 723 |
+
(model for model in models if model.get("fit") is not None),
|
| 724 |
+
key=lambda model: model["lane_index"],
|
| 725 |
+
)
|
| 726 |
+
source_ids = {int(model["lane_index"]) for model in ordered}
|
| 727 |
+
target_ids = tuple(range(4))
|
| 728 |
+
synthetic_ids = [lane_id for lane_id in target_ids if lane_id not in source_ids]
|
| 729 |
+
report = {
|
| 730 |
+
"applied": False,
|
| 731 |
+
"forced": True,
|
| 732 |
+
"activation": "complete_four_parallel",
|
| 733 |
+
"scope": "bev_only",
|
| 734 |
+
"reference_lane": None,
|
| 735 |
+
"reference_score": None,
|
| 736 |
+
"reference_selection": "maximum_shared_domain_then_confidence",
|
| 737 |
+
"reference_domain_m": None,
|
| 738 |
+
"required_domain_m": None,
|
| 739 |
+
"lane_width_m": float(nominal_lane_width_m),
|
| 740 |
+
"lane_width_source": "nominal",
|
| 741 |
+
"source_lane_ids": [f"P{i}" for i in sorted(source_ids)],
|
| 742 |
+
"completed_lane_ids": [f"P{i}" for i in target_ids],
|
| 743 |
+
"synthetic_lane_ids": [f"P{i}" for i in synthetic_ids],
|
| 744 |
+
"trigger_pairs": [],
|
| 745 |
+
"pairs_before": [],
|
| 746 |
+
"pairs_after": [],
|
| 747 |
+
"method": None,
|
| 748 |
+
"offsets_m": {},
|
| 749 |
+
"passthrough_lanes": [],
|
| 750 |
+
"validation_passed": False,
|
| 751 |
+
}
|
| 752 |
+
if not ordered:
|
| 753 |
+
report["failure"] = "no valid measured BEV fit is available"
|
| 754 |
+
return {}, report
|
| 755 |
+
|
| 756 |
+
if len(ordered) >= 2:
|
| 757 |
+
lane_width_m, width_source = _estimate_lane_width(
|
| 758 |
+
ordered, nominal_lane_width_m, sample_step_m
|
| 759 |
+
)
|
| 760 |
+
pairs_before, trigger_pairs = analyze_lane_topology(
|
| 761 |
+
ordered, lane_width_m, sample_step_m=sample_step_m
|
| 762 |
+
)
|
| 763 |
+
else:
|
| 764 |
+
lane_width_m = float(nominal_lane_width_m)
|
| 765 |
+
width_source = "nominal_single_reference"
|
| 766 |
+
pairs_before, trigger_pairs = [], []
|
| 767 |
+
|
| 768 |
+
def shared_domain_support(model):
|
| 769 |
+
fit = model["fit"]
|
| 770 |
+
return sum(
|
| 771 |
+
max(
|
| 772 |
+
0.0,
|
| 773 |
+
min(float(fit["x_max"]), float(other["fit"]["x_max"]))
|
| 774 |
+
- max(float(fit["x_min"]), float(other["fit"]["x_min"])),
|
| 775 |
+
)
|
| 776 |
+
for other in ordered
|
| 777 |
+
if other is not model
|
| 778 |
+
)
|
| 779 |
+
|
| 780 |
+
reference = max(
|
| 781 |
+
ordered,
|
| 782 |
+
key=lambda model: (
|
| 783 |
+
shared_domain_support(model),
|
| 784 |
+
float(model["score"]),
|
| 785 |
+
float(model["fit"]["x_max"])
|
| 786 |
+
- float(model["fit"]["x_min"]),
|
| 787 |
+
-int(model["lane_index"]),
|
| 788 |
+
),
|
| 789 |
+
)
|
| 790 |
+
reference_id = int(reference["lane_index"])
|
| 791 |
+
reference_fit = reference["fit"]
|
| 792 |
+
offsets = {
|
| 793 |
+
lane_id: float((reference_id - lane_id) * lane_width_m)
|
| 794 |
+
for lane_id in target_ids
|
| 795 |
+
}
|
| 796 |
+
|
| 797 |
+
completed = {}
|
| 798 |
+
normal_fit_ok = True
|
| 799 |
+
for lane_id in target_ids:
|
| 800 |
+
if lane_id == reference_id:
|
| 801 |
+
completed[lane_id] = _copy_fit(reference_fit)
|
| 802 |
+
continue
|
| 803 |
+
fit = _parallel_offset_fit(
|
| 804 |
+
reference_fit, reference_fit, offsets[lane_id], sample_step_m
|
| 805 |
+
)
|
| 806 |
+
if fit is None:
|
| 807 |
+
normal_fit_ok = False
|
| 808 |
+
break
|
| 809 |
+
completed[lane_id] = fit
|
| 810 |
+
|
| 811 |
+
method = "normal_offset_cubic"
|
| 812 |
+
if normal_fit_ok:
|
| 813 |
+
completed_models = [
|
| 814 |
+
{"lane_index": lane_id, "score": 1.0, "fit": completed[lane_id]}
|
| 815 |
+
for lane_id in target_ids
|
| 816 |
+
]
|
| 817 |
+
pairs_after, remaining_triggers = analyze_lane_topology(
|
| 818 |
+
completed_models, lane_width_m, sample_step_m=sample_step_m
|
| 819 |
+
)
|
| 820 |
+
else:
|
| 821 |
+
pairs_after = []
|
| 822 |
+
remaining_triggers = ["normal_offset_fit_failed"]
|
| 823 |
+
|
| 824 |
+
if remaining_triggers:
|
| 825 |
+
# Identical c1..c3 with constant c0 spacing guarantees four ordered,
|
| 826 |
+
# non-crossing lanes over the complete reference domain.
|
| 827 |
+
method = "shared_shape_vertical_offset_fallback"
|
| 828 |
+
completed = {}
|
| 829 |
+
for lane_id in target_ids:
|
| 830 |
+
fit = _copy_fit(reference_fit)
|
| 831 |
+
fit["coefficients"][0] += offsets[lane_id]
|
| 832 |
+
fit["rmse"] = 0.0
|
| 833 |
+
fit["repair_fit_rmse"] = 0.0
|
| 834 |
+
completed[lane_id] = fit
|
| 835 |
+
completed_models = [
|
| 836 |
+
{"lane_index": lane_id, "score": 1.0, "fit": completed[lane_id]}
|
| 837 |
+
for lane_id in target_ids
|
| 838 |
+
]
|
| 839 |
+
pairs_after, remaining_triggers = analyze_lane_topology(
|
| 840 |
+
completed_models, lane_width_m, sample_step_m=sample_step_m
|
| 841 |
+
)
|
| 842 |
+
|
| 843 |
+
validation_passed = not remaining_triggers and all(
|
| 844 |
+
pair["minimum_gap_m"] is None or pair["minimum_gap_m"] > 0.0
|
| 845 |
+
for pair in pairs_after
|
| 846 |
+
)
|
| 847 |
+
required_x_min = min(float(model["fit"]["x_min"]) for model in ordered)
|
| 848 |
+
required_x_max = max(float(model["fit"]["x_max"]) for model in ordered)
|
| 849 |
+
report.update({
|
| 850 |
+
"applied": bool(validation_passed),
|
| 851 |
+
"reference_lane": f"P{reference_id}",
|
| 852 |
+
"reference_score": float(reference["score"]),
|
| 853 |
+
"reference_domain_m": [
|
| 854 |
+
float(reference_fit["x_min"]), float(reference_fit["x_max"])
|
| 855 |
+
],
|
| 856 |
+
"required_domain_m": [required_x_min, required_x_max],
|
| 857 |
+
"lane_width_m": float(lane_width_m),
|
| 858 |
+
"lane_width_source": width_source,
|
| 859 |
+
"trigger_pairs": trigger_pairs,
|
| 860 |
+
"pairs_before": pairs_before,
|
| 861 |
+
"pairs_after": pairs_after,
|
| 862 |
+
"method": method,
|
| 863 |
+
"offsets_m": {f"P{k}": v for k, v in offsets.items()},
|
| 864 |
+
"validation_passed": bool(validation_passed),
|
| 865 |
+
})
|
| 866 |
+
if not validation_passed:
|
| 867 |
+
report["failure"] = "completed topology did not pass ordering validation"
|
| 868 |
+
return {}, report
|
| 869 |
+
|
| 870 |
+
for lane_id, fit in completed.items():
|
| 871 |
+
fit["parallel_repair"] = True
|
| 872 |
+
fit["parallel_reference_lane"] = reference_id
|
| 873 |
+
fit["parallel_offset_m"] = offsets[lane_id]
|
| 874 |
+
fit["parallel_repair_method"] = method
|
| 875 |
+
fit["synthetic_lane"] = lane_id in synthetic_ids
|
| 876 |
+
return completed, report
|
| 877 |
+
|
| 878 |
+
|
| 879 |
+
def _self_test():
|
| 880 |
+
calibration = CameraCalibration()
|
| 881 |
+
bev_range = BevRange()
|
| 882 |
+
ground = np.array(
|
| 883 |
+
((5.0, 0.0), (20.0, 3.5), (50.0, -3.5), (150.0, 2.0),
|
| 884 |
+
(299.0, 0.0)),
|
| 885 |
+
dtype=np.float64,
|
| 886 |
+
)
|
| 887 |
+
pixels = ground_to_pixels(ground, calibration)
|
| 888 |
+
reconstructed, valid = pixels_to_ground(pixels, calibration, bev_range)
|
| 889 |
+
assert valid.all()
|
| 890 |
+
assert np.allclose(reconstructed, ground, atol=1e-5), (
|
| 891 |
+
reconstructed, ground
|
| 892 |
+
)
|
| 893 |
+
|
| 894 |
+
x = np.linspace(5.0, 180.0, 80)
|
| 895 |
+
truth = np.array((1.8, 1.5e-2, -1.2e-4, 3.0e-7))
|
| 896 |
+
y = evaluate_cubic(truth, x)
|
| 897 |
+
y[20] += 4.0 # one large synthetic outlier
|
| 898 |
+
fitted = fit_cubic_lane(np.column_stack((x, y)))
|
| 899 |
+
assert fitted is not None
|
| 900 |
+
prediction = evaluate_cubic(fitted["coefficients"], x)
|
| 901 |
+
clean = np.ones(len(x), dtype=bool)
|
| 902 |
+
clean[20] = False
|
| 903 |
+
assert np.sqrt(np.mean((prediction[clean] - evaluate_cubic(truth, x[clean])) ** 2)) < 0.02
|
| 904 |
+
|
| 905 |
+
# Four truly parallel curves must remain untouched.
|
| 906 |
+
base = np.array((1.0, 0.03, 1.0e-4, -1.0e-7))
|
| 907 |
+
models = []
|
| 908 |
+
for lane_id in range(4):
|
| 909 |
+
coefficients = base.copy()
|
| 910 |
+
coefficients[0] += (1 - lane_id) * 3.5
|
| 911 |
+
models.append({
|
| 912 |
+
"lane_index": lane_id,
|
| 913 |
+
"score": 0.9 - lane_id * 0.01,
|
| 914 |
+
"fit": {
|
| 915 |
+
"coefficients": coefficients,
|
| 916 |
+
"x_min": 5.0, "x_max": 150.0, "rmse": 0.02,
|
| 917 |
+
"point_count": 80, "inlier_count": 80,
|
| 918 |
+
"inlier_ratio": 1.0,
|
| 919 |
+
},
|
| 920 |
+
})
|
| 921 |
+
unchanged, clean_report = repair_parallel_lane_fits(models)
|
| 922 |
+
assert not clean_report["applied"]
|
| 923 |
+
assert all(
|
| 924 |
+
np.allclose(unchanged[i]["coefficients"], models[i]["fit"]["coefficients"])
|
| 925 |
+
for i in range(4)
|
| 926 |
+
)
|
| 927 |
+
|
| 928 |
+
# Always-parallel mode must rebuild even a healthy frame while preserving
|
| 929 |
+
# ordered, non-crossing lane geometry.
|
| 930 |
+
forced, forced_report = repair_parallel_lane_fits(
|
| 931 |
+
models, force_repair=True
|
| 932 |
+
)
|
| 933 |
+
assert forced_report["applied"] and forced_report["forced"]
|
| 934 |
+
forced_models = [
|
| 935 |
+
{**model, "fit": forced[model["lane_index"]]} for model in models
|
| 936 |
+
]
|
| 937 |
+
forced_pairs, forced_triggers = analyze_lane_topology(
|
| 938 |
+
forced_models, forced_report["lane_width_m"]
|
| 939 |
+
)
|
| 940 |
+
assert not forced_triggers
|
| 941 |
+
assert all(pair["minimum_gap_m"] > 0.0 for pair in forced_pairs)
|
| 942 |
+
|
| 943 |
+
# Completing from only the two ego boundaries must synthesize both outer
|
| 944 |
+
# markings and produce a valid four-lane topology.
|
| 945 |
+
completed, complete_report = complete_four_parallel_lane_fits(models[1:3])
|
| 946 |
+
assert complete_report["applied"]
|
| 947 |
+
assert set(completed) == {0, 1, 2, 3}
|
| 948 |
+
assert complete_report["synthetic_lane_ids"] == ["P0", "P3"]
|
| 949 |
+
completed_models = [
|
| 950 |
+
{"lane_index": lane_id, "fit": completed[lane_id]}
|
| 951 |
+
for lane_id in range(4)
|
| 952 |
+
]
|
| 953 |
+
completed_pairs, completed_triggers = analyze_lane_topology(
|
| 954 |
+
completed_models, complete_report["lane_width_m"]
|
| 955 |
+
)
|
| 956 |
+
assert not completed_triggers
|
| 957 |
+
assert all(pair["minimum_gap_m"] > 0.0 for pair in completed_pairs)
|
| 958 |
+
|
| 959 |
+
# Corrupt P3 so it merges into and crosses P2 in the far range.
|
| 960 |
+
corrupted = [{**model, "fit": _copy_fit(model["fit"])} for model in models]
|
| 961 |
+
corrupted[2]["score"] = 0.98 # P2 must become the exact reference.
|
| 962 |
+
corrupted[3]["fit"]["coefficients"] = np.array(
|
| 963 |
+
(-3.2, 0.03, 3.0e-4, 1.5e-6)
|
| 964 |
+
)
|
| 965 |
+
repaired, repair_report = repair_parallel_lane_fits(corrupted)
|
| 966 |
+
assert repair_report["applied"]
|
| 967 |
+
assert repair_report["reference_lane"] == "P2"
|
| 968 |
+
assert repair_report["validation_passed"]
|
| 969 |
+
repaired_models = [
|
| 970 |
+
{**model, "fit": repaired[model["lane_index"]]}
|
| 971 |
+
for model in corrupted
|
| 972 |
+
]
|
| 973 |
+
repaired_pairs, repaired_triggers = analyze_lane_topology(
|
| 974 |
+
repaired_models, repair_report["lane_width_m"]
|
| 975 |
+
)
|
| 976 |
+
assert not repaired_triggers
|
| 977 |
+
assert all(pair["minimum_gap_m"] > 0.0 for pair in repaired_pairs)
|
| 978 |
+
|
| 979 |
+
# A short, high-confidence lane must not beat a slightly lower-confidence
|
| 980 |
+
# reference that actually covers the required far-range repair domain.
|
| 981 |
+
coverage_case = [
|
| 982 |
+
{**model, "fit": _copy_fit(model["fit"])} for model in corrupted
|
| 983 |
+
]
|
| 984 |
+
coverage_case[0]["score"] = 0.999
|
| 985 |
+
coverage_case[0]["fit"]["x_max"] = 50.0
|
| 986 |
+
coverage_case[1]["fit"]["x_max"] = 100.0
|
| 987 |
+
coverage_case[2]["score"] = 0.80
|
| 988 |
+
coverage_case[3]["fit"]["x_max"] = 100.0
|
| 989 |
+
_, coverage_report = repair_parallel_lane_fits(coverage_case)
|
| 990 |
+
assert coverage_report["applied"]
|
| 991 |
+
assert coverage_report["reference_lane"] == "P2"
|
| 992 |
+
assert coverage_report["reference_selection"] == (
|
| 993 |
+
"highest_confidence_with_domain_coverage"
|
| 994 |
+
)
|
| 995 |
+
|
| 996 |
+
runaway_fit = {
|
| 997 |
+
"coefficients": np.array((-5.0, 0.4, -0.013, 1.15e-4)),
|
| 998 |
+
"x_min": 10.0, "x_max": 130.0, "rmse": 0.1,
|
| 999 |
+
"point_count": 50, "inlier_count": 50, "inlier_ratio": 1.0,
|
| 1000 |
+
}
|
| 1001 |
+
clipped, funnel_report = clip_cubic_fit_to_funnel(runaway_fit)
|
| 1002 |
+
assert clipped is not None and funnel_report["clipped"]
|
| 1003 |
+
assert clipped["x_max"] < runaway_fit["x_max"]
|
| 1004 |
+
check_x = np.linspace(clipped["x_min"], clipped["x_max"], 300)
|
| 1005 |
+
check_y = evaluate_cubic(clipped["coefficients"], check_x)
|
| 1006 |
+
funnel_half_width = (check_x - 1.0) * np.tan(np.deg2rad(15.0))
|
| 1007 |
+
assert np.all(np.abs(check_y) <= funnel_half_width + 0.11)
|
| 1008 |
+
print("OK -- pixel/ground projection round trip")
|
| 1009 |
+
print("OK -- robust metric cubic lane fit")
|
| 1010 |
+
print("OK -- BEV parallel-lane topology repair")
|
| 1011 |
+
print("OK -- reference coverage and camera-funnel guard")
|
| 1012 |
+
|
| 1013 |
+
|
| 1014 |
+
if __name__ == "__main__":
|
| 1015 |
+
_self_test()
|
experiments/bev_parallel_modes/test_video_bev_parallel_legacy.py
ADDED
|
@@ -0,0 +1,931 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Run RCLane ONNX and export ego-centric cubic lane models in BEV.
|
| 2 |
+
|
| 3 |
+
Each JSONL frame stores one metric polynomial per visible marking:
|
| 4 |
+
|
| 5 |
+
Y(X) = c0 + c1*X + c2*X^2 + c3*X^3
|
| 6 |
+
|
| 7 |
+
X points forward from ego and Y points left. Coefficients are valid only inside
|
| 8 |
+
the exported ``x_domain_m``; the implementation never extrapolates a detected
|
| 9 |
+
lane to the full 300 m visualization range.
|
| 10 |
+
"""
|
| 11 |
+
|
| 12 |
+
import argparse
|
| 13 |
+
import json
|
| 14 |
+
import os
|
| 15 |
+
import sys
|
| 16 |
+
import time
|
| 17 |
+
from collections import Counter
|
| 18 |
+
from pathlib import Path
|
| 19 |
+
|
| 20 |
+
# Keep this archived runner self-contained while importing the current project
|
| 21 |
+
# modules. Position 0 remains this directory so ``from bev`` resolves to the
|
| 22 |
+
# matching legacy BEV implementation stored beside this script.
|
| 23 |
+
PROJECT_ROOT = Path(__file__).resolve().parents[2]
|
| 24 |
+
if str(PROJECT_ROOT) not in sys.path:
|
| 25 |
+
sys.path.insert(1, str(PROJECT_ROOT))
|
| 26 |
+
|
| 27 |
+
import cv2
|
| 28 |
+
import numpy as np
|
| 29 |
+
|
| 30 |
+
from bev import (
|
| 31 |
+
BevRange,
|
| 32 |
+
CameraCalibration,
|
| 33 |
+
clip_cubic_fit_to_funnel,
|
| 34 |
+
complete_four_parallel_lane_fits,
|
| 35 |
+
evaluate_cubic,
|
| 36 |
+
fit_cubic_lane,
|
| 37 |
+
model_lane_to_ground,
|
| 38 |
+
repair_parallel_lane_fits,
|
| 39 |
+
)
|
| 40 |
+
from dataset import normalize_image_numpy
|
| 41 |
+
from decode import configure_decode_threads, decode, warmup_decode_backend
|
| 42 |
+
from test_video_onnx import (
|
| 43 |
+
MODEL_HEIGHT,
|
| 44 |
+
MODEL_WIDTH,
|
| 45 |
+
OUTPUT_NAMES,
|
| 46 |
+
create_session,
|
| 47 |
+
draw_predictions,
|
| 48 |
+
softmax_foreground,
|
| 49 |
+
timing_summary,
|
| 50 |
+
)
|
| 51 |
+
|
| 52 |
+
|
| 53 |
+
LANE_COLORS = {
|
| 54 |
+
0: (255, 255, 0),
|
| 55 |
+
1: (0, 255, 0),
|
| 56 |
+
2: (0, 165, 255),
|
| 57 |
+
3: (255, 255, 0),
|
| 58 |
+
}
|
| 59 |
+
|
| 60 |
+
|
| 61 |
+
def parse_args():
|
| 62 |
+
parser = argparse.ArgumentParser(description=__doc__)
|
| 63 |
+
parser.add_argument("--model", required=True)
|
| 64 |
+
parser.add_argument("--video", required=True)
|
| 65 |
+
parser.add_argument("--output", default="runs/video_bev_e19.mp4")
|
| 66 |
+
parser.add_argument("--polynomials", default=None,
|
| 67 |
+
help="per-frame JSONL; defaults next to output")
|
| 68 |
+
parser.add_argument("--summary", default=None)
|
| 69 |
+
parser.add_argument(
|
| 70 |
+
"--provider", choices=("tensorrt", "cuda", "cpu"),
|
| 71 |
+
default="tensorrt",
|
| 72 |
+
)
|
| 73 |
+
parser.add_argument("--trt-cache-dir", default="exports/trt_cache")
|
| 74 |
+
parser.add_argument("--allow-tf32", action="store_true")
|
| 75 |
+
parser.add_argument(
|
| 76 |
+
"--start-frame", type=int, default=0,
|
| 77 |
+
help="zero-based source frame at which processing starts",
|
| 78 |
+
)
|
| 79 |
+
parser.add_argument("--max-frames", type=int, default=None)
|
| 80 |
+
parser.add_argument("--x-min", type=float, default=0.0)
|
| 81 |
+
parser.add_argument("--x-max", type=float, default=300.0)
|
| 82 |
+
parser.add_argument("--y-min", type=float, default=-85.0)
|
| 83 |
+
parser.add_argument("--y-max", type=float, default=85.0)
|
| 84 |
+
parser.add_argument(
|
| 85 |
+
"--max-cubic-rmse", type=float, default=0.5,
|
| 86 |
+
help="reject a cubic visualization above this metric RMSE",
|
| 87 |
+
)
|
| 88 |
+
parser.add_argument("--decode-seg-threshold", type=float, default=0.5)
|
| 89 |
+
parser.add_argument("--decode-seed-threshold", type=float, default=None)
|
| 90 |
+
parser.add_argument("--decode-seed-min-dist", type=int, default=2)
|
| 91 |
+
parser.add_argument("--decode-score-thresh", type=float, default=0.10)
|
| 92 |
+
parser.add_argument("--decode-nms-iou", type=float, default=0.5)
|
| 93 |
+
parser.add_argument("--decode-max-seeds", type=int, default=1024)
|
| 94 |
+
parser.add_argument(
|
| 95 |
+
"--decode-crawl-backend", choices=("auto", "numba", "numpy"),
|
| 96 |
+
default="auto",
|
| 97 |
+
)
|
| 98 |
+
parser.add_argument("--decode-cpu-threads", type=int, default=8)
|
| 99 |
+
parser.add_argument("--decode-nms-max-lanes", type=int, default=128)
|
| 100 |
+
parser.add_argument("--decode-nms-scale", type=float, default=0.25)
|
| 101 |
+
parser.add_argument("--max-ego-lanes", type=int, default=4)
|
| 102 |
+
parser.add_argument(
|
| 103 |
+
"--disable-parallel-repair", action="store_true",
|
| 104 |
+
help="export raw BEV cubics without overlap/crossing repair",
|
| 105 |
+
)
|
| 106 |
+
parser.add_argument(
|
| 107 |
+
"--always-parallel-repair", action="store_true",
|
| 108 |
+
help="force every valid detected BEV lane to be a normal offset",
|
| 109 |
+
)
|
| 110 |
+
parser.add_argument(
|
| 111 |
+
"--complete-four-parallel-lanes", action="store_true",
|
| 112 |
+
help="always output parallel P0-P3, synthesizing missing BEV lanes",
|
| 113 |
+
)
|
| 114 |
+
parser.add_argument(
|
| 115 |
+
"--nominal-lane-width", type=float, default=3.5,
|
| 116 |
+
help="fallback marking-to-marking distance in metres",
|
| 117 |
+
)
|
| 118 |
+
parser.add_argument(
|
| 119 |
+
"--repair-trigger-gap-ratio", type=float, default=0.55,
|
| 120 |
+
help="flag a pair below this fraction of expected lane width",
|
| 121 |
+
)
|
| 122 |
+
parser.add_argument(
|
| 123 |
+
"--repair-minimum-gap", type=float, default=1.0,
|
| 124 |
+
help="absolute lower bound used by the collapse detector (metres)",
|
| 125 |
+
)
|
| 126 |
+
parser.add_argument(
|
| 127 |
+
"--repair-minimum-run", type=float, default=4.0,
|
| 128 |
+
help="minimum collapsed longitudinal run that activates repair (metres)",
|
| 129 |
+
)
|
| 130 |
+
parser.add_argument(
|
| 131 |
+
"--repair-max-reference-extrapolation", type=float, default=2.0,
|
| 132 |
+
help="coverage tolerance when selecting a repair reference (metres)",
|
| 133 |
+
)
|
| 134 |
+
parser.add_argument(
|
| 135 |
+
"--funnel-margin", type=float, default=0.10,
|
| 136 |
+
help="metric tolerance around the calibrated camera funnel",
|
| 137 |
+
)
|
| 138 |
+
return parser.parse_args()
|
| 139 |
+
|
| 140 |
+
|
| 141 |
+
def lane_to_record(lane, calibration, bev_range, max_cubic_rmse=0.5):
|
| 142 |
+
points, scores = model_lane_to_ground(
|
| 143 |
+
lane, MODEL_WIDTH, MODEL_HEIGHT, calibration, bev_range
|
| 144 |
+
)
|
| 145 |
+
fit = fit_cubic_lane(points, scores, iterations=3)
|
| 146 |
+
lane_id = int(lane.lane_id) if lane.lane_id is not None else None
|
| 147 |
+
valid_fit = fit is not None and fit["rmse"] <= max_cubic_rmse
|
| 148 |
+
record = {
|
| 149 |
+
"lane_id": f"P{lane_id}" if lane_id is not None else None,
|
| 150 |
+
"lane_index": lane_id,
|
| 151 |
+
"role": lane.lane_role,
|
| 152 |
+
"score": float(lane.score),
|
| 153 |
+
"projected_point_count": int(len(points)),
|
| 154 |
+
"valid_fit": valid_fit,
|
| 155 |
+
"fit_status": (
|
| 156 |
+
"ok" if valid_fit else
|
| 157 |
+
"rmse_above_limit" if fit is not None else
|
| 158 |
+
"insufficient_geometry"
|
| 159 |
+
),
|
| 160 |
+
}
|
| 161 |
+
if fit is not None:
|
| 162 |
+
record.update({
|
| 163 |
+
"polynomial": "Y(X)=c0+c1*X+c2*X^2+c3*X^3",
|
| 164 |
+
"coefficients_c0_to_c3": [
|
| 165 |
+
float(value) for value in fit["coefficients"]
|
| 166 |
+
],
|
| 167 |
+
"x_domain_m": [fit["x_min"], fit["x_max"]],
|
| 168 |
+
"rmse_m": fit["rmse"],
|
| 169 |
+
"fit_point_count": fit["point_count"],
|
| 170 |
+
"inlier_count": fit["inlier_count"],
|
| 171 |
+
"inlier_ratio": fit["inlier_ratio"],
|
| 172 |
+
})
|
| 173 |
+
return {
|
| 174 |
+
"record": record,
|
| 175 |
+
"points": points,
|
| 176 |
+
"fit": fit if valid_fit else None,
|
| 177 |
+
}
|
| 178 |
+
|
| 179 |
+
|
| 180 |
+
def _apply_complete_four_results(lane_results, repaired_fits, report):
|
| 181 |
+
"""Replace measured BEV fits and append explicitly synthetic lane records."""
|
| 182 |
+
for result in lane_results:
|
| 183 |
+
record = result["record"]
|
| 184 |
+
lane_id = record["lane_index"]
|
| 185 |
+
source_fit = result["fit"]
|
| 186 |
+
if source_fit is None or lane_id not in repaired_fits:
|
| 187 |
+
continue
|
| 188 |
+
repaired_fit = repaired_fits[lane_id]
|
| 189 |
+
record.update({
|
| 190 |
+
"fit_status": "parallel_repaired_bev",
|
| 191 |
+
"source_coefficients_c0_to_c3": [
|
| 192 |
+
float(value) for value in source_fit["coefficients"]
|
| 193 |
+
],
|
| 194 |
+
"source_x_domain_m": [
|
| 195 |
+
float(source_fit["x_min"]), float(source_fit["x_max"])
|
| 196 |
+
],
|
| 197 |
+
"source_rmse_m": float(source_fit["rmse"]),
|
| 198 |
+
"coefficients_c0_to_c3": [
|
| 199 |
+
float(value) for value in repaired_fit["coefficients"]
|
| 200 |
+
],
|
| 201 |
+
"x_domain_m": [
|
| 202 |
+
float(repaired_fit["x_min"]), float(repaired_fit["x_max"])
|
| 203 |
+
],
|
| 204 |
+
"rmse_m": float(repaired_fit["rmse"]),
|
| 205 |
+
"parallel_repair_applied": True,
|
| 206 |
+
"parallel_repair_forced": True,
|
| 207 |
+
"parallel_reference_lane": report["reference_lane"],
|
| 208 |
+
"parallel_offset_m": float(report["offsets_m"][f"P{lane_id}"]),
|
| 209 |
+
"parallel_repair_method": report["method"],
|
| 210 |
+
"synthetic_bev_lane": False,
|
| 211 |
+
})
|
| 212 |
+
result["fit"] = repaired_fit
|
| 213 |
+
|
| 214 |
+
roles = {0: "left_2", 1: "ego_left", 2: "ego_right", 3: "right_2"}
|
| 215 |
+
synthetic_ids = {
|
| 216 |
+
int(label[1:]) for label in report.get("synthetic_lane_ids", ())
|
| 217 |
+
}
|
| 218 |
+
for lane_id in sorted(synthetic_ids):
|
| 219 |
+
fit = repaired_fits[lane_id]
|
| 220 |
+
synthetic_record = {
|
| 221 |
+
"lane_id": f"P{lane_id}",
|
| 222 |
+
"lane_index": lane_id,
|
| 223 |
+
"role": roles[lane_id],
|
| 224 |
+
"score": None,
|
| 225 |
+
"projected_point_count": 0,
|
| 226 |
+
"valid_fit": True,
|
| 227 |
+
"fit_status": "parallel_synthesized_bev",
|
| 228 |
+
"polynomial": "Y(X)=c0+c1*X+c2*X^2+c3*X^3",
|
| 229 |
+
"coefficients_c0_to_c3": [
|
| 230 |
+
float(value) for value in fit["coefficients"]
|
| 231 |
+
],
|
| 232 |
+
"x_domain_m": [float(fit["x_min"]), float(fit["x_max"])],
|
| 233 |
+
"rmse_m": float(fit["rmse"]),
|
| 234 |
+
"fit_point_count": int(fit.get("point_count", 0)),
|
| 235 |
+
"inlier_count": int(fit.get("inlier_count", 0)),
|
| 236 |
+
"inlier_ratio": float(fit.get("inlier_ratio", 1.0)),
|
| 237 |
+
"parallel_repair_applied": True,
|
| 238 |
+
"parallel_repair_forced": True,
|
| 239 |
+
"parallel_reference_lane": report["reference_lane"],
|
| 240 |
+
"parallel_offset_m": float(report["offsets_m"][f"P{lane_id}"]),
|
| 241 |
+
"parallel_repair_method": report["method"],
|
| 242 |
+
"synthetic_bev_lane": True,
|
| 243 |
+
}
|
| 244 |
+
existing = next(
|
| 245 |
+
(
|
| 246 |
+
result for result in lane_results
|
| 247 |
+
if result["record"]["lane_index"] == lane_id
|
| 248 |
+
),
|
| 249 |
+
None,
|
| 250 |
+
)
|
| 251 |
+
if existing is None:
|
| 252 |
+
lane_results.append({
|
| 253 |
+
"record": synthetic_record,
|
| 254 |
+
"points": np.empty((0, 2), dtype=np.float64),
|
| 255 |
+
"fit": fit,
|
| 256 |
+
})
|
| 257 |
+
else:
|
| 258 |
+
existing["record"] = synthetic_record
|
| 259 |
+
existing["fit"] = fit
|
| 260 |
+
lane_results.sort(key=lambda result: result["record"]["lane_index"])
|
| 261 |
+
|
| 262 |
+
|
| 263 |
+
def apply_parallel_repair(lane_results, args):
|
| 264 |
+
"""Replace only BEV fits; decoded image-space lanes remain untouched."""
|
| 265 |
+
models = [
|
| 266 |
+
{
|
| 267 |
+
"lane_index": result["record"]["lane_index"],
|
| 268 |
+
"score": result["record"]["score"],
|
| 269 |
+
"fit": result["fit"],
|
| 270 |
+
}
|
| 271 |
+
for result in lane_results
|
| 272 |
+
if result["fit"] is not None
|
| 273 |
+
and result["record"]["lane_index"] is not None
|
| 274 |
+
]
|
| 275 |
+
complete_four = getattr(args, "complete_four_parallel_lanes", False)
|
| 276 |
+
if args.disable_parallel_repair:
|
| 277 |
+
report = {
|
| 278 |
+
"applied": False,
|
| 279 |
+
"scope": "bev_only",
|
| 280 |
+
"disabled": True,
|
| 281 |
+
"validation_passed": True,
|
| 282 |
+
"trigger_pairs": [],
|
| 283 |
+
}
|
| 284 |
+
elif complete_four:
|
| 285 |
+
repaired_fits, report = complete_four_parallel_lane_fits(
|
| 286 |
+
models, nominal_lane_width_m=args.nominal_lane_width
|
| 287 |
+
)
|
| 288 |
+
if report["applied"]:
|
| 289 |
+
_apply_complete_four_results(lane_results, repaired_fits, report)
|
| 290 |
+
else:
|
| 291 |
+
repaired_fits, report = repair_parallel_lane_fits(
|
| 292 |
+
models,
|
| 293 |
+
nominal_lane_width_m=args.nominal_lane_width,
|
| 294 |
+
trigger_gap_ratio=args.repair_trigger_gap_ratio,
|
| 295 |
+
minimum_gap_m=args.repair_minimum_gap,
|
| 296 |
+
minimum_bad_run_m=args.repair_minimum_run,
|
| 297 |
+
maximum_reference_extrapolation_m=(
|
| 298 |
+
args.repair_max_reference_extrapolation
|
| 299 |
+
),
|
| 300 |
+
force_repair=args.always_parallel_repair,
|
| 301 |
+
)
|
| 302 |
+
if report["applied"]:
|
| 303 |
+
passthrough_lanes = set(report.get("passthrough_lanes", ()))
|
| 304 |
+
for result in lane_results:
|
| 305 |
+
record = result["record"]
|
| 306 |
+
lane_id = record["lane_index"]
|
| 307 |
+
source_fit = result["fit"]
|
| 308 |
+
if source_fit is None or lane_id not in repaired_fits:
|
| 309 |
+
continue
|
| 310 |
+
if record["lane_id"] in passthrough_lanes:
|
| 311 |
+
record.update({
|
| 312 |
+
"parallel_repair_applied": False,
|
| 313 |
+
"parallel_repair_forced": True,
|
| 314 |
+
"parallel_repair_passthrough": "disjoint_x_domain",
|
| 315 |
+
})
|
| 316 |
+
continue
|
| 317 |
+
repaired_fit = repaired_fits[lane_id]
|
| 318 |
+
record.update({
|
| 319 |
+
"fit_status": "parallel_repaired_bev",
|
| 320 |
+
"source_coefficients_c0_to_c3": [
|
| 321 |
+
float(value) for value in source_fit["coefficients"]
|
| 322 |
+
],
|
| 323 |
+
"source_x_domain_m": [
|
| 324 |
+
float(source_fit["x_min"]),
|
| 325 |
+
float(source_fit["x_max"]),
|
| 326 |
+
],
|
| 327 |
+
"source_rmse_m": float(source_fit["rmse"]),
|
| 328 |
+
"coefficients_c0_to_c3": [
|
| 329 |
+
float(value) for value in repaired_fit["coefficients"]
|
| 330 |
+
],
|
| 331 |
+
"x_domain_m": [
|
| 332 |
+
float(repaired_fit["x_min"]),
|
| 333 |
+
float(repaired_fit["x_max"]),
|
| 334 |
+
],
|
| 335 |
+
"rmse_m": float(repaired_fit["rmse"]),
|
| 336 |
+
"parallel_repair_applied": True,
|
| 337 |
+
"parallel_reference_lane": report["reference_lane"],
|
| 338 |
+
"parallel_offset_m": float(
|
| 339 |
+
report["offsets_m"][f"P{lane_id}"]
|
| 340 |
+
),
|
| 341 |
+
"parallel_repair_method": report["method"],
|
| 342 |
+
"parallel_repair_forced": bool(report.get("forced", False)),
|
| 343 |
+
"synthetic_bev_lane": False,
|
| 344 |
+
})
|
| 345 |
+
result["fit"] = repaired_fit
|
| 346 |
+
|
| 347 |
+
funnel_report = {
|
| 348 |
+
"camera_x_m": float(CameraCalibration().camera_to_vehicle_matrix[0, 3]),
|
| 349 |
+
"horizontal_fov_deg": float(CameraCalibration().horizontal_fov_deg),
|
| 350 |
+
"margin_m": float(args.funnel_margin),
|
| 351 |
+
"clipped_lanes": [],
|
| 352 |
+
"rejected_lanes": [],
|
| 353 |
+
"bypassed": bool(complete_four),
|
| 354 |
+
}
|
| 355 |
+
if complete_four:
|
| 356 |
+
report["funnel_guard"] = funnel_report
|
| 357 |
+
return report
|
| 358 |
+
for result in lane_results:
|
| 359 |
+
fit = result["fit"]
|
| 360 |
+
record = result["record"]
|
| 361 |
+
if fit is None:
|
| 362 |
+
continue
|
| 363 |
+
clipped_fit, clip_report = clip_cubic_fit_to_funnel(
|
| 364 |
+
fit,
|
| 365 |
+
camera_x_m=funnel_report["camera_x_m"],
|
| 366 |
+
horizontal_fov_deg=funnel_report["horizontal_fov_deg"],
|
| 367 |
+
margin_m=args.funnel_margin,
|
| 368 |
+
)
|
| 369 |
+
lane_label = record["lane_id"]
|
| 370 |
+
if clipped_fit is None:
|
| 371 |
+
result["fit"] = None
|
| 372 |
+
record.update({
|
| 373 |
+
"valid_fit": False,
|
| 374 |
+
"fit_status": "outside_camera_funnel",
|
| 375 |
+
"funnel_guard": clip_report,
|
| 376 |
+
})
|
| 377 |
+
funnel_report["rejected_lanes"].append(lane_label)
|
| 378 |
+
continue
|
| 379 |
+
result["fit"] = clipped_fit
|
| 380 |
+
record["x_domain_m"] = [
|
| 381 |
+
float(clipped_fit["x_min"]), float(clipped_fit["x_max"])
|
| 382 |
+
]
|
| 383 |
+
record["funnel_guard"] = clip_report
|
| 384 |
+
if clip_report["clipped"]:
|
| 385 |
+
funnel_report["clipped_lanes"].append(lane_label)
|
| 386 |
+
report["funnel_guard"] = funnel_report
|
| 387 |
+
return report
|
| 388 |
+
|
| 389 |
+
|
| 390 |
+
def _bev_pixel(x_forward, y_left, bev_range, bounds):
|
| 391 |
+
left, top, right, bottom = bounds
|
| 392 |
+
px = left + (bev_range.y_max - y_left) / (
|
| 393 |
+
bev_range.y_max - bev_range.y_min
|
| 394 |
+
) * (right - left)
|
| 395 |
+
py = top + (bev_range.x_max - x_forward) / (
|
| 396 |
+
bev_range.x_max - bev_range.x_min
|
| 397 |
+
) * (bottom - top)
|
| 398 |
+
return np.column_stack((px, py))
|
| 399 |
+
|
| 400 |
+
|
| 401 |
+
def _draw_dashed_curve(canvas, points, color, thickness=4,
|
| 402 |
+
dash_points=7, gap_points=4):
|
| 403 |
+
stride = dash_points + gap_points
|
| 404 |
+
for start in range(0, len(points) - 1, stride):
|
| 405 |
+
segment = points[start:min(start + dash_points + 1, len(points))]
|
| 406 |
+
if len(segment) >= 2:
|
| 407 |
+
cv2.polylines(
|
| 408 |
+
canvas, [segment], False, (0, 0, 0), thickness + 4,
|
| 409 |
+
cv2.LINE_AA,
|
| 410 |
+
)
|
| 411 |
+
cv2.polylines(
|
| 412 |
+
canvas, [segment], False, color, thickness, cv2.LINE_AA,
|
| 413 |
+
)
|
| 414 |
+
|
| 415 |
+
|
| 416 |
+
def draw_bev(lane_results, bev_range, calibration, topology_report=None,
|
| 417 |
+
width=640, height=1080):
|
| 418 |
+
canvas = np.full((height, width, 3), (35, 19, 10), dtype=np.uint8)
|
| 419 |
+
bounds = (72, 62, width - 24, height - 70)
|
| 420 |
+
left, top, right, bottom = bounds
|
| 421 |
+
|
| 422 |
+
# Requested sensor-style ROI funnel: ego at the apex and the configured
|
| 423 |
+
# metric ROI at its far edge. It is a visualization of the output ROI, not
|
| 424 |
+
# a replacement for the calibrated camera ray/ground intersection.
|
| 425 |
+
camera_x = float(calibration.camera_to_vehicle_matrix[0, 3])
|
| 426 |
+
half_fov_radians = np.deg2rad(
|
| 427 |
+
calibration.horizontal_fov_deg * 0.5
|
| 428 |
+
)
|
| 429 |
+
fov_half_width = min(
|
| 430 |
+
bev_range.y_max,
|
| 431 |
+
max(0.0, bev_range.x_max - camera_x) * np.tan(half_fov_radians),
|
| 432 |
+
)
|
| 433 |
+
roi_metric = np.array(
|
| 434 |
+
((camera_x, 0.0),
|
| 435 |
+
(bev_range.x_max, fov_half_width),
|
| 436 |
+
(bev_range.x_max, -fov_half_width)),
|
| 437 |
+
dtype=np.float64,
|
| 438 |
+
)
|
| 439 |
+
roi_pixels = np.round(_bev_pixel(
|
| 440 |
+
roi_metric[:, 0], roi_metric[:, 1], bev_range, bounds
|
| 441 |
+
)).astype(np.int32)
|
| 442 |
+
overlay = canvas.copy()
|
| 443 |
+
cv2.fillPoly(overlay, [roi_pixels], (105, 74, 105), cv2.LINE_AA)
|
| 444 |
+
cv2.addWeighted(overlay, 0.72, canvas, 0.28, 0.0, canvas)
|
| 445 |
+
cv2.polylines(canvas, [roi_pixels], True, (150, 120, 165), 2, cv2.LINE_AA)
|
| 446 |
+
|
| 447 |
+
for x in np.arange(
|
| 448 |
+
np.ceil(bev_range.x_min / 50.0) * 50.0,
|
| 449 |
+
bev_range.x_max + 0.1,
|
| 450 |
+
50.0,
|
| 451 |
+
):
|
| 452 |
+
half_width = min(
|
| 453 |
+
fov_half_width,
|
| 454 |
+
max(0.0, x - camera_x) * np.tan(half_fov_radians),
|
| 455 |
+
)
|
| 456 |
+
y_at_left_edge = half_width
|
| 457 |
+
y_at_right_edge = -half_width
|
| 458 |
+
range_line = _bev_pixel(
|
| 459 |
+
np.array([x, x]),
|
| 460 |
+
np.array([y_at_left_edge, y_at_right_edge]),
|
| 461 |
+
bev_range,
|
| 462 |
+
bounds,
|
| 463 |
+
)
|
| 464 |
+
range_line = np.round(range_line).astype(np.int32)
|
| 465 |
+
row = int(round(range_line[0, 1]))
|
| 466 |
+
cv2.line(
|
| 467 |
+
canvas, tuple(range_line[0]), tuple(range_line[1]),
|
| 468 |
+
(118, 91, 120), 1, cv2.LINE_AA,
|
| 469 |
+
)
|
| 470 |
+
cv2.putText(
|
| 471 |
+
canvas, f"{x:.0f}m", (8, min(bottom, max(top + 12, row + 5))),
|
| 472 |
+
cv2.FONT_HERSHEY_SIMPLEX, 0.42, (205, 190, 210), 1,
|
| 473 |
+
cv2.LINE_AA,
|
| 474 |
+
)
|
| 475 |
+
|
| 476 |
+
zero = _bev_pixel(
|
| 477 |
+
np.array([bev_range.x_min]), np.array([0.0]), bev_range, bounds
|
| 478 |
+
)[0].astype(int)
|
| 479 |
+
far_center = _bev_pixel(
|
| 480 |
+
np.array([bev_range.x_max]), np.array([0.0]), bev_range, bounds
|
| 481 |
+
)[0].astype(int)
|
| 482 |
+
for y0 in range(zero[1] - 12, far_center[1], -28):
|
| 483 |
+
y1 = max(far_center[1], y0 - 15)
|
| 484 |
+
cv2.line(canvas, (zero[0], y0), (far_center[0], y1),
|
| 485 |
+
(225, 215, 230), 2, cv2.LINE_AA)
|
| 486 |
+
ego = np.array(
|
| 487 |
+
((zero[0] - 10, bottom - 22), (zero[0] + 10, bottom - 22),
|
| 488 |
+
(zero[0] + 14, bottom), (zero[0] - 14, bottom)),
|
| 489 |
+
dtype=np.int32,
|
| 490 |
+
)
|
| 491 |
+
cv2.fillPoly(canvas, [ego], (238, 238, 238), cv2.LINE_AA)
|
| 492 |
+
cv2.polylines(canvas, [ego], True, (20, 20, 20), 2, cv2.LINE_AA)
|
| 493 |
+
|
| 494 |
+
formula_rows = []
|
| 495 |
+
for result in lane_results:
|
| 496 |
+
record = result["record"]
|
| 497 |
+
lane_id = record["lane_index"]
|
| 498 |
+
color = LANE_COLORS.get(lane_id, (220, 220, 220))
|
| 499 |
+
points = result["points"]
|
| 500 |
+
# On repaired frames the raw IPM samples may themselves overlap. Keep
|
| 501 |
+
# them in JSON diagnostics, but omit them from the final BEV panel so
|
| 502 |
+
# the displayed/exported geometry is unambiguously the repaired cubic.
|
| 503 |
+
if len(points) and not (topology_report or {}).get("applied", False):
|
| 504 |
+
pixels = _bev_pixel(
|
| 505 |
+
points[:, 0], points[:, 1], bev_range, bounds
|
| 506 |
+
)
|
| 507 |
+
pixels = np.round(pixels).astype(np.int32)
|
| 508 |
+
for pixel in pixels[::max(1, len(pixels) // 40)]:
|
| 509 |
+
cv2.circle(canvas, tuple(pixel), 2, color, -1, cv2.LINE_AA)
|
| 510 |
+
|
| 511 |
+
fit = result["fit"]
|
| 512 |
+
if fit is None:
|
| 513 |
+
rmse = record.get("rmse_m")
|
| 514 |
+
reason = (
|
| 515 |
+
f"rejected rmse={rmse:.2f}m" if rmse is not None
|
| 516 |
+
else "insufficient geometry"
|
| 517 |
+
)
|
| 518 |
+
formula_rows.append((color, f"{record['lane_id']}: {reason}"))
|
| 519 |
+
continue
|
| 520 |
+
x = np.linspace(fit["x_min"], fit["x_max"], 160)
|
| 521 |
+
y = evaluate_cubic(fit["coefficients"], x)
|
| 522 |
+
valid = (
|
| 523 |
+
np.isfinite(y)
|
| 524 |
+
& (y >= bev_range.y_min)
|
| 525 |
+
& (y <= bev_range.y_max)
|
| 526 |
+
)
|
| 527 |
+
curve = _bev_pixel(x[valid], y[valid], bev_range, bounds)
|
| 528 |
+
if len(curve) >= 2:
|
| 529 |
+
curve = np.round(curve).astype(np.int32)
|
| 530 |
+
_draw_dashed_curve(canvas, curve, color)
|
| 531 |
+
cv2.putText(
|
| 532 |
+
canvas, record["lane_id"], tuple(curve[-1]),
|
| 533 |
+
cv2.FONT_HERSHEY_SIMPLEX, 0.48, color, 2, cv2.LINE_AA,
|
| 534 |
+
)
|
| 535 |
+
formula_rows.append((
|
| 536 |
+
color,
|
| 537 |
+
f"{record['lane_id']}: X={fit['x_min']:.1f}..{fit['x_max']:.1f}m "
|
| 538 |
+
f"rmse={fit['rmse']:.2f}m",
|
| 539 |
+
))
|
| 540 |
+
|
| 541 |
+
cv2.putText(
|
| 542 |
+
canvas,
|
| 543 |
+
f"CAMERA FOV {calibration.horizontal_fov_deg:.0f}deg: "
|
| 544 |
+
"X forward / Y left",
|
| 545 |
+
(20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.54,
|
| 546 |
+
(255, 255, 255), 2, cv2.LINE_AA,
|
| 547 |
+
)
|
| 548 |
+
if (topology_report or {}).get("applied", False):
|
| 549 |
+
if topology_report.get("activation") == "complete_four_parallel":
|
| 550 |
+
repair_mode = "BEV COMPLETE-4"
|
| 551 |
+
elif topology_report.get("forced", False):
|
| 552 |
+
repair_mode = "BEV ALWAYS-PARALLEL"
|
| 553 |
+
else:
|
| 554 |
+
repair_mode = "BEV REPAIRED"
|
| 555 |
+
cv2.putText(
|
| 556 |
+
canvas,
|
| 557 |
+
"{} | ref={} | width={:.2f}m".format(
|
| 558 |
+
repair_mode,
|
| 559 |
+
topology_report["reference_lane"],
|
| 560 |
+
topology_report["lane_width_m"],
|
| 561 |
+
),
|
| 562 |
+
(20, 52), cv2.FONT_HERSHEY_SIMPLEX, 0.44,
|
| 563 |
+
(80, 220, 255), 1, cv2.LINE_AA,
|
| 564 |
+
)
|
| 565 |
+
cv2.putText(
|
| 566 |
+
canvas, "Y left (+) Y right (-)",
|
| 567 |
+
(left, height - 16), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
|
| 568 |
+
(210, 210, 210), 1, cv2.LINE_AA,
|
| 569 |
+
)
|
| 570 |
+
for row, (color, text) in enumerate(formula_rows[:4]):
|
| 571 |
+
cv2.putText(
|
| 572 |
+
canvas, text, (82, 82 + row * 18),
|
| 573 |
+
cv2.FONT_HERSHEY_SIMPLEX, 0.33, color, 1, cv2.LINE_AA,
|
| 574 |
+
)
|
| 575 |
+
return canvas
|
| 576 |
+
|
| 577 |
+
|
| 578 |
+
def make_composite(frame, bev_canvas, frame_index, fit_count, pipeline_ms,
|
| 579 |
+
topology_report=None):
|
| 580 |
+
output = np.zeros((1080, 1920, 3), dtype=np.uint8)
|
| 581 |
+
bev_width = 640
|
| 582 |
+
output[:, :bev_width] = cv2.resize(
|
| 583 |
+
bev_canvas, (bev_width, 1080), interpolation=cv2.INTER_AREA
|
| 584 |
+
)
|
| 585 |
+
camera_width = 1920 - bev_width
|
| 586 |
+
scaled_height = int(round(frame.shape[0] * camera_width / frame.shape[1]))
|
| 587 |
+
camera_view = cv2.resize(frame, (camera_width, scaled_height),
|
| 588 |
+
interpolation=cv2.INTER_AREA)
|
| 589 |
+
top = (1080 - scaled_height) // 2
|
| 590 |
+
output[top:top + scaled_height, bev_width:] = camera_view
|
| 591 |
+
cv2.line(output, (bev_width, 0), (bev_width, 1079), (255, 255, 255), 2)
|
| 592 |
+
repair_label = ""
|
| 593 |
+
if (topology_report or {}).get("applied", False):
|
| 594 |
+
if topology_report.get("activation") == "complete_four_parallel":
|
| 595 |
+
mode = "complete-4"
|
| 596 |
+
elif topology_report.get("forced", False):
|
| 597 |
+
mode = "always-parallel"
|
| 598 |
+
else:
|
| 599 |
+
mode = "repair"
|
| 600 |
+
repair_label = (
|
| 601 |
+
f" | BEV {mode} ref={topology_report['reference_lane']}"
|
| 602 |
+
)
|
| 603 |
+
algorithm_fps = 1000.0 / max(float(pipeline_ms), 1e-9)
|
| 604 |
+
runtime_label = (
|
| 605 |
+
f"frame={frame_index} | cubic lanes={fit_count} | "
|
| 606 |
+
f"pipeline={pipeline_ms:.1f}ms | algorithm={algorithm_fps:.1f} FPS"
|
| 607 |
+
f"{repair_label}"
|
| 608 |
+
)
|
| 609 |
+
cv2.putText(
|
| 610 |
+
output, runtime_label,
|
| 611 |
+
(bev_width + 24, 42), cv2.FONT_HERSHEY_SIMPLEX, 0.78,
|
| 612 |
+
(0, 0, 0), 5, cv2.LINE_AA,
|
| 613 |
+
)
|
| 614 |
+
cv2.putText(
|
| 615 |
+
output, runtime_label,
|
| 616 |
+
(bev_width + 24, 42), cv2.FONT_HERSHEY_SIMPLEX, 0.78,
|
| 617 |
+
(255, 255, 255), 2, cv2.LINE_AA,
|
| 618 |
+
)
|
| 619 |
+
cv2.putText(
|
| 620 |
+
output, "CAMERA: raw decode (not back-projected)",
|
| 621 |
+
(bev_width + 24, 72), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
|
| 622 |
+
(0, 0, 0), 4, cv2.LINE_AA,
|
| 623 |
+
)
|
| 624 |
+
cv2.putText(
|
| 625 |
+
output, "CAMERA: raw decode (not back-projected)",
|
| 626 |
+
(bev_width + 24, 72), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
|
| 627 |
+
(255, 255, 255), 1, cv2.LINE_AA,
|
| 628 |
+
)
|
| 629 |
+
return output
|
| 630 |
+
|
| 631 |
+
|
| 632 |
+
def main():
|
| 633 |
+
args = parse_args()
|
| 634 |
+
if args.max_frames is not None and args.max_frames <= 0:
|
| 635 |
+
raise ValueError("--max-frames must be positive")
|
| 636 |
+
if args.start_frame < 0:
|
| 637 |
+
raise ValueError("--start-frame must be non-negative")
|
| 638 |
+
if args.max_cubic_rmse <= 0:
|
| 639 |
+
raise ValueError("--max-cubic-rmse must be positive")
|
| 640 |
+
if not args.x_min < args.x_max or not args.y_min < args.y_max:
|
| 641 |
+
raise ValueError("invalid BEV range")
|
| 642 |
+
if args.nominal_lane_width <= 0:
|
| 643 |
+
raise ValueError("--nominal-lane-width must be positive")
|
| 644 |
+
if not 0 < args.repair_trigger_gap_ratio < 1:
|
| 645 |
+
raise ValueError("--repair-trigger-gap-ratio must be in (0, 1)")
|
| 646 |
+
if args.repair_minimum_gap <= 0 or args.repair_minimum_run < 0:
|
| 647 |
+
raise ValueError("repair gap/run arguments must be non-negative")
|
| 648 |
+
if args.repair_max_reference_extrapolation < 0:
|
| 649 |
+
raise ValueError(
|
| 650 |
+
"--repair-max-reference-extrapolation must be non-negative"
|
| 651 |
+
)
|
| 652 |
+
if args.funnel_margin < 0:
|
| 653 |
+
raise ValueError("--funnel-margin must be non-negative")
|
| 654 |
+
if args.disable_parallel_repair and (
|
| 655 |
+
args.always_parallel_repair or args.complete_four_parallel_lanes
|
| 656 |
+
):
|
| 657 |
+
raise ValueError(
|
| 658 |
+
"--disable-parallel-repair conflicts with parallel completion"
|
| 659 |
+
)
|
| 660 |
+
|
| 661 |
+
model_path = Path(args.model).expanduser().resolve()
|
| 662 |
+
video_path = Path(args.video).expanduser().resolve()
|
| 663 |
+
output_path = Path(args.output).expanduser().resolve()
|
| 664 |
+
polynomial_path = (
|
| 665 |
+
Path(args.polynomials).expanduser().resolve()
|
| 666 |
+
if args.polynomials else output_path.with_suffix(".lanes.jsonl")
|
| 667 |
+
)
|
| 668 |
+
summary_path = (
|
| 669 |
+
Path(args.summary).expanduser().resolve()
|
| 670 |
+
if args.summary else output_path.with_suffix(".json")
|
| 671 |
+
)
|
| 672 |
+
for path in (model_path, video_path):
|
| 673 |
+
if not path.is_file():
|
| 674 |
+
raise FileNotFoundError(path)
|
| 675 |
+
for path in (output_path, polynomial_path, summary_path):
|
| 676 |
+
path.parent.mkdir(parents=True, exist_ok=True)
|
| 677 |
+
|
| 678 |
+
calibration = CameraCalibration()
|
| 679 |
+
bev_range = BevRange(args.x_min, args.x_max, args.y_min, args.y_max)
|
| 680 |
+
cv2.setNumThreads(1)
|
| 681 |
+
configure_decode_threads(args.decode_cpu_threads)
|
| 682 |
+
session = create_session(
|
| 683 |
+
model_path, args.provider, args.allow_tf32, args.trt_cache_dir
|
| 684 |
+
)
|
| 685 |
+
warmup_decode_backend(args.decode_crawl_backend)
|
| 686 |
+
warmup = np.zeros((1, 3, MODEL_HEIGHT, MODEL_WIDTH), dtype=np.float32)
|
| 687 |
+
for _ in range(5):
|
| 688 |
+
session.run(list(OUTPUT_NAMES), {"images": warmup})
|
| 689 |
+
|
| 690 |
+
capture = cv2.VideoCapture(str(video_path))
|
| 691 |
+
if not capture.isOpened():
|
| 692 |
+
raise RuntimeError(f"cannot open video: {video_path}")
|
| 693 |
+
width = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH))
|
| 694 |
+
height = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
|
| 695 |
+
fps = float(capture.get(cv2.CAP_PROP_FPS) or 20.0)
|
| 696 |
+
source_frames = int(capture.get(cv2.CAP_PROP_FRAME_COUNT))
|
| 697 |
+
if (width, height) != (calibration.width, calibration.height):
|
| 698 |
+
capture.release()
|
| 699 |
+
raise ValueError(
|
| 700 |
+
f"calibration is {calibration.width}x{calibration.height}, "
|
| 701 |
+
f"video is {width}x{height}; crop/resize must be calibrated"
|
| 702 |
+
)
|
| 703 |
+
if args.start_frame >= source_frames:
|
| 704 |
+
capture.release()
|
| 705 |
+
raise ValueError(
|
| 706 |
+
f"--start-frame {args.start_frame} is outside {source_frames} frames"
|
| 707 |
+
)
|
| 708 |
+
if args.start_frame:
|
| 709 |
+
capture.set(cv2.CAP_PROP_POS_FRAMES, args.start_frame)
|
| 710 |
+
available_frames = source_frames - args.start_frame
|
| 711 |
+
target_frames = min(available_frames, args.max_frames or available_frames)
|
| 712 |
+
|
| 713 |
+
temporary_output = output_path.with_name(
|
| 714 |
+
output_path.stem + ".tmp" + output_path.suffix
|
| 715 |
+
)
|
| 716 |
+
temporary_polynomials = polynomial_path.with_suffix(
|
| 717 |
+
polynomial_path.suffix + ".tmp"
|
| 718 |
+
)
|
| 719 |
+
writer = cv2.VideoWriter(
|
| 720 |
+
str(temporary_output), cv2.VideoWriter_fourcc(*"mp4v"), fps,
|
| 721 |
+
(1920, 1080),
|
| 722 |
+
)
|
| 723 |
+
if not writer.isOpened():
|
| 724 |
+
capture.release()
|
| 725 |
+
raise RuntimeError(f"cannot open writer: {temporary_output}")
|
| 726 |
+
|
| 727 |
+
timings = {
|
| 728 |
+
"preprocess": [], "inference": [], "decode": [], "bev_fit": [],
|
| 729 |
+
"pipeline": [],
|
| 730 |
+
}
|
| 731 |
+
fit_counts = Counter()
|
| 732 |
+
repair_methods = Counter()
|
| 733 |
+
repair_references = Counter()
|
| 734 |
+
repair_trigger_pairs = Counter()
|
| 735 |
+
funnel_clipped_lanes = Counter()
|
| 736 |
+
funnel_rejected_lanes = Counter()
|
| 737 |
+
lane_counts = []
|
| 738 |
+
frame_index = args.start_frame
|
| 739 |
+
processed_frames = 0
|
| 740 |
+
started = time.perf_counter()
|
| 741 |
+
polynomial_file = temporary_polynomials.open("w")
|
| 742 |
+
try:
|
| 743 |
+
while processed_frames < target_frames:
|
| 744 |
+
ok, frame = capture.read()
|
| 745 |
+
if not ok:
|
| 746 |
+
break
|
| 747 |
+
pipeline_start = time.perf_counter()
|
| 748 |
+
|
| 749 |
+
stage = time.perf_counter()
|
| 750 |
+
images = normalize_image_numpy(frame, MODEL_WIDTH, MODEL_HEIGHT)
|
| 751 |
+
preprocess_ms = (time.perf_counter() - stage) * 1000.0
|
| 752 |
+
|
| 753 |
+
stage = time.perf_counter()
|
| 754 |
+
outputs = session.run(list(OUTPUT_NAMES), {"images": images})
|
| 755 |
+
inference_ms = (time.perf_counter() - stage) * 1000.0
|
| 756 |
+
|
| 757 |
+
stage = time.perf_counter()
|
| 758 |
+
lanes = decode(
|
| 759 |
+
softmax_foreground(outputs[0])[0], outputs[1][0], outputs[2][0],
|
| 760 |
+
outputs[3][0], outputs[4][0],
|
| 761 |
+
seg_threshold=args.decode_seg_threshold,
|
| 762 |
+
seed_threshold=args.decode_seed_threshold,
|
| 763 |
+
seed_min_dist=args.decode_seed_min_dist,
|
| 764 |
+
score_thresh=args.decode_score_thresh,
|
| 765 |
+
iou_thresh=args.decode_nms_iou,
|
| 766 |
+
max_seeds=args.decode_max_seeds,
|
| 767 |
+
nms_max_lanes=args.decode_nms_max_lanes,
|
| 768 |
+
nms_scale=args.decode_nms_scale,
|
| 769 |
+
max_output_lanes=args.max_ego_lanes,
|
| 770 |
+
crawl_backend=args.decode_crawl_backend,
|
| 771 |
+
)
|
| 772 |
+
decode_ms = (time.perf_counter() - stage) * 1000.0
|
| 773 |
+
|
| 774 |
+
stage = time.perf_counter()
|
| 775 |
+
lane_results = [
|
| 776 |
+
lane_to_record(
|
| 777 |
+
lane, calibration, bev_range, args.max_cubic_rmse
|
| 778 |
+
)
|
| 779 |
+
for lane in lanes
|
| 780 |
+
]
|
| 781 |
+
topology_report = apply_parallel_repair(lane_results, args)
|
| 782 |
+
valid_records = [
|
| 783 |
+
result["record"] for result in lane_results
|
| 784 |
+
if result["record"]["valid_fit"]
|
| 785 |
+
]
|
| 786 |
+
for record in valid_records:
|
| 787 |
+
fit_counts[record["lane_id"]] += 1
|
| 788 |
+
if topology_report.get("applied", False):
|
| 789 |
+
repair_methods[topology_report["method"]] += 1
|
| 790 |
+
repair_references[topology_report["reference_lane"]] += 1
|
| 791 |
+
repair_trigger_pairs.update(topology_report["trigger_pairs"])
|
| 792 |
+
funnel_clipped_lanes.update(
|
| 793 |
+
topology_report["funnel_guard"]["clipped_lanes"]
|
| 794 |
+
)
|
| 795 |
+
funnel_rejected_lanes.update(
|
| 796 |
+
topology_report["funnel_guard"]["rejected_lanes"]
|
| 797 |
+
)
|
| 798 |
+
bev_canvas = draw_bev(
|
| 799 |
+
lane_results, bev_range, calibration, topology_report
|
| 800 |
+
)
|
| 801 |
+
bev_fit_ms = (time.perf_counter() - stage) * 1000.0
|
| 802 |
+
|
| 803 |
+
pipeline_ms = (time.perf_counter() - pipeline_start) * 1000.0
|
| 804 |
+
draw_predictions(frame, lanes)
|
| 805 |
+
composite = make_composite(
|
| 806 |
+
frame, bev_canvas, frame_index, len(valid_records), pipeline_ms,
|
| 807 |
+
topology_report,
|
| 808 |
+
)
|
| 809 |
+
writer.write(composite)
|
| 810 |
+
polynomial_file.write(json.dumps({
|
| 811 |
+
"frame_index": frame_index,
|
| 812 |
+
"timestamp_seconds": frame_index / fps,
|
| 813 |
+
"coordinate_system": {"X": "forward_m", "Y": "left_m"},
|
| 814 |
+
"parallel_repair": topology_report,
|
| 815 |
+
"lanes": [result["record"] for result in lane_results],
|
| 816 |
+
}) + "\n")
|
| 817 |
+
|
| 818 |
+
timings["preprocess"].append(preprocess_ms)
|
| 819 |
+
timings["inference"].append(inference_ms)
|
| 820 |
+
timings["decode"].append(decode_ms)
|
| 821 |
+
timings["bev_fit"].append(bev_fit_ms)
|
| 822 |
+
timings["pipeline"].append(pipeline_ms)
|
| 823 |
+
lane_counts.append(len(lanes))
|
| 824 |
+
frame_index += 1
|
| 825 |
+
processed_frames += 1
|
| 826 |
+
if processed_frames % 25 == 0 or processed_frames == target_frames:
|
| 827 |
+
print(
|
| 828 |
+
f"frame {processed_frames}/{target_frames} "
|
| 829 |
+
f"(source={frame_index - 1}) | "
|
| 830 |
+
f"lanes={len(lanes)} cubic={len(valid_records)} "
|
| 831 |
+
f"repair={topology_report.get('applied', False)} "
|
| 832 |
+
f"bev={bev_fit_ms:.1f}ms pipeline={pipeline_ms:.1f}ms "
|
| 833 |
+
f"elapsed={time.perf_counter() - started:.1f}s",
|
| 834 |
+
flush=True,
|
| 835 |
+
)
|
| 836 |
+
finally:
|
| 837 |
+
capture.release()
|
| 838 |
+
writer.release()
|
| 839 |
+
polynomial_file.close()
|
| 840 |
+
|
| 841 |
+
if processed_frames == 0:
|
| 842 |
+
temporary_output.unlink(missing_ok=True)
|
| 843 |
+
temporary_polynomials.unlink(missing_ok=True)
|
| 844 |
+
raise RuntimeError("input video produced no frames")
|
| 845 |
+
os.replace(temporary_output, output_path)
|
| 846 |
+
os.replace(temporary_polynomials, polynomial_path)
|
| 847 |
+
|
| 848 |
+
summary = {
|
| 849 |
+
"model": str(model_path),
|
| 850 |
+
"video": str(video_path),
|
| 851 |
+
"output_video": str(output_path),
|
| 852 |
+
"output_polynomials": str(polynomial_path),
|
| 853 |
+
"processed_frames": processed_frames,
|
| 854 |
+
"start_frame": args.start_frame,
|
| 855 |
+
"source_frames": source_frames,
|
| 856 |
+
"fps": fps,
|
| 857 |
+
"provider": args.provider,
|
| 858 |
+
"coordinate_system": {
|
| 859 |
+
"X": "forward from ego, metres",
|
| 860 |
+
"Y": "left of ego, metres",
|
| 861 |
+
"Z": "not exported; local road plane is Z=0",
|
| 862 |
+
},
|
| 863 |
+
"camera": {
|
| 864 |
+
"resolution": [calibration.width, calibration.height],
|
| 865 |
+
"horizontal_fov_deg": calibration.horizontal_fov_deg,
|
| 866 |
+
"intrinsic": calibration.intrinsic.tolist(),
|
| 867 |
+
"camera_to_vehicle": calibration.camera_to_vehicle_matrix.tolist(),
|
| 868 |
+
"distortion": None,
|
| 869 |
+
},
|
| 870 |
+
"bev_range_m": {
|
| 871 |
+
"X": [bev_range.x_min, bev_range.x_max],
|
| 872 |
+
"Y": [bev_range.y_min, bev_range.y_max],
|
| 873 |
+
},
|
| 874 |
+
"polynomial": {
|
| 875 |
+
"formula": "Y(X)=c0+c1*X+c2*X^2+c3*X^3",
|
| 876 |
+
"coefficient_order": ["c0", "c1", "c2", "c3"],
|
| 877 |
+
"max_accepted_rmse_m": args.max_cubic_rmse,
|
| 878 |
+
"fit_counts_by_lane": dict(sorted(fit_counts.items())),
|
| 879 |
+
},
|
| 880 |
+
"parallel_repair": {
|
| 881 |
+
"enabled": not args.disable_parallel_repair,
|
| 882 |
+
"always_parallel": args.always_parallel_repair,
|
| 883 |
+
"complete_four_parallel_lanes": args.complete_four_parallel_lanes,
|
| 884 |
+
"scope": "bev_only; camera overlay remains raw decode",
|
| 885 |
+
"nominal_lane_width_m": args.nominal_lane_width,
|
| 886 |
+
"trigger_gap_ratio": args.repair_trigger_gap_ratio,
|
| 887 |
+
"minimum_gap_m": args.repair_minimum_gap,
|
| 888 |
+
"minimum_bad_run_m": args.repair_minimum_run,
|
| 889 |
+
"maximum_reference_extrapolation_m": (
|
| 890 |
+
args.repair_max_reference_extrapolation
|
| 891 |
+
),
|
| 892 |
+
"funnel_margin_m": args.funnel_margin,
|
| 893 |
+
"funnel_clipped_lane_fits": int(
|
| 894 |
+
sum(funnel_clipped_lanes.values())
|
| 895 |
+
),
|
| 896 |
+
"funnel_clipped_by_lane": dict(
|
| 897 |
+
sorted(funnel_clipped_lanes.items())
|
| 898 |
+
),
|
| 899 |
+
"funnel_rejected_lane_fits": int(
|
| 900 |
+
sum(funnel_rejected_lanes.values())
|
| 901 |
+
),
|
| 902 |
+
"funnel_rejected_by_lane": dict(
|
| 903 |
+
sorted(funnel_rejected_lanes.items())
|
| 904 |
+
),
|
| 905 |
+
"repaired_frames": int(sum(repair_methods.values())),
|
| 906 |
+
"methods": dict(sorted(repair_methods.items())),
|
| 907 |
+
"references": dict(sorted(repair_references.items())),
|
| 908 |
+
"trigger_pairs": dict(sorted(repair_trigger_pairs.items())),
|
| 909 |
+
},
|
| 910 |
+
"lane_count": {
|
| 911 |
+
"mean": float(np.mean(lane_counts)),
|
| 912 |
+
"min": int(min(lane_counts)),
|
| 913 |
+
"max": int(max(lane_counts)),
|
| 914 |
+
},
|
| 915 |
+
"timings": {
|
| 916 |
+
name: timing_summary(values) for name, values in timings.items()
|
| 917 |
+
},
|
| 918 |
+
"wall_time_seconds": time.perf_counter() - started,
|
| 919 |
+
}
|
| 920 |
+
temporary_summary = summary_path.with_suffix(summary_path.suffix + ".tmp")
|
| 921 |
+
with temporary_summary.open("w") as handle:
|
| 922 |
+
json.dump(summary, handle, indent=2)
|
| 923 |
+
handle.write("\n")
|
| 924 |
+
os.replace(temporary_summary, summary_path)
|
| 925 |
+
print(f"video OK: {output_path}")
|
| 926 |
+
print(f"polynomials OK: {polynomial_path}")
|
| 927 |
+
print(f"summary: {summary_path}")
|
| 928 |
+
|
| 929 |
+
|
| 930 |
+
if __name__ == "__main__":
|
| 931 |
+
main()
|