dsh/dual_camera_system/panorama_camera.py

"""
全景摄像头模块
负责获取视频流和物体检测
"""

import cv2
import numpy as np
import threading
import queue
import time
from typing import Optional, List, Tuple, Dict, Any
from dataclasses import dataclass

from config import PANORAMA_CAMERA, DETECTION_CONFIG
from dahua_sdk import DahuaSDK, PTZCommand


@dataclass
class DetectedObject:
    """检测到的物体"""
    class_name: str          # 类别名称
    confidence: float        # 置信度
    bbox: Tuple[int, int, int, int]  # 边界框 (x, y, width, height)
    center: Tuple[int, int]  # 中心点坐标
    track_id: Optional[int] = None  # 跟踪ID


class PanoramaCamera:
    """全景摄像头类"""
    
    def __init__(self, sdk: DahuaSDK, camera_config: Dict = None):
        """
        初始化全景摄像头
        Args:
            sdk: 大华SDK实例
            camera_config: 摄像头配置
        """
        self.sdk = sdk
        self.config = camera_config or PANORAMA_CAMERA
        
        self.login_handle = None
        self.play_handle = None
        self.connected = False
        
        # 视频流
        self.frame_queue = queue.Queue(maxsize=10)
        self.current_frame = None
        self.frame_lock = threading.Lock()
        self.rtsp_cap = None  # RTSP视频捕获
        
        # 检测器
        self.detector = None
        
        # 控制标志
        self.running = False
        self.stream_thread = None
        
        # 断线重连
        self.auto_reconnect = True
        self.reconnect_interval = 5.0  # 重连间隔(秒)
        self.max_reconnect_attempts = 3  # 最大重连次数
    
    def connect(self) -> bool:
        """
        连接摄像头
        Returns:
            是否成功
        """
        login_handle, error = self.sdk.login(
            self.config['ip'],
            self.config['port'],
            self.config['username'],
            self.config['password']
        )
        
        if login_handle is None:
            print(f"连接全景摄像头失败: IP={self.config['ip']}, 错误码={error}")
            return False
        
        self.login_handle = login_handle
        self.connected = True
        print(f"成功连接全景摄像头: {self.config['ip']}")
        return True
    
    def disconnect(self):
        """断开连接"""
        self.stop_stream()
        if self.login_handle:
            self.sdk.logout(self.login_handle)
            self.login_handle = None
        self.connected = False
    
    def start_stream(self) -> bool:
        """
        开始视频流
        Returns:
            是否成功
        """
        if not self.connected:
            return False
        
        self.play_handle = self.sdk.real_play(
            self.login_handle, 
            self.config['channel']
        )
        
        if self.play_handle is None:
            print("启动视频流失败")
            return False
        
        self.running = True
        self.stream_thread = threading.Thread(target=self._stream_worker, daemon=True)
        self.stream_thread.start()
        
        print("视频流已启动")
        return True
    
    def start_stream_rtsp(self, rtsp_url: str = None) -> bool:
        """
        通过RTSP协议获取视频流
        Args:
            rtsp_url: RTSP地址，格式: rtsp://user:pass@ip:port/channel
        Returns:
            是否成功
        """
        if rtsp_url is None:
            # 构建RTSP地址
            rtsp_url = f"rtsp://{self.config['username']}:{self.config['password']}@{self.config['ip']}:554/h264/ch{self.config['channel']}/main/av_stream"
        
        try:
            self.rtsp_cap = cv2.VideoCapture(rtsp_url)
            if not self.rtsp_cap.isOpened():
                print(f"无法打开RTSP流: {rtsp_url}")
                return False
            
            self.running = True
            self.stream_thread = threading.Thread(target=self._rtsp_stream_worker, daemon=True)
            self.stream_thread.start()
            print(f"RTSP视频流已启动: {rtsp_url}")
            return True
        except Exception as e:
            print(f"RTSP流启动失败: {e}")
            return False
    
    def _stream_worker(self):
        """视频流工作线程 (SDK模式)"""
        retry_count = 0
        max_retries = 10
        
        while self.running:
            try:
                # 尝试从 SDK 帧缓冲区获取帧 (如果可用)
                frame_buffer = self.sdk.get_video_frame_buffer(self.config['channel'])
                
                if frame_buffer:
                    frame_info = frame_buffer.get(timeout=0.1)
                    if frame_info and frame_info.get('data'):
                        # 解码帧数据 (如果需要)
                        # 注意: SDK回调返回的是编码数据，需要解码
                        # 这里暂时跳过，因为解码需要额外处理
                        pass
                
                # RTSP 模式获取帧 (推荐方式)
                if self.rtsp_cap is not None and self.rtsp_cap.isOpened():
                    ret, frame = self.rtsp_cap.read()
                    if ret and frame is not None:
                        with self.frame_lock:
                            self.current_frame = frame.copy()
                        
                        try:
                            self.frame_queue.put(frame.copy(), block=False)
                        except queue.Full:
                            pass
                        
                        retry_count = 0  # 重置重试计数
                        time.sleep(0.001)  # 减少CPU占用
                        continue
                
                # 如果 RTSP 不可用，尝试自动连接
                if retry_count < max_retries:
                    rtsp_url = self._build_rtsp_url()
                    try:
                        if self.rtsp_cap is None:
                            self.rtsp_cap = cv2.VideoCapture(rtsp_url)
                            self.rtsp_cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)  # 减少缓冲延迟
                        
                        if self.rtsp_cap.isOpened():
                            retry_count = 0
                            continue
                    except Exception as e:
                        pass
                    
                    retry_count += 1
                    time.sleep(1.0)  # 重试间隔
                else:
                    # 超过最大重试次数，使用模拟帧
                    frame = np.zeros((1080, 1920, 3), dtype=np.uint8)
                    
                    with self.frame_lock:
                        self.current_frame = frame
                    
                    try:
                        self.frame_queue.put(frame, block=False)
                    except queue.Full:
                        pass
                    
                    time.sleep(0.1)
                
            except Exception as e:
                print(f"视频流错误: {e}")
                time.sleep(0.1)
    
    def _build_rtsp_url(self) -> str:
        """构建 RTSP URL"""
        return f"rtsp://{self.config['username']}:{self.config['password']}@{self.config['ip']}:554/h264/ch{self.config['channel']}/main/av_stream"
    
    def _rtsp_stream_worker(self):
        """RTSP视频流工作线程"""
        while self.running:
            try:
                if self.rtsp_cap is None or not self.rtsp_cap.isOpened():
                    time.sleep(0.1)
                    continue
                
                ret, frame = self.rtsp_cap.read()
                if not ret or frame is None:
                    time.sleep(0.01)
                    continue
                
                with self.frame_lock:
                    self.current_frame = frame.copy()
                
                try:
                    self.frame_queue.put(frame, block=False)
                except queue.Full:
                    pass
                    
            except Exception as e:
                print(f"RTSP视频流错误: {e}")
                time.sleep(0.1)
    
    def stop_stream(self):
        """停止视频流"""
        self.running = False
        if self.stream_thread:
            self.stream_thread.join(timeout=2)
        if self.play_handle:
            self.sdk.stop_real_play(self.play_handle)
            self.play_handle = None
        if self.rtsp_cap:
            self.rtsp_cap.release()
            self.rtsp_cap = None
    
    def get_frame(self) -> Optional[np.ndarray]:
        """
        获取当前帧
        Returns:
            当前帧图像
        """
        with self.frame_lock:
            return self.current_frame.copy() if self.current_frame is not None else None
    
    def get_frame_from_queue(self, timeout: float = 0.1) -> Optional[np.ndarray]:
        """
        从帧队列获取帧 (用于批量处理)
        Args:
            timeout: 等待超时时间
        Returns:
            帧图像或None
        """
        try:
            return self.frame_queue.get(timeout=timeout)
        except:
            return None
    
    def get_frame_buffer(self, count: int = 5) -> List[np.ndarray]:
        """
        获取帧缓冲 (用于运动检测等需要多帧的场景)
        Args:
            count: 获取帧数
        Returns:
            帧列表
        """
        frames = []
        while len(frames) < count:
            frame = self.get_frame_from_queue(timeout=0.05)
            if frame is not None:
                frames.append(frame)
            else:
                break
        return frames
    
    def set_detector(self, detector):
        """设置物体检测器"""
        self.detector = detector
    
    def detect_objects(self, frame: np.ndarray = None) -> List[DetectedObject]:
        """
        检测物体
        Args:
            frame: 输入帧,如果为None则使用当前帧
        Returns:
            检测到的物体列表
        """
        if frame is None:
            frame = self.get_frame()
        
        if frame is None or self.detector is None:
            return []
        
        return self.detector.detect(frame)
    
    def get_detection_position(self, obj: DetectedObject, 
                               frame_size: Tuple[int, int]) -> Tuple[float, float]:
        """
        获取检测物体在画面中的相对位置
        Args:
            obj: 检测到的物体
            frame_size: 画面尺寸 (width, height)
        Returns:
            相对位置 (x_ratio, y_ratio) 范围0-1
        """
        width, height = frame_size
        x_ratio = obj.center[0] / width
        y_ratio = obj.center[1] / height
        return (x_ratio, y_ratio)


