From 4bf18b8b420bd19ed25761bde75876f033b31cbb Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 27 Apr 2026 14:55:08 -0300 Subject: [PATCH] testes com track objects oak-d --- .../Forms/Operacoes/frmResultadosOperacao.cs | 81 ++-- .../Models/Operadores/VisualWorkerModel.cs | 18 +- .../workers/camera_worker/camera_oak.py | 107 +++++- .../workers/visual_worker/camera_manager.py | 42 ++- .../Scripts/workers/visual_worker/config.py | 1 + Python/OAK/spatial_depth_tracker.py | 316 ++++++++++++++++ Python/OAK/spatial_depth_tracker_test.py | 263 +++++++++++++ Python/OAK/spatial_hybrid_tracker_test.py | 353 ++++++++++++++++++ Python/OAK/spatial_yolo_tracker_test.py | 137 +++++++ 9 files changed, 1277 insertions(+), 41 deletions(-) create mode 100644 Python/OAK/spatial_depth_tracker.py create mode 100644 Python/OAK/spatial_depth_tracker_test.py create mode 100644 Python/OAK/spatial_hybrid_tracker_test.py create mode 100644 Python/OAK/spatial_yolo_tracker_test.py diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs index 9aeb40294..1600b5103 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs @@ -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,42 +1587,45 @@ namespace AgroBase.Forms.Operacoes // //picSonarRadar.Image = Log.Leitura.obj.radar_2d.PlotarAnalsie((Bitmap)picSonarRadar.Image.Clone()); //} - var LogLvx = LogsOperacao[idxMomentoAtual].LivoxLidar; - // MID-360 - var g = gridObstaculos; - if (g.Columns.Count == 0) + if (false) { - g.Columns.Clear(); - g.Columns.Add("clDist", "Dist (m)"); - g.Columns.Add("clFrente", "Frente (m)"); - g.Columns.Add("clLateral", "Lat (m)"); - g.Columns.Add("clW", "W (m)"); - g.Columns.Add("clD", "D (m)"); - g.Columns.Add("clH", "H (m)"); - g.Columns.Add("clVoxels", "Voxels"); - g.Columns.Add("clTempo", "t (ms)"); - } - g.Rows.Clear(); - if (LogLvx != null) - { - foreach (var b in LogLvx.bboxes.OrderBy(x => x.dist_xy_m)) + var LogLvx = LogsOperacao[idxMomentoAtual].LivoxLidar; + // MID-360 + var g = gridObstaculos; + if (g.Columns.Count == 0) { - g.Rows.Add( - b.dist_xy_m.ToString("0.00"), - b.frente_m.ToString("0.00"), - b.cy.ToString("0.00"), - b.w.ToString("0.00"), - b.d.ToString("0.00"), - b.h.ToString("0.00"), - b.voxels, - b.t_ms - ); + g.Columns.Clear(); + g.Columns.Add("clDist", "Dist (m)"); + g.Columns.Add("clFrente", "Frente (m)"); + g.Columns.Add("clLateral", "Lat (m)"); + g.Columns.Add("clW", "W (m)"); + g.Columns.Add("clD", "D (m)"); + g.Columns.Add("clH", "H (m)"); + g.Columns.Add("clVoxels", "Voxels"); + g.Columns.Add("clTempo", "t (ms)"); } + g.Rows.Clear(); + if (LogLvx != null) + { + foreach (var b in LogLvx.bboxes.OrderBy(x => x.dist_xy_m)) + { + g.Rows.Add( + b.dist_xy_m.ToString("0.00"), + b.frente_m.ToString("0.00"), + b.cy.ToString("0.00"), + b.w.ToString("0.00"), + b.d.ToString("0.00"), + b.h.ToString("0.00"), + b.voxels, + b.t_ms + ); + } - view_top_down.SetBboxes(LogLvx.bboxes); - view_frontal.SetBboxes(LogLvx.bboxes); - pnlTridimensional.Invalidate(); - view_3d.SetBboxes(LogLvx.bboxes); + view_top_down.SetBboxes(LogLvx.bboxes); + view_frontal.SetBboxes(LogLvx.bboxes); + pnlTridimensional.Invalidate(); + view_3d.SetBboxes(LogLvx.bboxes); + } } } diff --git a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs index 8be438f9b..1f32169e9 100644 --- a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs +++ b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs @@ -719,6 +719,14 @@ namespace AgroBase.Models.Operadores public double conf { get; set; } public List bbox_norm { get; set; } public List bbox_px { get; set; } + public int track_id { get; set; } + public string track_status { get; set; } + public bool tem_tracker { get; set; } + public List 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 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, }; } } diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py index dce9ff4e1..3a239faae 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py @@ -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} diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py index 59bed88c3..c72c5963b 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py @@ -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) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py index 0b2949850..8e26513bc 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py @@ -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], diff --git a/Python/OAK/spatial_depth_tracker.py b/Python/OAK/spatial_depth_tracker.py new file mode 100644 index 000000000..9f5db4645 --- /dev/null +++ b/Python/OAK/spatial_depth_tracker.py @@ -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() \ No newline at end of file diff --git a/Python/OAK/spatial_depth_tracker_test.py b/Python/OAK/spatial_depth_tracker_test.py new file mode 100644 index 000000000..14d4a5a8a --- /dev/null +++ b/Python/OAK/spatial_depth_tracker_test.py @@ -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() \ No newline at end of file diff --git a/Python/OAK/spatial_hybrid_tracker_test.py b/Python/OAK/spatial_hybrid_tracker_test.py new file mode 100644 index 000000000..f05e849ce --- /dev/null +++ b/Python/OAK/spatial_hybrid_tracker_test.py @@ -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() \ No newline at end of file diff --git a/Python/OAK/spatial_yolo_tracker_test.py b/Python/OAK/spatial_yolo_tracker_test.py new file mode 100644 index 000000000..66ca3e61f --- /dev/null +++ b/Python/OAK/spatial_yolo_tracker_test.py @@ -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() \ No newline at end of file