186 lines
6.7 KiB
Python
186 lines
6.7 KiB
Python
import json
|
|
import time
|
|
from typing import Tuple, Dict, List, Iterable, Optional
|
|
import numpy as np
|
|
from scipy.optimize import curve_fit
|
|
import statistics
|
|
import math
|
|
from aaredaqlib.logger_config import setup_logger
|
|
from aaredaqlib.models import MLBoxModel, MLOutputModel
|
|
|
|
logger = setup_logger(__name__, '/tmp/mxlogs')
|
|
|
|
def box_height_from_tuple(box: tuple[float, float, float, float]) -> float:
|
|
x1, y1, x2, y2 = box
|
|
return abs(y2 - y1)
|
|
|
|
def box_area_from_tuple(box: tuple[float, float, float, float]) -> float:
|
|
x1, y1, x2, y2 = box
|
|
return abs(y2 - y1) * abs(x2 -x1)
|
|
|
|
def prepare_samples(boxes_by_angle: dict[int, tuple[float,float,float,float]],
|
|
area = False) -> List[Tuple[float, float]]:
|
|
# angles in degrees -> (theta_rad, height)
|
|
samples = []
|
|
for deg, box in boxes_by_angle.items():
|
|
v = box_area_from_tuple(box) if area else box_height_from_tuple(box)
|
|
samples.append((float(deg), v))
|
|
return samples
|
|
|
|
def cos_model(theta_deg: float | np.ndarray, A: float, B: float, phi_rad: float, C: float):
|
|
return A + B * np.cos(C * np.deg2rad(theta_deg) - phi_rad)
|
|
|
|
def mad_filter(samples: List[Tuple[float, float]], k: float = 3.5) -> List[Tuple[float, float]]:
|
|
if not samples:
|
|
return samples
|
|
ys = [y for _, y in samples]
|
|
med = statistics.median(ys)
|
|
mad = statistics.median([abs(y - med) for y in ys]) or 1.0
|
|
return [(d, y) for d, y in samples if abs(y - med) <= k * mad]
|
|
|
|
def samples_to_json(samples):
|
|
output_data = {
|
|
'timestamp': time.ctime(),
|
|
'scan_results': samples,
|
|
'total_results': len(samples)
|
|
}
|
|
with open("cos_test.json", 'w') as f:
|
|
json.dump(output_data, f, indent=2)
|
|
return
|
|
|
|
def fit_metrics(y_true: np.ndarray, y_pred: np.ndarray) -> tuple[float, float, float]:
|
|
resid = y_true - y_pred
|
|
rmse = float(np.sqrt(np.mean(resid**2)))
|
|
mae = float(np.mean(np.abs(resid)))
|
|
# R² with protection against zero variance
|
|
ss_tot = float(np.sum((y_true - np.mean(y_true))**2))
|
|
r2 = float(1.0 - np.sum(resid**2) / ss_tot) if ss_tot > 0 else float("nan")
|
|
return rmse, mae, r2
|
|
|
|
|
|
def fit_cosine(samples: List[Tuple[float, float]]) -> dict:
|
|
samples = mad_filter(samples, k=3.5)
|
|
if len(samples) < 3:
|
|
A = sum(y for _, y in samples) / max(1, len(samples))
|
|
return {"A": A, "B": 1.0, "phi_rad": 0.0, "C": 1.0,
|
|
"rmse": None, "mae": None, "r2": None}
|
|
|
|
degs = np.array([d for d, _ in samples], dtype=float)
|
|
ys = np.array([y for _, y in samples], dtype=float)
|
|
|
|
# Initial guess via linearized cosine/sine fit
|
|
cosv = np.cos(np.deg2rad(degs))
|
|
sinv = np.sin(np.deg2rad(degs))
|
|
X = np.column_stack([np.ones_like(degs), cosv, sinv]) # [A, C, S]
|
|
try:
|
|
beta, _, _, _ = np.linalg.lstsq(X, ys, rcond=None)
|
|
A0, C0, S0 = beta.tolist()
|
|
except Exception:
|
|
A0, C0, S0 = float(np.mean(ys)), 0.0, 0.0
|
|
|
|
B0 = float(math.hypot(C0, S0))
|
|
phi0 = float(math.atan2(S0, C0))
|
|
C0 = 1.0
|
|
# Bounds to keep B reasonable and phi in [-pi, pi]
|
|
y_std = float(np.std(ys)) or 1.0
|
|
B_max = 10.0 * y_std
|
|
bounds = ([-np.inf, 0.0, -math.pi, -np.inf], [np.inf, B_max, math.pi, np.inf])
|
|
|
|
try:
|
|
popt, _ = curve_fit(cos_model, degs, ys, p0=[A0, B0, phi0, C0], bounds=bounds, maxfev=10000)
|
|
logger.info(f"fit cosine: {popt}")
|
|
A, B, phi, C = map(float, popt)
|
|
B = max(0.0, B)
|
|
yhat = cos_model(degs, A, B, phi, C)
|
|
rmse, mae, r2 = fit_metrics(ys, yhat)
|
|
return {"A": A, "B": B, "phi_rad": phi, "C": C,
|
|
"rmse": rmse, "mae": mae, "r2": r2}
|
|
|
|
except Exception as e:
|
|
# Fallback to initial
|
|
logger.info(f"error in curve fit {e}")
|
|
yhat0 = cos_model(degs, A0, max(0.0, B0), phi0, C0)
|
|
rmse0, mae0, r2_0 = fit_metrics(ys, yhat0)
|
|
return {"A": float(A0), "B": max(0.0, float(B0)), "phi_rad": float(phi0), "C": float(C0),
|
|
"rmse": rmse0, "mae": mae0, "r2": r2_0}
|
|
|
|
def get_samples_out(boxes):
|
|
samples_out = []
|
|
for deg, box in boxes.items():
|
|
h = box_height_from_tuple(box)
|
|
a = box_area_from_tuple(box)
|
|
samples_out.append({"angle_deg": float(deg), "height": float(h), "area": float(a)})
|
|
samples_out.sort(key=lambda x: x["angle_deg"])
|
|
return samples_out
|
|
|
|
def choose_best_fit(fits_by_name: Dict[str, Dict]) -> Tuple[Optional[float], Optional[Dict], Optional[str]]:
|
|
|
|
|
|
def key(entry: Dict):
|
|
params = entry.get("params") or {}
|
|
rmse = params.get("rmse")
|
|
mae = params.get("mae")
|
|
r2 = params.get("r2")
|
|
logger.info(f"params: {params}")
|
|
# Treat None/NaN as non-comparable
|
|
if rmse is None or mae is None or r2 is None:
|
|
return None
|
|
try:
|
|
logger.info(f"rmse: {rmse}, mae: {mae}, r2: {r2}")
|
|
return float(rmse), float(mae), -float(r2)
|
|
except Exception:
|
|
return None
|
|
|
|
best_name = None
|
|
best_fit = None
|
|
best_key = None
|
|
|
|
for name, entry in fits_by_name.items():
|
|
k = key(entry)
|
|
logger.info(f"name: {name}, entry: {entry}, key : {k}")
|
|
if k is None:
|
|
continue
|
|
if best_key is None or k < best_key:
|
|
best_key = k
|
|
best_fit = entry
|
|
best_name = name
|
|
|
|
if best_fit is None:
|
|
return None, None, None
|
|
|
|
best_angle = best_fit.get("angle")
|
|
best_angle = float(best_angle) if isinstance(best_angle, (int, float)) else None
|
|
|
|
|
|
return (best_angle,
|
|
best_fit,
|
|
best_name)
|
|
|
|
def get_flat_face(boxes: dict[int, tuple[float,float,float,float]], start_angle:int, end_angle:int, area:bool = False) -> tuple[int, dict]:
|
|
samples = prepare_samples(boxes, area=area)
|
|
parameters = fit_cosine(samples)
|
|
|
|
def safe_best_angle(params: dict) -> int:
|
|
if not params:
|
|
return 0
|
|
search_grid = range(start_angle, end_angle, 1)
|
|
A = float(params.get("A", 0.0))
|
|
B = float(max(0.0, params.get("B", 0.0)))
|
|
C = float(params.get("C", 0.0))
|
|
phi = float(params.get("phi_rad", 0.0))
|
|
if B <= 1e-9 or not math.isfinite(A) or not math.isfinite(phi):
|
|
return 0
|
|
return max(search_grid, key=lambda d: cos_model(d, A, B, phi, C))
|
|
|
|
best_fit_angle= safe_best_angle(parameters)
|
|
return best_fit_angle, parameters
|
|
|
|
def chose_best_angle(boxes: dict[int, tuple[float,float,float,float]], fit_results) -> int:
|
|
measured_angles = list(boxes.keys())
|
|
choose_best_fit(fit_results)
|
|
candidates = [a for a in (fit_results["Area"]["angle"], fit_results["Height"]["angle"]) if isinstance(a, (int, float))]
|
|
if measured_angles and candidates:
|
|
chosen = min(candidates, key=lambda a: min(abs(((a - m + 180) % 360) - 180) for m in measured_angles))
|
|
else:
|
|
chosen = candidates[0] if candidates else 0
|
|
return chosen |