第02章 - 低空 AI 巡检实战

# metrics.py —— 事件级口径计算（只依赖 numpy / pandas）
import numpy as np
import pandas as pd

def haversine_m(lat1, lon1, lat2, lon2):
    """两个经纬度点之间的球面距离（米），WGS84 平均半径。"""
    r = 6371008.8
    p1, p2 = np.radians(lat1), np.radians(lat2)
    dp = p2 - p1
    dl = np.radians(lon2) - np.radians(lon1)
    a = np.sin(dp / 2) ** 2 + np.cos(p1) * np.cos(p2) * np.sin(dl / 2) ** 2
    return 2 * r * np.arcsin(np.sqrt(a))

def greedy_match(gt, det, dist_m=30.0, tol_s=10.0):
    """按「同类 + 时间容差 + 距离容差」做一对一贪心匹配，返回匹配数与命中掩码。"""
    det = det.reset_index(drop=True)
    used = np.zeros(len(det), dtype=bool)
    for _, g in gt.iterrows():
        d = haversine_m(g["lat"], g["lon"], det["lat"].to_numpy(), det["lon"].to_numpy())
        t = (det["ts"] - g["ts"]).dt.total_seconds().abs().to_numpy()
        cand = np.flatnonzero((d <= dist_m) & (t <= tol_s) & (det["cls"] == g["cls"]) & ~used)
        if cand.size:
            used[cand[np.argmin(d[cand])]] = True
    return used

gt = pd.read_csv("gt_events.csv", parse_dates=["ts"])
det = pd.read_csv("alarm_events.csv", parse_dates=["ts"])
hit = greedy_match(gt, det)
tp = int(hit.sum())
print(f"recall={tp / max(len(gt), 1):.3f}  fpr={(len(det) - tp) / max(len(det), 1):.3f}")
matched = det.loc[hit].merge(gt, on="cls", suffixes=("_det", "_gt"))
lat_ms = (matched["ts_det"] - matched["ts_gt"]).dt.total_seconds()
print(f"latency_p50={lat_ms.quantile(0.5):.2f}s  p95={lat_ms.quantile(0.95):.2f}s")

# cloud_mqtt.py —— 订阅 OSD / 状态 / 事件，打印最小可读摘要
import json
import time
import paho.mqtt.client as mqtt

BROKER = "mqtt.example.com"
PORT = 8883                      # 8883 为 TLS 端口，按现场调整
GW_SN = "8UADZCK00A0XYZ"         # 机场网关序列号

def on_connect(client, userdata, flags, rc, properties=None):
    print("connected rc=", rc)
    if rc != 0:
        return
    # OSD 频率高，用 QoS 0 省流量；状态与事件用 QoS 1 保证可达
    client.subscribe(f"thing/product/{GW_SN}/osd", qos=0)
    client.subscribe(f"thing/product/{GW_SN}/state", qos=1)
    client.subscribe(f"thing/product/{GW_SN}/events", qos=1)
    client.subscribe(f"thing/product/{GW_SN}/services_reply", qos=1)

def on_message(client, userdata, msg):
    try:
        payload = json.loads(msg.payload)
    except json.JSONDecodeError:
        print("non-json", msg.topic, msg.payload[:64])
        return
    method = payload.get("method", "?")
    data = payload.get("data", {}) or {}
    if msg.topic.endswith("/osd"):
        battery = (data.get("battery") or {}).get("capacity_percent")
        print(f"[OSD] {method} battery={battery}%")
    elif msg.topic.endswith("/state"):
        print(f"[STATE] {data.get('name')} online={data.get('online')}")
    elif msg.topic.endswith("/events"):
        print(f"[EVENT] {method} need_reply={payload.get('need_reply')} "
              f"{json.dumps(data, ensure_ascii=False)[:120]}")
    else:
        print(f"[REPLY] tid={payload.get('tid')} {json.dumps(data)[:120]}")

client = mqtt.Client(client_id=f"platform-{int(time.time())}", protocol=mqtt.MQTTv5)
client.username_pw_set("platform_user", "REPLACE_ME")
client.tls_set()                 # 需要私有 CA 时传 ca_certs="/path/ca.pem"
client.on_connect = on_connect
client.on_message = on_message
client.connect(BROKER, PORT, keepalive=60)
client.loop_forever()

