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()