import os import time import json import math import ctypes as C from ctypes import wintypes as W from datetime import datetime import numpy as np import cv2 # ============================================================ # GAL5000 dataset capture + robust software auto-exposure # # This version is aligned with our working VT SDK findings: # - Uses ExposureRaw (0x3010) for exposure control. # - Uses Digital Gain Raw (0x302A) as the secondary control. # - Does NOT attempt to set Analog Gain via 0x3020 (known 4109). # - Reads initial values via GET when available, then tracks locally. # # Keys: # C / SPACE : save sample now # A : toggle auto-save # E : toggle auto-exposure # M : toggle preview upscale (speed/clarity) # Q / ESC : quit # When AE is OFF: # +/- : exposure +/- # [ ] : exposure fast +/- # V / X : digital gain +/- # ============================================================ # ========================= # CONFIG # ========================= SDK_DIR = os.path.join(os.path.dirname(__file__), "dlls") DLL_NAME = "VT_SDK64.dll" # Output dataset root OUT_ROOT = os.path.join(os.path.dirname(__file__), "dataset") SESSION_DIR = os.path.join(OUT_ROOT, datetime.now().strftime("%Y%m%d")) os.makedirs(SESSION_DIR, exist_ok=True) # Camera scan/open DEVICE_UDEF = 0 DEVICE_INDEX = 0 DATA_RAW = 0 # RAW geometry RAW_W = 2592 RAW_H = 2056 TIMEOUT_MS = 2000 WINDOW_NAME = "GAL5000 Dataset Capture (C/SPACE=save | A=auto-save | E=AE toggle | Q=quit)" # Preview UPSCALE = 2 # Auto-save CAPTURE_INTERVAL_S = 1.0 # ========================= # PARAM IDs # ========================= BUF_SIZE = 256 PARAM_ID_SENSOR_EXPOSURETIMERAW = 0x00003010 PARAM_ID_SENSOR_GAINANALOGRAW = 0x00003020 # GET may work; SET may fail (4109) PARAM_ID_SENSOR_GAINDIGITRAW = 0x0000302A # Optional SFNC info (may fail on some firmware) PARAM_ID_SFNC_SENSORWIDTH = 0x00001101 PARAM_ID_SFNC_SENSORHEIGHT = 0x00001102 PARAM_ID_SFNC_WIDTHMAX = 0x00001106 PARAM_ID_SFNC_HEIGHTMAX = 0x00001107 PARAM_ID_SFNC_WIDTH = 0x00001111 PARAM_ID_SFNC_HEIGHT = 0x00001112 PARAM_ID_SFNC_OFFSETX = 0x00001113 PARAM_ID_SFNC_OFFSETY = 0x00001114 # PARAM_VALUETYPE VALUE_INT = 0 VALUE_FLOAT = 1 VALUE_STR = 2 # ========================= # LIMITS # ========================= EXP_MIN = 1 EXP_MAX = 20000 EXP_STEP = 200 EXP_STEP_FAST = 1000 # Digital gain (we already confirmed GET/SET works) GAIN_D_MIN, GAIN_D_MAX = 0, 8 # keep conservative; expand if you verify bigger range GAIN_D_STEP = 1 # ROI for AE metrics (camera looks at ground) ROI_Y0_FRAC = 0.55 ROI_Y1_FRAC = 0.95 ROI_X0_FRAC = 0.15 ROI_X1_FRAC = 0.85 # AE targets TARGET_P95 = 140.0 DEADBAND = 6.0 SAT_LIMIT = 0.01 # AE controller tuning (log-domain multiplicative) K_LOG = 0.12 MAX_STEP = 0.10 EMA_ALPHA = 0.20 def clamp(v, lo, hi): return lo if v < lo else hi if v > hi else v def ts_name() -> str: return datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3] # ========================= # VT SDK structures # ========================= class VT_FRAMEINFO(C.Structure): _fields_ = [ ("lFrameID", W.DWORD), ("lBufSize", W.DWORD), ("lWidth", W.DWORD), ("lHeight", W.DWORD), ("lPixBits", C.c_ubyte), ("_pad0", C.c_ubyte * 3), ("pBufPtr", C.POINTER(C.c_ubyte)), ("lFrameStatus", W.DWORD), ("lPixType", W.DWORD), ("lTimeStamp", W.DWORD), ("_reserve", W.DWORD * 8), ] class VT_DEVPARAM(C.Structure): _fields_ = [ ("bUseName", W.BOOL), ("lParamByID", W.DWORD), ("lParamByName", C.c_char * BUF_SIZE), ] def devparam_by_id(pid: int) -> VT_DEVPARAM: p = VT_DEVPARAM() p.bUseName = False p.lParamByID = pid p.lParamByName = b"" return p # ========================= # DLL load + prototypes # ========================= os.add_dll_directory(SDK_DIR) dll = C.WinDLL(os.path.join(SDK_DIR, DLL_NAME)) print("DLL carregada OK:", dll) dll.VT_DeviceScan.argtypes = [C.POINTER(C.c_ubyte), C.c_int] dll.VT_DeviceScan.restype = C.c_int dll.VT_DeviceOpen.argtypes = [C.c_void_p, C.POINTER(W.HANDLE), C.c_int, C.c_int] dll.VT_DeviceOpen.restype = C.c_int dll.VT_SingleFrameCapture.argtypes = [W.HANDLE, C.POINTER(VT_FRAMEINFO), C.c_int, C.c_int, W.BOOL] dll.VT_SingleFrameCapture.restype = C.c_int dll.VT_DeviceClose.argtypes = [C.POINTER(W.HANDLE)] dll.VT_DeviceClose.restype = C.c_int dll.VT_ParamGetValue.argtypes = [W.HANDLE, VT_DEVPARAM, C.c_void_p, C.c_int] dll.VT_ParamGetValue.restype = C.c_int dll.VT_ParamSetValue.argtypes = [W.HANDLE, VT_DEVPARAM, C.c_void_p, C.c_int] dll.VT_ParamSetValue.restype = C.c_int def ck(ret: int, name: str): if ret != 0: raise RuntimeError(f"{name} falhou, ret={ret}") def param_get_int(h: W.HANDLE, pid: int) -> int: p = devparam_by_id(pid) v = C.c_int(0) ret = dll.VT_ParamGetValue(h, p, C.byref(v), VALUE_INT) ck(ret, f"VT_ParamGetValue({hex(pid)})") return int(v.value) def param_set_int(h: W.HANDLE, pid: int, value: int): p = devparam_by_id(pid) v = C.c_int(int(value)) ret = dll.VT_ParamSetValue(h, p, C.byref(v), VALUE_INT) ck(ret, f"VT_ParamSetValue({hex(pid)})") # ========================= # Capture + preview # ========================= def capture_raw8(h: W.HANDLE) -> np.ndarray: fi = VT_FRAMEINFO() ret = dll.VT_SingleFrameCapture(h, C.byref(fi), DATA_RAW, TIMEOUT_MS, True) ck(ret, "VT_SingleFrameCapture") w, hh = int(fi.lWidth), int(fi.lHeight) buf = C.string_at(fi.pBufPtr, fi.lBufSize) arr = np.frombuffer(buf, dtype=np.uint8) needed = w * hh if arr.size < needed: arr = np.pad(arr, (0, needed - arr.size), mode="constant", constant_values=0) arr = arr[:needed].reshape(hh, w) return arr def norm8(x, p_lo=2, p_hi=98): lo = np.percentile(x, p_lo) hi = np.percentile(x, p_hi) if hi <= lo + 1: return x.astype(np.uint8) y = (x.astype(np.float32) - lo) * (255.0 / (hi - lo)) return np.clip(y, 0, 255).astype(np.uint8) def make_rgb_preview(raw: np.ndarray, upscale=2) -> np.ndarray: # Pattern: # R G # IR B R = raw[0::2, 0::2] G = raw[0::2, 1::2] B = raw[1::2, 1::2] Rn, Gn, Bn = norm8(R), norm8(G), norm8(B) bgr = np.dstack([Bn, Gn, Rn]) if upscale and upscale != 1: bgr = cv2.resize( bgr, (bgr.shape[1] * upscale, bgr.shape[0] * upscale), interpolation=cv2.INTER_NEAREST, ) return bgr def overlay_hud(img_bgr, lines): y = 28 for s in lines: cv2.putText(img_bgr, s, (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 0), 3, cv2.LINE_AA) cv2.putText(img_bgr, s, (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) y += 28 # ========================= # AE metrics + controller # ========================= def measure_raw_g_metrics(raw: np.ndarray): """Measure p90/p95 and saturation ratio on RAW green channel within ROI.""" G = raw[0::2, 1::2] # H/2 x W/2 h2, w2 = G.shape y0, y1 = int(h2 * ROI_Y0_FRAC), int(h2 * ROI_Y1_FRAC) x0, x1 = int(w2 * ROI_X0_FRAC), int(w2 * ROI_X1_FRAC) roi = G[y0:y1, x0:x1] p90 = float(np.percentile(roi, 90)) p95 = float(np.percentile(roi, 95)) sat = float(np.mean(roi >= 250)) return p90, p95, sat class AEController: """Industrial-ish software AE (multiplicative in log space, with EMA + deadband). Primary actuator: ExposureRaw Secondary actuator (only when exp hits limits): DigitalGainRaw """ def __init__( self, exp_min=EXP_MIN, exp_max=EXP_MAX, target_p95=TARGET_P95, deadband=DEADBAND, k=K_LOG, max_step=MAX_STEP, ema_alpha=EMA_ALPHA, sat_limit=SAT_LIMIT, gain_d_min=GAIN_D_MIN, gain_d_max=GAIN_D_MAX, gain_d_step=GAIN_D_STEP, ): self.exp_min = exp_min self.exp_max = exp_max self.target = target_p95 self.deadband = deadband self.k = k self.max_step = max_step self.ema_alpha = ema_alpha self.sat_limit = sat_limit self.gain_d_min = gain_d_min self.gain_d_max = gain_d_max self.gain_d_step = gain_d_step self.p95_ema = None def reset(self): self.p95_ema = None def step(self, raw: np.ndarray, exp_raw: int, gain_d: int): p90, p95, sat = measure_raw_g_metrics(raw) # EMA if self.p95_ema is None: self.p95_ema = p95 else: self.p95_ema = (1.0 - self.ema_alpha) * self.p95_ema + self.ema_alpha * p95 e = self.target - self.p95_ema # deadband hold if abs(e) <= self.deadband and sat <= self.sat_limit: dbg = {"p90": p90, "p95": p95, "p95_ema": self.p95_ema, "sat": sat, "step": 0.0, "hold": True} return exp_raw, gain_d, dbg # compute multiplicative exposure step if sat > self.sat_limit: step = -min(self.max_step, 0.12) else: ratio = (self.target + 1e-6) / (self.p95_ema + 1e-6) step = self.k * math.log(ratio) step = clamp(step, -self.max_step, +self.max_step) new_exp = int(round(exp_raw * math.exp(step))) new_exp = int(clamp(new_exp, self.exp_min, self.exp_max)) new_gain_d = gain_d # Secondary: adjust digital gain only when exposure is saturated at limits if new_exp >= self.exp_max and self.p95_ema < (self.target - self.deadband): new_gain_d = int(clamp(gain_d + self.gain_d_step, self.gain_d_min, self.gain_d_max)) if new_exp <= self.exp_min and (self.p95_ema > (self.target + self.deadband) or sat > self.sat_limit): new_gain_d = int(clamp(gain_d - self.gain_d_step, self.gain_d_min, self.gain_d_max)) dbg = {"p90": p90, "p95": p95, "p95_ema": self.p95_ema, "sat": sat, "step": step, "hold": False} return new_exp, new_gain_d, dbg # ========================= # Saving # ========================= def save_sample(raw: np.ndarray, bgr_preview: np.ndarray, meta: dict): name = ts_name() raw_path = os.path.join(SESSION_DIR, f"{name}.raw") png_path = os.path.join(SESSION_DIR, f"{name}.png") json_path = os.path.join(SESSION_DIR, f"{name}.json") raw.tofile(raw_path) cv2.imwrite(png_path, bgr_preview) with open(json_path, "w", encoding="utf-8") as f: json.dump(meta, f, ensure_ascii=False, indent=2) return raw_path, png_path, json_path # ========================= # MAIN # ========================= def main(): # scan n = C.c_ubyte(0) ck(dll.VT_DeviceScan(C.byref(n), DEVICE_UDEF), "VT_DeviceScan") if n.value == 0: raise RuntimeError("Nenhuma câmera encontrada.") # open idx = C.c_ubyte(0) h = W.HANDLE() ck(dll.VT_DeviceOpen(C.byref(idx), C.byref(h), DEVICE_INDEX, DEVICE_UDEF), "VT_DeviceOpen") print("DeviceOpen OK, handle=", h.value) print("Saving to:", SESSION_DIR) # camera info (best-effort) try: sensor_w = param_get_int(h, PARAM_ID_SFNC_SENSORWIDTH) sensor_h = param_get_int(h, PARAM_ID_SFNC_SENSORHEIGHT) roi_w = param_get_int(h, PARAM_ID_SFNC_WIDTH) roi_h = param_get_int(h, PARAM_ID_SFNC_HEIGHT) roi_x = param_get_int(h, PARAM_ID_SFNC_OFFSETX) roi_y = param_get_int(h, PARAM_ID_SFNC_OFFSETY) width_max = param_get_int(h, PARAM_ID_SFNC_WIDTHMAX) height_max = param_get_int(h, PARAM_ID_SFNC_HEIGHTMAX) print(f"[CAM] sensor={sensor_w}x{sensor_h} roi={roi_w}x{roi_h}+{roi_x},{roi_y} max={width_max}x{height_max}") except Exception as e: print("[CAM] Info SFNC indisponível:", e) cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL) # local preview scale (avoid global mutation inside the loop) upscale = UPSCALE # Read initial values (best-effort) try: exp_raw = param_get_int(h, PARAM_ID_SENSOR_EXPOSURETIMERAW) except Exception: exp_raw = 1500 # gain_a only for metadata (may be readable, but we won't set it) try: gain_a = param_get_int(h, PARAM_ID_SENSOR_GAINANALOGRAW) except Exception: gain_a = 0 try: gain_d = param_get_int(h, PARAM_ID_SENSOR_GAINDIGITRAW) except Exception: gain_d = 2 print(f"[INIT] exp_raw={exp_raw} gainA(readonly?)={gain_a} gainD={gain_d}") # Apply initial exposure + digital gain (ignore failures gracefully) try: param_set_int(h, PARAM_ID_SENSOR_EXPOSURETIMERAW, int(exp_raw)) except Exception as e: print("[WARN] Falhou set exp inicial:", e) try: param_set_int(h, PARAM_ID_SENSOR_GAINDIGITRAW, int(gain_d)) except Exception as e: print("[WARN] Falhou set gainD inicial:", e) ae = AEController() ae_on = True auto_save = False last_auto_t = 0.0 # FPS t0 = time.time() frames = 0 fps = 0.0 last_msg = "" last_msg_t = 0.0 def set_exposure(new_exp: int) -> int: new_exp = int(clamp(int(new_exp), EXP_MIN, EXP_MAX)) param_set_int(h, PARAM_ID_SENSOR_EXPOSURETIMERAW, new_exp) return new_exp def set_gain_d(new_gain: int) -> int: new_gain = int(clamp(int(new_gain), GAIN_D_MIN, GAIN_D_MAX)) param_set_int(h, PARAM_ID_SENSOR_GAINDIGITRAW, new_gain) return new_gain try: while True: raw = capture_raw8(h) # best-effort refresh analog gain (read-only meta) try: gain_a = param_get_int(h, PARAM_ID_SENSOR_GAINANALOGRAW) except Exception: pass # software AE ae_dbg = {} if ae_on: new_exp, new_gain_d, ae_dbg = ae.step(raw, exp_raw, gain_d) if new_exp != exp_raw: try: exp_raw = set_exposure(new_exp) except Exception as e: print("[ERR] set exposure:", e) ae_on = False if new_gain_d != gain_d: try: gain_d = set_gain_d(new_gain_d) except Exception as e: print("[ERR] set gainD:", e) ae_on = False else: ae_dbg = {} # clean preview (this is what we save) rgb_clean = make_rgb_preview(raw, upscale=upscale) bgr = rgb_clean.copy() # FPS frames += 1 dt = time.time() - t0 if dt >= 1.0: fps = frames / dt frames = 0 t0 = time.time() # HUD p95_disp = ae_dbg.get("p95_ema", ae_dbg.get("p95", 0.0)) lines = [ f"AE: {'ON' if ae_on else 'OFF'} | AutoSave: {'ON' if auto_save else 'OFF'} | Interval: {CAPTURE_INTERVAL_S:.1f}s", f"exp_raw={exp_raw} gain_d={gain_d} (gain_a={gain_a}) | FPS={fps:.1f}", f"AEdbg: p95={p95_disp:.1f} sat={ae_dbg.get('sat', 0):.3f} hold={ae_dbg.get('hold', False)}", "Keys: C/SPACE=save | A=autosave | E=AE | M=toggle preview | Q quit", "(AE OFF): +/- exp | [ ] exp fast | V/C gainD", ] overlay_hud(bgr, lines) # post-save message if last_msg and (time.time() - last_msg_t) < 2.0: cv2.putText( bgr, last_msg, (12, bgr.shape[0] - 18), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA, ) cv2.imshow(WINDOW_NAME, bgr) # autosave now = time.time() if auto_save and (now - last_auto_t) >= CAPTURE_INTERVAL_S: meta = { "ts": datetime.now().isoformat(timespec="milliseconds"), "raw_w": RAW_W, "raw_h": RAW_H, "exp_raw": int(exp_raw), "gain_a": int(gain_a), "gain_d": int(gain_d), "ae_on": bool(ae_on), "ae_dbg": {k: (float(v) if isinstance(v, (int, float, np.floating)) else v) for k, v in ae_dbg.items()}, "note": "autosave", } raw_path, _, _ = save_sample(raw, rgb_clean, meta) last_msg = f"SAVED: {os.path.basename(raw_path)}" last_msg_t = now last_auto_t = now k = cv2.waitKey(1) & 0xFF if k in (ord("q"), ord("Q"), 27): break elif k in (ord("a"), ord("A")): auto_save = not auto_save last_msg = f"AutoSave -> {'ON' if auto_save else 'OFF'}" last_msg_t = time.time() elif k in (ord("e"), ord("E")): ae_on = not ae_on if ae_on: ae.reset() last_msg = f"AE -> {'ON' if ae_on else 'OFF'}" last_msg_t = time.time() elif k in (ord("m"), ord("M")): # quick toggle upscale (helps on slower PCs) upscale = 0 if upscale else 2 last_msg = f"Preview UPSCALE -> {upscale}" last_msg_t = time.time() elif k in (ord("c"), ord("C"), 32): # C or SPACE meta = { "ts": datetime.now().isoformat(timespec="milliseconds"), "raw_w": RAW_W, "raw_h": RAW_H, "exp_raw": int(exp_raw), "gain_a": int(gain_a), "gain_d": int(gain_d), "ae_on": bool(ae_on), "ae_dbg": {k2: (float(v2) if isinstance(v2, (int, float, np.floating)) else v2) for k2, v2 in ae_dbg.items()}, "note": "manual", } raw_path, _, _ = save_sample(raw, rgb_clean, meta) last_msg = f"SAVED: {os.path.basename(raw_path)}" last_msg_t = time.time() # manual controls only when AE is off elif not ae_on: if k in (ord("+"), ord("=")): exp_raw = int(clamp(exp_raw + EXP_STEP, EXP_MIN, EXP_MAX)) try: exp_raw = set_exposure(exp_raw) except Exception as e: print("[ERR] manual exp +:", e) elif k in (ord("-"), ord("_")): exp_raw = int(clamp(exp_raw - EXP_STEP, EXP_MIN, EXP_MAX)) try: exp_raw = set_exposure(exp_raw) except Exception as e: print("[ERR] manual exp -:", e) elif k == ord("]"): exp_raw = int(clamp(exp_raw + EXP_STEP_FAST, EXP_MIN, EXP_MAX)) try: exp_raw = set_exposure(exp_raw) except Exception as e: print("[ERR] manual exp fast +:", e) elif k == ord("["): exp_raw = int(clamp(exp_raw - EXP_STEP_FAST, EXP_MIN, EXP_MAX)) try: exp_raw = set_exposure(exp_raw) except Exception as e: print("[ERR] manual exp fast -:", e) elif k in (ord("v"), ord("V")): try: gain_d = set_gain_d(gain_d + GAIN_D_STEP) print(f"[MANUAL] GainD -> {gain_d}") except Exception as e: print("[ERR] manual gainD +:", e) elif k in (ord("c"), ord("C")): # Note: C is already save; we keep this branch unreachable. pass elif k in (ord("x"), ord("X")): # convenience: use X as gainD - (since C is save) try: gain_d = set_gain_d(gain_d - GAIN_D_STEP) print(f"[MANUAL] GainD -> {gain_d}") except Exception as e: print("[ERR] manual gainD -:", e) finally: try: ret = dll.VT_DeviceClose(C.byref(h)) if ret != 0: print("VT_DeviceClose retornou:", ret) except Exception as e: print("Erro ao fechar:", e) cv2.destroyAllWindows() print("Fim.") if __name__ == "__main__": main()