import depthai as dai import numpy as np import cv2 import time import json # Função para calcular a moda de um array unidimensional def calc_mode(values): # Arredonda os valores para eliminar pequenas variações rounded = np.round(values, 0) vals, counts = np.unique(rounded, return_counts=True) return vals[np.argmax(counts)] # Configuração de filtros NUM_FRAMES_SMOOTH = 5 # Número de frames para suavização temporal SMOOTH_KERNEL = (5, 5) # Tamanho do kernel para suavização espacial OUTLIER_THRESHOLD = 50 # Limiar para remoção de outliers depth_buffer = [] # Buffer para armazenar os últimos frames de profundidade # Suavização Temporal def smooth_depth(depth_frame): global depth_buffer if len(depth_buffer) >= NUM_FRAMES_SMOOTH: depth_buffer.pop(0) # Remove o frame mais antigo depth_buffer.append(depth_frame) # Adiciona o novo frame return np.mean(depth_buffer, axis=0).astype(np.uint16) # Retorna a média # Suavização Espacial (Filtro Gaussiano) def gaussian_smooth(depth_frame): return cv2.GaussianBlur(depth_frame, SMOOTH_KERNEL, 0) # Filtro de Mediana para Remover Ruídos def median_filter(depth_frame): return cv2.medianBlur(depth_frame, 5) # Remover Outliers (Saltos Extremos) def remove_outliers(depth_frame): depth_median = cv2.medianBlur(depth_frame, 5) diff = np.abs(depth_frame - depth_median) depth_frame[diff > OUTLIER_THRESHOLD] = depth_median[diff > OUTLIER_THRESHOLD] return depth_frame # Aplicar filtros no depthFrame def apply_depth_filters(depth_frame): if filtro1: depth_frame = smooth_depth(depth_frame) # 1. Suavização Temporal if filtro2: depth_frame = gaussian_smooth(depth_frame) # 2. Suavização Espacial if filtro3: depth_frame = median_filter(depth_frame) # 3. Filtro de Mediana if filtro4: depth_frame = remove_outliers(depth_frame) # 4. Remoção de Outliers return depth_frame # Função para gerar o mapa de calor da profundidade def generate_heatmap(depth_frame): normalized_depth = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX) heatmap = cv2.applyColorMap(normalized_depth.astype(np.uint8), cv2.COLORMAP_JET) return heatmap def gerar_json_subdivisoes(): data = { "x_max": WIDTH, "y_max": HEIGHT, "subdivisoes_cima": { "num_linhas": AIR_ROWS, "num_colunas": AIR_COLS, "celulas": [] }, "subdivisoes_chao": { "num_linhas": GROUND_ROWS, "num_colunas": GROUND_CELLS, "celulas": [] } } # Subdivisões da região aérea row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS for i in range(AIR_ROWS): for j in range(AIR_COLS): x_start = j * cell_width_air y_start = i * row_height_air data["subdivisoes_cima"]["celulas"].append({ "linha": i, "coluna": j, "x": x_start, "y": y_start, "largura": cell_width_air, "altura": row_height_air, "profundidade_media": smoothed_air[i, j], "profundidade_calibragem": air_reference[i, j] }) # Subdivisões da região do solo row_height_ground = GROUND_REGION_HEIGHT // GROUND_ROWS for i in range(GROUND_ROWS): scale = (GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE)) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground start_x = int(WIDTH / 2 - effective_width / 2) for j in range(GROUND_CELLS): x_start = int(start_x + j * cell_width) data["subdivisoes_chao"]["celulas"].append({ "linha": i, "coluna": j, "x": x_start, "y": y_start, "largura": int(cell_width), "altura": row_height_ground, "profundidade_media": smoothed_ground[i, j], "profundidade_calibragem": ground_reference[i, j] }) return json.dumps(data, indent=4) # ─────────────────────────────────────────────── # Configurações da câmera e das matrizes # ─────────────────────────────────────────────── WIDTH, HEIGHT = 320, 240 # Região do solo (grid A - parte inferior): ocupa 50% da altura da imagem GROUND_ROWS = 10 # Número de linhas da matriz do solo GROUND_CELLS = 10 # Número de células por linha GROUND_REGION_HEIGHT = int(HEIGHT * 0.5) # 50% da altura da imagem GROUND_TOP_SCALE = 0.5 # A linha mais distante (superior da região) terá 35% da largura total DEPTH_LIMIT = 50 # (Valor de referência para comparação, mas agora usamos calibração) # Região aérea (parte superior): ocupa o restante da imagem AIR_ROWS = 10 # Número de linhas na matriz aérea AIR_COLS = 15 # Número de colunas na matriz aérea AIR_REGION_HEIGHT = HEIGHT - GROUND_REGION_HEIGHT # Altura da região aérea # ─────────────────────────────────────────────── # Parâmetros para calibração e detecção com moda # ─────────────────────────────────────────────── NUM_CALIB_FRAMES = 100 # Número de frames para calibração NUM_DETECT_FRAMES = 10 # Número de frames para acumulação antes de calcular a moda na detecção # Variável global para armazenar o frame de profundidade atual (para uso no callback) current_depth_frame = None show_grid = False filtro1 = True filtro2 = True filtro3 = True filtro4 = True # ─────────────────────────────────────────────── # Criação do pipeline DepthAI # ─────────────────────────────────────────────── pipeline = dai.Pipeline() # Nó da câmera RGB cam_rgb = pipeline.create(dai.node.ColorCamera) cam_rgb.setPreviewSize(640, 480) 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) # Nó de profundidade mono_left = pipeline.create(dai.node.MonoCamera) mono_right = pipeline.create(dai.node.MonoCamera) mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P) mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P) mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT) mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT) stereo = pipeline.create(dai.node.StereoDepth) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.ROBOTICS) #stereo.initialConfig.setConfidenceThreshold(250) # Remove ruído, mantendo apenas pontos confiáveis #stereo.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) # Usa filtro de mediana forte #stereo.setLeftRightCheck(True) # Ativa verificação para evitar erros #stereo.setExtendedDisparity(False) # Reduz ruído em distâncias curtas #stereo.setSubpixel(True) # Aumenta a precisão da profundidade 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) # ─────────────────────────────────────────────── # Callback de Mouse para exibir informações da célula clicada # ─────────────────────────────────────────────── def mouse_callback(event, x, y, flags, param): global current_depth_frame, ground_reference, air_reference, last_detect_ground, last_detect_air if event != cv2.EVENT_LBUTTONDOWN or current_depth_frame is None: return # Se o clique for na região do solo: if y >= HEIGHT - GROUND_REGION_HEIGHT: local_y = y - (HEIGHT - GROUND_REGION_HEIGHT) row_height_ground = GROUND_REGION_HEIGHT // GROUND_ROWS row = int(local_y // row_height_ground) if row < 0 or row >= GROUND_ROWS: print("Clique fora da região do solo.") return # Calcular a escala para essa linha scale = GROUND_TOP_SCALE + (row / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) start_x = int(WIDTH / 2 - effective_width / 2) if x < start_x or x > start_x + effective_width: print("Clique fora das células da região do solo.") return cell_width = effective_width / GROUND_CELLS col = int((x - start_x) // cell_width) ref_val = ground_reference[row, col] detect_val = last_detect_ground[row, col] pixel_val = current_depth_frame[y, x] print(f"Ground cell [{row}, {col}]: Calibração: {ref_val:.2f}, Média detectada: {detect_val:.2f}, Pixel: {pixel_val:.2f}") # Se o clique for na região aérea: elif y < AIR_REGION_HEIGHT: row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS row = int(y // row_height_air) col = int(x // cell_width_air) if row < 0 or row >= AIR_ROWS or col < 0 or col >= AIR_COLS: print("Clique fora da região aérea.") return ref_val = air_reference[row, col] detect_val = last_detect_air[row, col] pixel_val = current_depth_frame[y, x] print(f"Air cell [{row}, {col}]: Calibração: {ref_val:.2f}, Média detectada: {detect_val:.2f}, Pixel: {pixel_val:.2f}") else: print("Clique fora das regiões definidas.") cv2.namedWindow("Deteccao de Obstaculos - OAK-D Lite") cv2.setMouseCallback("Deteccao de Obstaculos - OAK-D Lite", mouse_callback) cv2.namedWindow("Mapa de Calor da Profundidade") cv2.setMouseCallback("Mapa de Calor da Profundidade", mouse_callback) # ─────────────────────────────────────────────── # Execução do dispositivo e funções de calibração # ─────────────────────────────────────────────── 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) # Função para calibrar a região do solo utilizando a moda def calibrate_ground(): print("Calibrando região do solo: coletando {} frames...".format(NUM_CALIB_FRAMES)) calib_data = np.zeros((GROUND_ROWS, GROUND_CELLS, NUM_CALIB_FRAMES)) for frame_idx in range(NUM_CALIB_FRAMES): depth_frame = depth_queue.get().getFrame() depth_frame = apply_depth_filters(depth_frame) for i in range(GROUND_ROWS): y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground y_end = y_start + row_height_ground scale = GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS for j in range(GROUND_CELLS): x_start = int(WIDTH / 2 - effective_width / 2 + j * cell_width) x_end = int(x_start + cell_width) calib_data[i, j, frame_idx] = np.mean(depth_frame[y_start:y_end, x_start:x_end]) ground_ref = np.zeros((GROUND_ROWS, GROUND_CELLS)) for i in range(GROUND_ROWS): for j in range(GROUND_CELLS): ground_ref[i, j] = calc_mode(calib_data[i, j, :]) print("Calibração da região do solo concluída!") return ground_ref # Função para calibrar a região aérea utilizando a moda def calibrate_air(): print("Calibrando região aérea: coletando {} frames...".format(NUM_CALIB_FRAMES)) calib_data_air = np.zeros((AIR_ROWS, AIR_COLS, NUM_CALIB_FRAMES)) row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS for frame_idx in range(NUM_CALIB_FRAMES): depth_frame = depth_queue.get().getFrame() depth_frame = apply_depth_filters(depth_frame) for i in range(AIR_ROWS): y_start = i * row_height_air y_end = y_start + row_height_air for j in range(AIR_COLS): x_start = j * cell_width_air x_end = x_start + cell_width_air calib_data_air[i, j, frame_idx] = np.mean(depth_frame[y_start:y_end, x_start:x_end]) air_ref = np.zeros((AIR_ROWS, AIR_COLS)) for i in range(AIR_ROWS): for j in range(AIR_COLS): air_ref[i, j] = calc_mode(calib_data_air[i, j, :]) print("Calibração da região aérea concluída!") return air_ref # Função para calibrar a distância utilizando a moda def calibrate_distance(alvo=2500): global GROUND_REGION_HEIGHT, AIR_REGION_HEIGHT # Garantir que estamos alterando as variáveis globais NUM_LINHAS_ANALISE = 80 # Número de linhas horizontais para análise ALTURA_LINHA = HEIGHT // NUM_LINHAS_ANALISE # Altura de cada linha PROFUNDIDADE_ALVO = alvo # Profundidade alvo em cm para definir a região do solo dinamicamente media_profundidade_acumulada = np.zeros((NUM_LINHAS_ANALISE, NUM_CALIB_FRAMES)) print("Calibrando profundidade média com {} frames...".format(NUM_CALIB_FRAMES)) for frame_idx in range(NUM_CALIB_FRAMES): depth_frame = depth_queue.get().getFrame() depth_frame = apply_depth_filters(depth_frame) for i in range(NUM_LINHAS_ANALISE): y_start = HEIGHT - (i + 1) * ALTURA_LINHA y_end = y_start + ALTURA_LINHA x_start_crop = int(WIDTH * 0.0) x_end_crop = int(WIDTH * 1.0) region_values = depth_frame[y_start:y_end, x_start_crop:x_end_crop].flatten() valid_values = region_values[(region_values > 0) & (region_values < 10000)] if valid_values.size > 0: media_profundidade_acumulada[i, frame_idx] = np.mean(valid_values) else: media_profundidade_acumulada[i, frame_idx] = 9999 media_profundidade = np.array([calc_mode(media_profundidade_acumulada[i, :]) for i in range(NUM_LINHAS_ANALISE)]) #print("Médias de profundidade por linha (usando moda):", media_profundidade) erro_minimo = float('inf') linha_alvo = None for i in range(NUM_LINHAS_ANALISE): erro = abs(media_profundidade[i] - PROFUNDIDADE_ALVO) if erro < erro_minimo: erro_minimo = erro linha_alvo = i if linha_alvo is not None: linha_alvo += 1 GROUND_REGION_HEIGHT = HEIGHT - ((NUM_LINHAS_ANALISE - linha_alvo) * ALTURA_LINHA) else: GROUND_REGION_HEIGHT = HEIGHT // 2 AIR_REGION_HEIGHT = HEIGHT - GROUND_REGION_HEIGHT v = GROUND_REGION_HEIGHT // GROUND_ROWS print("Novo GROUND_REGION_HEIGHT:", GROUND_REGION_HEIGHT) print("Novo AIR_REGION_HEIGHT:", AIR_REGION_HEIGHT) print("Novo tamanho das linhas do solo:", v) return v # Calibração inicial row_height_ground = calibrate_distance() ground_reference = calibrate_ground() air_reference = calibrate_air() # Preparação para a detecção usando moda (acumula dados de alguns frames) detect_counter = 0 detect_data_ground = np.zeros((GROUND_ROWS, GROUND_CELLS, NUM_DETECT_FRAMES)) detect_data_air = np.zeros((AIR_ROWS, AIR_COLS, NUM_DETECT_FRAMES)) last_detect_ground = np.zeros((GROUND_ROWS, GROUND_CELLS)) last_detect_air = np.zeros((AIR_ROWS, AIR_COLS)) # Parâmetro para o filtro de média móvel alpha = 0.4 # ajuste entre 0 e 1 (valores menores = mais suave) # Inicialize as grids filtradas com os valores de calibração (ou com zeros, se preferir) smoothed_ground = ground_reference.copy() smoothed_air = air_reference.copy() last_time = time.time() target_fps = 30 # Limita o FPS while True: 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() # Aplicação dos filtros em sequência depth_frame = apply_depth_filters(depth_frame) # Gera o mapa de calor heatmap = generate_heatmap(depth_frame) # Cria uma cópia do heatmap para desenhar o overlay heatmap_overlay = heatmap.copy() # Atualiza o frame global para o callback de mouse current_depth_frame = depth_frame.copy() overlay = rgb_frame.copy() # ───────────────────────────────────────────── # Atualiza a grid do SOLO (região inferior) com média móvel # ───────────────────────────────────────────── for i in range(GROUND_ROWS): y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground y_end = y_start + row_height_ground scale = (GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE)) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS for j in range(GROUND_CELLS): x_start = int(WIDTH / 2 - effective_width / 2 + j * cell_width) x_end = int(x_start + cell_width) # Obtém os valores válidos da região region_values = depth_frame[y_start:y_end, x_start:x_end].flatten() valid_values = region_values[(region_values > 0) & (region_values < 10000)] # Remove valores inválidos # Calcula a média da célula e atualiza as variáveis if valid_values.size > 0: measurement = np.mean(valid_values) else: measurement = 9999 # Define um valor alto se não houver dados válidos # Atualiza a grid suavizada com a média móvel smoothed_ground[i, j] = alpha * measurement + (1 - alpha) * smoothed_ground[i, j] # Atualiza a matriz last_detect_ground com a média real da célula last_detect_ground[i, j] = measurement # ───────────────────────────────────────────── # Atualiza a grid da REGIÃO AÉREA (parte superior) com média móvel # ───────────────────────────────────────────── row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS for i in range(AIR_ROWS): y_start = i * row_height_air y_end = y_start + row_height_air for j in range(AIR_COLS): x_start = j * cell_width_air x_end = x_start + cell_width_air # Obtém os valores válidos da região region_values = depth_frame[y_start:y_end, x_start:x_end].flatten() valid_values = region_values[(region_values > 0) & (region_values < 10000)] # Remove valores inválidos # Calcula a média da célula e atualiza as variáveis if valid_values.size > 0: measurement = np.mean(valid_values) else: measurement = 9999 # Define um valor alto se não houver dados válidos # Atualiza a grid suavizada com a média móvel smoothed_air[i, j] = alpha * measurement + (1 - alpha) * smoothed_air[i, j] # Atualiza a matriz last_detect_air com a média real da célula last_detect_air[i, j] = measurement # ───────────────────────────────────────────── # Processamento da detecção: Região do SOLO # ───────────────────────────────────────────── for i in range(GROUND_ROWS): y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground y_end = y_start + row_height_ground scale = (GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE)) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS for j in range(GROUND_CELLS): x_start = int(WIDTH / 2 - effective_width / 2 + j * cell_width) x_end = int(x_start + cell_width) # Compara o valor suavizado com a referência calibrada if smoothed_ground[i, j] < (ground_reference[i, j] - DEPTH_LIMIT): # Obstáculo (valor menor: objeto mais próximo) cv2.rectangle(overlay, (x_start, y_start), (x_end, y_end), (0, 0, 255), -1) cv2.rectangle(heatmap_overlay, (x_start, y_start), (x_end, y_end), (0, 0, 255), -1) elif smoothed_ground[i, j] > (ground_reference[i, j] + DEPTH_LIMIT): # Erosão (valor maior: superfície rebaixada) cv2.rectangle(overlay, (x_start, y_start), (x_end, y_end), (0, 255, 255), -1) cv2.rectangle(heatmap_overlay, (x_start, y_start), (x_end, y_end), (0, 255, 255), -1) cv2.rectangle(rgb_frame, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) if show_grid: cv2.rectangle(heatmap, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) # ───────────────────────────────────────────── # Processamento da detecção: Região AÉREA # ───────────────────────────────────────────── for i in range(AIR_ROWS): y_start = i * row_height_air y_end = y_start + row_height_air for j in range(AIR_COLS): x_start = j * cell_width_air x_end = x_start + cell_width_air if smoothed_air[i, j] < (air_reference[i, j] - DEPTH_LIMIT): cv2.rectangle(overlay, (x_start, y_start), (x_end, y_end), (0, 255, 0), -1) cv2.rectangle(heatmap_overlay, (x_start, y_start), (x_end, y_end), (0, 255, 0), -1) cv2.rectangle(rgb_frame, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) if show_grid: cv2.rectangle(heatmap, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) cv2.addWeighted(overlay, 0.4, rgb_frame, 0.6, 0, rgb_frame) cv2.imshow("Deteccao de Obstaculos - OAK-D Lite", rgb_frame) if show_grid: cv2.addWeighted(heatmap_overlay, 0.4, heatmap, 0.6, 0, heatmap) cv2.imshow("Mapa de Calor da Profundidade", heatmap) key = cv2.waitKey(1) if key == ord('q'): break elif key == ord('c'): print("Recalibrando regiões...") row_height_ground = calibrate_distance() ground_reference = calibrate_ground() air_reference = calibrate_air() elif key == ord('g'): show_grid = True if show_grid == False else False print(f"Grade {'Ligada' if show_grid else 'Desligada'}") elif key == ord('1'): filtro1 = True if filtro1 == False else False print(f"Filtro suavizacao temporal {'Ligado' if filtro1 else 'Desligado'}") elif key == ord('2'): filtro2 = True if filtro2 == False else False print(f"Filtro suavizacao espacial {'Ligado' if filtro2 else 'Desligado'}") elif key == ord('3'): filtro3 = True if filtro3 == False else False print(f"Filtro mediana {'Ligado' if filtro3 else 'Desligado'}") elif key == ord('4'): filtro4 = True if filtro4 == False else False print(f"Filtro remocao outliers {'Ligado' if filtro4 else 'Desligado'}") cv2.destroyAllWindows()