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