# cloud_task.py —— 下发飞行任务（对照 services_reply 判结果）
import json
import time
import uuid

def publish_service(client, gw_sn, method, data, bid="task-001"):
    tid = str(uuid.uuid4())
    body = {"tid": tid, "bid": bid, "timestamp": int(time.time() * 1000),
            "method": method, "data": data}
    client.publish(f"thing/product/{gw_sn}/services",
                   json.dumps(body, ensure_ascii=False), qos=1)
    return tid

# ① 准备任务：航线文件必须是设备可访问的 HTTPS/KMZ 地址
prepare = {
    "flight_id": "fl-20261113-001",
    "execute_time": 0,                     # 0 表示立即执行
    "task_type": 0,
    "file": {"url": "https://oss.example.com/routes/inspect-01.kmz"},
}
tid1 = publish_service(client, GW_SN, "flighttask_prepare", prepare)
print("prepare tid", tid1)   # 收到 services_reply 且 result==0 后才执行下一步

# ② 执行任务（必须在 prepare 成功之后）
tid2 = publish_service(client, GW_SN, "flighttask_execute",
                       {"flight_id": "fl-20261113-001"})
print("execute tid", tid2)

# export_parity.py —— 比对 PyTorch 与 ONNX 的原始输出（不是检测框）
import argparse
import numpy as np
import onnxruntime as ort
import torch
from ultralytics import YOLO

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--pt", required=True)
    ap.add_argument("--onnx", required=True)
    ap.add_argument("--imgsz", type=int, default=1024)
    ap.add_argument("--tol", type=float, default=1e-3)
    args = ap.parse_args()

    model = YOLO(args.pt)
    torch_model = model.model.float().eval()
    dummy = torch.rand(1, 3, args.imgsz, args.imgsz)
    with torch.no_grad():
        ref = torch_model(dummy)
    ref = (ref[0] if isinstance(ref, (list, tuple)) else ref).numpy()

    sess = ort.InferenceSession(args.onnx, providers=["CPUExecutionProvider"])
    out = sess.run(None, {sess.get_inputs()[0].name: dummy.numpy()})
    out = np.asarray(out[0])

    print("shape ref", ref.shape, "shape onnx", out.shape)
    if ref.shape != out.shape:
        raise SystemExit("shape mismatch: 先检查导出时的 imgsz 与输出节点")
    diff = np.abs(ref - out)
    print(f"max_abs_diff={diff.max():.6f}  mean_abs_diff={diff.mean():.6f}")
    if diff.max() > args.tol:
        raise SystemExit(f"diff 超过阈值 {args.tol}：检查 opset、simplify 与前后处理")
    print("parity OK")

if __name__ == "__main__":
    main()

# pipeline.py —— 有界队列 + 抽帧 + 时间戳透传的最小实现
import threading
from collections import deque
import cv2

class FrameGrabber(threading.Thread):
    def __init__(self, url, every_n=2, maxsize=8):
        super().__init__(daemon=True)
        self.url = url
        self.cap = cv2.VideoCapture(url)
        self.every_n = every_n
        self.q = deque(maxlen=maxsize)   # 有界：满了自动丢最旧
        self.lock = threading.Condition()
        self.running = True

    def _reopen(self):
        """断流后按退避重连，避免死循环空转。"""
        backoff = 1.0
        while self.running and backoff <= 30.0:
            self.cap.release()
            if self.cap.open(self.url):
                return True
            threading.Event().wait(backoff)
            backoff *= 2
        return False

    def run(self):
        idx = 0
        while self.running:
            ok, frame = self.cap.read()
            if not ok:
                if not self._reopen():
                    break
                continue
            idx += 1
            if idx % self.every_n:
                continue
            ts = self.cap.get(cv2.CAP_PROP_POS_MSEC) / 1000.0   # 采集时间，不是处理时间
            with self.lock:
                self.q.append((ts, frame))
                self.lock.notify()

    def get(self, timeout=1.0):
        with self.lock:
            if not self.q:
                self.lock.wait(timeout)
            return self.q.popleft() if self.q else None

