49 lines
1.5 KiB
Python
49 lines
1.5 KiB
Python
import numpy as np
|
|
|
|
def detectar_obstaculos_em_frente(depth_frame, fx, baseline, faixa_altura=(0.45, 0.55), max_distancia_mm=2500):
|
|
"""
|
|
Detecta obstáculos com base no perfil de profundidade frontal.
|
|
|
|
Retorna uma lista de obstáculos com:
|
|
- posição_percentual (0 à 100)
|
|
- distancia (em metros)
|
|
- largura_aproximada (em px)
|
|
"""
|
|
|
|
altura, largura = depth_frame.shape
|
|
y1 = int(altura * faixa_altura[0])
|
|
y2 = int(altura * faixa_altura[1])
|
|
|
|
faixa = depth_frame[y1:y2, :]
|
|
|
|
profundidade_media = np.median(faixa, axis=0)
|
|
with np.errstate(divide='ignore'):
|
|
distancias_mm = (fx * baseline * 1000) / profundidade_media
|
|
distancias_mm = np.clip(distancias_mm, 0, max_distancia_mm)
|
|
|
|
# Simplifica usando limiar: onde há objetos "próximos"
|
|
mascara = (distancias_mm > 100) & (distancias_mm < max_distancia_mm)
|
|
|
|
obstaculos = []
|
|
inicio = None
|
|
|
|
for x in range(largura):
|
|
if mascara[x]:
|
|
if inicio is None:
|
|
inicio = x
|
|
elif inicio is not None:
|
|
fim = x
|
|
centro = (inicio + fim) // 2
|
|
largura_px = fim - inicio
|
|
distancia = np.min(distancias_mm[inicio:fim])
|
|
posicao_pct = 100 * centro / largura
|
|
|
|
obstaculos.append({
|
|
"posicao": round(float(posicao_pct), 1),
|
|
"distancia": round(float(distancia) / 1000, 2),
|
|
"largura_px": largura_px
|
|
})
|
|
inicio = None
|
|
|
|
return obstaculos
|