535 lines
25 KiB
Python
535 lines
25 KiB
Python
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()
|