# sam3_review.py —— 用文本概念复核一张命中帧（官方 sam3 包接口示意，以当前版本为准）
from PIL import Image
from sam3 import build_sam3_image_model, Sam3Processor

model = build_sam3_image_model(ckpt_path="sam3.1.pt", device="cuda")
proc = Sam3Processor(model)

image = Image.open("hit_frames/000123.jpg").convert("RGB")
state = proc.set_image(image)
# 概念写成「名词 + 属性」，不要写「危险」「异常」这类抽象词
out = proc.set_text_prompt(state=state, prompt="bird nest on transmission tower")
instances = out["instances"]
print("命中实例数", len(instances), "最高得分", float(instances["scores"].max()))

# dedup.py —— 按 track_id 做告警去重与冷却（与检测/跟踪器解耦）
import time
from dataclasses import dataclass, field

@dataclass
class AlarmGate:
    cooldown_s: float = 30.0
    min_hits: int = 3
    _last: dict = field(default_factory=dict)
    _hits: dict = field(default_factory=dict)

    def accept(self, track_id: int, cls: str, now: float | None = None) -> bool:
        now = now if now is not None else time.time()
        key = (track_id, cls)
        # 连续命中计数：抑制单帧误检造成的瞬时告警
        self._hits[key] = self._hits.get(key, 0) + 1
        if self._hits[key] < self.min_hits:
            return False
        last = self._last.get(key)
        if last is not None and now - last < self.cooldown_s:
            return False
        self._last[key] = now
        return True

gate = AlarmGate(cooldown_s=30.0, min_hits=3)
assert gate.accept(7, "person", now=100.0) is False   # 第 1 次命中
assert gate.accept(7, "person", now=100.1) is False   # 第 2 次命中
assert gate.accept(7, "person", now=100.2) is True    # 第 3 次命中 → 告警
assert gate.accept(7, "person", now=105.0) is False   # 冷却期内

# visdrone2yolo.py —— VisDrone 检测标注转 YOLO，带体检统计
import argparse
import json
from collections import Counter
from pathlib import Path

from PIL import Image

CLASS_MAP = {1: 0, 2: 1, 3: 2, 4: 3, 5: 4, 6: 5, 7: 6, 8: 7, 9: 8, 10: 9}
NAMES = ["pedestrian", "people", "bicycle", "car", "van",
         "truck", "tricycle", "awning-tricycle", "bus", "motor"]

def convert(src_img_dir: Path, src_ann_dir: Path, dst_root: Path, split: str):
    img_out = dst_root / "images" / split
    lbl_out = dst_root / "labels" / split
    img_out.mkdir(parents=True, exist_ok=True)
    lbl_out.mkdir(parents=True, exist_ok=True)

    stat = Counter()
    for ann in sorted(src_ann_dir.glob("*.txt")):
        img_path = None
        for ext in (".jpg", ".png"):
            cand = src_img_dir / (ann.stem + ext)
            if cand.exists():
                img_path = cand
                break
        if img_path is None:
            stat["missing_image"] += 1
            continue
        with Image.open(img_path) as im:
            w, h = im.size
        rows = []
        for line in ann.read_text(encoding="utf-8").splitlines():
            parts = line.strip().split(",")
            if len(parts) < 8:
                continue
            x, y, bw, bh = map(float, parts[:4])
            score, cat = float(parts[4]), int(parts[5])
            if cat not in CLASS_MAP or score <= 0:      # 忽略区域与无效框
                stat["dropped"] += 1
                continue
            if bw <= 1 or bh <= 1:
                stat["tiny_drop"] += 1
                continue
            cx, cy = (x + bw / 2) / w, (y + bh / 2) / h
            rows.append(f"{CLASS_MAP[cat]} {cx:.6f} {cy:.6f} {bw / w:.6f} {bh / h:.6f}")
            stat[NAMES[CLASS_MAP[cat]]] += 1
        (lbl_out / (ann.stem + ".txt")).write_text("\n".join(rows), encoding="utf-8")
        # 图片用符号链接省磁盘；Windows 无权限时退化为真实复制
        link = img_out / img_path.name
        if not link.exists():
            try:
                link.symlink_to(img_path.resolve())
            except OSError:
                link.write_bytes(img_path.read_bytes())
    return stat

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--src", required=True, help="VisDrone2019-DET-train 目录")
    ap.add_argument("--dst", required=True, help="输出目录")
    args = ap.parse_args()
    src = Path(args.src)
    stat = convert(src / "images", src / "annotations", Path(args.dst), "train")
    print(json.dumps(dict(stat), ensure_ascii=False, indent=2))

