303 lines
10 KiB
Python
303 lines
10 KiB
Python
import time
|
|
import json
|
|
import numpy as np
|
|
|
|
|
|
class RadiometricController:
|
|
def __init__(
|
|
self,
|
|
client,
|
|
enabled=True,
|
|
config_json_path=None,
|
|
interval_s=0.5,
|
|
strip_y0_pct=0.95,
|
|
strip_y1_pct=1.0,
|
|
patch_x0_pct=0.35,
|
|
patch_x1_pct=0.75,
|
|
target_mean=0.70,
|
|
deadband=0.03,
|
|
alpha=0.20,
|
|
exp_min_us=100,
|
|
exp_max_us=80000,
|
|
gain_min=1.0,
|
|
gain_max=8.0,
|
|
exp_step_gain=0.65,
|
|
prefer_exposure=True,
|
|
verbose=False,
|
|
):
|
|
self.client = client
|
|
|
|
cfg = self._load_config_json(config_json_path)
|
|
|
|
interval_s = cfg.get("interval_s", interval_s)
|
|
strip_y0_pct = cfg.get("strip_y0_pct", strip_y0_pct)
|
|
strip_y1_pct = cfg.get("strip_y1_pct", strip_y1_pct)
|
|
patch_x0_pct = cfg.get("patch_x0_pct", patch_x0_pct)
|
|
patch_x1_pct = cfg.get("patch_x1_pct", patch_x1_pct)
|
|
target_mean = cfg.get("target_mean", target_mean)
|
|
deadband = cfg.get("deadband", deadband)
|
|
alpha = cfg.get("alpha", alpha)
|
|
exp_min_us = cfg.get("exp_min_us", exp_min_us)
|
|
exp_max_us = cfg.get("exp_max_us", exp_max_us)
|
|
gain_min = cfg.get("gain_min", gain_min)
|
|
gain_max = cfg.get("gain_max", gain_max)
|
|
exp_step_gain = cfg.get("exp_step_gain", exp_step_gain)
|
|
prefer_exposure = cfg.get("prefer_exposure", prefer_exposure)
|
|
verbose = cfg.get("verbose", verbose)
|
|
exp_apply_threshold_us = cfg.get("exp_apply_threshold_us", 50)
|
|
gain_apply_threshold = cfg.get("gain_apply_threshold", 0.02)
|
|
|
|
self.enabled = bool(enabled)
|
|
self.interval_s = float(interval_s)
|
|
|
|
self.strip_y0_pct = float(strip_y0_pct)
|
|
self.strip_y1_pct = float(strip_y1_pct)
|
|
self.patch_x0_pct = float(patch_x0_pct)
|
|
self.patch_x1_pct = float(patch_x1_pct)
|
|
|
|
self.target_mean = float(target_mean)
|
|
self.deadband = float(deadband)
|
|
self.alpha = float(alpha)
|
|
|
|
self.exp_min_us = int(exp_min_us)
|
|
self.exp_max_us = int(exp_max_us)
|
|
self.gain_min = float(gain_min)
|
|
self.gain_max = float(gain_max)
|
|
|
|
self.exp_step_gain = float(exp_step_gain)
|
|
self.prefer_exposure = bool(prefer_exposure)
|
|
self.verbose = bool(verbose)
|
|
|
|
self.exp_apply_threshold_us = int(exp_apply_threshold_us)
|
|
self.gain_apply_threshold = float(gain_apply_threshold)
|
|
|
|
self.last_update_ts = 0.0
|
|
self.last_result = {}
|
|
|
|
self.state = {
|
|
"cam0": {"exp": 15000, "gain": 1.0},
|
|
"cam1": {"exp": 15000, "gain": 1.0},
|
|
"cam2": {"exp": 15000, "gain": 1.0},
|
|
}
|
|
self._ae_disabled = set()
|
|
self._last_applied = {
|
|
"cam0": {"exp": None, "gain": None},
|
|
"cam1": {"exp": None, "gain": None},
|
|
"cam2": {"exp": None, "gain": None},
|
|
}
|
|
|
|
def _load_config_json(self, path):
|
|
if not path:
|
|
return {}
|
|
|
|
try:
|
|
with open(path, "r", encoding="utf-8") as f:
|
|
data = json.load(f)
|
|
except Exception:
|
|
return {}
|
|
|
|
cfg = data.get("radiometric_config", {})
|
|
return cfg if isinstance(cfg, dict) else {}
|
|
|
|
def sync_from_camera_controls(self, camera_controls: dict | None):
|
|
if not isinstance(camera_controls, dict):
|
|
return
|
|
|
|
for cam_id, ctrl in camera_controls.items():
|
|
if cam_id not in self.state:
|
|
continue
|
|
|
|
exp = ctrl.get("exposure_time_us")
|
|
gain = ctrl.get("analogue_gain")
|
|
|
|
if exp is not None:
|
|
self.state[cam_id]["exp"] = int(exp)
|
|
|
|
if gain is not None:
|
|
self.state[cam_id]["gain"] = float(gain)
|
|
|
|
def update(self, decoded: dict, meta: dict | None = None):
|
|
if not self.enabled:
|
|
return None
|
|
|
|
now = time.perf_counter()
|
|
if now - self.last_update_ts < self.interval_s:
|
|
return None
|
|
|
|
self.last_update_ts = now
|
|
|
|
results = {}
|
|
|
|
for cam_id in ("cam2", "cam0", "cam1"):
|
|
if cam_id not in decoded:
|
|
continue
|
|
|
|
img = decoded[cam_id].get("image")
|
|
if img is None:
|
|
continue
|
|
|
|
metrics = self.measure_reference_patch(img)
|
|
decision = self.compute_control(cam_id, metrics)
|
|
apply_resp = self.apply_control(cam_id, decision)
|
|
|
|
results[cam_id] = {
|
|
"metrics": metrics,
|
|
"decision": decision,
|
|
"apply": apply_resp,
|
|
}
|
|
|
|
self.last_result = results
|
|
return results
|
|
|
|
def measure_reference_patch(self, img01: np.ndarray) -> dict:
|
|
if img01.ndim == 3:
|
|
# RGB: usa luminância simples
|
|
img_gray = (
|
|
0.299 * img01[:, :, 0] +
|
|
0.587 * img01[:, :, 1] +
|
|
0.114 * img01[:, :, 2]
|
|
).astype(np.float32)
|
|
else:
|
|
img_gray = img01.astype(np.float32)
|
|
|
|
h, w = img_gray.shape[:2]
|
|
|
|
y0 = int(h * self.strip_y0_pct)
|
|
y1 = int(h * self.strip_y1_pct)
|
|
x0 = int(w * self.patch_x0_pct)
|
|
x1 = int(w * self.patch_x1_pct)
|
|
|
|
y0 = max(0, min(h - 1, y0))
|
|
y1 = max(y0 + 1, min(h, y1))
|
|
x0 = max(0, min(w - 1, x0))
|
|
x1 = max(x0 + 1, min(w, x1))
|
|
|
|
patch = img_gray[y0:y1, x0:x1]
|
|
arr = patch.reshape(-1)
|
|
|
|
return {
|
|
"valid": arr.size > 0,
|
|
"mean": float(arr.mean()) if arr.size else 0.0,
|
|
"p05": float(np.percentile(arr, 5)) if arr.size else 0.0,
|
|
"p95": float(np.percentile(arr, 95)) if arr.size else 0.0,
|
|
"sat_pct": float((arr >= 0.98).mean() * 100.0) if arr.size else 0.0,
|
|
"dark_pct": float((arr <= 0.02).mean() * 100.0) if arr.size else 0.0,
|
|
"roi": [x0, y0, x1, y1],
|
|
}
|
|
|
|
def compute_control(self, cam_id: str, metrics: dict) -> dict:
|
|
st = self.state.setdefault(cam_id, {"exp": 15000, "gain": 1.0})
|
|
|
|
old_exp = int(st["exp"])
|
|
old_gain = float(st["gain"])
|
|
|
|
if not metrics.get("valid"):
|
|
return {
|
|
"action": "hold",
|
|
"reason": "patch inválido",
|
|
"old_exp": old_exp,
|
|
"new_exp": old_exp,
|
|
"old_gain": old_gain,
|
|
"new_gain": old_gain,
|
|
}
|
|
|
|
mean = float(metrics["mean"])
|
|
p95 = float(metrics["p95"])
|
|
sat_pct = float(metrics["sat_pct"])
|
|
error = self.target_mean - mean
|
|
|
|
new_exp = old_exp
|
|
new_gain = old_gain
|
|
action = "hold"
|
|
reason = "dentro da faixa morta"
|
|
|
|
# Proteção contra saturação
|
|
if sat_pct > 1.0 or p95 > 0.96:
|
|
desired_exp = max(self.exp_min_us, int(old_exp * 0.85))
|
|
new_exp = self._smooth_int(old_exp, desired_exp)
|
|
action = "decrease_exposure"
|
|
reason = f"saturação detectada: sat={sat_pct:.2f}% p95={p95:.3f}"
|
|
|
|
elif abs(error) > self.deadband:
|
|
factor = 1.0 + self.exp_step_gain * error
|
|
factor = max(0.70, min(1.35, factor))
|
|
|
|
if self.prefer_exposure:
|
|
desired_exp = int(old_exp * factor)
|
|
desired_exp = self._clamp(desired_exp, self.exp_min_us, self.exp_max_us)
|
|
new_exp = self._smooth_int(old_exp, desired_exp)
|
|
|
|
# Se exposição bateu limite e ainda precisa clarear/escurecer, mexe no ganho
|
|
if desired_exp in (self.exp_min_us, self.exp_max_us):
|
|
desired_gain = old_gain * factor
|
|
desired_gain = self._clamp(desired_gain, self.gain_min, self.gain_max)
|
|
new_gain = self._smooth_float(old_gain, desired_gain)
|
|
|
|
action = "increase_exposure" if error > 0 else "decrease_exposure"
|
|
reason = f"corrigindo erro radiométrico: error={error:.3f}"
|
|
else:
|
|
desired_gain = old_gain * factor
|
|
desired_gain = self._clamp(desired_gain, self.gain_min, self.gain_max)
|
|
new_gain = self._smooth_float(old_gain, desired_gain)
|
|
action = "increase_gain" if error > 0 else "decrease_gain"
|
|
reason = f"corrigindo ganho: error={error:.3f}"
|
|
|
|
new_exp = int(self._clamp(new_exp, self.exp_min_us, self.exp_max_us))
|
|
new_gain = float(self._clamp(new_gain, self.gain_min, self.gain_max))
|
|
|
|
return {
|
|
"action": action,
|
|
"reason": reason,
|
|
"mean": mean,
|
|
"target_mean": self.target_mean,
|
|
"error": error,
|
|
"old_exp": old_exp,
|
|
"new_exp": new_exp,
|
|
"old_gain": old_gain,
|
|
"new_gain": new_gain,
|
|
}
|
|
|
|
def apply_control(self, cam_id: str, decision: dict):
|
|
new_exp = int(decision["new_exp"])
|
|
new_gain = float(decision["new_gain"])
|
|
|
|
self.state[cam_id]["exp"] = new_exp
|
|
self.state[cam_id]["gain"] = new_gain
|
|
|
|
responses = {}
|
|
last = self._last_applied.setdefault(cam_id, {"exp": None, "gain": None})
|
|
|
|
try:
|
|
if cam_id not in self._ae_disabled:
|
|
responses["ae"] = self.client.svc.set_ae_enable(cam_id, False)
|
|
|
|
if cam_id == "cam2":
|
|
responses["awb"] = self.client.svc.set_awb_enable(cam_id, False)
|
|
|
|
self._ae_disabled.add(cam_id)
|
|
|
|
if last["exp"] is None or abs(new_exp - last["exp"]) >= self.exp_apply_threshold_us:
|
|
responses["exposure"] = self.client.svc.set_exposure_time(cam_id, new_exp)
|
|
last["exp"] = new_exp
|
|
|
|
if last["gain"] is None or abs(new_gain - last["gain"]) >= self.gain_apply_threshold:
|
|
responses["gain"] = self.client.svc.set_analogue_gain(cam_id, new_gain)
|
|
last["gain"] = new_gain
|
|
|
|
except Exception as e:
|
|
responses["error"] = str(e)
|
|
|
|
if self.verbose:
|
|
print(f"[RAD] {cam_id}: {json.dumps(decision, ensure_ascii=False)} | apply={responses}")
|
|
|
|
return responses
|
|
|
|
def _smooth_int(self, old, desired):
|
|
return int(round((1.0 - self.alpha) * old + self.alpha * desired))
|
|
|
|
def _smooth_float(self, old, desired):
|
|
return float((1.0 - self.alpha) * old + self.alpha * desired)
|
|
|
|
@staticmethod
|
|
def _clamp(v, lo, hi):
|
|
return max(lo, min(hi, v)) |