huyluongngoc commited on
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()