155 lines
6.2 KiB
Python
155 lines
6.2 KiB
Python
import depthai as dai
|
|
import numpy as np
|
|
import cv2
|
|
import time
|
|
|
|
# 📏 Definições da resolução da câmera e das matrizes
|
|
WIDTH, HEIGHT = 640, 400
|
|
GROUND_ROWS = 10 # 🔹 Número de linhas da matriz do solo
|
|
GROUND_COLS = 10 # 🔹 Todas as linhas terão 10 colunas (células podem ter larguras diferentes)
|
|
GROUND_PERCENTAGE = 0.25 # 🔹 35% da tela representa 2m
|
|
GROUND_HEIGHT = int(HEIGHT * GROUND_PERCENTAGE) # 🔹 Altura da matriz do solo
|
|
AIR_ROWS = 3 # Linhas superiores para obstáculos elevados
|
|
AIR_COLS = 6 # Colunas para obstáculos suspensos
|
|
GROUND_DEPTH_LIMIT = 2000 # Distância limite do solo (2m)
|
|
|
|
selected_cell = None # Variável global para armazenar célula selecionada no clique
|
|
|
|
# Criando o pipeline
|
|
pipeline = dai.Pipeline()
|
|
|
|
# 📷 Criar nó da câmera RGB
|
|
cam_rgb = pipeline.create(dai.node.ColorCamera)
|
|
cam_rgb.setPreviewSize(WIDTH, HEIGHT)
|
|
cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB)
|
|
cam_rgb.setInterleaved(False)
|
|
|
|
xout_rgb = pipeline.create(dai.node.XLinkOut)
|
|
xout_rgb.setStreamName("rgb")
|
|
cam_rgb.preview.link(xout_rgb.input)
|
|
|
|
# 📏 Criar nó de profundidade
|
|
mono_left = pipeline.create(dai.node.MonoCamera)
|
|
mono_right = pipeline.create(dai.node.MonoCamera)
|
|
stereo = pipeline.create(dai.node.StereoDepth)
|
|
|
|
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
|
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
|
|
|
mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT)
|
|
mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT)
|
|
|
|
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
|
|
stereo.setLeftRightCheck(True)
|
|
stereo.setSubpixel(True)
|
|
|
|
mono_left.out.link(stereo.left)
|
|
mono_right.out.link(stereo.right)
|
|
|
|
xout_depth = pipeline.create(dai.node.XLinkOut)
|
|
xout_depth.setStreamName("depth")
|
|
stereo.depth.link(xout_depth.input)
|
|
|
|
# 🚀 Função para capturar clique do mouse
|
|
def mouse_callback(event, x, y, flags, param):
|
|
global selected_cell
|
|
if event == cv2.EVENT_LBUTTONDOWN:
|
|
selected_cell = (x, y)
|
|
|
|
# 🚀 Inicializar Dispositivo
|
|
with dai.Device(pipeline) as device:
|
|
rgb_queue = device.getOutputQueue(name="rgb", maxSize=1, blocking=False)
|
|
depth_queue = device.getOutputQueue(name="depth", maxSize=1, blocking=False)
|
|
|
|
print("📏 Calibrando profundidade média das subdivisões...")
|
|
|
|
# 🔹 Pegar um frame inicial para calibração
|
|
depth_frame = depth_queue.get().getFrame()
|
|
ground_reference = np.zeros((GROUND_ROWS, GROUND_COLS))
|
|
|
|
# 🚀 Gerar matriz afunilada corretamente (base larga, topo estreito)
|
|
for i in range(GROUND_ROWS):
|
|
# 🔹 A base (i = 0) começa larga e a parte superior (i = GROUND_ROWS-1) é mais estreita
|
|
y_start = HEIGHT - (GROUND_HEIGHT - (i * (GROUND_HEIGHT // GROUND_ROWS))) # 🔹 Começa na base e sobe
|
|
y_end = HEIGHT - (GROUND_HEIGHT - ((i + 1) * (GROUND_HEIGHT // GROUND_ROWS)))
|
|
|
|
# 🔹 A base começa larga e vai afunilando progressivamente para cima
|
|
row_width = WIDTH - (i * (WIDTH // (GROUND_ROWS // 2))) # 🔹 Agora o afunilamento está correto
|
|
cell_width = row_width // GROUND_COLS # 🔹 Mantém 10 células por linha
|
|
|
|
for j in range(GROUND_COLS):
|
|
x_start = (WIDTH // 2) - (row_width // 2) + (j * cell_width)
|
|
x_end = x_start + cell_width
|
|
|
|
ground_reference[i, j] = np.mean(depth_frame[y_start:y_end, x_start:x_end])
|
|
|
|
|
|
|
|
|
|
print("✅ Calibração concluída!")
|
|
|
|
last_time = time.time()
|
|
target_fps = 15 # 🔹 Limita o FPS para evitar travamentos
|
|
|
|
cv2.namedWindow("Detecção de Obstáculos - OAK-D Lite")
|
|
cv2.setMouseCallback("Detecção de Obstáculos - OAK-D Lite", mouse_callback)
|
|
|
|
while True:
|
|
# 🔹 Controle de FPS
|
|
current_time = time.time()
|
|
if current_time - last_time < 1 / target_fps:
|
|
time.sleep(0.01)
|
|
continue
|
|
last_time = current_time
|
|
|
|
in_rgb = rgb_queue.tryGet()
|
|
in_depth = depth_queue.tryGet()
|
|
|
|
if in_rgb is None or in_depth is None:
|
|
continue
|
|
|
|
rgb_frame = in_rgb.getCvFrame()
|
|
depth_frame = in_depth.getFrame()
|
|
|
|
# 🔹 Criar camada para sobrepor na imagem
|
|
overlay = rgb_frame.copy()
|
|
|
|
# 🚀 Analisar o solo (matriz "A" dentro dos 35%)
|
|
for i in range(GROUND_ROWS):
|
|
y_start = HEIGHT - GROUND_HEIGHT + (i * (GROUND_HEIGHT // GROUND_ROWS))
|
|
y_end = HEIGHT - GROUND_HEIGHT + ((i + 1) * (GROUND_HEIGHT // GROUND_ROWS))
|
|
|
|
row_width = WIDTH - (i * (WIDTH // GROUND_ROWS)) # 🔹 Afunilamento progressivo
|
|
cell_width = row_width // GROUND_COLS
|
|
|
|
for j in range(GROUND_COLS):
|
|
x_start = (WIDTH // 2) - (row_width // 2) + (j * cell_width)
|
|
x_end = x_start + cell_width
|
|
|
|
region_depth = np.mean(depth_frame[y_start:y_end, x_start:x_end])
|
|
|
|
# 🚨 Comparação com a calibração inicial
|
|
if region_depth < (ground_reference[i, j] - 200): # 🔹 Diferença de 20 cm
|
|
cv2.rectangle(overlay, (x_start, y_start), (x_end, y_end), (0, 0, 255), -1) # 🔴 Vermelho semi-opaco
|
|
|
|
cv2.rectangle(rgb_frame, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) # 🔹 Grade branca
|
|
|
|
# 🚀 Exibir profundidade ao clicar
|
|
if selected_cell and (x_start <= selected_cell[0] <= x_end) and (y_start <= selected_cell[1] <= y_end):
|
|
depth_at_pixel = depth_frame[selected_cell[1], selected_cell[0]]
|
|
cv2.putText(rgb_frame, f"Prof: {region_depth:.1f} mm", (x_start, y_start - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
|
cv2.putText(rgb_frame, f"Esperado: {ground_reference[i, j]:.1f} mm", (x_start, y_start + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
|
cv2.putText(rgb_frame, f"Pixel: {depth_at_pixel:.1f} mm", (x_start, y_start + 25), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 0, 0), 1)
|
|
|
|
# 🔹 Aplicar transparência na sobreposição
|
|
alpha = 0.4
|
|
cv2.addWeighted(overlay, alpha, rgb_frame, 1 - alpha, 0, rgb_frame)
|
|
|
|
# 🚀 Exibir imagem processada
|
|
cv2.imshow("Detecção de Obstáculos - OAK-D Lite", rgb_frame)
|
|
|
|
if cv2.waitKey(1) == ord('q'):
|
|
break
|
|
|
|
cv2.destroyAllWindows()
|