if __name__ == "__main__":
    main()

# bucket_recall.py —— 按目标像素面积分桶统计召回（YOLO 标签格式，简化匹配口径）
import argparse
from collections import defaultdict
from pathlib import Path

BUCKETS = [("xs", 0, 16), ("s", 16, 32), ("m", 32, 96), ("l", 96, 10**9)]

def area_bucket(area: float) -> str:
    for name, lo, hi in BUCKETS:
        if lo <= area < hi:
            return name
    return "l"

def read_labels(path: Path, w: int, h: int):
    out = []
    if not path.exists():
        return out
    for line in path.read_text().splitlines():
        parts = line.split()
        if len(parts) < 5:
            continue
        _, cx, cy, bw, bh = parts[:5]
        out.append((int(parts[0]), float(bw) * w, float(bh) * h))
    return out

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--gt", required=True, help="labels/val")
    ap.add_argument("--pred", required=True, help="推理结果 labels 目录")
    ap.add_argument("--size", type=int, default=1024)
    args = ap.parse_args()

    total, hit = defaultdict(int), defaultdict(int)
    for gt_file in Path(args.gt).glob("*.txt"):
        preds = read_labels(Path(args.pred) / gt_file.name, args.size, args.size)
        for cls, bw, bh in read_labels(gt_file, args.size, args.size):
            key = area_bucket(bw * bh)
            total[key] += 1
            # 简化匹配：同类 + 宽高差小于较大框的一半，视为命中
            ok = any(pc == cls and abs(pw - bw) < max(bw, 1) * 0.5
                     and abs(ph - bh) < max(bh, 1) * 0.5 for pc, pw, ph in preds)
            hit[key] += int(ok)
    for name, _, _ in BUCKETS:
        n = total[name]
        print(f"{name:>2}  数量={n:>6}  召回={hit[name] / n:.3f}" if n else f"{name:>2}  数量=0")

if __name__ == "__main__":
    main()

# sliced_infer.py —— 切片推理：切图 → 批量推理 → 坐标还原 → 跨块 NMS 合并
from dataclasses import dataclass

import numpy as np
from ultralytics import YOLO

@dataclass
class Det:
    cls: int
    conf: float
    xyxy: tuple[float, float, float, float]   # 原图坐标系

def slice_windows(w: int, h: int, size: int, overlap: float):
    """生成 (x0, y0, x1, y1) 滑窗，保证覆盖整图且边界块贴边。"""
    step = int(size * (1 - overlap))
    xs = list(range(0, max(w - size, 0) + 1, step)) or [0]
    ys = list(range(0, max(h - size, 0) + 1, step)) or [0]
    if xs[-1] + size < w:
        xs.append(max(w - size, 0))
    if ys[-1] + size < h:
        ys.append(max(h - size, 0))
    for y in ys:
        for x in xs:
            yield x, y, min(x + size, w), min(y + size, h)

def nms(dets: list[Det], iou_thres: float = 0.55) -> list[Det]:
    """按类别分组的纯 numpy NMS，输入输出都用原图坐标。"""
    keep_all = []
    for cls in {d.cls for d in dets}:
        group = [d for d in dets if d.cls == cls]
        group.sort(key=lambda d: -d.conf)
        keep = []
        while group:
            best = group.pop(0)
            keep.append(best)
            rest = []
            for d in group:
                if iou(best.xyxy, d.xyxy) <= iou_thres:
                    rest.append(d)
            group = rest
        keep_all.extend(keep)
    keep_all.sort(key=lambda d: -d.conf)
    return keep_all

