893 lines
28 KiB
Python
893 lines
28 KiB
Python
# gal5000_camera.py
|
|
# Driver da câmera GAL5000-60ucNIR com:
|
|
# - abertura/fechamento
|
|
# - captura RAW8 mosaic
|
|
# - conversão para RAW4 normalizado
|
|
# - autoexposure (exposição + ganhos)
|
|
#
|
|
# Uso típico:
|
|
#
|
|
# from gal5000_camera import Gal5000Camera
|
|
#
|
|
# cam = Gal5000Camera(
|
|
# dll_dir=r"C:\ZendionInc\agrobot_base\Python\gal5000\dlls",
|
|
# raw_w=2592,
|
|
# raw_h=2056,
|
|
# )
|
|
# with cam:
|
|
# raw4, dbg = cam.grab_raw4(512, 512)
|
|
# # raw4 = np.ndarray (4,512,512) float32 em 0..1
|
|
# # dbg = dict com exp_raw, gain_a, gain_d, p95 etc.
|
|
|
|
import os
|
|
import math
|
|
import time
|
|
import ctypes as C
|
|
from ctypes import wintypes as W
|
|
from collections import deque
|
|
import threading
|
|
|
|
import numpy as np
|
|
import cv2
|
|
|
|
# -----------------------------
|
|
# Constantes de parâmetros
|
|
# -----------------------------
|
|
|
|
PARAM_ID_SENSOR_EXPOSURETIMERAW = 0x00003010
|
|
PARAM_ID_SENSOR_GAINANALOGRAW = 0x00003020
|
|
PARAM_ID_SENSOR_GAINDIGITRAW = 0x0000302A
|
|
PARAM_ID_SFNC_BINNINGHORIZONTAL = 0x00001119
|
|
PARAM_ID_SFNC_BINNINGVERTICAL = 0x0000111B
|
|
PARAM_ID_SFNC_DECIMATIONHORIZONTAL = 0x0000111D
|
|
PARAM_ID_SFNC_DECIMATIONVERTICAL = 0x0000111F
|
|
PARAM_ID_SFNC_ACQUISITIONFRAMERATE = 0x00001208
|
|
PARAM_ID_SFNC_ACQUISITIONFRAMERATEENABLE = 0x00001209
|
|
PARAM_ID_SFNC_SENSORWIDTH = 0x00001101
|
|
PARAM_ID_SFNC_SENSORHEIGHT = 0x00001102
|
|
PARAM_ID_SFNC_WIDTH = 0x00001111
|
|
PARAM_ID_SFNC_HEIGHT = 0x00001112
|
|
PARAM_ID_SFNC_OFFSETX = 0x00001113
|
|
PARAM_ID_SFNC_OFFSETY = 0x00001114
|
|
|
|
BUF_SIZE = 256
|
|
VALUE_INT = 0
|
|
VALUE_FLOAT = 1
|
|
|
|
DEVICE_UDEF = 0
|
|
DEVICE_INDEX = 0
|
|
DATA_RAW = 0
|
|
|
|
# Limites de exposição em unidades RAW (linhas)
|
|
EXP_MIN = 1
|
|
EXP_MAX = 20000
|
|
EXP_MARGIN = 200 # exemplo, em unidades de exp_raw
|
|
# Ganho analógico
|
|
GAIN_A_MIN = 0
|
|
GAIN_A_MAX = 50
|
|
GAIN_A_BASE = 0
|
|
# Ganho digital
|
|
GAIN_D_MIN = 0
|
|
GAIN_D_MAX = 8
|
|
|
|
# ROI para análise de brilho
|
|
ROI_Y0_FRAC = 0.0
|
|
ROI_Y1_FRAC = 1.0
|
|
ROI_X0_FRAC = 0.0
|
|
ROI_X1_FRAC = 1.0
|
|
|
|
# Alvo de brilho / saturação
|
|
TARGET_P95 = 140.0 # alvo de brilho (0..255)
|
|
DEADBAND = 6.0 # zona morta
|
|
SAT_LIMIT = 0.02 # máx fração de pixels saturados
|
|
|
|
# Controle log / suavização
|
|
K_LOG = 0.12
|
|
MAX_STEP = 0.10
|
|
EMA_ALPHA = 0.20
|
|
|
|
|
|
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),
|
|
]
|
|
|
|
FRAME_CALLBACK = C.WINFUNCTYPE(
|
|
W.DWORD, # retorno
|
|
W.HANDLE, # hDev
|
|
VT_FRAMEINFO, # frame info (by value)
|
|
C.c_void_p, # contexto
|
|
)
|
|
|
|
class VT_DEVPARAM(C.Structure):
|
|
_fields_ = [
|
|
("bUseName", W.BOOL),
|
|
("lParamByID", W.DWORD),
|
|
("lParamByName", C.c_char * BUF_SIZE),
|
|
]
|
|
|
|
|
|
# -----------------------------
|
|
# Helpers de DLL / parâmetros
|
|
# -----------------------------
|
|
|
|
def _load_gal_dll(dll_dir: str, dll_name: str):
|
|
if dll_dir is None:
|
|
raise RuntimeError("dll_dir é obrigatório para carregar a VT_SDK64.dll")
|
|
|
|
os.add_dll_directory(dll_dir)
|
|
dll = C.WinDLL(os.path.join(dll_dir, dll_name))
|
|
|
|
# funções principais
|
|
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_CaptureStart.argtypes = [W.HANDLE]
|
|
dll.VT_CaptureStart.restype = C.c_int
|
|
|
|
dll.VT_CaptureStop.argtypes = [W.HANDLE]
|
|
dll.VT_CaptureStop.restype = C.c_int
|
|
|
|
dll.VT_SetFrameCallback.argtypes = [W.HANDLE, FRAME_CALLBACK, C.c_void_p, C.c_int]
|
|
dll.VT_SetFrameCallback.restype = C.c_int
|
|
|
|
# parâmetros
|
|
dll.VT_ParamGetValue.argtypes = [W.HANDLE, C.POINTER(VT_DEVPARAM), C.c_void_p, C.c_int]
|
|
dll.VT_ParamGetValue.restype = C.c_int
|
|
|
|
dll.VT_ParamSetValue.argtypes = [W.HANDLE, C.POINTER(VT_DEVPARAM), C.c_void_p, C.c_int]
|
|
dll.VT_ParamSetValue.restype = C.c_int
|
|
|
|
return dll
|
|
|
|
|
|
def _ck(ret: int, name: str):
|
|
if ret != 0:
|
|
print(f"{name} falhou, ret={ret}")
|
|
raise RuntimeError(f"{name} falhou, ret={ret}")
|
|
|
|
|
|
def _clamp(v, lo, hi):
|
|
return lo if v < lo else hi if v > hi else v
|
|
|
|
|
|
def _devparam_by_id(pid: int) -> VT_DEVPARAM:
|
|
p = VT_DEVPARAM()
|
|
p.bUseName = False
|
|
p.lParamByID = pid
|
|
p.lParamByName = b""
|
|
return p
|
|
|
|
|
|
def _devparam_by_name(name: str) -> VT_DEVPARAM:
|
|
p = VT_DEVPARAM()
|
|
p.bUseName = True
|
|
p.lParamByID = 0
|
|
# garante que preenche o buffer todo com zeros depois da string
|
|
encoded = name.encode("ascii")
|
|
p.lParamByName[:len(encoded)] = encoded
|
|
return p
|
|
|
|
|
|
def _param_get_int(dll, 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(dll, 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)})")
|
|
|
|
|
|
def _param_get_float_by_name(dll, h: W.HANDLE, name: str) -> float:
|
|
p = _devparam_by_name(name)
|
|
v = C.c_double(0.0)
|
|
ret = dll.VT_ParamGetValue(h, p, C.byref(v), VALUE_FLOAT)
|
|
_ck(ret, f"VT_ParamGetValue({name})")
|
|
return float(v.value)
|
|
|
|
def _param_set_float(dll, h: W.HANDLE, pid: int, value: float):
|
|
p = _devparam_by_id(pid)
|
|
v = C.c_double(float(value))
|
|
ret = dll.VT_ParamSetValue(h, p, C.byref(v), VALUE_FLOAT)
|
|
_ck(ret, f"VT_ParamSetValue({hex(pid)})")
|
|
|
|
def _param_set_float_by_name(dll, h: W.HANDLE, name: str, value: float):
|
|
p = _devparam_by_name(name)
|
|
v = C.c_double(float(value))
|
|
ret = dll.VT_ParamSetValue(h, p, C.byref(v), VALUE_FLOAT)
|
|
_ck(ret, f"VT_ParamSetValue({name})")
|
|
|
|
|
|
def _gal_open(dll) -> W.HANDLE:
|
|
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.")
|
|
idx = C.c_ubyte(0)
|
|
h = W.HANDLE()
|
|
_ck(dll.VT_DeviceOpen(C.byref(idx), C.byref(h), DEVICE_INDEX, DEVICE_UDEF), "VT_DeviceOpen")
|
|
return h
|
|
|
|
|
|
def _gal_close(dll, h: W.HANDLE):
|
|
try:
|
|
dll.VT_DeviceClose(C.byref(h))
|
|
except Exception:
|
|
pass
|
|
|
|
|
|
def _gal_capture_raw8_mosaic(dll, h: W.HANDLE, raw_w: int, raw_h: int, timeout_ms: int) -> np.ndarray:
|
|
fi = VT_FRAMEINFO()
|
|
_ck(dll.VT_SingleFrameCapture(h, C.byref(fi), DATA_RAW, timeout_ms, True), "VT_SingleFrameCapture")
|
|
|
|
w, hh = int(fi.lWidth), int(fi.lHeight)
|
|
if (w != raw_w) or (hh != raw_h):
|
|
# só avisa, pode mudar ROI e afins
|
|
raw_w, raw_h = w, hh
|
|
|
|
buf = C.string_at(fi.pBufPtr, fi.lBufSize)
|
|
arr = np.frombuffer(buf, dtype=np.uint8)
|
|
|
|
needed = raw_w * raw_h
|
|
if arr.size < needed:
|
|
arr = np.pad(arr, (0, needed - arr.size), mode="constant", constant_values=0)
|
|
arr = arr[:needed].reshape(raw_h, raw_w)
|
|
return arr
|
|
|
|
|
|
class AEController:
|
|
"""
|
|
Controlador de Auto Exposure em cima do MOSAIC cru.
|
|
Ajusta exposição, e opcionalmente ganho analógico/digital.
|
|
"""
|
|
|
|
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,
|
|
use_gain=True,
|
|
subsample=2):
|
|
"""
|
|
subsample:
|
|
1 -> usa todos os pixels do canal G
|
|
2 -> usa 1/4 dos pixels (subamostragem 2x2)
|
|
3 -> usa 1/9 dos pixels, etc.
|
|
|
|
Na prática, 2 costuma ser um ótimo equilíbrio
|
|
(muito rápido, métricas quase idênticas).
|
|
"""
|
|
self.run_each = 0.05
|
|
self._last_time = 0.0
|
|
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.use_gain = use_gain
|
|
|
|
self.subsample = max(1, int(subsample))
|
|
|
|
self.p95_ema = None
|
|
|
|
# Buffers reutilizáveis para o histograma
|
|
self._hist = np.zeros(256, dtype=np.int32)
|
|
self._cdf = np.zeros(256, dtype=np.int32)
|
|
|
|
# --------- medição rápida de p90/p95/sat ---------
|
|
|
|
def _measure_raw_g_metrics_fast(self, mosaic_u8: np.ndarray):
|
|
"""
|
|
Mede p90, p95 e saturação usando:
|
|
- apenas canal G do mosaico
|
|
- histogram + CDF
|
|
- subamostragem opcional
|
|
|
|
Retorna: (p90, p95, sat) onde:
|
|
p90, p95 em escala 0..255 (float)
|
|
sat = fração de pixels saturados (=255) em 0..1
|
|
"""
|
|
if mosaic_u8.ndim != 2 or mosaic_u8.dtype != np.uint8:
|
|
g = np.asarray(mosaic_u8, dtype=np.uint8)
|
|
else:
|
|
g = mosaic_u8
|
|
|
|
# Extrai canal G do mosaico:
|
|
# padrão:
|
|
# R G
|
|
# IR B
|
|
# então G está em [0::2, 1::2]
|
|
g = g[0::2, 1::2]
|
|
|
|
# Subamostragem espacial opcional
|
|
s = self.subsample
|
|
if s > 1:
|
|
g = g[::s, ::s]
|
|
|
|
# Histogram 0..255 usando buffer interno
|
|
hist = self._hist
|
|
hist.fill(0)
|
|
# np.add.at acumula contagens sem criar array novo
|
|
np.add.at(hist, g.ravel(), 1)
|
|
|
|
total = int(hist.sum())
|
|
if total == 0:
|
|
# fallback besta, mas evita divisão por zero
|
|
return 0.0, 0.0, 0.0
|
|
|
|
# CDF no buffer
|
|
cdf = self._cdf
|
|
np.cumsum(hist, out=cdf)
|
|
|
|
# índices para 90% e 95% dos pixels
|
|
thr90 = 0.90 * total
|
|
thr95 = 0.95 * total
|
|
|
|
idx90 = int(np.searchsorted(cdf, thr90))
|
|
idx95 = int(np.searchsorted(cdf, thr95))
|
|
|
|
# saturação: fração de pixels em 255
|
|
sat = hist[255] / float(total)
|
|
|
|
return float(idx90), float(idx95), float(sat)
|
|
|
|
# --------- lógica de controle (quase igual a sua) ---------
|
|
|
|
def step(self,
|
|
mosaic_u8: np.ndarray,
|
|
exp_raw: int,
|
|
gain_a: int,
|
|
gain_d: int):
|
|
"""
|
|
Retorna (new_exp, new_gain_a, new_gain_d, dbg)
|
|
"""
|
|
|
|
# Aqui trocamos a função por uma versão rápida
|
|
p90, p95, sat = self._measure_raw_g_metrics_fast(mosaic_u8)
|
|
|
|
# EMA do p95
|
|
if self.p95_ema is None:
|
|
self.p95_ema = p95
|
|
else:
|
|
a = self.ema_alpha
|
|
self.p95_ema = (1.0 - a) * self.p95_ema + a * p95
|
|
|
|
e = self.target - self.p95_ema
|
|
|
|
# deadband: se está perto do alvo e sem saturação, não mexe
|
|
if abs(e) <= self.deadband and sat <= self.sat_limit:
|
|
new_gain_a = gain_a
|
|
|
|
if self.use_gain:
|
|
# Relaxar ganho em direção ao baseline quando está tudo ok
|
|
if gain_a > GAIN_A_BASE:
|
|
new_gain_a = max(GAIN_A_BASE, gain_a - 1)
|
|
elif gain_a < GAIN_A_BASE:
|
|
new_gain_a = min(GAIN_A_BASE, gain_a + 1)
|
|
|
|
dbg = {
|
|
"p90": p90,
|
|
"p95": p95,
|
|
"p95_ema": self.p95_ema,
|
|
"sat": sat,
|
|
"step": 0.0,
|
|
"hold": True,
|
|
}
|
|
self._last_time = time.time()
|
|
return exp_raw, new_gain_a, gain_d, dbg
|
|
|
|
# cálculo do passo em log
|
|
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 = _clamp(new_exp, self.exp_min, self.exp_max)
|
|
|
|
new_gain_a = gain_a
|
|
new_gain_d = gain_d
|
|
|
|
if self.use_gain:
|
|
# 1) Muito escuro e exp no teto -> sobe ganho
|
|
if new_exp >= self.exp_max - EXP_MARGIN and self.p95_ema < (self.target - self.deadband):
|
|
new_gain_a = _clamp(gain_a + 2, GAIN_A_MIN, GAIN_A_MAX)
|
|
|
|
# 2) Muito claro e exp no piso -> desce ganho
|
|
elif new_exp <= self.exp_min + EXP_MARGIN and (self.p95_ema > (self.target + self.deadband) or sat > self.sat_limit):
|
|
new_gain_a = _clamp(gain_a - 2, GAIN_A_MIN, GAIN_A_MAX)
|
|
|
|
dbg = {
|
|
"p90": p90,
|
|
"p95": p95,
|
|
"p95_ema": self.p95_ema,
|
|
"sat": sat,
|
|
"step": step,
|
|
"hold": False,
|
|
}
|
|
self._last_time = time.time()
|
|
return new_exp, new_gain_a, new_gain_d, dbg
|
|
|
|
|
|
# -----------------------------
|
|
# Conversão MOSAIC -> RAW4
|
|
# -----------------------------
|
|
|
|
_m2r_hw = None # (H, W) do mosaico atual (pares)
|
|
_m2r_out_hw = None # (out_h, out_w)
|
|
_m2r_tmp4_u8 = None # (H2, W2, 4) uint8
|
|
_m2r_resized4_u8 = None # (out_h, out_w, 4) uint8
|
|
_m2r_raw4_f32 = None # (4, out_h, out_w) float32
|
|
def mosaic_to_raw4_resized_buf(
|
|
mosaic_u8: np.ndarray,
|
|
out_h: int,
|
|
out_w: int,
|
|
interpolation=cv2.INTER_AREA,
|
|
) -> np.ndarray:
|
|
"""
|
|
mosaic_u8: (H,W) uint8 com padrão 2x2:
|
|
R G
|
|
IR B
|
|
|
|
Retorna raw4: (4,out_h,out_w) float32 em 0..1, ordem [R,G,IR,B].
|
|
|
|
Qualidade:
|
|
- Separa canais primeiro (H/2,W/2), depois faz resize 4ch.
|
|
- Não mistura canais. É equivalente a 4 resizes separados.
|
|
Performance:
|
|
- 1 resize apenas.
|
|
- Buffers reutilizáveis para evitar alocações.
|
|
"""
|
|
global _m2r_hw, _m2r_out_hw, _m2r_tmp4_u8, _m2r_resized4_u8, _m2r_raw4_f32
|
|
if mosaic_u8.ndim != 2 or mosaic_u8.dtype != np.uint8:
|
|
# Se vier com shape diferente, adapta aqui ou faz assert.
|
|
mosaic_u8 = np.asarray(mosaic_u8, dtype=np.uint8)
|
|
if mosaic_u8.ndim != 2:
|
|
raise ValueError(f"Esperava mosaico 2D uint8 (H,W), veio {mosaic_u8.shape}")
|
|
|
|
H, W = mosaic_u8.shape[:2]
|
|
|
|
# Garante dimensões pares (corte mínimo, sem interpolar mosaico)
|
|
if (H % 2) != 0:
|
|
H -= 1
|
|
if (W % 2) != 0:
|
|
W -= 1
|
|
if H != mosaic_u8.shape[0] or W != mosaic_u8.shape[1]:
|
|
mosaic_u8 = mosaic_u8[:H, :W]
|
|
|
|
H2, W2 = H // 2, W // 2
|
|
|
|
# (Re)aloca buffers se mudou H,W ou out_h,out_w
|
|
if _m2r_hw != (H, W) or _m2r_out_hw != (out_h, out_w):
|
|
_m2r_hw = (H, W)
|
|
_m2r_out_hw = (out_h, out_w)
|
|
|
|
_m2r_tmp4_u8 = np.empty((H2, W2, 4), dtype=np.uint8)
|
|
_m2r_resized4_u8 = np.empty((out_h, out_w, 4), dtype=np.uint8)
|
|
_m2r_raw4_f32 = np.empty((4, out_h, out_w), dtype=np.float32)
|
|
|
|
tmp4 = _m2r_tmp4_u8
|
|
|
|
# Separa canais (views) do mosaico (H2,W2)
|
|
# Importante: isso não copia; é slicing em visão
|
|
r = mosaic_u8[0::2, 0::2]
|
|
g = mosaic_u8[0::2, 1::2]
|
|
ir = mosaic_u8[1::2, 0::2]
|
|
b = mosaic_u8[1::2, 1::2]
|
|
|
|
# Empacota em 4ch uint8 (H2,W2,4)
|
|
tmp4[..., 0] = r
|
|
tmp4[..., 1] = g
|
|
tmp4[..., 2] = ir
|
|
tmp4[..., 3] = b
|
|
|
|
# UM resize multi-canal para (out_h,out_w,4)
|
|
# Usa buffer de saída para reduzir alocação
|
|
resized4 = cv2.resize(tmp4, (out_w, out_h), interpolation=interpolation)
|
|
|
|
# Normaliza e transpõe para (4,H,W) float32 em 0..1
|
|
# Evita stack/astype extra
|
|
raw4 = _m2r_raw4_f32
|
|
# resized4 é uint8 (out_h,out_w,4)
|
|
# Transpõe para (4,out_h,out_w) e converte
|
|
# astype aqui cria cópia; mas a gente já escreve no buffer raw4, então:
|
|
raw4[0, :, :] = resized4[:, :, 0].astype(np.float32) * (1.0 / 255.0)
|
|
raw4[1, :, :] = resized4[:, :, 1].astype(np.float32) * (1.0 / 255.0)
|
|
raw4[2, :, :] = resized4[:, :, 2].astype(np.float32) * (1.0 / 255.0)
|
|
raw4[3, :, :] = resized4[:, :, 3].astype(np.float32) * (1.0 / 255.0)
|
|
|
|
return raw4
|
|
|
|
def mosaic_to_raw4_resized(
|
|
mosaic_u8: np.ndarray,
|
|
out_h: int,
|
|
out_w: int,
|
|
interpolation = cv2.INTER_AREA,
|
|
) -> np.ndarray:
|
|
"""
|
|
mosaic_u8: (H,W) uint8, padrão:
|
|
R G
|
|
IR B
|
|
Retorna raw4 float32 (4,out_h,out_w) em 0..1.
|
|
"""
|
|
H, W = mosaic_u8.shape[:2]
|
|
if (H % 2) != 0 or (W % 2) != 0:
|
|
mosaic_u8 = mosaic_u8[:H - (H % 2), :W - (W % 2)]
|
|
|
|
r = mosaic_u8[0::2, 0::2]
|
|
g = mosaic_u8[0::2, 1::2]
|
|
ir = mosaic_u8[1::2, 0::2]
|
|
b = mosaic_u8[1::2, 1::2]
|
|
|
|
r = cv2.resize(r, (out_w, out_h), interpolation=interpolation)
|
|
g = cv2.resize(g, (out_w, out_h), interpolation=interpolation)
|
|
ir = cv2.resize(ir, (out_w, out_h), interpolation=interpolation)
|
|
b = cv2.resize(b, (out_w, out_h), interpolation=interpolation)
|
|
|
|
raw4 = np.stack([r, g, ir, b], axis=0).astype(np.float32) / 255.0
|
|
return np.clip(raw4, 0.0, 1.0)
|
|
|
|
|
|
# -----------------------------
|
|
# Classe principal: Gal5000Camera
|
|
# -----------------------------
|
|
|
|
class Gal5000Camera:
|
|
def __init__(
|
|
self,
|
|
dll_dir: str = r"C:\ZendionInc\agrobot_base\Python\gal5000\dlls",
|
|
dll_name: str = "VT_SDK64.dll",
|
|
raw_w: int = 2592,
|
|
raw_h: int = 2056,
|
|
use_auto_exposure: bool = True,
|
|
):
|
|
self.dll_dir = dll_dir
|
|
self.dll_name = dll_name
|
|
self.raw_w = raw_w
|
|
self.raw_h = raw_h
|
|
|
|
self.dll = _load_gal_dll(dll_dir, dll_name)
|
|
self.handle: W.HANDLE | None = None
|
|
|
|
self.exp_raw: int | None = 1500
|
|
self.gain_a: int | None = 0
|
|
self.gain_d: int | None = 0
|
|
|
|
self.ae = AEController(exp_max=10000)
|
|
self.ae_enabled = use_auto_exposure
|
|
|
|
self._streaming = False
|
|
self._frame_queue = deque(maxlen=1)
|
|
self._frame_lock = threading.Lock()
|
|
self._frame_cb_c = None # segura a ref do callback
|
|
|
|
# context manager
|
|
def __enter__(self):
|
|
self.open()
|
|
return self
|
|
|
|
def __exit__(self, exc_type, exc, tb):
|
|
self.close()
|
|
|
|
# lifecycle
|
|
def open(self):
|
|
if self.handle is not None:
|
|
return
|
|
self.handle = _gal_open(self.dll)
|
|
# tenta ler parâmetros atuais
|
|
self._init_params()
|
|
|
|
def close(self):
|
|
if self.handle is None:
|
|
return
|
|
if self._streaming:
|
|
self.stop_streaming()
|
|
_gal_close(self.dll, self.handle)
|
|
self.handle = None
|
|
|
|
def configure_fps(self, fps: int):
|
|
# 1) tenta habilitar frame rate, mas se não tiver suporte, só avisa e segue
|
|
try:
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_ACQUISITIONFRAMERATEENABLE, 1)
|
|
except Exception as e:
|
|
print(f"[WARN] ACQ_FRAMERATE_ENABLE não suportado: {e}")
|
|
# 2) tenta primeiro via SFNC ID
|
|
try:
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_ACQUISITIONFRAMERATE, int(fps))
|
|
print(f"[INFO] AcquisitionFrameRate (SFNC) setado para {fps} fps")
|
|
except Exception as e_id:
|
|
print(f"[WARN] SFNC AcquisitionFrameRate falhou: {e_id}")
|
|
# 3) fallback via nome 'AcquisitionFrameRateAbs'
|
|
try:
|
|
_param_set_float_by_name(self.dll, self.handle, "AcquisitionFrameRateAbs", float(fps))
|
|
print(f"[INFO] AcquisitionFrameRateAbs setado para {fps} fps")
|
|
except Exception as e_name:
|
|
print(f"[WARN] AcquisitionFrameRateAbs também falhou: {e_name}")
|
|
|
|
def configure_binning_full_fov(self, bin_factor: int, fps: float | None = None):
|
|
if self.handle is None:
|
|
return
|
|
|
|
# 1) lê o tamanho máximo atual que o SDK considera como 'sensor'
|
|
sensor_w = _param_get_int(self.dll, self.handle, PARAM_ID_SFNC_SENSORWIDTH)
|
|
sensor_h = _param_get_int(self.dll, self.handle, PARAM_ID_SFNC_SENSORHEIGHT)
|
|
|
|
# 2) seta o binning (igual ao combo do viewer)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_BINNINGHORIZONTAL, bin_factor)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_BINNINGVERTICAL, bin_factor)
|
|
|
|
# 3) offset zerado para garantir FOV máximo
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_OFFSETX, 0)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_OFFSETY, 0)
|
|
|
|
# 4) width/height cobrindo tudo
|
|
# OBS: dependendo do SDK, SensorWidth já pode estar "pós-binning".
|
|
# Se ao dividir por bin_factor você perder FOV, teste também sem dividir.
|
|
width = sensor_w // bin_factor
|
|
height = sensor_h // bin_factor
|
|
|
|
print(f'Bin factor: {bin_factor}, Sensor: {sensor_w}x{sensor_h}, Shape: {width}x{height}')
|
|
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_WIDTH, width)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SFNC_HEIGHT, height)
|
|
|
|
# 5) fps opcional, como você já fez
|
|
if fps is not None:
|
|
self.configure_fps(fps)
|
|
|
|
# leitura inicial de exp/gain
|
|
def _init_params(self):
|
|
if self.handle is None:
|
|
return
|
|
try:
|
|
self.set_exposure(self.exp_raw)
|
|
self.set_gain_a(self.gain_a)
|
|
self.set_gain_d(self.gain_d)
|
|
except Exception:
|
|
print("Erro ao definir parametros iniciais de AE")
|
|
try:
|
|
self.exp_raw = _param_get_int(self.dll, self.handle, PARAM_ID_SENSOR_EXPOSURETIMERAW)
|
|
except Exception:
|
|
self.exp_raw = 1500
|
|
try:
|
|
self.gain_a = _param_get_int(self.dll, self.handle, PARAM_ID_SENSOR_GAINANALOGRAW)
|
|
except Exception:
|
|
self.gain_a = 0
|
|
try:
|
|
self.gain_d = _param_get_int(self.dll, self.handle, PARAM_ID_SENSOR_GAINDIGITRAW)
|
|
except Exception:
|
|
self.gain_d = 0
|
|
|
|
# getters / setters exp/gain
|
|
def get_exposure(self) -> int:
|
|
return int(self.exp_raw) if self.exp_raw is not None else 0
|
|
|
|
def set_exposure(self, new_exp: int) -> int:
|
|
if self.handle is None:
|
|
return 0
|
|
new_exp = _clamp(int(new_exp), EXP_MIN, EXP_MAX)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SENSOR_EXPOSURETIMERAW, new_exp)
|
|
self.exp_raw = new_exp
|
|
return new_exp
|
|
|
|
def get_gain_a(self) -> int:
|
|
return int(self.gain_a) if self.gain_a is not None else 0
|
|
|
|
def set_gain_a(self, new_gain: int) -> int:
|
|
if self.handle is None:
|
|
return 0
|
|
new_gain = _clamp(int(new_gain), GAIN_A_MIN, GAIN_A_MAX)
|
|
#_param_set_int(self.dll, self.handle, PARAM_ID_SENSOR_GAINANALOGRAW, new_gain)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SENSOR_GAINDIGITRAW, new_gain)
|
|
self.gain_a = new_gain
|
|
return new_gain
|
|
|
|
def get_gain_d(self) -> int:
|
|
return int(self.gain_d) if self.gain_d is not None else 0
|
|
|
|
def set_gain_d(self, new_gain: int) -> int:
|
|
if self.handle is None:
|
|
return 0
|
|
new_gain = _clamp(int(new_gain), GAIN_D_MIN, GAIN_D_MAX)
|
|
_param_set_int(self.dll, self.handle, PARAM_ID_SENSOR_GAINDIGITRAW, new_gain)
|
|
self.gain_d = new_gain
|
|
return new_gain
|
|
|
|
# auto exposure
|
|
def enable_auto_exposure(self, enabled: bool = True):
|
|
self.ae_enabled = enabled
|
|
|
|
def is_auto_exposure_enabled(self) -> bool:
|
|
return self.ae_enabled
|
|
|
|
# captura bruta
|
|
def grab_mosaic(self, timeout_ms: int = 2000) -> np.ndarray:
|
|
if self.handle is None:
|
|
raise RuntimeError("Câmera não está aberta. Chame open() antes.")
|
|
return _gal_capture_raw8_mosaic(self.dll, self.handle, self.raw_w, self.raw_h, timeout_ms)
|
|
|
|
# captura + AE + conversão para RAW4
|
|
def grab_raw4(
|
|
self,
|
|
out_h: int,
|
|
out_w: int,
|
|
timeout_ms: int = 2000,
|
|
do_ae: bool = True,
|
|
):
|
|
"""
|
|
Captura um frame, aplica AE se habilitado,
|
|
converte para RAW4 normalizado e retorna:
|
|
|
|
raw4: np.ndarray (4,out_h,out_w) float32 em 0..1
|
|
dbg: dict com métricas de AE (p95, sat, exp, gains)
|
|
"""
|
|
t0 = time.time()
|
|
if self._streaming:
|
|
mosaic = self.grab_mosaic_stream(timeout_ms)
|
|
else:
|
|
mosaic = self.grab_mosaic(timeout_ms)
|
|
t1 = time.time()
|
|
|
|
dbg_ae = None
|
|
do_ae_now = (
|
|
do_ae
|
|
and self.ae_enabled
|
|
and self.exp_raw is not None
|
|
and (t1 - self.ae._last_time) >= self.ae.run_each # máximo 10 Hz de AE
|
|
)
|
|
if do_ae_now and self.ae_enabled and self.exp_raw is not None:
|
|
new_exp, new_ga, new_gd, dbg_ae = self.ae.step(
|
|
mosaic,
|
|
self.exp_raw,
|
|
self.gain_a or 0,
|
|
self.gain_d or 0,
|
|
)
|
|
if new_exp != self.exp_raw:
|
|
self.set_exposure(new_exp)
|
|
if new_ga != self.gain_a:
|
|
self.set_gain_a(new_ga)
|
|
if new_gd != self.gain_d:
|
|
self.set_gain_d(new_gd)
|
|
t2 = time.time()
|
|
|
|
raw4 = mosaic_to_raw4_resized_buf(mosaic, out_h, out_w)
|
|
t3 = time.time()
|
|
|
|
dbg = {
|
|
"raw_shape": mosaic.shape,
|
|
"ae": dbg_ae,
|
|
"exp_raw": self.exp_raw,
|
|
"gain_a": self.gain_a,
|
|
"gain_d": self.gain_d,
|
|
"t_capture": t1 - t0,
|
|
"t_ae": t2 - t1,
|
|
"t_convert": t3 - t2,
|
|
"latency_s": t3 - t0,
|
|
}
|
|
return raw4, dbg
|
|
|
|
def get_status(self) -> dict:
|
|
"""
|
|
Retorna um snapshot simples do estado da câmera.
|
|
"""
|
|
return {
|
|
"opened": self.handle is not None,
|
|
"exp_raw": self.exp_raw,
|
|
"gain_a": self.gain_a,
|
|
"gain_d": self.gain_d,
|
|
"ae_enabled": self.ae_enabled,
|
|
"raw_w": self.raw_w,
|
|
"raw_h": self.raw_h,
|
|
}
|
|
|
|
# streaming
|
|
def _on_frame(self, hDev, fi: VT_FRAMEINFO, ctx):
|
|
"""
|
|
Callback chamado pelo SDK a cada frame.
|
|
Converte o buffer RAW8 mosaic para np.ndarray e põe na fila.
|
|
Mantém o trabalho aqui o mais leve possível.
|
|
"""
|
|
try:
|
|
w = int(fi.lWidth)
|
|
h = int(fi.lHeight)
|
|
|
|
size = int(fi.lBufSize)
|
|
|
|
buf = C.string_at(fi.pBufPtr, size)
|
|
arr = np.frombuffer(buf, dtype=np.uint8)
|
|
|
|
needed = w * h
|
|
if arr.size < needed:
|
|
arr = np.pad(arr, (0, needed - arr.size), mode="constant", constant_values=0)
|
|
elif arr.size > needed:
|
|
arr = arr[:needed]
|
|
|
|
mosaic = arr.reshape(h, w)
|
|
|
|
with self._frame_lock:
|
|
self._frame_queue.append((mosaic, time.time()))
|
|
|
|
except Exception as e:
|
|
print(f"[FRAME_CB ERROR] {e}")
|
|
|
|
return 0
|
|
|
|
def start_streaming(self):
|
|
if self.handle is None:
|
|
raise RuntimeError("Câmera não está aberta.")
|
|
|
|
if self._streaming:
|
|
return
|
|
|
|
# cria callback C e segura referência
|
|
self._frame_cb_c = FRAME_CALLBACK(self._on_frame)
|
|
|
|
_ck(self.dll.VT_SetFrameCallback(self.handle, self._frame_cb_c, None, DATA_RAW),
|
|
"VT_SetFrameCallback")
|
|
_ck(self.dll.VT_CaptureStart(self.handle), "VT_CaptureStart")
|
|
|
|
self._streaming = True
|
|
|
|
def stop_streaming(self):
|
|
if not self._streaming or self.handle is None:
|
|
return
|
|
try:
|
|
self.dll.VT_CaptureStop(self.handle)
|
|
except Exception:
|
|
pass
|
|
self._streaming = False
|
|
|
|
def grab_mosaic_stream(self, timeout_ms: int = 2000) -> np.ndarray:
|
|
"""
|
|
Lê o último frame da fila de streaming.
|
|
"""
|
|
if not self._streaming:
|
|
raise RuntimeError("Streaming não está ativo. Chame start_streaming().")
|
|
|
|
deadline = time.time() + timeout_ms / 1000.0
|
|
last = None
|
|
|
|
while time.time() < deadline:
|
|
with self._frame_lock:
|
|
if self._frame_queue:
|
|
last = self._frame_queue[-1]
|
|
if last is not None:
|
|
mosaic, t_cap = last
|
|
return mosaic
|
|
time.sleep(0.001)
|
|
|
|
raise TimeoutError("Timeout aguardando frame de streaming.")
|