class ObjectDetector:
    """
    物体检测器
    使用YOLO11模型进行人体检测
    """
    
    def __init__(self, model_path: str = None, use_gpu: bool = True, model_size: str = 'n'):
        """
        初始化检测器
        Args:
            model_path: 模型路径 (自定义模型)
            use_gpu: 是否使用GPU
            model_size: 模型尺寸 ('n', 's', 'm', 'l', 'x')
        """
        self.model = None
        self.model_path = model_path
        self.use_gpu = use_gpu
        self.model_size = model_size
        self.config = DETECTION_CONFIG
        self.device = 'cuda:0' if use_gpu else 'cpu'
        
        self._load_model()
    
    def _load_model(self):
        """加载YOLO11检测模型"""
        try:
            # 使用ultralytics YOLO11
            from ultralytics import YOLO
            
            if self.model_path:
                # 使用自定义模型
                self.model = YOLO(self.model_path)
            else:
                # 使用YOLO11预训练模型
                # YOLO11模型命名: yolo11n.pt, yolo11s.pt, yolo11m.pt, yolo11l.pt, yolo11x.pt
                model_name = f'yolo11{self.model_size}.pt'
                self.model = YOLO(model_name)
            
            # 预热模型
            dummy = np.zeros((640, 640, 3), dtype=np.uint8)
            self.model(dummy, device=self.device, verbose=False)
            
            print(f"成功加载YOLO11检测模型 (device={self.device})")
        except ImportError:
            print("未安装ultralytics,请运行: pip install ultralytics")
            self._load_opencv_model()
        except Exception as e:
            print(f"加载YOLO11模型失败: {e}")
            self._load_opencv_model()
    
    def _load_opencv_model(self):
        """使用OpenCV加载模型"""
        # 可以加载ONNX模型
        pass
    
    def detect(self, frame: np.ndarray) -> List[DetectedObject]:
        """
        使用YOLO11检测物体
        Args:
            frame: 输入图像
        Returns:
            检测结果列表
        """
        if self.model is None or frame is None:
            return []
        
        results = []
        
        try:
            # YOLO11推理
            detections = self.model(
                frame, 
                device=self.device, 
                verbose=False,
                conf=self.config['confidence_threshold']
            )
            
            for det in detections:
                boxes = det.boxes
                if boxes is None:
                    continue
                    
                for i in range(len(boxes)):
                    # 获取类别
                    cls_id = int(boxes.cls[i])
                    cls_name = det.names[cls_id]
                    
                    # 过滤目标类别
                    if cls_name not in self.config['target_classes']:
                        continue
                    
                    # 获取置信度
                    conf = float(boxes.conf[i])
                    
                    # 获取边界框
                    xyxy = boxes.xyxy[i].cpu().numpy()
                    x1, y1, x2, y2 = map(int, xyxy)
                    width = x2 - x1
                    height = y2 - y1
                    
                    # 过滤过小的检测框
                    if width < 10 or height < 10:
                        continue
                    
                    # 计算中心点
                    center_x = x1 + width // 2
                    center_y = y1 + height // 2
                    
                    obj = DetectedObject(
                        class_name=cls_name,
                        confidence=conf,
                        bbox=(x1, y1, width, height),
                        center=(center_x, center_y)
                    )
                    results.append(obj)
                    
        except Exception as e:
            print(f"YOLO11检测错误: {e}")
        
        return results
    
    def detect_with_keypoints(self, frame: np.ndarray) -> List[DetectedObject]:
        """
        使用YOLO11-pose检测人体并返回关键点
        Args:
            frame: 输入图像
        Returns:
            带关键点的检测结果列表
        """
        # 如果使用pose模型,可以获取人体关键点
        # 用于更精确的人体定位
        return self.detect(frame)
    
    def detect_persons(self, frame: np.ndarray) -> List[DetectedObject]:
        """
        检测人体
        Args:
            frame: 输入图像
        Returns:
            检测到的人体列表
        """
        results = self.detect(frame)
        return [obj for obj in results if obj.class_name == 'person']