def iou(a, b) -> float:
    ax0, ay0, ax1, ay1 = a
    bx0, by0, bx1, by1 = b
    ix0, iy0 = max(ax0, bx0), max(ay0, by0)
    ix1, iy1 = min(ax1, bx1), min(ay1, by1)
    iw, ih = max(ix1 - ix0, 0), max(iy1 - iy0, 0)
    inter = iw * ih
    union = (ax1 - ax0) * (ay1 - ay0) + (bx1 - bx0) * (by1 - by0) - inter
    return inter / union if union > 0 else 0.0

def infer_sliced(model: YOLO, img: np.ndarray, size: int = 1024,
                 overlap: float = 0.2, conf: float = 0.25, batch: int = 8):
    h, w = img.shape[:2]
    wins = list(slice_windows(w, h, size, overlap))
    tiles, metas = [], []
    for x0, y0, x1, y1 in wins:
        tiles.append(img[y0:y1, x0:x1])
        metas.append((x0, y0))

    dets: list[Det] = []
    for i in range(0, len(tiles), batch):
        chunk, meta = tiles[i:i + batch], metas[i:i + batch]
        results = model.predict(chunk, imgsz=size, conf=conf, verbose=False)
        for res, (x0, y0) in zip(results, meta):
            for box in res.boxes:
                bx0, by0, bx1, by1 = box.xyxy[0].tolist()
                dets.append(Det(int(box.cls.item()), float(box.conf.item()),
                                (bx0 + x0, by0 + y0, bx1 + x0, by1 + y0)))
    return nms(dets, iou_thres=0.55)   # 跨块重复框在这里被合并

if __name__ == "__main__":
    import cv2
    model = YOLO("runs/visdrone/small1024/weights/best.pt")
    img = cv2.imread("data/visdrone/images/val/0000006_00159_d_0000014.jpg")
    for d in infer_sliced(model, img)[:5]:
        print(d.cls, round(d.conf, 3), [round(v, 1) for v in d.xyxy])

# geo_project.py —— 像素框底边中点 → 经纬度（地平面假设）
import math

import numpy as np

R_EARTH = 6378137.0

def euler_to_R(yaw_deg: float, pitch_deg: float, roll_deg: float) -> np.ndarray:
    """ZYX 顺序：先偏航、再俯仰、最后横滚，返回机体→东北天的旋转矩阵。"""
    y, p, r = np.radians([yaw_deg, pitch_deg, roll_deg])
    rz = np.array([[math.cos(y), -math.sin(y), 0], [math.sin(y), math.cos(y), 0], [0, 0, 1]])
    ry = np.array([[math.cos(p), 0, math.sin(p)], [0, 1, 0], [-math.sin(p), 0, math.cos(p)]])
    rx = np.array([[1, 0, 0], [0, math.cos(r), -math.sin(r)], [0, math.sin(r), math.cos(r)]])
    return rz @ ry @ rx

def pixel_to_ground(u, v, K, R, cam_alt, ground_alt=0.0):
    """返回目标点相对相机的东向/北向偏移（米）。K=(fx,fy,cx,cy)。"""
    fx, fy, cx, cy = K
    ray_cam = np.array([(u - cx) / fx, (v - cy) / fy, 1.0])
    d = R @ ray_cam                       # 东北天方向（未归一化）
    dz = d[2]
    if dz > -1e-6:                        # 射线朝上，打不到地面
        raise ValueError("ray does not hit ground: 检查云台俯仰与 R 的符号")
    t = (ground_alt - cam_alt) / dz       # t > 0
    return d[0] * t, d[1] * t

def enu_to_lonlat(lon0, lat0, east, north):
    """局部 ENU 近似：300 m 内误差在厘米级，够巡检用。"""
    dlat = north / R_EARTH
    dlon = east / (R_EARTH * math.cos(math.radians(lat0)))
    return lon0 + math.degrees(dlon), lat0 + math.degrees(dlat)

# 例：云台俯仰 -90°（垂直下视），画面中心点直接落在正下方
K = (1400.0, 1400.0, 960.0, 540.0)
R = euler_to_R(yaw_deg=0, pitch_deg=-90, roll_deg=0)
east, north = pixel_to_ground(960, 540, K, R, cam_alt=100.0, ground_alt=0.0)
print(round(east, 3), round(north, 3))          # 期望 (0, 0)
lon, lat = enu_to_lonlat(116.397, 39.908, east, north)
print(round(lon, 7), round(lat, 7))