From 1e42d76ded6cb7b458ffb375b27070f376fcd6bb Mon Sep 17 00:00:00 2001 From: Filip Leonarski Date: Fri, 17 Oct 2025 16:11:05 +0200 Subject: [PATCH] DAQ: added face detection --- daq/src/aaredaq/daq.py | 232 +++++++++++++++++++++++++++++++++++++-- daq/src/aaredaq/mlbox.py | 18 ++- 2 files changed, 232 insertions(+), 18 deletions(-) diff --git a/daq/src/aaredaq/daq.py b/daq/src/aaredaq/daq.py index bf45d736..b8dbdab0 100644 --- a/daq/src/aaredaq/daq.py +++ b/daq/src/aaredaq/daq.py @@ -1,9 +1,10 @@ import copy import json +import math import time from datetime import datetime from math import ceil -from typing import List, Tuple, Optional, Callable +from typing import List, Tuple, Optional, Callable, Dict import secrets import os @@ -1024,13 +1025,13 @@ class AareDAQ: #curr_image = self.camera_image #box = self.__mlbox.predict(curr_image) bgr_image = cv2.cvtColor(self.camera_image, cv2.COLOR_RGB2BGR) - box = self.__mlbox.predict(bgr_image) + box = self.__mlbox.predict(bgr_image, pref_class=(3,0)) if box is None: if filename is not None: cv2.imwrite(f"{filename}_no_detection.jpg", bgr_image) self.__aare.upload_image(sample_id, f"{filename}_no_detection", bgr_image) return None - x1, y1, x2, y2 = box + _, x1, y1, x2, y2 = box if filename is not None: cv2.rectangle(bgr_image, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2) cv2.imwrite(f"{filename}.jpg", bgr_image) @@ -1066,7 +1067,7 @@ class AareDAQ: self.__aare.upload_image(sample_id, f"{filename}_no_detection", bgr_image) return None - x1, y1, x2, y2 = box + cls, x1, y1, x2, y2 = box if filename is not None: #cv2.rectangle(bgr_image, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2) @@ -1074,8 +1075,21 @@ class AareDAQ: self.__aare.upload_image(sample_id, filename, bgr_image) geom = self.sample_geometry - centre_coord = y1 + (y2 - y1)/2 - coord = geom.picture_to_smargon(Coordinate(x=x1, y=centre_coord)) + + if cls == 0: # loop_all + centre_y = y1 + (y2 - y1)/2 + centre_x = x1 + elif cls == 1: # pin + centre_y = y1 + (y2 - y1)/2 + centre_x = x1 + elif cls == 2 or cls == 3: #crystal or loop_face + centre_y = y1 + (y2 - y1)/2 + centre_x = x1 + (x2 - x1)/2 + else: + print(f"unknown box class {cls}") + return None + + coord = geom.picture_to_smargon(Coordinate(x=centre_x, y=centre_y)) return SmargonCoordinate(sh_mm=coord) def ml_bounding_box(self, sample_id: int | None = None, filename: str | None = None) -> RasterGridRequest | None: @@ -1142,6 +1156,207 @@ class AareDAQ: ) return SmargonCoordinate(sh_mm=coord) + def box_height_from_tuple(self, box: Tuple[float, float, float, float]) -> float: + x1, y1, x2, y2 = box + return abs(y2 - y1) + + def box_area_from_tuple(self, box: Tuple[float, float, float, float]) -> float: + x1, y1, x2, y2 = box + return abs(y2 - y1) * abs(x2-x1) + + def prepare_height_samples(self, boxes_by_angle: Dict[float, 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(): + if not area: + h = self.box_height_from_tuple(box) + else: + h = self.box_area_from_tuple(box) + samples.append((math.radians(deg), h)) + return samples + + def fit_area_vs_angle(self, areas_by_angle_deg: List[Tuple[float, float]]) -> Tuple[Callable[[float], float], float, dict]: + """ + Fit area(θ) = A + B*cos(θ - φ) using a linear fit on cos/sin terms. + Input: + areas_by_angle_deg: { angle_deg: area } + Returns: + (area_fn, best_angle_deg, params) + area_fn(theta_deg) -> predicted area + best_angle_deg: angle (deg) maximizing fitted curve (in [0, 360)) + params: {"A": A, "B": B, "phi_rad": phi} + """ + if not areas_by_angle_deg: + return (lambda _: 0.0, 0.0, {"A": 0.0, "B": 0.0, "phi_rad": 0.0}) + + samples = [(math.radians(deg), float(area)) for deg, area in areas_by_angle_deg] + + if len(samples) < 3: + # Fallback: constant model at mean, choose best measured angle + mean_area = sum(a for _, a in samples) / len(samples) + best_measured = max(areas_by_angle_deg, key=lambda kv: kv[1])[0] % 360 + return (lambda _: mean_area, best_measured, {"A": mean_area, "B": 0.0, "phi_rad": 0.0}) + + # Accumulate sums for normal equations + n = len(samples) + sum1 = n + sum_cos = sum(math.cos(t) for t, _ in samples) + sum_sin = sum(math.sin(t) for t, _ in samples) + sum_y = sum(y for _, y in samples) + sum_cos2 = sum((math.cos(t)) ** 2 for t, _ in samples) + sum_sin2 = sum((math.sin(t)) ** 2 for t, _ in samples) + sum_cossin = sum(math.cos(t) * math.sin(t) for t, _ in samples) + sum_ycos = sum(y * math.cos(t) for t, y in samples) + sum_ysin = sum(y * math.sin(t) for t, y in samples) + + # Solve for [A, C, S] in: + # [ n sum_cos sum_sin ] [A] = [ sum_y ] + # [ sum_cos sum_cos2 sum_cossin ] [C] [ sum_ycos] + # [ sum_sin sum_cossin sum_sin2 ] [S] [ sum_ysin] + def det3(m): + return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) + - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])) + + M = [ + [sum1, sum_cos, sum_sin], + [sum_cos, sum_cos2, sum_cossin], + [sum_sin, sum_cossin, sum_sin2], + ] + b = [sum_y, sum_ycos, sum_ysin] + + def replace_col(M, col, vec): + R = [row[:] for row in M] + for i in range(3): + R[i][col] = vec[i] + return R + + D = det3(M) or 1e-12 + A = det3(replace_col(M, 0, b)) / D + C = det3(replace_col(M, 1, b)) / D + S = det3(replace_col(M, 2, b)) / D + + B = math.hypot(C, S) + phi = math.atan2(S, C) # C = B cosφ, S = B sinφ + + def area_fn(theta_deg: float) -> float: + return A + B * math.cos(math.radians(theta_deg) - phi) + + # Best angle occurs at θ = φ (convert to degrees, normalize) + best_angle_deg = (math.degrees(phi)) % 360 + + return area_fn, best_angle_deg, {"A": A, "B": B, "phi_rad": phi} + + def fit_cosine_height(self, samples: List[Tuple[float, float]]) -> Tuple[float, float, float]: + """ + Fit h(θ) = A + C*cosθ + S*sinθ via linear least squares, then convert to A + B*cos(θ - φ). + Returns (A, B, phi) where phi in radians. + """ + if len(samples) < 3: + # fallback: constant model + A = sum(h for _, h in samples) / max(1, len(samples)) + return (A, 0.0, 0.0) + + # Build normal equations for [A, C, S] + sum1 = len(samples) + sum_cos = sum(math.cos(t) for t, _ in samples) + sum_sin = sum(math.sin(t) for t, _ in samples) + sum_h = sum(h for _, h in samples) + sum_cos2 = sum(math.cos(t) ** 2 for t, _ in samples) + sum_sin2 = sum(math.sin(t) ** 2 for t, _ in samples) + sum_cossin = sum(math.cos(t) * math.sin(t) for t, _ in samples) + sum_hcos = sum(h * math.cos(t) for t, h in samples) + sum_hsin = sum(h * math.sin(t) for t, h in samples) + + # Solve 3x3 linear system: + # [ sum1 sum_cos sum_sin ] [A] = [ sum_h ] + # [ sum_cos sum_cos2 sum_cossin ] [C] [ sum_hcos ] + # [ sum_sin sum_cossin sum_sin2 ] [S] [ sum_hsin ] + # Use Cramer's rule or a tiny solver since numpy may not be allowed externally. + def det3(m): + return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) + - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])) + + M = [ + [sum1, sum_cos, sum_sin], + [sum_cos, sum_cos2, sum_cossin], + [sum_sin, sum_cossin, sum_sin2], + ] + bA = [sum_h, sum_hcos, sum_hsin] + + # Matrices with columns replaced + def replace_col(M, col_idx, vec): + R = [row[:] for row in M] + for i in range(3): + R[i][col_idx] = vec[i] + return R + + D = det3(M) or 1e-12 + A = det3(replace_col(M, 0, bA)) / D + C = det3(replace_col(M, 1, bA)) / D + S = det3(replace_col(M, 2, bA)) / D + + # Convert A + C cosθ + S sinθ to A + B cos(θ - φ) + B = math.hypot(C, S) + phi = math.atan2(S, C) # since C = B cosφ, S = B sinφ + + return (A, B, phi) + + def __face_detection_sequence(self, zoom_value: int = 280): + self.__set_state(BeamlineStateEnum.SampleAlignment) + self.__devs.lamp_light = 2.5 + self.__cfg.zoom_mode = ZoomModeEnum.LoopCenter + zoom_settings = self.__cfg.zoom_settings.z + zoom_value = 280 + print('new loop') + exposure = zoom_settings[zoom_value].exposure + gain = zoom_settings[zoom_value].gain + self.__devs.samcam_settings = SampleCameraSettings(exposure=exposure, gain=gain) + self.__devs.zoom_sync(zoom_value) + boxes = {} + angles = (90, 75, 60, 45, 30, 15, 0, -15, -30, -45, -60, -75, -90) + for angle in angles: + self.__devs.aerotech.move(angle, wait=True) + curr_image = cv2.cvtColor(self.camera_image, cv2.COLOR_RGB2BGR) + box = self.__mlbox.predict(curr_image, "", pref_class = (3,0)) + if box: + cls, x1, y1, x2, y2 = box + if cls == 0: + #logger.info(f"loop all for angle {angle}") + boxes[angle] = (x1, y1, x2, y2) + + if not boxes: + print("no boxes found") + return + + samples = self.prepare_height_samples(boxes, area=True) + #logger.info(f"height vs angle samples: {samples}") + #area_fn, best_angle_deg, params = self.fit_area_vs_angle(samples) + A, B, phi = self.fit_cosine_height(samples) + #logger.info(f"cosine fit: A={A}, B={B}, phi={phi}") + #A, B, phi = params["A"], params["B"], params["phi_rad"] + + def height_deg(theta_deg: float) -> float: + return A + B * math.cos(math.radians(theta_deg) - phi) + + search_grid = range(-90, 90, 1) + best_fit_angle = max(search_grid, key=lambda d: height_deg(d)) + best_fit_height = height_deg(best_fit_angle) + + flat_face_angle = max(boxes.keys(), key=lambda a: self.box_height_from_tuple(boxes[a])) + flat_face_box = boxes[flat_face_angle] + #logger.info(f'Best fitted angle: {best_fit_angle}, fitted height: {best_fit_height:.3f}') + #logger.info(f'Flat face angle: {flat_face_angle}, box: {flat_face_box}') + + # Move to fitted best angle and return both measured and fitted info and predictor + self.__devs.aerotech.move(best_fit_angle, wait=True) + return + + #return flat_face_angle, flat_face_box + + def __loop_center_sequence(self, sample_id: int | None = None) -> bool: self.__set_state(BeamlineStateEnum.SampleAlignment) self.__devs.lamp_light = 2.5 @@ -1207,8 +1422,8 @@ class AareDAQ: else: if found >= 3 or targets_found_this_attempt >= 3: print(f"sucessfully found {found} or {targets_found_this_attempt} targets in {attempt} attempts") - return True - if found_flag and found_angle is not None: + break + if found_flag is not None and found_angle is not None: print(f"found a target at angle {found_angle} in attempt {attempt}") angles = (found_angle, found_angle + 45, found_angle + 90) attempt += 1 @@ -1218,6 +1433,7 @@ class AareDAQ: #i += 1 print("alc success") + self.__face_detection_sequence() return True except Exception as e: print(f"Error in loop centering: {e}") diff --git a/daq/src/aaredaq/mlbox.py b/daq/src/aaredaq/mlbox.py index 7451307c..5420363e 100644 --- a/daq/src/aaredaq/mlbox.py +++ b/daq/src/aaredaq/mlbox.py @@ -28,7 +28,7 @@ class MlBox: return response.json() @staticmethod - def __get_pred(results) -> None | Tuple[int, float, float, float, float]: + def __get_pred(results, pref_class = None) -> None | Tuple[int, float, float, float, float]: best_by_class: dict[int, tuple[float, float, float, float, float]] = {} for pred in results: @@ -46,8 +46,11 @@ class MlBox: if not best_by_class: return None - - for preferred_cls in (2, 3, 0, 1): + if pref_class: + classes = pref_class + else: + classes = (2, 3, 0, 1) + for preferred_cls in classes: if preferred_cls in best_by_class: x1, y1, x2, y2, _ = best_by_class[preferred_cls] return preferred_cls, x1, y1, x2, y2 @@ -69,15 +72,10 @@ class MlBox: return all_detections - def predict(self, image, filename: str | None = None) -> None | Tuple[int, float, float, float, float]: - print("running ml_box") - print("results:") + def predict(self, image, filename: str | None = None, pref_class = None) -> None | Tuple[int, float, float, float, float]: results = self.get_response(image) - print(results) - #results = self.__model.predict(source=image, conf=0.5) - #detections = self.get_all_detections(results.results) preds = results.get("results") if isinstance(results, dict) else None if not preds: return None - pred = self.__get_pred(preds) + pred = self.__get_pred(preds, pref_class) return pred \ No newline at end of file