testes com track objects oak-d
This commit is contained in:
Diego Freitas
parent
8bbe6c4978
commit
4bf18b8b42
|
|
@ -1542,12 +1542,16 @@ namespace AgroBase.Forms.Operacoes
|
|||
txtSonarDecisao.Text = (Log.matriz_confianca?.block?.reason_detail ?? "");
|
||||
|
||||
// OAK-D Lite
|
||||
/*if (gridObstaculos.Columns.Count == 0)
|
||||
if (gridObstaculos.Columns.Count == 0)
|
||||
{
|
||||
gridObstaculos.Columns.Clear();
|
||||
|
||||
gridObstaculos.Columns.Add("clDeteccao", "Detecção");
|
||||
gridObstaculos.Columns.Add("clConfianca", "Confiança");
|
||||
gridObstaculos.Columns.Add("clTrack", "Track");
|
||||
gridObstaculos.Columns.Add("clLateral", "Lateral");
|
||||
gridObstaculos.Columns.Add("clAltura", "Altura");
|
||||
gridObstaculos.Columns.Add("clDistancia", "Distancia");
|
||||
}
|
||||
gridObstaculos.Rows.Clear();
|
||||
if (Log?.deteccao != null)
|
||||
|
|
@ -1557,10 +1561,14 @@ namespace AgroBase.Forms.Operacoes
|
|||
gridObstaculos.Rows.Add
|
||||
(
|
||||
obstaculo.label,
|
||||
obstaculo.conf
|
||||
$"{obstaculo.conf:F2}",
|
||||
obstaculo.track_status,
|
||||
$"{obstaculo.lateral_m:F2}",
|
||||
$"{obstaculo.altura_relativa_m:F2}",
|
||||
$"{obstaculo.distancia_m:F2}"
|
||||
);
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -1579,6 +1587,8 @@ namespace AgroBase.Forms.Operacoes
|
|||
// //picSonarRadar.Image = Log.Leitura.obj.radar_2d.PlotarAnalsie((Bitmap)picSonarRadar.Image.Clone());
|
||||
//}
|
||||
|
||||
if (false)
|
||||
{
|
||||
var LogLvx = LogsOperacao[idxMomentoAtual].LivoxLidar;
|
||||
// MID-360
|
||||
var g = gridObstaculos;
|
||||
|
|
@ -1616,6 +1626,7 @@ namespace AgroBase.Forms.Operacoes
|
|||
pnlTridimensional.Invalidate();
|
||||
view_3d.SetBboxes(LogLvx.bboxes);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -719,6 +719,14 @@ namespace AgroBase.Models.Operadores
|
|||
public double conf { get; set; }
|
||||
public List<double> bbox_norm { get; set; }
|
||||
public List<double> bbox_px { get; set; }
|
||||
public int track_id { get; set; }
|
||||
public string track_status { get; set; }
|
||||
public bool tem_tracker { get; set; }
|
||||
public List<double> xyz_m { get; set; }
|
||||
public double distancia_m { get; set; }
|
||||
public double lateral_m { get; set; }
|
||||
public double altura_relativa_m { get; set; }
|
||||
public List<double> bbox_full { get; set; }
|
||||
|
||||
public VisualWorkerMessageDeteccaoModel Clone()
|
||||
{
|
||||
|
|
@ -728,7 +736,15 @@ namespace AgroBase.Models.Operadores
|
|||
label = label,
|
||||
bbox_norm = bbox_norm,
|
||||
bbox_px = bbox_px,
|
||||
conf = conf
|
||||
conf = conf,
|
||||
track_id = track_id,
|
||||
track_status = track_status,
|
||||
tem_tracker = tem_tracker,
|
||||
xyz_m = xyz_m,
|
||||
distancia_m = distancia_m,
|
||||
lateral_m = lateral_m,
|
||||
altura_relativa_m = altura_relativa_m,
|
||||
bbox_full = bbox_full,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -97,6 +97,12 @@ class CameraOak:
|
|||
|
||||
if self.modelo_ia_det is not None:
|
||||
self.q_det = self.device.getOutputQueue(name="det", maxSize=1, blocking=False)
|
||||
self.q_det_track = None
|
||||
if self.modelo_ia_det.get("com_track", False):
|
||||
try:
|
||||
self.q_det_track = self.device.getOutputQueue(name="det_track", maxSize=1, blocking=False)
|
||||
except:
|
||||
pass
|
||||
|
||||
if self.dispositivo == T_Code.Snr:
|
||||
dadosSnr = ContextoGlobalRedis.get_operacao().get("Snr", {})
|
||||
|
|
@ -405,6 +411,7 @@ class CameraOak:
|
|||
ROI_INICIO = self.modelo_ia_det["ia_roi_begin"]
|
||||
ROI_TAMANHO = self.modelo_ia_det["ia_roi_size"]
|
||||
CONF = self.modelo_ia_det["ia_conf"]
|
||||
TRACK = self.modelo_ia_det["com_track"]
|
||||
blob_path = self.modelo_ia_det["ia_model_path"]
|
||||
|
||||
y1 = 1.0 - (ROI_INICIO + ROI_TAMANHO)
|
||||
|
|
@ -418,6 +425,7 @@ class CameraOak:
|
|||
manip_det.initialConfig.setFrameType(dai.RawImgFrame.Type.RGB888p)
|
||||
|
||||
det = pipeline.createMobileNetDetectionNetwork()
|
||||
#det = pipeline.createMobileNetSpatialDetectionNetwork()
|
||||
det.setBlobPath(blob_path)
|
||||
det.setConfidenceThreshold(CONF)
|
||||
det.setNumInferenceThreads(2)
|
||||
|
|
@ -428,6 +436,32 @@ class CameraOak:
|
|||
xout_det = pipeline.createXLinkOut()
|
||||
xout_det.setStreamName("det")
|
||||
det.out.link(xout_det.input)
|
||||
#stereo.depth.link(det.inputDepth)
|
||||
|
||||
# Tracker oficial da OAK, sem alterar a saída "det"
|
||||
if TRACK:
|
||||
manip_track = pipeline.createImageManip()
|
||||
manip_track.initialConfig.setCropRect(0.0, y1, 1.0, y2)
|
||||
manip_track.initialConfig.setResize(RESOLUCAO[0], RESOLUCAO[1])
|
||||
manip_track.initialConfig.setKeepAspectRatio(True)
|
||||
manip_track.initialConfig.setFrameType(dai.RawImgFrame.Type.BGR888p)
|
||||
|
||||
tracker = pipeline.create(dai.node.ObjectTracker)
|
||||
tracker.setTrackerType(dai.TrackerType.ZERO_TERM_COLOR_HISTOGRAM)
|
||||
tracker.setTrackerIdAssignmentPolicy(dai.TrackerIdAssignmentPolicy.SMALLEST_ID)
|
||||
|
||||
# Mesmo frame usado pela detecção, mas convertido para BGR aceito pelo tracker
|
||||
script.outputs['toDet'].link(manip_track.inputImage)
|
||||
|
||||
manip_track.out.link(tracker.inputTrackerFrame)
|
||||
manip_track.out.link(tracker.inputDetectionFrame)
|
||||
|
||||
# As detecções reais continuam vindo do modelo
|
||||
det.out.link(tracker.inputDetections)
|
||||
|
||||
xout_track = pipeline.createXLinkOut()
|
||||
xout_track.setStreamName("det_track")
|
||||
tracker.out.link(xout_track.input)
|
||||
|
||||
script.outputs['toDet'].link(manip_det.inputImage)
|
||||
#cam.video.link(manip_det.inputImage)
|
||||
|
|
@ -628,10 +662,18 @@ class CameraOak:
|
|||
"bbox_px": [x0p, y0p, x1p, y1p],
|
||||
}
|
||||
|
||||
# Se for SpatialDetectionNetwork, adiciona XYZ (em metros)
|
||||
# Se for SpatialDetectionNetwork, adiciona XYZ em metros
|
||||
if hasattr(d, "spatialCoordinates"):
|
||||
sc = d.spatialCoordinates
|
||||
item["xyz_m"] = [float(sc.x) / 1000.0, float(sc.y) / 1000.0, float(sc.z) / 1000.0]
|
||||
|
||||
x_m = float(sc.x) / 1000.0
|
||||
y_m = float(sc.y) / 1000.0
|
||||
z_m = float(sc.z) / 1000.0
|
||||
|
||||
item["xyz_m"] = [x_m, y_m, z_m]
|
||||
item["lateral_m"] = x_m
|
||||
item["altura_relativa_m"] = y_m
|
||||
item["distancia_m"] = z_m
|
||||
|
||||
# Opcional: mapear para o frame completo (leva em conta ROI da detecção)
|
||||
if mapear_para_fullframe:
|
||||
|
|
@ -641,6 +683,67 @@ class CameraOak:
|
|||
|
||||
dets.append(item)
|
||||
|
||||
tracklets = []
|
||||
if getattr(self, "q_det_track", None) is not None:
|
||||
pkt_track = self.q_det_track.tryGet()
|
||||
if pkt_track is not None:
|
||||
tracklets = getattr(pkt_track, "tracklets", [])
|
||||
|
||||
def _iou(a, b):
|
||||
ax0, ay0, ax1, ay1 = a
|
||||
bx0, by0, bx1, by1 = b
|
||||
|
||||
ix0 = max(ax0, bx0)
|
||||
iy0 = max(ay0, by0)
|
||||
ix1 = min(ax1, bx1)
|
||||
iy1 = min(ay1, by1)
|
||||
|
||||
iw = max(0, ix1 - ix0)
|
||||
ih = max(0, iy1 - iy0)
|
||||
inter = iw * ih
|
||||
|
||||
area_a = max(0, ax1 - ax0) * max(0, ay1 - ay0)
|
||||
area_b = max(0, bx1 - bx0) * max(0, by1 - by0)
|
||||
|
||||
union = area_a + area_b - inter
|
||||
return inter / union if union > 0 else 0.0
|
||||
|
||||
for d in dets:
|
||||
best_t = None
|
||||
best_iou = 0.0
|
||||
|
||||
db = d["bbox_norm"]
|
||||
d_label = d.get("label_id", -1)
|
||||
|
||||
for t in tracklets:
|
||||
if int(t.label) != int(d_label):
|
||||
continue
|
||||
|
||||
roi = t.roi
|
||||
tb = [
|
||||
float(roi.topLeft().x),
|
||||
float(roi.topLeft().y),
|
||||
float(roi.bottomRight().x),
|
||||
float(roi.bottomRight().y),
|
||||
]
|
||||
|
||||
score = _iou(db, tb)
|
||||
|
||||
if score > best_iou:
|
||||
best_iou = score
|
||||
best_t = t
|
||||
|
||||
if best_t is not None and best_iou > 0.2:
|
||||
d["track_id"] = int(best_t.id)
|
||||
d["track_status"] = best_t.status.name
|
||||
d["tem_tracker"] = True
|
||||
d["track_iou"] = float(best_iou)
|
||||
else:
|
||||
d["track_id"] = None
|
||||
d["track_status"] = None
|
||||
d["tem_tracker"] = False
|
||||
d["track_iou"] = 0.0
|
||||
|
||||
dur = time.time() - start
|
||||
self.timestamp_ultima_deteccao = time.time()
|
||||
return dets, {"erro": None, "duracao": dur, "frame_valido": True}
|
||||
|
|
|
|||
|
|
@ -566,6 +566,17 @@ class CameraManager:
|
|||
x1r, y1r = int(rx2 * W), int(ry2 * H)
|
||||
cv2.rectangle(img, (x0r, y0r), (x1r, y1r), (60, 60, 60), 1)
|
||||
|
||||
cv2.putText(
|
||||
img,
|
||||
f"dets recebidas: {len(dets)}",
|
||||
(10, 48),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.6,
|
||||
(0, 255, 255),
|
||||
2,
|
||||
cv2.LINE_AA
|
||||
)
|
||||
|
||||
# desenhar detecções
|
||||
for d in dets:
|
||||
if d.get("conf", 0.0) < conf_thr:
|
||||
|
|
@ -574,9 +585,21 @@ class CameraManager:
|
|||
# bbox em px do frame
|
||||
if "bbox_full" in d and d["bbox_full"]:
|
||||
x0, y0, x1, y1 = d["bbox_full"]
|
||||
# clamp se necessário
|
||||
x0 = max(0, min(W - 1, int(x0))); x1 = max(0, min(W - 1, int(x1)))
|
||||
y0 = max(0, min(H - 1, int(y0))); y1 = max(0, min(H - 1, int(y1)))
|
||||
|
||||
# bbox_full provavelmente está no frame original 1920x1080
|
||||
src_w = getattr(self, "frame_size", (1920, 1080))[0]
|
||||
src_h = getattr(self, "frame_size", (1920, 1080))[1]
|
||||
|
||||
sx = W / float(src_w)
|
||||
sy = H / float(src_h)
|
||||
|
||||
x0 = int(round(x0 * sx))
|
||||
x1 = int(round(x1 * sx))
|
||||
y0 = int(round(y0 * sy))
|
||||
y1 = int(round(y1 * sy))
|
||||
|
||||
x0 = max(0, min(W - 1, x0)); x1 = max(0, min(W - 1, x1))
|
||||
y0 = max(0, min(H - 1, y0)); y1 = max(0, min(H - 1, y1))
|
||||
else:
|
||||
bn = d.get("bbox_norm", None)
|
||||
if not bn:
|
||||
|
|
@ -592,7 +615,20 @@ class CameraManager:
|
|||
cv2.rectangle(img, (x0, y0), (x1, y1), color, 2)
|
||||
|
||||
name = d.get("label", None)
|
||||
dist = d.get("distancia_m", None)
|
||||
track_id = d.get("track_id", None)
|
||||
status = d.get("track_status", None)
|
||||
|
||||
txt = f"{name or f'id:{lid}'} {d.get('conf', 0.0):.2f}"
|
||||
|
||||
if track_id is not None:
|
||||
txt += f" T:{track_id}"
|
||||
|
||||
if status:
|
||||
txt += f" {status}"
|
||||
|
||||
if dist is not None:
|
||||
txt += f" Z:{dist:.2f}m"
|
||||
_put_label(img, txt, x0, y0, color)
|
||||
|
||||
# FPS (EMA)
|
||||
|
|
|
|||
|
|
@ -92,6 +92,7 @@ def reload_seg_config():
|
|||
def load_det_config():
|
||||
_CONFIG_DET = {
|
||||
"debug_visual": False,
|
||||
"com_track": False,
|
||||
"ia_roi_begin": 0.0,
|
||||
"ia_roi_size": 1.0,
|
||||
"ia_resolution": [300,300],
|
||||
|
|
|
|||
|
|
@ -0,0 +1,316 @@
|
|||
import cv2
|
||||
import depthai as dai
|
||||
import numpy as np
|
||||
import time
|
||||
import math
|
||||
|
||||
|
||||
# =========================
|
||||
# CONFIGURAÇÕES
|
||||
# =========================
|
||||
|
||||
MIN_DEPTH_MM = 300
|
||||
MAX_DEPTH_MM = 3500
|
||||
DANGER_DEPTH_MM = 1600
|
||||
|
||||
MIN_AREA_PX = 250
|
||||
MAX_LOST_FRAMES = 10
|
||||
TRACK_MAX_DIST = 80
|
||||
|
||||
ROI_TOP = 0.25
|
||||
ROI_BOTTOM = 0.95
|
||||
ROI_LEFT = 0.15
|
||||
ROI_RIGHT = 0.85
|
||||
|
||||
# Quanto o pixel precisa ser "mais perto" que o chão esperado
|
||||
# para ser considerado saliência/obstáculo.
|
||||
GROUND_DELTA_MM = 220
|
||||
|
||||
# Confirmação temporal simples
|
||||
MIN_TRACK_AGE_FOR_DANGER = 2
|
||||
|
||||
|
||||
class SimpleBlobTracker:
|
||||
def __init__(self):
|
||||
self.next_id = 1
|
||||
self.tracks = {}
|
||||
|
||||
def update(self, detections):
|
||||
updated = []
|
||||
used_tracks = set()
|
||||
|
||||
for det in detections:
|
||||
best_id = None
|
||||
best_dist = 999999
|
||||
|
||||
for tid, tr in self.tracks.items():
|
||||
if tid in used_tracks:
|
||||
continue
|
||||
|
||||
dx = det["cx"] - tr["cx"]
|
||||
dy = det["cy"] - tr["cy"]
|
||||
dz = (det["z_mm"] - tr["z_mm"]) / 30.0
|
||||
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
|
||||
|
||||
if dist < best_dist:
|
||||
best_dist = dist
|
||||
best_id = tid
|
||||
|
||||
if best_id is not None and best_dist < TRACK_MAX_DIST:
|
||||
tid = best_id
|
||||
used_tracks.add(tid)
|
||||
self.tracks[tid].update(det)
|
||||
self.tracks[tid]["lost"] = 0
|
||||
self.tracks[tid]["age"] += 1
|
||||
else:
|
||||
tid = self.next_id
|
||||
self.next_id += 1
|
||||
self.tracks[tid] = dict(det)
|
||||
self.tracks[tid]["lost"] = 0
|
||||
self.tracks[tid]["age"] = 1
|
||||
|
||||
out = dict(self.tracks[tid])
|
||||
out["id"] = tid
|
||||
updated.append(out)
|
||||
|
||||
for tid in list(self.tracks.keys()):
|
||||
if tid not in used_tracks and all(d.get("id") != tid for d in updated):
|
||||
self.tracks[tid]["lost"] += 1
|
||||
if self.tracks[tid]["lost"] > MAX_LOST_FRAMES:
|
||||
del self.tracks[tid]
|
||||
|
||||
return updated
|
||||
|
||||
|
||||
def build_ground_model_by_row(roi_depth):
|
||||
"""
|
||||
Estima a profundidade esperada do chão em cada linha da ROI.
|
||||
Usa percentil alto, porque o chão costuma ser a superfície mais distante
|
||||
dentro da linha quando há obstáculos mais próximos.
|
||||
"""
|
||||
rh, rw = roi_depth.shape
|
||||
ground = np.zeros(rh, dtype=np.float32)
|
||||
|
||||
for y in range(rh):
|
||||
row = roi_depth[y, :]
|
||||
valid = row[(row > MIN_DEPTH_MM) & (row < MAX_DEPTH_MM)]
|
||||
|
||||
if len(valid) < 20:
|
||||
ground[y] = np.nan
|
||||
else:
|
||||
ground[y] = np.percentile(valid, 75)
|
||||
|
||||
# Interpola linhas inválidas
|
||||
idx = np.arange(rh)
|
||||
good = np.isfinite(ground)
|
||||
|
||||
if np.count_nonzero(good) < 5:
|
||||
return None
|
||||
|
||||
ground = np.interp(idx, idx[good], ground[good])
|
||||
|
||||
# Suaviza o perfil do chão
|
||||
ground = cv2.GaussianBlur(ground.reshape(-1, 1), (1, 31), 0).reshape(-1)
|
||||
|
||||
return ground
|
||||
|
||||
|
||||
# =========================
|
||||
# PIPELINE OAK-D LITE
|
||||
# =========================
|
||||
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
cam_rgb = pipeline.create(dai.node.ColorCamera)
|
||||
cam_rgb.setPreviewSize(640, 400)
|
||||
cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
|
||||
cam_rgb.setInterleaved(False)
|
||||
cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
|
||||
cam_rgb.setFps(30)
|
||||
|
||||
mono_left = pipeline.create(dai.node.MonoCamera)
|
||||
mono_right = pipeline.create(dai.node.MonoCamera)
|
||||
|
||||
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
|
||||
mono_left.setBoardSocket(dai.CameraBoardSocket.CAM_B)
|
||||
mono_right.setBoardSocket(dai.CameraBoardSocket.CAM_C)
|
||||
|
||||
stereo = pipeline.create(dai.node.StereoDepth)
|
||||
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT)
|
||||
stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A)
|
||||
stereo.setSubpixel(True)
|
||||
stereo.setLeftRightCheck(True)
|
||||
|
||||
mono_left.out.link(stereo.left)
|
||||
mono_right.out.link(stereo.right)
|
||||
|
||||
xout_rgb = pipeline.create(dai.node.XLinkOut)
|
||||
xout_depth = pipeline.create(dai.node.XLinkOut)
|
||||
|
||||
xout_rgb.setStreamName("rgb")
|
||||
xout_depth.setStreamName("depth")
|
||||
|
||||
cam_rgb.preview.link(xout_rgb.input)
|
||||
stereo.depth.link(xout_depth.input)
|
||||
|
||||
|
||||
tracker = SimpleBlobTracker()
|
||||
|
||||
with dai.Device(pipeline) as device:
|
||||
q_rgb = device.getOutputQueue("rgb", maxSize=1, blocking=True)
|
||||
q_depth = device.getOutputQueue("depth", maxSize=1, blocking=True)
|
||||
|
||||
last = time.time()
|
||||
fps = 0
|
||||
|
||||
while True:
|
||||
frame = q_rgb.get().getCvFrame()
|
||||
depth = q_depth.get().getFrame()
|
||||
|
||||
h, w = frame.shape[:2]
|
||||
|
||||
if depth.shape[:2] != (h, w):
|
||||
depth = cv2.resize(depth, (w, h), interpolation=cv2.INTER_NEAREST)
|
||||
|
||||
now = time.time()
|
||||
fps = 0.9 * fps + 0.1 * (1 / max(now - last, 1e-6))
|
||||
last = now
|
||||
|
||||
x1_roi = int(w * ROI_LEFT)
|
||||
x2_roi = int(w * ROI_RIGHT)
|
||||
y1_roi = int(h * ROI_TOP)
|
||||
y2_roi = int(h * ROI_BOTTOM)
|
||||
|
||||
roi_depth = depth[y1_roi:y2_roi, x1_roi:x2_roi]
|
||||
rh, rw = roi_depth.shape
|
||||
|
||||
ground = build_ground_model_by_row(roi_depth)
|
||||
|
||||
if ground is None:
|
||||
obstacle_mask = np.zeros_like(roi_depth, dtype=np.uint8)
|
||||
ground_vis = np.zeros_like(roi_depth, dtype=np.uint8)
|
||||
detections = []
|
||||
else:
|
||||
ground_2d = np.repeat(ground[:, None], rw, axis=1)
|
||||
|
||||
valid_mask = (
|
||||
(roi_depth > MIN_DEPTH_MM) &
|
||||
(roi_depth < MAX_DEPTH_MM)
|
||||
)
|
||||
|
||||
# Obstáculo = pixel válido significativamente mais perto que o chão esperado
|
||||
diff = ground_2d - roi_depth
|
||||
|
||||
obstacle_mask = np.zeros_like(roi_depth, dtype=np.uint8)
|
||||
obstacle_mask[(valid_mask) & (diff > GROUND_DELTA_MM)] = 255
|
||||
|
||||
kernel = np.ones((5, 5), np.uint8)
|
||||
obstacle_mask = cv2.morphologyEx(obstacle_mask, cv2.MORPH_OPEN, kernel)
|
||||
obstacle_mask = cv2.morphologyEx(obstacle_mask, cv2.MORPH_CLOSE, kernel)
|
||||
|
||||
contours, _ = cv2.findContours(
|
||||
obstacle_mask,
|
||||
cv2.RETR_EXTERNAL,
|
||||
cv2.CHAIN_APPROX_SIMPLE
|
||||
)
|
||||
|
||||
detections = []
|
||||
|
||||
for cnt in contours:
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < MIN_AREA_PX:
|
||||
continue
|
||||
|
||||
x, y, bw, bh = cv2.boundingRect(cnt)
|
||||
|
||||
gx = x + x1_roi
|
||||
gy = y + y1_roi
|
||||
gcx = gx + bw // 2
|
||||
gcy = gy + bh // 2
|
||||
|
||||
blob_depth = roi_depth[y:y + bh, x:x + bw]
|
||||
valid = blob_depth[
|
||||
(blob_depth > MIN_DEPTH_MM) &
|
||||
(blob_depth < MAX_DEPTH_MM)
|
||||
]
|
||||
|
||||
if len(valid) < 50:
|
||||
continue
|
||||
|
||||
z_mm = float(np.median(valid))
|
||||
saliencia_mm = float(np.median(diff[y:y + bh, x:x + bw][obstacle_mask[y:y + bh, x:x + bw] > 0]))
|
||||
|
||||
detections.append({
|
||||
"bbox": (gx, gy, gx + bw, gy + bh),
|
||||
"cx": gcx,
|
||||
"cy": gcy,
|
||||
"z_mm": z_mm,
|
||||
"area": area,
|
||||
"saliencia_mm": saliencia_mm
|
||||
})
|
||||
|
||||
ground_vis = np.clip(diff, 0, 800)
|
||||
ground_vis = (ground_vis / 800.0 * 255).astype(np.uint8)
|
||||
ground_vis = cv2.applyColorMap(ground_vis, cv2.COLORMAP_JET)
|
||||
|
||||
tracked = tracker.update(detections)
|
||||
|
||||
perigo = False
|
||||
|
||||
cv2.rectangle(frame, (x1_roi, y1_roi), (x2_roi, y2_roi), (255, 255, 0), 1)
|
||||
|
||||
for obj in tracked:
|
||||
x1, y1, x2, y2 = obj["bbox"]
|
||||
z_m = obj["z_mm"] / 1000.0
|
||||
center_percent = obj["cx"] / w * 100.0
|
||||
sal = obj.get("saliencia_mm", 0)
|
||||
|
||||
danger = (
|
||||
obj["z_mm"] < DANGER_DEPTH_MM and
|
||||
obj["age"] >= MIN_TRACK_AGE_FOR_DANGER
|
||||
)
|
||||
|
||||
if danger:
|
||||
perigo = True
|
||||
|
||||
color = (0, 0, 255) if danger else (0, 255, 0)
|
||||
|
||||
texto = (
|
||||
f"ID {obj['id']} | Z={z_m:.2f}m | "
|
||||
f"X={center_percent:.0f}% | sal={sal:.0f}mm | age={obj['age']}"
|
||||
)
|
||||
|
||||
cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)
|
||||
cv2.circle(frame, (obj["cx"], obj["cy"]), 4, (0, 255, 255), -1)
|
||||
cv2.putText(frame, texto, (x1, max(20, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.42, color, 1)
|
||||
|
||||
cv2.putText(frame, f"FPS: {fps:.1f}", (10, 25),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
|
||||
|
||||
cv2.putText(frame, f"Obstaculos: {len(tracked)} | Ground delta: {GROUND_DELTA_MM}mm",
|
||||
(10, 52), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
|
||||
(255, 255, 255), 1)
|
||||
|
||||
if perigo:
|
||||
cv2.putText(frame, "PERIGO: saliencia no caminho - PARAR", (10, 82),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
|
||||
|
||||
depth_vis = depth.copy()
|
||||
depth_vis[depth_vis == 0] = MAX_DEPTH_MM
|
||||
depth_vis = np.clip(depth_vis, MIN_DEPTH_MM, MAX_DEPTH_MM)
|
||||
depth_vis = ((MAX_DEPTH_MM - depth_vis) / (MAX_DEPTH_MM - MIN_DEPTH_MM) * 255).astype(np.uint8)
|
||||
depth_vis = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET)
|
||||
|
||||
cv2.imshow("OAK-D Lite - Ground Obstacle Tracker", frame)
|
||||
cv2.imshow("Depth view", depth_vis)
|
||||
cv2.imshow("Obstacle mask", obstacle_mask)
|
||||
cv2.imshow("Ground diff / saliencia", ground_vis)
|
||||
|
||||
key = cv2.waitKey(1)
|
||||
if key == ord("q") or key == 27:
|
||||
break
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
|
|
@ -0,0 +1,263 @@
|
|||
import cv2
|
||||
import depthai as dai
|
||||
import numpy as np
|
||||
import time
|
||||
import math
|
||||
|
||||
|
||||
# =========================
|
||||
# CONFIGURAÇÕES DO TESTE
|
||||
# =========================
|
||||
|
||||
MIN_DEPTH_MM = 300 # ignora muito perto
|
||||
MAX_DEPTH_MM = 3000 # só olha até 3 m
|
||||
DANGER_DEPTH_MM = 1500 # abaixo disso marca perigo
|
||||
|
||||
MIN_AREA_PX = 450 # área mínima do blob
|
||||
MAX_LOST_FRAMES = 10 # quantos frames mantém ID sem ver
|
||||
TRACK_MAX_DIST = 90 # distância máxima em pixels para associar ID
|
||||
|
||||
ROI_TOP = 0.25 # começa em 25% da altura
|
||||
ROI_BOTTOM = 0.95 # termina em 95% da altura
|
||||
ROI_LEFT = 0.15 # ignora bordas
|
||||
ROI_RIGHT = 0.85
|
||||
|
||||
|
||||
# =========================
|
||||
# TRACKER SIMPLES POR CENTRO
|
||||
# =========================
|
||||
|
||||
class SimpleBlobTracker:
|
||||
def __init__(self):
|
||||
self.next_id = 1
|
||||
self.tracks = {}
|
||||
|
||||
def update(self, detections):
|
||||
# detections: lista de dicts com cx, cy, z_mm, bbox, area
|
||||
updated = []
|
||||
used_tracks = set()
|
||||
|
||||
for det in detections:
|
||||
best_id = None
|
||||
best_dist = 999999
|
||||
|
||||
for tid, tr in self.tracks.items():
|
||||
if tid in used_tracks:
|
||||
continue
|
||||
|
||||
dx = det["cx"] - tr["cx"]
|
||||
dy = det["cy"] - tr["cy"]
|
||||
dz = (det["z_mm"] - tr["z_mm"]) / 30.0 # peso leve para profundidade
|
||||
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
|
||||
|
||||
if dist < best_dist:
|
||||
best_dist = dist
|
||||
best_id = tid
|
||||
|
||||
if best_id is not None and best_dist < TRACK_MAX_DIST:
|
||||
tid = best_id
|
||||
used_tracks.add(tid)
|
||||
|
||||
self.tracks[tid].update(det)
|
||||
self.tracks[tid]["lost"] = 0
|
||||
self.tracks[tid]["age"] += 1
|
||||
else:
|
||||
tid = self.next_id
|
||||
self.next_id += 1
|
||||
|
||||
self.tracks[tid] = dict(det)
|
||||
self.tracks[tid]["lost"] = 0
|
||||
self.tracks[tid]["age"] = 1
|
||||
|
||||
out = dict(self.tracks[tid])
|
||||
out["id"] = tid
|
||||
updated.append(out)
|
||||
|
||||
# envelhece tracks não usados
|
||||
for tid in list(self.tracks.keys()):
|
||||
if tid not in used_tracks and all(d.get("id") != tid for d in updated):
|
||||
self.tracks[tid]["lost"] += 1
|
||||
if self.tracks[tid]["lost"] > MAX_LOST_FRAMES:
|
||||
del self.tracks[tid]
|
||||
|
||||
return updated
|
||||
|
||||
|
||||
# =========================
|
||||
# PIPELINE OAK-D LITE
|
||||
# =========================
|
||||
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
# RGB
|
||||
cam_rgb = pipeline.create(dai.node.ColorCamera)
|
||||
cam_rgb.setPreviewSize(640, 400)
|
||||
cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
|
||||
cam_rgb.setInterleaved(False)
|
||||
cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
|
||||
cam_rgb.setFps(30)
|
||||
|
||||
# Mono stereo
|
||||
mono_left = pipeline.create(dai.node.MonoCamera)
|
||||
mono_right = pipeline.create(dai.node.MonoCamera)
|
||||
|
||||
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
|
||||
mono_left.setBoardSocket(dai.CameraBoardSocket.CAM_B)
|
||||
mono_right.setBoardSocket(dai.CameraBoardSocket.CAM_C)
|
||||
|
||||
# Depth
|
||||
stereo = pipeline.create(dai.node.StereoDepth)
|
||||
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT)
|
||||
stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A)
|
||||
stereo.setSubpixel(True)
|
||||
stereo.setLeftRightCheck(True)
|
||||
|
||||
mono_left.out.link(stereo.left)
|
||||
mono_right.out.link(stereo.right)
|
||||
|
||||
# Outputs
|
||||
xout_rgb = pipeline.create(dai.node.XLinkOut)
|
||||
xout_depth = pipeline.create(dai.node.XLinkOut)
|
||||
|
||||
xout_rgb.setStreamName("rgb")
|
||||
xout_depth.setStreamName("depth")
|
||||
|
||||
cam_rgb.preview.link(xout_rgb.input)
|
||||
stereo.depth.link(xout_depth.input)
|
||||
|
||||
|
||||
# =========================
|
||||
# PROCESSAMENTO
|
||||
# =========================
|
||||
|
||||
tracker = SimpleBlobTracker()
|
||||
|
||||
with dai.Device(pipeline) as device:
|
||||
q_rgb = device.getOutputQueue("rgb", maxSize=4, blocking=False)
|
||||
q_depth = device.getOutputQueue("depth", maxSize=4, blocking=False)
|
||||
|
||||
last = time.time()
|
||||
fps = 0
|
||||
|
||||
while True:
|
||||
in_rgb = q_rgb.get()
|
||||
in_depth = q_depth.get()
|
||||
|
||||
frame = in_rgb.getCvFrame()
|
||||
depth = in_depth.getFrame() # uint16 em mm
|
||||
|
||||
h, w = frame.shape[:2]
|
||||
|
||||
if depth.shape[:2] != (h, w):
|
||||
depth = cv2.resize(depth, (w, h), interpolation=cv2.INTER_NEAREST)
|
||||
|
||||
now = time.time()
|
||||
fps = 0.9 * fps + 0.1 * (1 / max(now - last, 1e-6))
|
||||
last = now
|
||||
|
||||
# ROI
|
||||
x1_roi = int(w * ROI_LEFT)
|
||||
x2_roi = int(w * ROI_RIGHT)
|
||||
y1_roi = int(h * ROI_TOP)
|
||||
y2_roi = int(h * ROI_BOTTOM)
|
||||
|
||||
roi_depth = depth[y1_roi:y2_roi, x1_roi:x2_roi]
|
||||
|
||||
# Máscara de pixels com profundidade válida/próxima
|
||||
mask = np.zeros_like(roi_depth, dtype=np.uint8)
|
||||
mask[(roi_depth > MIN_DEPTH_MM) & (roi_depth < MAX_DEPTH_MM)] = 255
|
||||
|
||||
# Limpeza de ruído
|
||||
kernel = np.ones((5, 5), np.uint8)
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
|
||||
|
||||
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
detections = []
|
||||
|
||||
for cnt in contours:
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < MIN_AREA_PX:
|
||||
continue
|
||||
|
||||
x, y, bw, bh = cv2.boundingRect(cnt)
|
||||
|
||||
# Coordenadas globais
|
||||
gx = x + x1_roi
|
||||
gy = y + y1_roi
|
||||
gcx = gx + bw // 2
|
||||
gcy = gy + bh // 2
|
||||
|
||||
blob_depth = roi_depth[y:y + bh, x:x + bw]
|
||||
valid = blob_depth[(blob_depth > MIN_DEPTH_MM) & (blob_depth < MAX_DEPTH_MM)]
|
||||
|
||||
if len(valid) < 50:
|
||||
continue
|
||||
|
||||
z_mm = float(np.median(valid))
|
||||
|
||||
detections.append({
|
||||
"bbox": (gx, gy, gx + bw, gy + bh),
|
||||
"cx": gcx,
|
||||
"cy": gcy,
|
||||
"z_mm": z_mm,
|
||||
"area": area
|
||||
})
|
||||
|
||||
tracked = tracker.update(detections)
|
||||
|
||||
perigo = False
|
||||
|
||||
# Desenha ROI
|
||||
cv2.rectangle(frame, (x1_roi, y1_roi), (x2_roi, y2_roi), (255, 255, 0), 1)
|
||||
|
||||
for obj in tracked:
|
||||
x1, y1, x2, y2 = obj["bbox"]
|
||||
z_m = obj["z_mm"] / 1000.0
|
||||
center_percent = obj["cx"] / w * 100.0
|
||||
|
||||
danger = obj["z_mm"] < DANGER_DEPTH_MM
|
||||
if danger:
|
||||
perigo = True
|
||||
|
||||
color = (0, 0, 255) if danger else (0, 255, 0)
|
||||
|
||||
texto = (
|
||||
f"ID {obj['id']} | Z={z_m:.2f}m | "
|
||||
f"X={center_percent:.0f}% | area={int(obj['area'])}"
|
||||
)
|
||||
|
||||
cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)
|
||||
cv2.circle(frame, (obj["cx"], obj["cy"]), 4, (0, 255, 255), -1)
|
||||
cv2.putText(frame, texto, (x1, max(20, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.45, color, 1)
|
||||
|
||||
cv2.putText(frame, f"FPS: {fps:.1f}", (10, 25),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
|
||||
|
||||
cv2.putText(frame, f"Blobs: {len(tracked)}", (10, 52),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1)
|
||||
|
||||
if perigo:
|
||||
cv2.putText(frame, "PERIGO: obstaculo proximo - PARAR", (10, 82),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
|
||||
|
||||
# Preview depth colorido
|
||||
depth_vis = depth.copy()
|
||||
depth_vis[depth_vis == 0] = MAX_DEPTH_MM
|
||||
depth_vis = np.clip(depth_vis, MIN_DEPTH_MM, MAX_DEPTH_MM)
|
||||
depth_vis = ((MAX_DEPTH_MM - depth_vis) / (MAX_DEPTH_MM - MIN_DEPTH_MM) * 255).astype(np.uint8)
|
||||
depth_vis = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET)
|
||||
|
||||
cv2.imshow("OAK-D Lite - Depth Blob Tracker", frame)
|
||||
cv2.imshow("Depth view", depth_vis)
|
||||
cv2.imshow("Mask blobs", mask)
|
||||
|
||||
key = cv2.waitKey(1)
|
||||
if key == ord("q") or key == 27:
|
||||
break
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
|
|
@ -0,0 +1,353 @@
|
|||
import cv2
|
||||
import depthai as dai
|
||||
import blobconverter
|
||||
import numpy as np
|
||||
import time
|
||||
import math
|
||||
|
||||
|
||||
LABELS = [
|
||||
"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus",
|
||||
"car", "cat", "chair", "cow", "diningtable", "dog", "horse",
|
||||
"motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"
|
||||
]
|
||||
|
||||
|
||||
# =========================
|
||||
# CONFIG SEGURANÇA
|
||||
# =========================
|
||||
|
||||
MIN_DEPTH_MM = 300
|
||||
MAX_DEPTH_MM = 3500
|
||||
DANGER_DEPTH_MM = 1600
|
||||
|
||||
MIN_BLOB_AREA_PX = 120
|
||||
TRACK_MAX_DIST = 55
|
||||
MAX_LOST_FRAMES = 10
|
||||
|
||||
ROI_TOP = 0.25
|
||||
ROI_BOTTOM = 0.95
|
||||
ROI_LEFT = 0.12
|
||||
ROI_RIGHT = 0.88
|
||||
|
||||
AI_CONFIDENCE = 0.5
|
||||
AI_DANGER_CLASSES = {
|
||||
"person", "bicycle", "motorbike", "car", "bus", "dog", "cat", "cow", "horse", "sheep"
|
||||
}
|
||||
|
||||
|
||||
class SimpleBlobTracker:
|
||||
def __init__(self):
|
||||
self.next_id = 1
|
||||
self.tracks = {}
|
||||
|
||||
def update(self, detections):
|
||||
updated = []
|
||||
used_tracks = set()
|
||||
|
||||
for det in detections:
|
||||
best_id = None
|
||||
best_dist = 999999
|
||||
|
||||
for tid, tr in self.tracks.items():
|
||||
if tid in used_tracks:
|
||||
continue
|
||||
|
||||
dx = det["cx"] - tr["cx"]
|
||||
dy = det["cy"] - tr["cy"]
|
||||
dz = (det["z_mm"] - tr["z_mm"]) / 30.0
|
||||
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
|
||||
|
||||
if dist < best_dist:
|
||||
best_dist = dist
|
||||
best_id = tid
|
||||
|
||||
if best_id is not None and best_dist < TRACK_MAX_DIST:
|
||||
tid = best_id
|
||||
used_tracks.add(tid)
|
||||
self.tracks[tid].update(det)
|
||||
self.tracks[tid]["lost"] = 0
|
||||
self.tracks[tid]["age"] += 1
|
||||
else:
|
||||
tid = self.next_id
|
||||
self.next_id += 1
|
||||
self.tracks[tid] = dict(det)
|
||||
self.tracks[tid]["lost"] = 0
|
||||
self.tracks[tid]["age"] = 1
|
||||
|
||||
out = dict(self.tracks[tid])
|
||||
out["id"] = tid
|
||||
updated.append(out)
|
||||
|
||||
for tid in list(self.tracks.keys()):
|
||||
if tid not in used_tracks and all(d.get("id") != tid for d in updated):
|
||||
self.tracks[tid]["lost"] += 1
|
||||
if self.tracks[tid]["lost"] > MAX_LOST_FRAMES:
|
||||
del self.tracks[tid]
|
||||
|
||||
return updated
|
||||
|
||||
|
||||
# =========================
|
||||
# PIPELINE OAK
|
||||
# =========================
|
||||
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
cam_rgb = pipeline.create(dai.node.ColorCamera)
|
||||
cam_rgb.setPreviewSize(300, 300)
|
||||
cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
|
||||
cam_rgb.setInterleaved(False)
|
||||
cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
|
||||
cam_rgb.setFps(30)
|
||||
|
||||
mono_left = pipeline.create(dai.node.MonoCamera)
|
||||
mono_right = pipeline.create(dai.node.MonoCamera)
|
||||
|
||||
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
mono_left.setBoardSocket(dai.CameraBoardSocket.CAM_B)
|
||||
mono_right.setBoardSocket(dai.CameraBoardSocket.CAM_C)
|
||||
|
||||
stereo = pipeline.create(dai.node.StereoDepth)
|
||||
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT)
|
||||
stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A)
|
||||
stereo.setSubpixel(True)
|
||||
stereo.setLeftRightCheck(True)
|
||||
|
||||
mono_left.out.link(stereo.left)
|
||||
mono_right.out.link(stereo.right)
|
||||
|
||||
# IA espacial
|
||||
detection = pipeline.create(dai.node.MobileNetSpatialDetectionNetwork)
|
||||
detection.setBlobPath(blobconverter.from_zoo(
|
||||
name="mobilenet-ssd",
|
||||
shaves=3,
|
||||
version="2021.4"
|
||||
))
|
||||
detection.setConfidenceThreshold(AI_CONFIDENCE)
|
||||
detection.input.setBlocking(False)
|
||||
detection.setBoundingBoxScaleFactor(0.5)
|
||||
detection.setDepthLowerThreshold(MIN_DEPTH_MM)
|
||||
detection.setDepthUpperThreshold(8000)
|
||||
|
||||
cam_rgb.preview.link(detection.input)
|
||||
stereo.depth.link(detection.inputDepth)
|
||||
|
||||
# Tracker oficial para IA
|
||||
tracker_ai = pipeline.create(dai.node.ObjectTracker)
|
||||
tracker_ai.setTrackerType(dai.TrackerType.ZERO_TERM_COLOR_HISTOGRAM)
|
||||
tracker_ai.setTrackerIdAssignmentPolicy(dai.TrackerIdAssignmentPolicy.SMALLEST_ID)
|
||||
|
||||
detection.passthrough.link(tracker_ai.inputTrackerFrame)
|
||||
detection.passthrough.link(tracker_ai.inputDetectionFrame)
|
||||
detection.out.link(tracker_ai.inputDetections)
|
||||
|
||||
# Outputs
|
||||
xout_rgb = pipeline.create(dai.node.XLinkOut)
|
||||
xout_depth = pipeline.create(dai.node.XLinkOut)
|
||||
xout_ai = pipeline.create(dai.node.XLinkOut)
|
||||
|
||||
xout_rgb.setStreamName("rgb")
|
||||
xout_depth.setStreamName("depth")
|
||||
xout_ai.setStreamName("ai_tracklets")
|
||||
|
||||
cam_rgb.preview.link(xout_rgb.input)
|
||||
#stereo.depth.link(xout_depth.input)
|
||||
tracker_ai.out.link(xout_ai.input)
|
||||
|
||||
|
||||
# =========================
|
||||
# LOOP
|
||||
# =========================
|
||||
|
||||
blob_tracker = SimpleBlobTracker()
|
||||
|
||||
with dai.Device(pipeline) as device:
|
||||
q_rgb = device.getOutputQueue("rgb", maxSize=1, blocking=True)
|
||||
q_depth = device.getOutputQueue("depth", maxSize=1, blocking=False)
|
||||
q_ai = device.getOutputQueue("ai_tracklets", maxSize=1, blocking=False)
|
||||
|
||||
last_depth = None
|
||||
last_ai_tracklets = []
|
||||
|
||||
last = time.time()
|
||||
fps = 0
|
||||
|
||||
while True:
|
||||
in_rgb = q_rgb.get()
|
||||
frame = in_rgb.getCvFrame()
|
||||
|
||||
in_depth = q_depth.tryGet()
|
||||
if in_depth is not None:
|
||||
last_depth = in_depth.getFrame()
|
||||
|
||||
in_ai = q_ai.tryGet()
|
||||
if in_ai is not None:
|
||||
last_ai_tracklets = in_ai.tracklets
|
||||
|
||||
depth_ok = last_depth is not None
|
||||
|
||||
if depth_ok:
|
||||
depth = last_depth
|
||||
|
||||
if depth.shape[:2] != frame.shape[:2]:
|
||||
depth = cv2.resize(depth, (frame.shape[1], frame.shape[0]), interpolation=cv2.INTER_NEAREST)
|
||||
else:
|
||||
depth = None
|
||||
|
||||
depth = last_depth
|
||||
ai_tracklets = last_ai_tracklets
|
||||
|
||||
h, w = frame.shape[:2]
|
||||
|
||||
now = time.time()
|
||||
fps = 0.9 * fps + 0.1 * (1 / max(now - last, 1e-6))
|
||||
last = now
|
||||
|
||||
danger_ai = False
|
||||
danger_blob = False
|
||||
|
||||
# =========================
|
||||
# 1) IA + DEPTH + TRACKER
|
||||
# =========================
|
||||
|
||||
for t in ai_tracklets:
|
||||
roi = t.roi.denormalize(w, h)
|
||||
x1 = int(roi.topLeft().x)
|
||||
y1 = int(roi.topLeft().y)
|
||||
x2 = int(roi.bottomRight().x)
|
||||
y2 = int(roi.bottomRight().y)
|
||||
|
||||
label = LABELS[t.label] if t.label < len(LABELS) else str(t.label)
|
||||
z_mm = t.spatialCoordinates.z
|
||||
z_m = z_mm / 1000.0
|
||||
|
||||
is_danger_class = label in AI_DANGER_CLASSES
|
||||
is_close = MIN_DEPTH_MM < z_mm < DANGER_DEPTH_MM
|
||||
|
||||
if is_danger_class and is_close:
|
||||
danger_ai = True
|
||||
|
||||
color = (0, 0, 255) if is_danger_class and is_close else (0, 180, 255)
|
||||
|
||||
txt = f"AI ID {t.id} | {label} | Z={z_m:.2f}m | {t.status.name}"
|
||||
cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)
|
||||
cv2.putText(frame, txt, (x1, max(20, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.42, color, 1)
|
||||
|
||||
# =========================
|
||||
# 2) DEPTH BLOB SEM IA
|
||||
# =========================
|
||||
danger_blob = False
|
||||
tracked_blobs = []
|
||||
mask = None
|
||||
|
||||
if depth_ok:
|
||||
x1_roi = int(w * ROI_LEFT)
|
||||
x2_roi = int(w * ROI_RIGHT)
|
||||
y1_roi = int(h * ROI_TOP)
|
||||
y2_roi = int(h * ROI_BOTTOM)
|
||||
|
||||
roi_depth = depth[y1_roi:y2_roi, x1_roi:x2_roi]
|
||||
|
||||
mask = np.zeros_like(roi_depth, dtype=np.uint8)
|
||||
mask[(roi_depth > MIN_DEPTH_MM) & (roi_depth < MAX_DEPTH_MM)] = 255
|
||||
|
||||
kernel = np.ones((5, 5), np.uint8)
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
|
||||
|
||||
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
blob_detections = []
|
||||
|
||||
for cnt in contours:
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < MIN_BLOB_AREA_PX:
|
||||
continue
|
||||
|
||||
x, y, bw, bh = cv2.boundingRect(cnt)
|
||||
gx = x + x1_roi
|
||||
gy = y + y1_roi
|
||||
gcx = gx + bw // 2
|
||||
gcy = gy + bh // 2
|
||||
|
||||
blob_depth = roi_depth[y:y + bh, x:x + bw]
|
||||
valid = blob_depth[(blob_depth > MIN_DEPTH_MM) & (blob_depth < MAX_DEPTH_MM)]
|
||||
|
||||
if len(valid) < 50:
|
||||
continue
|
||||
|
||||
z_mm = float(np.median(valid))
|
||||
|
||||
blob_detections.append({
|
||||
"bbox": (gx, gy, gx + bw, gy + bh),
|
||||
"cx": gcx,
|
||||
"cy": gcy,
|
||||
"z_mm": z_mm,
|
||||
"area": area
|
||||
})
|
||||
|
||||
tracked_blobs = blob_tracker.update(blob_detections)
|
||||
|
||||
cv2.rectangle(frame, (x1_roi, y1_roi), (x2_roi, y2_roi), (255, 255, 0), 1)
|
||||
|
||||
for obj in tracked_blobs:
|
||||
x1, y1, x2, y2 = obj["bbox"]
|
||||
z_mm = obj["z_mm"]
|
||||
z_m = z_mm / 1000.0
|
||||
center_percent = obj["cx"] / w * 100.0
|
||||
|
||||
is_close = z_mm < DANGER_DEPTH_MM
|
||||
if is_close:
|
||||
danger_blob = True
|
||||
|
||||
color = (0, 0, 255) if is_close else (0, 255, 0)
|
||||
|
||||
txt = f"DEPTH ID {obj['id']} | Z={z_m:.2f}m | X={center_percent:.0f}% | area={int(obj['area'])}"
|
||||
cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)
|
||||
cv2.circle(frame, (obj["cx"], obj["cy"]), 4, (255, 0, 255), -1)
|
||||
cv2.putText(frame, txt, (x1, min(h - 10, y2 + 16)), cv2.FONT_HERSHEY_SIMPLEX, 0.42, color, 1)
|
||||
else:
|
||||
cv2.putText(frame, "Depth: aguardando/indisponivel", (10, 115), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 255), 1)
|
||||
|
||||
# =========================
|
||||
# 3) DECISÃO FINAL
|
||||
# =========================
|
||||
|
||||
danger_final = danger_ai or danger_blob
|
||||
|
||||
cv2.putText(frame, f"FPS: {fps:.1f}", (10, 25),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
|
||||
|
||||
cv2.putText(frame, f"AI danger: {danger_ai} | Depth danger: {danger_blob}",
|
||||
(10, 52), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
|
||||
(255, 255, 255), 1)
|
||||
|
||||
if danger_final:
|
||||
cv2.putText(frame, "PARAR: risco detectado", (10, 85),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.85, (0, 0, 255), 2)
|
||||
else:
|
||||
cv2.putText(frame, "Livre", (10, 85),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.85, (0, 255, 0), 2)
|
||||
|
||||
if depth_ok:
|
||||
depth_vis = depth.copy()
|
||||
depth_vis[depth_vis == 0] = MAX_DEPTH_MM
|
||||
depth_vis = np.clip(depth_vis, MIN_DEPTH_MM, MAX_DEPTH_MM)
|
||||
depth_vis = ((MAX_DEPTH_MM - depth_vis) / (MAX_DEPTH_MM - MIN_DEPTH_MM) * 255).astype(np.uint8)
|
||||
depth_vis = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET)
|
||||
cv2.imshow("Depth", depth_vis)
|
||||
|
||||
if mask is not None:
|
||||
cv2.imshow("Depth Blob Mask", mask)
|
||||
|
||||
cv2.imshow("OAK-D Lite - Hybrid Safety Tracker", frame)
|
||||
|
||||
|
||||
key = cv2.waitKey(1)
|
||||
if key == ord("q") or key == 27:
|
||||
break
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
import cv2
|
||||
import depthai as dai
|
||||
import blobconverter
|
||||
import time
|
||||
|
||||
# COCO labels do MobileNet-SSD
|
||||
LABELS = [
|
||||
"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus",
|
||||
"car", "cat", "chair", "cow", "diningtable", "dog", "horse",
|
||||
"motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"
|
||||
]
|
||||
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
# RGB
|
||||
cam_rgb = pipeline.create(dai.node.ColorCamera)
|
||||
cam_rgb.setPreviewSize(300, 300)
|
||||
cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
|
||||
cam_rgb.setInterleaved(False)
|
||||
cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
|
||||
cam_rgb.setFps(30)
|
||||
|
||||
# Mono stereo
|
||||
mono_left = pipeline.create(dai.node.MonoCamera)
|
||||
mono_right = pipeline.create(dai.node.MonoCamera)
|
||||
|
||||
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
|
||||
|
||||
mono_left.setBoardSocket(dai.CameraBoardSocket.CAM_B)
|
||||
mono_right.setBoardSocket(dai.CameraBoardSocket.CAM_C)
|
||||
|
||||
# Depth
|
||||
stereo = pipeline.create(dai.node.StereoDepth)
|
||||
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT)
|
||||
stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A)
|
||||
stereo.setSubpixel(True)
|
||||
|
||||
mono_left.out.link(stereo.left)
|
||||
mono_right.out.link(stereo.right)
|
||||
|
||||
# Spatial Detection Network - MobileNet SSD
|
||||
detection = pipeline.create(dai.node.MobileNetSpatialDetectionNetwork)
|
||||
|
||||
detection.setBlobPath(blobconverter.from_zoo(
|
||||
name="mobilenet-ssd",
|
||||
shaves=3,
|
||||
version="2021.4"
|
||||
))
|
||||
|
||||
detection.setConfidenceThreshold(0.5)
|
||||
detection.input.setBlocking(False)
|
||||
detection.setBoundingBoxScaleFactor(0.5)
|
||||
detection.setDepthLowerThreshold(200)
|
||||
detection.setDepthUpperThreshold(8000)
|
||||
|
||||
cam_rgb.preview.link(detection.input)
|
||||
stereo.depth.link(detection.inputDepth)
|
||||
|
||||
# Tracker
|
||||
tracker = pipeline.create(dai.node.ObjectTracker)
|
||||
|
||||
# Para obstáculo geral, rastreia tudo detectado.
|
||||
# Para só pessoas: tracker.setDetectionLabelsToTrack([15])
|
||||
tracker.setTrackerType(dai.TrackerType.ZERO_TERM_COLOR_HISTOGRAM)
|
||||
tracker.setTrackerIdAssignmentPolicy(dai.TrackerIdAssignmentPolicy.SMALLEST_ID)
|
||||
|
||||
detection.passthrough.link(tracker.inputTrackerFrame)
|
||||
detection.passthrough.link(tracker.inputDetectionFrame)
|
||||
detection.out.link(tracker.inputDetections)
|
||||
|
||||
# Outputs
|
||||
xout_rgb = pipeline.create(dai.node.XLinkOut)
|
||||
xout_track = pipeline.create(dai.node.XLinkOut)
|
||||
|
||||
xout_rgb.setStreamName("rgb")
|
||||
xout_track.setStreamName("tracklets")
|
||||
|
||||
tracker.passthroughTrackerFrame.link(xout_rgb.input)
|
||||
tracker.out.link(xout_track.input)
|
||||
|
||||
with dai.Device(pipeline) as device:
|
||||
q_rgb = device.getOutputQueue("rgb", maxSize=4, blocking=False)
|
||||
q_track = device.getOutputQueue("tracklets", maxSize=4, blocking=False)
|
||||
|
||||
last = time.time()
|
||||
fps = 0
|
||||
|
||||
while True:
|
||||
frame = q_rgb.get().getCvFrame()
|
||||
tracklets = q_track.get().tracklets
|
||||
|
||||
now = time.time()
|
||||
fps = 0.9 * fps + 0.1 * (1 / max(now - last, 1e-6))
|
||||
last = now
|
||||
|
||||
h, w = frame.shape[:2]
|
||||
|
||||
perigo = False
|
||||
|
||||
for t in tracklets:
|
||||
roi = t.roi.denormalize(w, h)
|
||||
x1 = int(roi.topLeft().x)
|
||||
y1 = int(roi.topLeft().y)
|
||||
x2 = int(roi.bottomRight().x)
|
||||
y2 = int(roi.bottomRight().y)
|
||||
|
||||
label = LABELS[t.label] if t.label < len(LABELS) else str(t.label)
|
||||
|
||||
x_mm = t.spatialCoordinates.x
|
||||
y_mm = t.spatialCoordinates.y
|
||||
z_mm = t.spatialCoordinates.z
|
||||
|
||||
dist_m = z_mm / 1000.0
|
||||
|
||||
if dist_m < 1.5:
|
||||
perigo = True
|
||||
|
||||
texto = f"ID {t.id} | {label} | {t.status.name} | Z={dist_m:.2f}m"
|
||||
|
||||
cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
cv2.putText(frame, texto, (x1, max(20, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (255, 255, 255), 1)
|
||||
|
||||
cv2.circle(frame, ((x1 + x2) // 2, (y1 + y2) // 2), 4, (0, 255, 255), -1)
|
||||
|
||||
cv2.putText(frame, f"FPS: {fps:.1f}", (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
|
||||
|
||||
if perigo:
|
||||
cv2.putText(frame, "PERIGO: objeto perto - PARAR", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)
|
||||
|
||||
cv2.imshow("OAK-D Lite Spatial Object Tracker", frame)
|
||||
|
||||
key = cv2.waitKey(1)
|
||||
if key == ord("q") or key == 27:
|
||||
break
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
Loading…
Reference in New Issue