DAQ: updated face detection code and included more logging statements

This commit is contained in:
2025-10-24 17:30:54 +02:00
parent d83fedc7cd
commit d658fe4336
+163 -161
View File
@@ -1,6 +1,7 @@
import copy
import json
import math
import statistics
import time
from datetime import datetime
from math import ceil
@@ -12,6 +13,7 @@ import cv2
import numpy as np
import redis
from scipy import ndimage
from scipy.optimize import curve_fit
from aaredaq import workflows
from aaredaq.aaredb import AareWrapper
@@ -243,7 +245,7 @@ class AareDAQ:
self.__devs.smargon.move_home(wait=True)
logger.info("moving aerotech to mount position")
self.__devs.abr_pos = ABR_POS_MOUNT
time.sleep(0.5)
time.sleep(0.1)
logger.info("checking beamstop")
if self.__devs.bsz.value < 24.0:
raise Exception("Beamstop Z below 24.0 mm - potentially unsafe with mounting")
@@ -251,7 +253,7 @@ class AareDAQ:
logger.info(f"Checking smargon position: {self.__devs.smargon.readback}")
logger.info(f"Checking abr position: {self.__devs.abr_pos}")
if self.__devs.magnet_position_sensor.value != 0:
time.sleep(5)
time.sleep(1)
logger.warning("!!!!!!!!!!!!!!!!!!!!!MAGNET CONTROLLER BROKE AGAIN!!!!!!!!!!!!!!!!!!")
logger.debug(f"Checking magnet position sensor positon: {self.__devs.magnet_position_sensor_readout.value}")
logger.debug(f"Checking smargon position: {self.__devs.smargon.readback}")
@@ -298,7 +300,6 @@ class AareDAQ:
# reset zoom
self.zoom = 1
# mount sample
logger.info(f"Moving tell to unmount")
self.__aare.sample_unmounted(curr_sample)
logger.info(f"Moving tell to mount")
self.__devs.tell.mount(
@@ -911,148 +912,116 @@ class AareDAQ:
# 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))
v = self.box_area_from_tuple(box) if area else self.box_height_from_tuple(box)
samples.append((float(deg), v))
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})
@staticmethod
def _cos_model(theta_deg: float | np.ndarray, A: float, B: float, phi_rad: float):
return A + B * np.cos(np.deg2rad(theta_deg) - phi_rad)
samples = [(math.radians(deg), float(area)) for deg, area in areas_by_angle_deg]
@staticmethod
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]
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})
def fit_area_vs_angle(self, areas_by_angle_deg: List[Tuple[float, float]]) -> dict:
# 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)
pts = [(float(d), float(a)) for d, a in areas_by_angle_deg]
pts = self._mad_filter(pts, k=3.5)
degs = np.array([d for d, _ in pts], dtype=float)
ys = np.array([a for _, a in pts], dtype=float)
# 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]))
# Initial guess via linearized 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))
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φ
# Bounds
y_std = float(np.std(ys)) or 1.0
B_max = 10.0 * y_std
bounds = ([-np.inf, 0.0, -math.pi], [np.inf, B_max, math.pi])
def area_fn(theta_deg: float) -> float:
return A + B * math.cos(math.radians(theta_deg) - phi)
try:
popt, _ = curve_fit(self._cos_model, degs, ys, p0=[A0, B0, phi0], bounds=bounds, maxfev=10000)
A, B, phi = map(float, popt)
except Exception as e:
logger.info(f"error in curve fit {e}")
A, B, phi = A0, max(0.0, B0), phi0
# Best angle occurs at θ = φ (convert to degrees, normalize)
best_angle_deg = (math.degrees(phi)) % 360
#best_angle_deg = float((math.degrees(phi)) % 360)
return {"A": A, "B": max(0.0, B), "phi_rad": phi}
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.
"""
def fit_cosine_height(self, samples: List[Tuple[float, float]]) -> dict:
samples = self._mad_filter(samples, k=3.5)
if len(samples) < 3:
# fallback: constant model
A = sum(h for _, h in samples) / max(1, len(samples))
A = sum(y for _, y 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)
degs = np.array([d for d, _ in samples], dtype=float)
ys = np.array([y for _, y in samples], dtype=float)
# 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]))
# 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))
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]
# 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, B_max, math.pi])
# 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
try:
popt, _ = curve_fit(self._cos_model, degs, ys, p0=[A0, B0, phi0], bounds=bounds, maxfev=10000)
A, B, phi = map(float, popt)
B = max(0.0, B)
except Exception as e:
# Fallback to initial
logger.info(f"error in curve fit {e}")
return {"A": A, "B": max(0.0, B), "phi_rad": phi}
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
def face_detection(self) -> dict:
self.__cfg.try_set_busy(timeout=360)
try:
logger.info("running face detection sequence")
result = self.__face_detection_sequence()
except Exception as e:
logger.error(f"error in face detection sequence {e}")
result = {
"samples": None,
"height_fit": None,
"area_fit": None,
}
self.__cfg.state_busy = False
return result
# 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):
def __face_detection_sequence(self) -> dict:
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')
zoom_value = self.__devs.zoom
logger.info('face detection sequence')
exposure = zoom_settings[zoom_value].exposure
gain = zoom_settings[zoom_value].gain
self.__devs.samcam_settings = SampleCameraSettings(exposure=exposure, gain=gain)
@@ -1065,38 +1034,64 @@ class AareDAQ:
box = self.__mlbox.predict(curr_image, "", pref_class = (3,0))
if box:
cls, x1, y1, x2, y2 = box
boxes[angle] = (x1, y1, x2, y2)
if cls == 0:
#logger.info(f"loop all for angle {angle}")
boxes[angle] = (x1, y1, x2, y2)
logger.info(f'box added to boxes: {boxes}')
elif cls == 3:
boxes[angle] = (x1, y1, x2, y2)
else:
logger.debug(f"no loop found only class: {cls} pin or xtal")
else:
logger.info(f'no box found for angle {angle}')
if not boxes:
print("no boxes found")
return
logger.info(f'no boxes found: {boxes}')
logger.info('no boxes found')
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"]
return {
"samples": None,
"height_fit": None,
"area_fit": None,
}
def height_deg(theta_deg: float) -> float:
samples_area = self.prepare_height_samples(boxes, area=True)
area_params = self.fit_area_vs_angle(samples_area)
samples_height = self.prepare_height_samples(boxes, area=False)
height_params= self.fit_cosine_height(samples_height)
search_grid = np.linspace(-90, 90, 400)
def model(theta_deg: float, A:float, B: float, phi: 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)
best_fit_angle_area = max(search_grid, key=lambda d: model(d, area_params['A'], area_params['B'], area_params['phi_rad']))
best_fit_angle_height = max(search_grid, key=lambda d: model(d, height_params['A'], height_params['B'], height_params['phi_rad']))
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
logger.info(f"best angle by area: {best_fit_angle_area}")
logger.info(f"best angle by height: {best_fit_angle_height}")
candidates = [
a for a in (best_fit_angle_area, best_fit_angle_height)
if a is not None and a in search_grid
]
angle = candidates[0] if candidates else 0
self.__devs.aerotech.move(angle, wait=True)
# Move to fitted best angle and return both measured and fitted info and predictor
self.__devs.aerotech.move(best_fit_angle, wait=True)
return
# Build return JSON
samples_out = []
for deg, box in boxes.items():
h = self.box_height_from_tuple(box)
a = self.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 flat_face_angle, flat_face_box
return {
"samples": samples_out,
"height_fit": {"A": height_params["A"], "B": height_params["B"], "phi_rad": height_params["phi_rad"], "best_angle_deg": best_fit_angle_height},
"area_fit": {"A": area_params["A"], "B": area_params["B"], "phi_rad": area_params["phi_rad"], "best_angle_deg": best_fit_angle_area},
}
def __loop_center_sequence(self, sample_id: int | None = None) -> bool:
@@ -1112,17 +1107,15 @@ class AareDAQ:
max_attempt = 2
attempt = 0
found = 0
angles = (0, -45, -90)
angles = (0, -90)
if sample_id is not None:
self.save_screenshot_db(sample_id, f"pre_alc")
while attempt < max_attempt:
print('new loop center settings')
logger.debug('new loop center settings')
self.__devs.samcam_settings = SampleCameraSettings(exposure=exposure, gain=gain)
self.__devs.zoom_sync(zoom_value)
print(f'zoom={zoom_value},gain={gain}, exp={exposure}')
if sample_id is not None:
self.save_screenshot_db(sample_id, f"pre_alc")
print(sample_id)
logger.debug(f'zoom={zoom_value},gain={gain}, exp={exposure}')
found_flag = False
found_angle = None
@@ -1131,12 +1124,13 @@ class AareDAQ:
gn = int(gain)
for loop, angle in enumerate(angles):
print(f"Moving to new omega: {angle}")
if sample_id is not None and i == 0:
self.save_screenshot_db(sample_id, f"pre_alc_{sample_id}_{angle}_{zoom_value:.0f}_{exp}_{gn}")
logger.debug(f"Moving to new omega: {angle}")
# if sample_id is not None and i == 0:
# self.save_screenshot_db(sample_id, f"pre_alc_{sample_id}_{angle}_{zoom_value:.0f}_{exp}_{gn}")
time_to_move_aerotech= time.perf_counter()
self.__devs.aerotech.move(angle, wait=True)
time.sleep(1)
logger.info(f"time to move: {time.perf_counter()-time_to_move_aerotech}")
#time.sleep(0.1)
filename = None
if sample_id is not None:
filename = f"{sample_id}_{angle}_{zoom_value:.0f}_{exp}_{gn}"
@@ -1163,22 +1157,22 @@ 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")
logger.debug(f"sucessfully found {found} or {targets_found_this_attempt} targets in {attempt} attempts")
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}")
logger.debug(f"found a target at angle {found_angle} in attempt {attempt}")
angles = (found_angle, found_angle + 45, found_angle + 90)
attempt += 1
print(f"attempt {attempt} of {max_attempt}")
attempt += 1
logger.debug(f"attempt {attempt} of {max_attempt}")
if attempt >= max_attempt:
raise LoopCenteringFailed
#i += 1
print("alc success")
self.__face_detection_sequence()
logger.debug("alc success")
#self.face_detection_sequence()
return True
except Exception as e:
print(f"Error in loop centering: {e}")
logger.info(f"Error in loop centering: {e}")
return False
def auto_loop_center(self, sample_id: int | None = None) -> float:
@@ -1236,10 +1230,16 @@ class AareDAQ:
)
try:
logger.info(f"setting busy at {time.perf_counter() - start}")
self.__cfg.try_set_busy(timeout=360)
logger.info(f"set busy at {time.perf_counter() - start}")
geom = self.sample_geometry
print(f"{time.ctime()} starting mount {sample.db_id}")
start_mount=time.perf_counter()
logger.info(f"starting mount {sample.db_id} at {time.ctime()}")
self.__mount(sample)
logger.info(f"mounting done at {time.perf_counter() - start_mount}, total time: {time.perf_counter() - start}")
alc_time = time.perf_counter() - start
logger.info(f"starting alc at {alc_time}")
if not self.__loop_center_sequence(sample.db_id):
self.__aare.alc_failed(sample)
print("alc failed")
@@ -1247,7 +1247,9 @@ class AareDAQ:
end = time.perf_counter()
return end - start
#raise LoopCenteringFailed
logger.info(f"alc done at {time.perf_counter() - start}")
self.__face_detection_sequence()
logger.info(f"face_detection done at {time.perf_counter() - start}")
self.zoom = 500
time.sleep(0.5)