testes com track objects oak-d

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;
+            if (false)
-            // MID-360
-            var g = gridObstaculos;
-            if (g.Columns.Count == 0)
             {
-                g.Columns.Clear();
+                var LogLvx = LogsOperacao[idxMomentoAtual].LivoxLidar;
-                g.Columns.Add("clDist", "Dist (m)");
+                // MID-360
-                g.Columns.Add("clFrente", "Frente (m)");
+                var g = gridObstaculos;
-                g.Columns.Add("clLateral", "Lat (m)");
+                if (g.Columns.Count == 0)
-                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(
+                    g.Columns.Clear();
-                        b.dist_xy_m.ToString("0.00"),
+                    g.Columns.Add("clDist", "Dist (m)");
-                        b.frente_m.ToString("0.00"),
+                    g.Columns.Add("clFrente", "Frente (m)");
-                        b.cy.ToString("0.00"),
+                    g.Columns.Add("clLateral", "Lat (m)");
-                        b.w.ToString("0.00"),
+                    g.Columns.Add("clW", "W (m)");
-                        b.d.ToString("0.00"),
+                    g.Columns.Add("clD", "D (m)");
-                        b.h.ToString("0.00"),
+                    g.Columns.Add("clH", "H (m)");
-                        b.voxels,
+                    g.Columns.Add("clVoxels", "Voxels");
-                        b.t_ms
+                    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_top_down.SetBboxes(LogLvx.bboxes);
-                view_frontal.SetBboxes(LogLvx.bboxes);
+                    view_frontal.SetBboxes(LogLvx.bboxes);
-                pnlTridimensional.Invalidate();
+                    pnlTridimensional.Invalidate();
-                view_3d.SetBboxes(LogLvx.bboxes);
+                    view_3d.SetBboxes(LogLvx.bboxes);
+                }
             }
             
         }

AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs

@@ -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,
             };
         }
     }

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}

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)))
+                    # bbox_full provavelmente está no frame original 1920x1080
-                    y0 = max(0, min(H - 1, int(y0))); y1 = max(0, min(H - 1, int(y1)))
+                    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)

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],

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()

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()

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()

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()