class PersonTracker:
    """
    人体跟踪器
    使用简单的质心跟踪算法
    """
    
    def __init__(self, max_disappeared: int = 30):
        """
        初始化跟踪器
        Args:
            max_disappeared: 最大消失帧数
        """
        self.max_disappeared = max_disappeared
        self.next_id = 0
        self.objects = {}  # id -> center
        self.disappeared = {}  # id -> disappeared count
    
    def update(self, detections: List[DetectedObject]) -> List[DetectedObject]:
        """
        更新跟踪状态
        Args:
            detections: 当前帧检测结果
        Returns:
            带有跟踪ID的检测结果
        """
        # 如果没有检测结果
        if len(detections) == 0:
            # 标记所有已跟踪对象为消失
            for obj_id in list(self.disappeared.keys()):
                self.disappeared[obj_id] += 1
                if self.disappeared[obj_id] > self.max_disappeared:
                    self._deregister(obj_id)
            return []
        
        # 计算当前检测中心点
        input_centers = np.array([d.center for d in detections])
        
        # 如果没有已跟踪对象
        if len(self.objects) == 0:
            for det in detections:
                self._register(det)
        else:
            # 计算距离矩阵
            object_ids = list(self.objects.keys())
            object_centers = np.array([self.objects[obj_id] for obj_id in object_ids])
            
            # 计算欧氏距离
            distances = np.linalg.norm(
                object_centers[:, np.newaxis] - input_centers, 
                axis=2
            )
            
            # 匈牙利算法匹配 (简化版: 贪心匹配)
            rows = distances.min(axis=1).argsort()
            cols = distances.argmin(axis=1)[rows]
            
            used_rows = set()
            used_cols = set()
            
            for (row, col) in zip(rows, cols):
                if row in used_rows or col in used_cols:
                    continue
                
                obj_id = object_ids[row]
                self.objects[obj_id] = input_centers[col]
                self.disappeared[obj_id] = 0
                detections[col].track_id = obj_id
                
                used_rows.add(row)
                used_cols.add(col)
            
            # 处理未匹配的已跟踪对象
            unused_rows = set(range(len(object_ids))) - used_rows
            for row in unused_rows:
                obj_id = object_ids[row]
                self.disappeared[obj_id] += 1
                if self.disappeared[obj_id] > self.max_disappeared:
                    self._deregister(obj_id)
            
            # 处理未匹配的新检测
            unused_cols = set(range(len(input_centers))) - used_cols
            for col in unused_cols:
                self._register(detections[col])
        
        return [d for d in detections if d.track_id is not None]
    
    def _register(self, detection: DetectedObject):
        """注册新对象"""
        detection.track_id = self.next_id
        self.objects[self.next_id] = detection.center
        self.disappeared[self.next_id] = 0
        self.next_id += 1
    
    def _deregister(self, obj_id: int):
        """注销对象"""
        del self.objects[obj_id]
        del self.disappeared[obj_id]