607 lines
22 KiB
Python
607 lines
22 KiB
Python
from __future__ import annotations
|
|
|
|
from itertools import product
|
|
|
|
import numpy as np
|
|
|
|
from .fitting import evaluate_line
|
|
from .preprocess import estimate_normals
|
|
from .structures import FittedLine, HoleCandidate, ReferenceLine
|
|
|
|
|
|
AXIS_INDEX = {"x": 0, "y": 1, "z": 2}
|
|
|
|
|
|
def _circle_from_three_points(points_2d: np.ndarray) -> tuple[np.ndarray, float] | None:
|
|
p1, p2, p3 = points_2d
|
|
temp = p2[0] ** 2 + p2[1] ** 2
|
|
bc = (p1[0] ** 2 + p1[1] ** 2 - temp) / 2.0
|
|
cd = (temp - p3[0] ** 2 - p3[1] ** 2) / 2.0
|
|
det = (p1[0] - p2[0]) * (p2[1] - p3[1]) - (p2[0] - p3[0]) * (p1[1] - p2[1])
|
|
if abs(det) < 1e-6:
|
|
return None
|
|
center_x = (bc * (p2[1] - p3[1]) - cd * (p1[1] - p2[1])) / det
|
|
center_y = ((p1[0] - p2[0]) * cd - (p2[0] - p3[0]) * bc) / det
|
|
center = np.array([center_x, center_y], dtype=np.float64)
|
|
radius = float(np.linalg.norm(center - p1))
|
|
return center, radius
|
|
|
|
|
|
def _circle_coverage(angles: np.ndarray, bins: int) -> float:
|
|
wrapped = np.mod(angles, 2.0 * np.pi)
|
|
hist, _ = np.histogram(wrapped, bins=bins, range=(0.0, 2.0 * np.pi))
|
|
return float(np.count_nonzero(hist) / max(bins, 1))
|
|
|
|
|
|
def _cluster_labels(features: np.ndarray, eps: float, min_samples: int) -> np.ndarray:
|
|
if len(features) == 0:
|
|
return np.zeros(0, dtype=np.int32)
|
|
labels = np.full(len(features), -1, dtype=np.int32)
|
|
cluster_id = 0
|
|
for index in range(len(features)):
|
|
if labels[index] >= 0:
|
|
continue
|
|
seed_neighbors = np.where(
|
|
np.linalg.norm(features - features[index], axis=1) <= eps
|
|
)[0]
|
|
if len(seed_neighbors) < min_samples:
|
|
continue
|
|
labels[seed_neighbors] = cluster_id
|
|
queue = list(seed_neighbors.tolist())
|
|
while queue:
|
|
current = queue.pop()
|
|
current_neighbors = np.where(
|
|
np.linalg.norm(features - features[current], axis=1) <= eps
|
|
)[0]
|
|
if len(current_neighbors) < min_samples:
|
|
continue
|
|
for neighbor in current_neighbors:
|
|
if labels[neighbor] < 0:
|
|
labels[neighbor] = cluster_id
|
|
queue.append(int(neighbor))
|
|
cluster_id += 1
|
|
return labels
|
|
|
|
|
|
def _detect_circle_proposals(
|
|
points: np.ndarray,
|
|
normals: np.ndarray,
|
|
config: dict,
|
|
*,
|
|
allowed_axes: list[str] | None = None,
|
|
) -> list[HoleCandidate]:
|
|
ref_cfg = config["reference"]
|
|
rng = np.random.default_rng(42)
|
|
proposals: list[dict] = []
|
|
if len(points) < ref_cfg["min_inliers"]:
|
|
return []
|
|
allowed_axis_set = set(allowed_axes or AXIS_INDEX.keys())
|
|
|
|
for axis_name, axis_index in AXIS_INDEX.items():
|
|
if axis_name not in allowed_axis_set:
|
|
continue
|
|
mask = np.abs(normals[:, axis_index]) <= ref_cfg["normal_axis_max_alignment"]
|
|
axis_points = points[mask] if np.count_nonzero(mask) >= ref_cfg["min_inliers"] else points
|
|
if len(axis_points) < ref_cfg["min_inliers"]:
|
|
continue
|
|
other_axes = [idx for idx in range(3) if idx != axis_index]
|
|
projected = axis_points[:, other_axes]
|
|
if len(projected) > ref_cfg["circle_sample_size"]:
|
|
sample_indices = rng.choice(len(projected), size=ref_cfg["circle_sample_size"], replace=False)
|
|
projected_sample = projected[sample_indices]
|
|
else:
|
|
projected_sample = projected
|
|
|
|
for _ in range(ref_cfg["circle_trials"]):
|
|
triple_indices = rng.choice(len(projected_sample), size=3, replace=False)
|
|
circle = _circle_from_three_points(projected_sample[triple_indices])
|
|
if circle is None:
|
|
continue
|
|
center_2d, radius = circle
|
|
if not (ref_cfg["radius_min_mm"] <= radius <= ref_cfg["radius_max_mm"]):
|
|
continue
|
|
radial_distance = np.linalg.norm(projected - center_2d[None, :], axis=1)
|
|
residual = np.abs(radial_distance - radius)
|
|
inlier_mask = residual <= ref_cfg["fit_tolerance_mm"]
|
|
inlier_count = int(np.count_nonzero(inlier_mask))
|
|
if inlier_count < ref_cfg["min_inliers"]:
|
|
continue
|
|
angles = np.arctan2(
|
|
projected[inlier_mask, 1] - center_2d[1],
|
|
projected[inlier_mask, 0] - center_2d[0],
|
|
)
|
|
coverage_ratio = _circle_coverage(angles, ref_cfg["coverage_bin_count"])
|
|
mean_residual = float(np.mean(residual[inlier_mask]))
|
|
score = (
|
|
inlier_count / max(len(projected), 1)
|
|
+ 1.35 * coverage_ratio
|
|
- mean_residual / max(ref_cfg["fit_tolerance_mm"], 1.0)
|
|
)
|
|
center_3d = np.zeros(3, dtype=np.float64)
|
|
center_3d[other_axes] = center_2d
|
|
center_3d[axis_index] = float(np.median(axis_points[inlier_mask, axis_index]))
|
|
proposals.append(
|
|
{
|
|
"axis": axis_name,
|
|
"center": center_3d,
|
|
"radius": radius,
|
|
"score": score,
|
|
"inlier_count": inlier_count,
|
|
"coverage_ratio": coverage_ratio,
|
|
"residual": mean_residual,
|
|
}
|
|
)
|
|
|
|
if not proposals:
|
|
return []
|
|
|
|
proposal_features = np.asarray(
|
|
[
|
|
np.hstack(
|
|
[
|
|
proposal["center"] / max(ref_cfg["cluster_eps_mm"], 1.0),
|
|
proposal["radius"] / max(ref_cfg["cluster_eps_mm"], 1.0),
|
|
]
|
|
)
|
|
for proposal in proposals
|
|
]
|
|
)
|
|
candidates: list[HoleCandidate] = []
|
|
labels = _cluster_labels(
|
|
proposal_features,
|
|
eps=1.0,
|
|
min_samples=ref_cfg["cluster_min_samples"],
|
|
)
|
|
for label in sorted(set(labels)):
|
|
if label < 0:
|
|
continue
|
|
members = [proposal for proposal, member_label in zip(proposals, labels) if member_label == label]
|
|
scores = np.asarray([member["score"] for member in members], dtype=np.float64)
|
|
weights = np.clip(scores - scores.min() + 1e-3, 1e-3, None)
|
|
centers = np.asarray([member["center"] for member in members], dtype=np.float64)
|
|
radii = np.asarray([member["radius"] for member in members], dtype=np.float64)
|
|
best_member = max(members, key=lambda item: item["score"])
|
|
center = np.average(centers, axis=0, weights=weights)
|
|
radius = float(np.average(radii, weights=weights))
|
|
residual_factor = float(
|
|
np.exp(
|
|
-best_member["residual"]
|
|
/ max(ref_cfg["fit_tolerance_mm"] * 3.5, 1.0)
|
|
)
|
|
)
|
|
radius_factor = float(
|
|
np.exp(
|
|
-max(radius - ref_cfg["radius_max_mm"] * 0.45, 0.0)
|
|
/ max(ref_cfg["radius_max_mm"] * 0.35, 1.0)
|
|
)
|
|
)
|
|
confidence = float(
|
|
np.clip(
|
|
(
|
|
0.25
|
|
+ 0.22 * best_member["coverage_ratio"]
|
|
+ 0.18 * best_member["inlier_count"] / max(len(points), 1)
|
|
+ 0.15 * (best_member["score"] / 3.0)
|
|
)
|
|
* residual_factor
|
|
* radius_factor
|
|
+ 0.1,
|
|
0.0,
|
|
1.0,
|
|
)
|
|
)
|
|
candidates.append(
|
|
HoleCandidate(
|
|
side="",
|
|
region="",
|
|
axis=best_member["axis"],
|
|
center=center,
|
|
radius=radius,
|
|
score=float(best_member["score"]),
|
|
inlier_count=int(best_member["inlier_count"]),
|
|
coverage_ratio=float(best_member["coverage_ratio"]),
|
|
residual=float(best_member["residual"]),
|
|
confidence=confidence,
|
|
)
|
|
)
|
|
candidates.sort(key=lambda item: item.score, reverse=True)
|
|
return candidates
|
|
|
|
|
|
def _score_candidates_against_beam(
|
|
candidates: list[HoleCandidate],
|
|
fitted_line: FittedLine,
|
|
side: str,
|
|
region: str,
|
|
config: dict,
|
|
region_bounds: dict[str, float],
|
|
) -> list[HoleCandidate]:
|
|
if not candidates:
|
|
return []
|
|
x_values = np.asarray([candidate.center[0] for candidate in candidates], dtype=np.float64)
|
|
y_expected, z_expected, _ = evaluate_line(fitted_line, x_values)
|
|
ref_cfg = config["reference"]
|
|
scored: list[HoleCandidate] = []
|
|
region_span = max(region_bounds["x_hi"] - region_bounds["x_lo"], 1.0)
|
|
preferred_axes = set(ref_cfg.get("preferred_axes", []))
|
|
axis_bonus_cfg = ref_cfg.get("axis_bonus", {})
|
|
for index, candidate in enumerate(candidates):
|
|
lateral_distance = abs(candidate.center[1] - y_expected[index])
|
|
vertical_distance = abs(candidate.center[2] - z_expected[index])
|
|
underside_bottom = candidate.center[2] - candidate.radius
|
|
underside_distance = abs(underside_bottom - z_expected[index])
|
|
if region == "front":
|
|
extremity = np.clip(
|
|
(region_bounds["x_hi"] - candidate.center[0]) / region_span,
|
|
0.0,
|
|
1.0,
|
|
)
|
|
else:
|
|
extremity = np.clip(
|
|
(candidate.center[0] - region_bounds["x_lo"]) / region_span,
|
|
0.0,
|
|
1.0,
|
|
)
|
|
penalty = (
|
|
ref_cfg["lateral_penalty_weight"]
|
|
* lateral_distance
|
|
/ max(ref_cfg["lateral_margin_mm"], 1.0)
|
|
+ ref_cfg["vertical_center_penalty_weight"]
|
|
* vertical_distance
|
|
/ max(ref_cfg["underside_upper_margin_mm"], 1.0)
|
|
+ ref_cfg["underside_penalty_weight"]
|
|
* underside_distance
|
|
/ max(ref_cfg["underside_upper_margin_mm"], 1.0)
|
|
)
|
|
adjusted_score = (
|
|
candidate.score
|
|
+ axis_bonus_cfg.get(candidate.axis, 0.0)
|
|
+ ref_cfg["extremity_reward"] * extremity
|
|
- penalty
|
|
)
|
|
axis_factor = 1.0 if candidate.axis in preferred_axes else 0.72
|
|
confidence = float(
|
|
np.clip(
|
|
candidate.confidence
|
|
* axis_factor
|
|
* (0.72 + 0.28 * extremity)
|
|
* np.exp(-0.22 * penalty),
|
|
0.0,
|
|
1.0,
|
|
)
|
|
)
|
|
scored.append(
|
|
HoleCandidate(
|
|
side=side,
|
|
region=region,
|
|
axis=candidate.axis,
|
|
center=candidate.center,
|
|
radius=candidate.radius,
|
|
score=float(adjusted_score),
|
|
inlier_count=candidate.inlier_count,
|
|
coverage_ratio=candidate.coverage_ratio,
|
|
residual=candidate.residual,
|
|
confidence=confidence,
|
|
)
|
|
)
|
|
scored.sort(key=lambda item: item.score, reverse=True)
|
|
return scored
|
|
|
|
|
|
def _collect_region_hole_candidates(
|
|
region_points: np.ndarray,
|
|
region_normals: np.ndarray,
|
|
config: dict,
|
|
allowed_axes: list[str],
|
|
) -> list[HoleCandidate]:
|
|
if len(region_points) == 0:
|
|
return []
|
|
ref_cfg = config["reference"]
|
|
candidate_pool: list[HoleCandidate] = []
|
|
x_min = float(region_points[:, 0].min())
|
|
x_max = float(region_points[:, 0].max())
|
|
span = max(x_max - x_min, 1.0)
|
|
window_count = max(int(ref_cfg.get("x_window_count", 1)), 1)
|
|
overlap = float(np.clip(ref_cfg.get("x_window_overlap_ratio", 0.0), 0.0, 0.9))
|
|
base_window = span / window_count
|
|
window_length = base_window * (1.0 + overlap)
|
|
|
|
candidate_pool.extend(
|
|
_detect_circle_proposals(
|
|
region_points,
|
|
region_normals,
|
|
config,
|
|
allowed_axes=allowed_axes,
|
|
)
|
|
)
|
|
|
|
if window_count <= 1:
|
|
return candidate_pool
|
|
|
|
for window_index in range(window_count):
|
|
x_lo = x_min + window_index * base_window
|
|
x_hi = x_lo + window_length
|
|
mask = (region_points[:, 0] >= x_lo) & (region_points[:, 0] <= x_hi)
|
|
sub_points = region_points[mask]
|
|
if len(sub_points) < ref_cfg["min_inliers"] * 2:
|
|
continue
|
|
sub_normals = region_normals[mask]
|
|
candidate_pool.extend(
|
|
_detect_circle_proposals(
|
|
sub_points,
|
|
sub_normals,
|
|
config,
|
|
allowed_axes=allowed_axes,
|
|
)
|
|
)
|
|
|
|
deduped: list[HoleCandidate] = []
|
|
for candidate in sorted(candidate_pool, key=lambda item: item.score, reverse=True):
|
|
if any(
|
|
candidate.axis == existing.axis
|
|
and np.linalg.norm(candidate.center - existing.center) <= max(ref_cfg["cluster_eps_mm"] * 0.6, 80.0)
|
|
and abs(candidate.radius - existing.radius) <= max(ref_cfg["cluster_eps_mm"] * 0.25, 20.0)
|
|
for existing in deduped
|
|
):
|
|
continue
|
|
deduped.append(candidate)
|
|
if len(deduped) >= 12:
|
|
break
|
|
return deduped
|
|
|
|
|
|
def _region_points_for_hole_search(
|
|
points_vehicle: np.ndarray,
|
|
fitted_line: FittedLine,
|
|
region: str,
|
|
config: dict,
|
|
) -> tuple[np.ndarray, dict[str, float]]:
|
|
ref_cfg = config["reference"]
|
|
x_min = float(fitted_line.x_support.min())
|
|
x_max = float(fitted_line.x_support.max())
|
|
span = max(x_max - x_min, 1.0)
|
|
region_length = ref_cfg["stable_region_fraction"] * span
|
|
extension = ref_cfg["search_extension_mm"]
|
|
vehicle_x_min = float(points_vehicle[:, 0].min())
|
|
vehicle_x_max = float(points_vehicle[:, 0].max())
|
|
if region == "front":
|
|
x_lo = max(vehicle_x_min, x_min - extension)
|
|
x_hi = min(vehicle_x_max, x_min + region_length)
|
|
x_eval_lo, x_eval_hi = x_min, min(x_max, x_min + region_length)
|
|
else:
|
|
x_lo = max(vehicle_x_min, x_max - region_length)
|
|
x_hi = min(vehicle_x_max, x_max + extension)
|
|
x_eval_lo, x_eval_hi = max(x_min, x_max - region_length), x_max
|
|
if x_eval_hi <= x_eval_lo:
|
|
x_grid = np.asarray([x_min if region == "front" else x_max], dtype=np.float64)
|
|
else:
|
|
x_grid = np.linspace(x_eval_lo, x_eval_hi, 16)
|
|
y_ref, z_ref, _ = evaluate_line(fitted_line, x_grid)
|
|
y_center = float(np.median(y_ref))
|
|
z_center = float(np.median(z_ref))
|
|
z_lo = max(
|
|
float(points_vehicle[:, 2].min()),
|
|
z_center - ref_cfg["underside_lower_margin_mm"],
|
|
)
|
|
z_hi = min(
|
|
float(points_vehicle[:, 2].max()),
|
|
z_center + ref_cfg["underside_upper_margin_mm"],
|
|
)
|
|
mask = (
|
|
(points_vehicle[:, 0] >= x_lo)
|
|
& (points_vehicle[:, 0] <= x_hi)
|
|
& (np.abs(points_vehicle[:, 1] - y_center) <= ref_cfg["lateral_margin_mm"])
|
|
& (points_vehicle[:, 2] >= z_lo)
|
|
& (points_vehicle[:, 2] <= z_hi)
|
|
)
|
|
return points_vehicle[mask], {
|
|
"x_lo": float(x_lo),
|
|
"x_hi": float(x_hi),
|
|
"y_center": y_center,
|
|
"z_center": z_center,
|
|
"z_lo": float(z_lo),
|
|
"z_hi": float(z_hi),
|
|
}
|
|
|
|
|
|
def _build_endpoint_reference_line(
|
|
fitted_line: FittedLine,
|
|
side: str,
|
|
) -> ReferenceLine:
|
|
x_front = float(fitted_line.x_support.min())
|
|
x_back = float(fitted_line.x_support.max())
|
|
yz_front, _ = evaluate_reference_line_from_fitted(fitted_line, np.asarray([x_front], dtype=np.float64))
|
|
yz_back, _ = evaluate_reference_line_from_fitted(fitted_line, np.asarray([x_back], dtype=np.float64))
|
|
point_front = np.array([x_front, yz_front[0, 0], yz_front[0, 1]], dtype=np.float64)
|
|
point_back = np.array([x_back, yz_back[0, 0], yz_back[0, 1]], dtype=np.float64)
|
|
direction = point_back - point_front
|
|
norm = float(np.linalg.norm(direction))
|
|
if norm <= 1e-9:
|
|
return ReferenceLine(
|
|
side=side,
|
|
available=False,
|
|
point_front=None,
|
|
point_back=None,
|
|
direction=None,
|
|
confidence=0.0,
|
|
residual=None,
|
|
candidates_front=[],
|
|
candidates_back=[],
|
|
alternates=[],
|
|
)
|
|
confidence = 1.0
|
|
residual = float(0.5 * (fitted_line.y_residual_rms + fitted_line.z_residual_rms))
|
|
return ReferenceLine(
|
|
side=side,
|
|
available=True,
|
|
point_front=point_front,
|
|
point_back=point_back,
|
|
direction=direction / norm,
|
|
confidence=confidence,
|
|
residual=residual,
|
|
candidates_front=[],
|
|
candidates_back=[],
|
|
alternates=[],
|
|
)
|
|
|
|
|
|
def evaluate_reference_line_from_fitted(
|
|
fitted_line: FittedLine,
|
|
x_grid: np.ndarray,
|
|
) -> tuple[np.ndarray, np.ndarray]:
|
|
y, z, confidence = evaluate_line(fitted_line, x_grid)
|
|
return np.column_stack([y, z]), confidence
|
|
|
|
|
|
def detect_reference_line(
|
|
points_vehicle: np.ndarray,
|
|
fitted_line: FittedLine,
|
|
config: dict,
|
|
side: str,
|
|
) -> ReferenceLine:
|
|
ref_cfg = config["reference"]
|
|
if ref_cfg.get("mode") == "line_endpoints":
|
|
return _build_endpoint_reference_line(fitted_line, side)
|
|
preferred_axes = ref_cfg.get("preferred_axes", ["y", "z"])
|
|
front_region, front_bounds = _region_points_for_hole_search(points_vehicle, fitted_line, "front", config)
|
|
back_region, back_bounds = _region_points_for_hole_search(points_vehicle, fitted_line, "back", config)
|
|
|
|
front_normals, _ = estimate_normals(front_region, k=min(18, max(len(front_region) - 1, 1)))
|
|
back_normals, _ = estimate_normals(back_region, k=min(18, max(len(back_region) - 1, 1)))
|
|
|
|
front_candidates = _score_candidates_against_beam(
|
|
_collect_region_hole_candidates(
|
|
front_region,
|
|
front_normals,
|
|
config,
|
|
allowed_axes=preferred_axes,
|
|
),
|
|
fitted_line,
|
|
side,
|
|
"front",
|
|
config,
|
|
front_bounds,
|
|
)
|
|
back_candidates = _score_candidates_against_beam(
|
|
_collect_region_hole_candidates(
|
|
back_region,
|
|
back_normals,
|
|
config,
|
|
allowed_axes=preferred_axes,
|
|
),
|
|
fitted_line,
|
|
side,
|
|
"back",
|
|
config,
|
|
back_bounds,
|
|
)
|
|
|
|
if not front_candidates or not back_candidates:
|
|
return ReferenceLine(
|
|
side=side,
|
|
available=False,
|
|
point_front=None,
|
|
point_back=None,
|
|
direction=None,
|
|
confidence=0.0,
|
|
residual=None,
|
|
candidates_front=front_candidates,
|
|
candidates_back=back_candidates,
|
|
alternates=[],
|
|
)
|
|
|
|
alternates = []
|
|
best = None
|
|
for front_candidate, back_candidate in product(front_candidates[:3], back_candidates[:3]):
|
|
direction = back_candidate.center - front_candidate.center
|
|
norm = float(np.linalg.norm(direction))
|
|
if norm < 1e-6:
|
|
continue
|
|
direction_unit = direction / norm
|
|
x_alignment = abs(direction_unit[0])
|
|
front_bottom = front_candidate.center[2] - front_candidate.radius
|
|
back_bottom = back_candidate.center[2] - back_candidate.radius
|
|
combo_score = (
|
|
front_candidate.score
|
|
+ back_candidate.score
|
|
+ 1.2 * x_alignment
|
|
- abs(front_candidate.center[1] - back_candidate.center[1]) / max(ref_cfg["lateral_margin_mm"], 1.0)
|
|
- 0.6 * abs(front_bottom - back_bottom) / max(ref_cfg["underside_upper_margin_mm"], 1.0)
|
|
+ (0.18 if front_candidate.axis == back_candidate.axis else 0.0)
|
|
)
|
|
residual_mean = 0.5 * (front_candidate.residual + back_candidate.residual)
|
|
combo_confidence = float(
|
|
np.clip(
|
|
min(front_candidate.confidence, back_candidate.confidence)
|
|
* x_alignment
|
|
* np.exp(-residual_mean / max(ref_cfg["fit_tolerance_mm"] * 3.0, 1.0)),
|
|
0.0,
|
|
1.0,
|
|
)
|
|
)
|
|
alternate = {
|
|
"front_center": front_candidate.center.tolist(),
|
|
"back_center": back_candidate.center.tolist(),
|
|
"direction": direction_unit.tolist(),
|
|
"score": float(combo_score),
|
|
"confidence": combo_confidence,
|
|
}
|
|
alternates.append(alternate)
|
|
if best is None or combo_score > best[0]:
|
|
best = (combo_score, front_candidate, back_candidate, direction_unit)
|
|
|
|
alternates.sort(key=lambda item: item["score"], reverse=True)
|
|
if best is None:
|
|
return ReferenceLine(
|
|
side=side,
|
|
available=False,
|
|
point_front=None,
|
|
point_back=None,
|
|
direction=None,
|
|
confidence=0.0,
|
|
residual=None,
|
|
candidates_front=front_candidates,
|
|
candidates_back=back_candidates,
|
|
alternates=alternates[:5],
|
|
)
|
|
|
|
_, front_best, back_best, direction = best
|
|
residual = float((front_best.residual + back_best.residual) / 2.0)
|
|
confidence = float(
|
|
np.clip(
|
|
min(front_best.confidence, back_best.confidence)
|
|
* abs(direction[0])
|
|
* np.exp(-residual / max(config["reference"]["fit_tolerance_mm"] * 3.0, 1.0)),
|
|
0.0,
|
|
1.0,
|
|
)
|
|
)
|
|
return ReferenceLine(
|
|
side=side,
|
|
available=True,
|
|
point_front=front_best.center,
|
|
point_back=back_best.center,
|
|
direction=direction,
|
|
confidence=confidence,
|
|
residual=residual,
|
|
candidates_front=front_candidates,
|
|
candidates_back=back_candidates,
|
|
alternates=alternates[:5],
|
|
)
|
|
|
|
|
|
def evaluate_reference_line(reference_line: ReferenceLine, x_grid: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
|
if not reference_line.available or reference_line.point_front is None or reference_line.point_back is None:
|
|
return np.full_like(x_grid, np.nan, dtype=np.float64), np.zeros_like(x_grid, dtype=np.float64)
|
|
front = reference_line.point_front
|
|
direction = reference_line.direction
|
|
assert direction is not None
|
|
dx = direction[0]
|
|
if abs(dx) < 1e-6:
|
|
return np.full_like(x_grid, np.nan, dtype=np.float64), np.zeros_like(x_grid, dtype=np.float64)
|
|
t = (x_grid - front[0]) / dx
|
|
y = front[1] + t * direction[1]
|
|
z = front[2] + t * direction[2]
|
|
confidence = np.full_like(x_grid, reference_line.confidence, dtype=np.float64)
|
|
return np.column_stack([y, z]), confidence
|