From 8bbe6c497861f8cb19bca3207b6666aaccfd5a47 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Fri, 24 Apr 2026 16:57:43 -0300 Subject: [PATCH] Adicionado controle AE radiometrico --- .../datasets/multiespec_module/_0_capture.py | 18 +- .../multiespec_module/_9_test_segformer.py | 11 +- .../multiespec_module/module_params.json | 17 + .../multiespec_module/multispectral_client.py | 38 +++ .../radiometric_controller.py | 303 ++++++++++++++++++ Python/raspi/build_module_params.py | 30 ++ Python/raspi/calibration/module_params.json | 19 +- Python/raspi/cam_3/multispectral_client.py | 123 +++++++ Python/raspi/cam_3/pi/raw_processor_core.py | 43 ++- Python/raspi/cam_3/radiometric_controller.py | 303 ++++++++++++++++++ Python/raspi/manual_fusion_calibrator.py | 3 +- Python/raspi/sensor_calibration_tool.py | 4 +- 12 files changed, 899 insertions(+), 13 deletions(-) create mode 100644 Python/OAK/datasets/multiespec_module/radiometric_controller.py create mode 100644 Python/raspi/cam_3/radiometric_controller.py diff --git a/Python/OAK/datasets/multiespec_module/_0_capture.py b/Python/OAK/datasets/multiespec_module/_0_capture.py index b90032496..53bf9cbef 100644 --- a/Python/OAK/datasets/multiespec_module/_0_capture.py +++ b/Python/OAK/datasets/multiespec_module/_0_capture.py @@ -154,6 +154,7 @@ def main(): parser.add_argument("--capture_mode", default="AUTO", choices=["AUTO", "SINGLE", "DOUBLE", "TRIPLE"], help="Modo de captura desejado no módulo.") parser.add_argument("--raw_policy", default="allow_single", choices=["allow_single", "require_triple"], help="Quando frame_type=RAW_BRUTO, define se o script aceita 1 câmera ou exige 3.") parser.add_argument("--module_calibration_json", default=MODULE_PARAMS, help="JSON salvo pelo calibrador de sensores com parâmetros fixos por câmera.") + parser.add_argument("--radiometric_ae", action="store_true", help="Liga controle automatico de exposicao radiometrico") args = parser.parse_args() @@ -223,11 +224,12 @@ def main(): capture_mode=effective_capture_mode, raw_policy=args.raw_policy, module_calibration_json=args.module_calibration_json, + radiometric_enabled=args.radiometric_ae, ) as cam: while True: t0 = time.time() - frame, meta = cam.get_next_frame(timeout=1.0) + frame, meta, decoded = cam.get_next_decoded(timeout=1.0) if meta is not None and frame is not None and meta.get("frame_id") != last_frame_id: last_frame_id = meta["frame_id"] @@ -336,6 +338,19 @@ def main(): t_view_fps = time.time() active_sources = meta.get("payload_sources") + + rad = getattr(cam, "radiometric_controller", None) + if rad and rad.enabled: + st = rad.state + line_rad = ( + f"RAD | " + f"RGB(exp={st['cam2']['exp']}, g={st['cam2']['gain']:.2f}) | " + f"RE(exp={st['cam0']['exp']}, g={st['cam0']['gain']:.2f}) | " + f"NIR(exp={st['cam1']['exp']}, g={st['cam1']['gain']:.2f})" + ) + else: + line_rad = "RAD | OFF" + lines = [ f"CANA: {args.cana} | HORA: {args.horario} | Pasta: {os.path.basename(session_dir)}", f"Type={meta.get('frame_type')} | CaptureMode={effective_capture_mode} | RAW policy={args.raw_policy}", @@ -343,6 +358,7 @@ def main(): f"frame_id={meta.get('frame_id')} | layout={meta.get('output_layout')} | dtype={meta.get('dtype') or meta.get('output_dtype')}", f"codec={meta.get('codec_name', meta.get('codec_family', '-'))} | comp={meta.get('dt_comp', 0):.4f}s | send={meta.get('dt_send_payload_prev', 0):.4f}s", f"CAM_PARAMS={os.path.basename(args.module_calibration_json)} | controles fixos aplicados", + line_rad, "Keys: C/SPACE=save | A=auto-save | M=preview | Q/Esc=quit" ] overlay_hud(preview_show, lines, base_h=raw_h) diff --git a/Python/OAK/datasets/multiespec_module/_9_test_segformer.py b/Python/OAK/datasets/multiespec_module/_9_test_segformer.py index b7ce9a612..3b4521f9d 100644 --- a/Python/OAK/datasets/multiespec_module/_9_test_segformer.py +++ b/Python/OAK/datasets/multiespec_module/_9_test_segformer.py @@ -97,6 +97,7 @@ def main(): parser.add_argument("--camera_frame_type", type=str, default="RAW_BRUTO", choices=["RAW_BRUTO", "RGB", "MULTISPEC"]) parser.add_argument("--camera_capture_mode", type=str, default="AUTO", choices=["AUTO", "SINGLE", "DOUBLE", "TRIPLE"]) parser.add_argument("--module_calibration_json", default=None, help="JSON salvo pelo calibrador de sensores com parâmetros fixos por câmera.") + parser.add_argument("--radiometric_ae", action="store_true", help="Liga controle automatico de exposicao radiometrico") args = parser.parse_args() @@ -227,9 +228,10 @@ def main(): capture_mode=args.camera_capture_mode, raw_policy="allow_single", module_calibration_json=MODULE_PARAMS, + radiometric_enabled=args.radiometric_ae ) as cam: while True: - frame, meta = cam.get_next_frame(timeout=0.5) + frame, meta, decoded = cam.get_next_decoded(0.5) if meta is None or frame is None: continue @@ -262,7 +264,12 @@ def main(): fps_pi = (1.0 - fps_smooth) * fps_pi + fps_smooth * inst_fps_pi try: - raw_np = cam.build_infer_tensor(frame, meta, channels_expected=CHANNELS, target_size=(W, H)) + raw_np = cam.build_infer_tensor_from_decoded( + decoded, + meta, + channels_expected=CHANNELS, + target_size=(W, H), + ) # ========================================================= # INFERÊNCIA diff --git a/Python/OAK/datasets/multiespec_module/module_params.json b/Python/OAK/datasets/multiespec_module/module_params.json index 6196e60fe..efd1aeaa1 100644 --- a/Python/OAK/datasets/multiespec_module/module_params.json +++ b/Python/OAK/datasets/multiespec_module/module_params.json @@ -73,5 +73,22 @@ "crop_valid_common": true, "resize_after_crop": true, "target_size": null + }, + "radiometric_config": { + "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.18, + "exp_min_us": 100, + "exp_max_us": 80000, + "gain_min": 1.0, + "gain_max": 8.0, + "verbose": true, + "exp_apply_threshold_us": 50, + "gain_apply_threshold": 0.02 } } \ No newline at end of file diff --git a/Python/OAK/datasets/multiespec_module/multispectral_client.py b/Python/OAK/datasets/multiespec_module/multispectral_client.py index a67ffe5cb..6544b9fc2 100644 --- a/Python/OAK/datasets/multiespec_module/multispectral_client.py +++ b/Python/OAK/datasets/multiespec_module/multispectral_client.py @@ -7,6 +7,7 @@ from multispectral_service import MultiSpectralService from stream_receiver import StreamReceiver from pi.raw_processor_core import RawProcessorCore from pi.raw_processor_preview import RawProcessorPreview +from radiometric_controller import RadiometricController class MultiSpectralClient: @@ -26,6 +27,7 @@ class MultiSpectralClient: capture_mode="AUTO", raw_policy="allow_single", module_calibration_json=None, + radiometric_enabled=False, ): self.pi_host = pi_host self.pc_host = pc_host @@ -71,6 +73,9 @@ class MultiSpectralClient: self.begin_resp = None self.applied_params = None + self.radiometric_enabled = bool(radiometric_enabled) + self.radiometric_controller = None + def __enter__(self): self.start() return self @@ -98,8 +103,27 @@ class MultiSpectralClient: self._apply_module_params(print_debug=print_debug) self._start_stream(print_debug=print_debug) + if self.radiometric_enabled: + self.enable_radiometric_controller() + return self + def enable_radiometric_controller(self): + self.radiometric_controller = RadiometricController( + client=self, + enabled=True, + config_json_path=self.module_calibration_json, + ) + self.radiometric_controller.sync_from_camera_controls(self.applied_params["camera_settings"]) + + return self.radiometric_controller + + def update_radiometry(self, decoded, meta=None): + if self.radiometric_controller is None: + return None + + return self.radiometric_controller.update(decoded, meta) + def _configure_module(self, print_debug=True): r0 = self.svc.set_resolution(self.width, self.height) r1 = self.svc.set_bayer(self.bayer) @@ -190,6 +214,15 @@ class MultiSpectralClient: raise TimeoutError("Timeout aguardando novo frame do stream.") + def get_next_decoded(self, timeout=2.0, update_radiometry=True): + frame, meta = self.get_next_frame(timeout=timeout) + decoded = self.core.decode_stream_cameras(frame, meta) + + if update_radiometry: + self.update_radiometry(decoded, meta) + + return frame, meta, decoded + def build_infer_tensor(self, frame, meta, channels_expected, target_size=None): return self.core.build_infer_tensor_from_stream( frame, @@ -198,6 +231,10 @@ class MultiSpectralClient: target_size=target_size, ) + def build_infer_tensor_from_decoded(self, decoded, meta, channels_expected, target_size=None): + tensor = self.core.fuse_multispec_cameras(decoded, meta, channels_expected) + return self.core.resize_tensor_chw(tensor, target_size=target_size) + def build_preview_from_raw_payload(self, frame, meta: dict): """ Gera preview priorizando a câmera RGB (cam2). @@ -282,6 +319,7 @@ class MultiSpectralClient: raise RuntimeError(f"Tipo de frame não suportado para preview: {type(frame)}") + def stop(self): try: self.svc.stop_stream() diff --git a/Python/OAK/datasets/multiespec_module/radiometric_controller.py b/Python/OAK/datasets/multiespec_module/radiometric_controller.py new file mode 100644 index 000000000..7514d42e0 --- /dev/null +++ b/Python/OAK/datasets/multiespec_module/radiometric_controller.py @@ -0,0 +1,303 @@ +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)) \ No newline at end of file diff --git a/Python/raspi/build_module_params.py b/Python/raspi/build_module_params.py index 88c5f1d55..57be6ddab 100644 --- a/Python/raspi/build_module_params.py +++ b/Python/raspi/build_module_params.py @@ -16,16 +16,45 @@ def main(): parser = argparse.ArgumentParser() parser.add_argument("--camera_json", default="calibration/sensor_calibration.json") parser.add_argument("--fusion_json", default="calibration/manual_offsets.json") + parser.add_argument("--radiometric_json", default="") parser.add_argument("--out", default="calibration/module_params.json") args = parser.parse_args() cam_data = load_json(args.camera_json) fusion_data = load_json(args.fusion_json) + radiometric_data = {} + if args.radiometric_json: + radiometric_data = load_json(args.radiometric_json) + camera_settings = cam_data.get("camera_settings") if not isinstance(camera_settings, dict): raise RuntimeError("camera_json sem camera_settings válido") + radiometric_config = radiometric_data.get("radiometric_config") + + if not isinstance(radiometric_config, dict): + radiometric_config = cam_data.get("radiometric_config") + + if not isinstance(radiometric_config, dict): + radiometric_config = { + "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.18, + "exp_min_us": 100, + "exp_max_us": 80000, + "gain_min": 1.0, + "gain_max": 8.0, + "verbose": True, + "exp_apply_threshold_us": 50, + "gain_apply_threshold": 0.02, + } + fusion_config = { "alignment_mode": fusion_data.get("alignment_mode", "manual_affine"), "baseline_mm": fusion_data.get("baseline_mm", 75.0), @@ -52,6 +81,7 @@ def main(): "camera_settings": camera_settings, "fusion_config": fusion_config, + "radiometric_config": radiometric_config, } with open(args.out, "w", encoding="utf-8") as f: diff --git a/Python/raspi/calibration/module_params.json b/Python/raspi/calibration/module_params.json index 6196e60fe..b36f8dc0a 100644 --- a/Python/raspi/calibration/module_params.json +++ b/Python/raspi/calibration/module_params.json @@ -1,6 +1,6 @@ { "schema": "multispec_module_params_v1", - "saved_at": "2026-04-24 10:46:59", + "saved_at": "2026-04-24 16:44:52", "frame_type": "RAW_BRUTO", "capture_mode_requested": "AUTO", "capture_mode_effective": "AUTO", @@ -73,5 +73,22 @@ "crop_valid_common": true, "resize_after_crop": true, "target_size": null + }, + "radiometric_config": { + "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.7, + "deadband": 0.03, + "alpha": 0.18, + "exp_min_us": 100, + "exp_max_us": 80000, + "gain_min": 1.0, + "gain_max": 8.0, + "verbose": true, + "exp_apply_threshold_us": 50, + "gain_apply_threshold": 0.02 } } \ No newline at end of file diff --git a/Python/raspi/cam_3/multispectral_client.py b/Python/raspi/cam_3/multispectral_client.py index e7cd1c806..ea1304f87 100644 --- a/Python/raspi/cam_3/multispectral_client.py +++ b/Python/raspi/cam_3/multispectral_client.py @@ -1,10 +1,13 @@ import time import json +import numpy as np + from cam_3.multispectral_service import MultiSpectralService from cam_3.stream_receiver import StreamReceiver from cam_3.pi.raw_processor_core import RawProcessorCore from cam_3.pi.raw_processor_preview import RawProcessorPreview +from cam_3.radiometric_controller import RadiometricController class MultiSpectralClient: @@ -24,6 +27,7 @@ class MultiSpectralClient: capture_mode="AUTO", raw_policy="allow_single", module_calibration_json=None, + radiometric_enabled=False, ): self.pi_host = pi_host self.pc_host = pc_host @@ -69,6 +73,9 @@ class MultiSpectralClient: self.begin_resp = None self.applied_params = None + self.radiometric_enabled = bool(radiometric_enabled) + self.radiometric_controller = None + def __enter__(self): self.start() return self @@ -96,8 +103,27 @@ class MultiSpectralClient: self._apply_module_params(print_debug=print_debug) self._start_stream(print_debug=print_debug) + if self.radiometric_enabled: + self.enable_radiometric_controller() + return self + def enable_radiometric_controller(self): + self.radiometric_controller = RadiometricController( + client=self, + enabled=True, + config_json_path=self.module_calibration_json, + ) + self.radiometric_controller.sync_from_camera_controls(self.applied_params["camera_settings"]) + + return self.radiometric_controller + + def update_radiometry(self, decoded, meta=None): + if self.radiometric_controller is None: + return None + + return self.radiometric_controller.update(decoded, meta) + def _configure_module(self, print_debug=True): r0 = self.svc.set_resolution(self.width, self.height) r1 = self.svc.set_bayer(self.bayer) @@ -188,6 +214,15 @@ class MultiSpectralClient: raise TimeoutError("Timeout aguardando novo frame do stream.") + def get_next_decoded(self, timeout=2.0, update_radiometry=True): + frame, meta = self.get_next_frame(timeout=timeout) + decoded = self.core.decode_stream_cameras(frame, meta) + + if update_radiometry: + self.update_radiometry(decoded, meta) + + return frame, meta, decoded + def build_infer_tensor(self, frame, meta, channels_expected, target_size=None): return self.core.build_infer_tensor_from_stream( frame, @@ -196,6 +231,94 @@ class MultiSpectralClient: target_size=target_size, ) + def build_infer_tensor_from_decoded(self, decoded, meta, channels_expected, target_size=None): + tensor = self.core.fuse_multispec_cameras(decoded, meta, channels_expected) + return self.core.resize_tensor_chw(tensor, target_size=target_size) + + def build_preview_from_raw_payload(self, frame, meta: dict): + """ + Gera preview priorizando a câmera RGB (cam2). + Se cam2 não estiver presente, cai para fallback usando a primeira câmera mono disponível. + Retorna: + preview_bgr + payload_float_preview + preview_source_id + """ + payload_sources = meta.get("payload_sources", []) or [] + + # Caso multi-payload: tenta usar cam2 primeiro + if isinstance(frame, dict): + if "cam2" in frame: + rgb_frame = frame["cam2"] + + if rgb_frame.ndim != 3 or rgb_frame.shape[2] != 3: + raise RuntimeError(f"cam2 recebida mas inválida para preview RGB: shape={rgb_frame.shape}") + + preview_bgr = rgb_frame.copy() + payload_float = rgb_frame[:, :, ::-1].astype(np.float32) / 255.0 + payload_float = np.transpose(payload_float, (2, 0, 1)) + + return preview_bgr, payload_float, "cam2" + + # fallback: usa a primeira câmera mono disponível + fallback_id = None + for cid in ("cam0", "cam1"): + if cid in frame: + fallback_id = cid + break + + if fallback_id is None: + raise RuntimeError("Nenhuma câmera disponível no payload para gerar preview") + + packed = frame[fallback_id] + if packed.ndim == 3 and packed.shape[2] == 1: + packed = packed[:, :, 0] + + cam_frames = meta.get("camera_frames", {}) or {} + cam_meta = cam_frames.get(fallback_id, {}) + bit_depth = int(cam_meta.get("bit_depth", 10)) + + raw16 = self.core.unpack_raw10_packed(packed) + preview_bgr = self.preview.raw16_to_preview_bgr(raw16, bit_depth=bit_depth) + + payload_float = self.core.build_training_rgb( + raw16, + output_dtype="float32", + bit_depth=bit_depth, + ) + + return preview_bgr, payload_float, fallback_id + + # Caso single-payload + if isinstance(frame, np.ndarray): + # Se vier HWC/3ch, tratamos como RGB USB + if frame.ndim == 3 and frame.shape[2] == 3: + preview_bgr = frame.copy() + payload_float = frame[:, :, ::-1].astype(np.float32) / 255.0 + payload_float = np.transpose(payload_float, (2, 0, 1)) + return preview_bgr, payload_float, "cam2" + + # Se vier mono packed, fallback antigo + packed = frame + if packed.ndim == 3 and packed.shape[2] == 1: + packed = packed[:, :, 0] + + source_camera = meta.get("source_camera") or {} + bit_depth = int(source_camera.get("bit_depth", meta.get("source_bit_depth", 10))) + + raw16 = self.core.unpack_raw10_packed(packed) + preview_bgr = self.preview.raw16_to_preview_bgr(raw16, bit_depth=bit_depth) + + payload_float = self.core.build_training_rgb( + raw16, + output_dtype="float32", + bit_depth=bit_depth, + ) + + return preview_bgr, payload_float, source_camera.get("id", "unknown") + + raise RuntimeError(f"Tipo de frame não suportado para preview: {type(frame)}") + def stop(self): try: self.svc.stop_stream() diff --git a/Python/raspi/cam_3/pi/raw_processor_core.py b/Python/raspi/cam_3/pi/raw_processor_core.py index 2c6707f9f..c8e3b56ef 100644 --- a/Python/raspi/cam_3/pi/raw_processor_core.py +++ b/Python/raspi/cam_3/pi/raw_processor_core.py @@ -184,14 +184,22 @@ class RawProcessorCore: return decoded - def build_multispectral_tensor(self, bins_data, bins_meta): + def build_multispectral_tensor(self, bins_data, bins_meta, target_size=None): decoded = self.decode_bins_cameras(bins_data, bins_meta) if "cam2" not in decoded: raise RuntimeError("RGB obrigatório") channel_names = self._channel_names_from_decoded(decoded) - tensor = self.fuse_multispec_cameras(decoded, meta=None, channels_expected=len(channel_names)) + + tensor = self.fuse_multispec_cameras( + decoded, + meta=None, + channels_expected=len(channel_names) + ) + + tensor = self.resize_tensor_chw(tensor, target_size=target_size) + return tensor, channel_names def build_infer_tensor_from_stream_old(self, frame, meta, channels_expected): @@ -309,15 +317,17 @@ class RawProcessorCore: raise RuntimeError(f"frame_type não suportado para inferência: {frame_type}") - def build_infer_tensor_from_stream(self, frame, meta, channels_expected): + def build_infer_tensor_from_stream(self, frame, meta, channels_expected, target_size=None): frame_type = meta.get("frame_type") if frame_type == "RAW_BRUTO": decoded = self.decode_stream_cameras(frame, meta) - return self.fuse_multispec_cameras(decoded, meta, channels_expected) + tensor = self.fuse_multispec_cameras(decoded, meta, channels_expected) + return self.resize_tensor_chw(tensor, target_size=target_size) if frame_type in ("RGB", "MULTISPEC"): - return self.build_infer_tensor_from_stream_old(frame, meta, channels_expected) + tensor = self.build_infer_tensor_from_stream_old(frame, meta, channels_expected) + return self.resize_tensor_chw(tensor, target_size=target_size) raise RuntimeError(f"frame_type não suportado para inferência: {frame_type}") @@ -554,6 +564,29 @@ class RawProcessorCore: return resized + def resize_tensor_chw(self, tensor, target_size=None): + if target_size is None: + return tensor + + target_w, target_h = target_size + + if tensor.ndim != 3: + raise RuntimeError(f"Tensor esperado em CHW. Veio shape={tensor.shape}") + + _, h, w = tensor.shape + + if (w, h) == (target_w, target_h): + return tensor.astype(np.float32, copy=False) + + interp = cv2.INTER_AREA if target_w < w or target_h < h else cv2.INTER_LINEAR + + chans = [] + for ch in tensor: + ch_res = cv2.resize(ch, (target_w, target_h), interpolation=interp) + chans.append(ch_res.astype(np.float32)) + + return np.stack(chans, axis=0) + def extract_camera_meta(self, meta_json: dict, cam_id: str) -> dict: cam_frames = meta_json.get("camera_frames", {}) or meta_json.get("stream_meta", {}).get("camera_frames", {}) diff --git a/Python/raspi/cam_3/radiometric_controller.py b/Python/raspi/cam_3/radiometric_controller.py new file mode 100644 index 000000000..7514d42e0 --- /dev/null +++ b/Python/raspi/cam_3/radiometric_controller.py @@ -0,0 +1,303 @@ +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)) \ No newline at end of file diff --git a/Python/raspi/manual_fusion_calibrator.py b/Python/raspi/manual_fusion_calibrator.py index 3911a2bfc..56cc47842 100644 --- a/Python/raspi/manual_fusion_calibrator.py +++ b/Python/raspi/manual_fusion_calibrator.py @@ -380,7 +380,7 @@ def main(): while True: t0 = time.time() - frame, meta = cam.get_next_frame(timeout=2.0) + frame, meta, decoded = cam.get_next_decoded(timeout=2.0) if meta is not None and frame is not None and meta.get("frame_id") != last_frame_id: last_frame_id = meta["frame_id"] @@ -388,7 +388,6 @@ def main(): if not isinstance(frame, dict): raise RuntimeError("Este calibrador espera RAW_BRUTO multi-payload como dict de câmeras.") - decoded = cam.core.decode_stream_cameras(frame, meta) decoded_last = decoded curr_frame_id = meta.get("frame_id") diff --git a/Python/raspi/sensor_calibration_tool.py b/Python/raspi/sensor_calibration_tool.py index 1939c817e..b22b57cf5 100644 --- a/Python/raspi/sensor_calibration_tool.py +++ b/Python/raspi/sensor_calibration_tool.py @@ -1003,7 +1003,7 @@ def main(): t0 = time.time() if live_mode: - frame, meta = cam.get_next_frame(timeout=2.0) + frame, meta, decoded = cam.get_next_decoded(timeout=2.0) if meta is not None and frame is not None and meta.get("frame_id") != last_frame_id: last_frame_id = meta["frame_id"] @@ -1011,7 +1011,7 @@ def main(): if not isinstance(frame, dict): raise RuntimeError("Este calibrador espera RAW_BRUTO multi-payload como dict de câmeras.") - decoded_last = cam.core.decode_stream_cameras(frame, meta) + decoded_last = decoded last_meta_stream = dict(meta) last_raw_frame = {cam_id: arr.copy() for cam_id, arr in frame.items()}