dsh/dual_camera_system/calibration.py

"""
相机校准模块
实现全景相机与球机的自动校准
建立画面坐标到PTZ角度的映射关系

核心改进：先发现视野重叠区域，再在重叠区内校准，
避免球机指向与全景画面无重叠的方向导致校准失败。
"""

import time
import math
import threading
import numpy as np
import cv2
from typing import List, Tuple, Dict, Optional, Callable
from dataclasses import dataclass, field
from enum import Enum

from ptz_camera import PTZCamera

# 加载PTZ配置
def _get_ptz_config():
    try:
        from config import PTZ_CONFIG
        return PTZ_CONFIG
    except ImportError:
        return {
            'mount_type': 'wall',
            'tilt_flip': False,
            'pan_flip': False
        }


class CalibrationState(Enum):
    IDLE = 0
    RUNNING = 1
    SUCCESS = 2
    FAILED = 3


@dataclass
class CalibrationPoint:
    pan: float
    tilt: float
    zoom: float = 1.0
    x_ratio: float = 0.0
    y_ratio: float = 0.0
    detected: bool = False


@dataclass
class CalibrationResult:
    success: bool
    points: List[CalibrationPoint]
    transform_matrix: Optional[np.ndarray] = None
    error_message: str = ""
    rms_error: float = 0.0


@dataclass
class OverlapRange:
    pan_start: float
    pan_end: float
    tilt_start: float
    tilt_end: float
    match_count: int
    panorama_center_x: float
    panorama_center_y: float


MIN_MATCH_THRESHOLD = 8


class OverlapDiscovery:
    """
    视野重叠发现器
    扫描球机视野范围，找出与全景画面有视觉重叠的角度区间
    """

    def __init__(self, feature_type: str = 'SIFT'):
        try:
            self.feature_detector = cv2.SIFT_create()
            self.feature_type = 'SIFT'
        except AttributeError:
            self.feature_detector = cv2.ORB_create(nfeatures=500)
            self.feature_type = 'ORB'

        norm_type = cv2.NORM_L2 if self.feature_type == 'SIFT' else cv2.NORM_HAMMING
        self.matcher = cv2.BFMatcher(norm_type)

    def match_frames(self, ptz_frame: np.ndarray, panorama_frame: np.ndarray
                     ) -> Tuple[bool, int, float, float]:
        """
        特征匹配球机画面与全景画面
        Returns: (是否匹配成功, 匹配点数, 全景画面中心x, 全景画面中心y)
        """
        if ptz_frame is None or panorama_frame is None:
            return (False, 0, 0.0, 0.0)

        try:
            ptz_gray = cv2.cvtColor(ptz_frame, cv2.COLOR_BGR2GRAY) if len(ptz_frame.shape) == 3 else ptz_frame
            pan_gray = cv2.cvtColor(panorama_frame, cv2.COLOR_BGR2GRAY) if len(panorama_frame.shape) == 3 else panorama_frame

            kp1, des1 = self.feature_detector.detectAndCompute(ptz_gray, None)
            kp2, des2 = self.feature_detector.detectAndCompute(pan_gray, None)

            if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4:
                return (False, 0, 0.0, 0.0)

            matches = self.matcher.knnMatch(des1, des2, k=2)

            good_matches = []
            for match_pair in matches:
                if len(match_pair) == 2:
                    m, n = match_pair
                    if m.distance < 0.75 * n.distance:
                        good_matches.append(m)

            if len(good_matches) < MIN_MATCH_THRESHOLD:
                return (False, len(good_matches), 0.0, 0.0)

            pan_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches])
            center_x = np.mean(pan_pts[:, 0])
            center_y = np.mean(pan_pts[:, 1])

            return (True, len(good_matches), center_x, center_y)

        except Exception as e:
            print(f"      特征匹配异常: {e}")
            return (False, 0, 0.0, 0.0)

    def discover_overlap_ranges(
        self,
        ptz: PTZCamera,
        get_panorama_frame: Callable[[], np.ndarray],
        ptz_capture: Callable[[], Optional[np.ndarray]],
        pan_range: Tuple[float, float] = (0, 360),
        tilt_range: Tuple[float, float] = (-30, 30),
        pan_step: float = 20,
        tilt_step: float = 15,
        stabilize_time: float = 2.0,
        on_progress: Callable[[int, int, str], None] = None,
        max_ranges: int = 3,
        min_positions_per_range: int = 3
    ) -> List[OverlapRange]:
        """
        扫描球机视野范围，发现与全景画面有重叠的角度区间

        1. 先拍一张全景参考帧
        2. 逐步移动球机到各个角度
        3. 在每个位置抓拍球机画面，与全景做特征匹配
        4. 记录有足够匹配点的角度
        5. 合并相邻的有重叠的角度形成区间
        """
        print(f"\n{'='*50}")
        print(f"阶段1: 视野重叠发现")
        print(f"扫描范围: pan={pan_range}, tilt={tilt_range}")
        print(f"步进: pan={pan_step}°, tilt={tilt_step}°")
        print(f"{'='*50}")

        # 1. 拍全景参考帧
        print("  获取全景参考帧...")
        ref_frames = []
        for _ in range(3):
            frame = get_panorama_frame()
            if frame is not None:
                ref_frames.append(frame)
            time.sleep(0.1)

        if not ref_frames:
            print("  错误: 无法获取全景参考帧!")
            return []

        panorama_ref = ref_frames[0]
        print(f"  全景参考帧: {panorama_ref.shape}")

        # 2. 扫描各个角度
        scan_results: List[Tuple[float, float, int, float, float]] = []

        pan_values = np.arange(pan_range[0], pan_range[1] + pan_step, pan_step)
        tilt_values = np.arange(tilt_range[0], tilt_range[1] + tilt_step, tilt_step)

        total_positions = len(pan_values) * len(tilt_values)
        current_idx = 0

        for pan in pan_values:
            for tilt in tilt_values:
                current_idx += 1
                pos_desc = f"pan={pan:.0f}°, tilt={tilt:.0f}°"

                if on_progress:
                    on_progress(current_idx, total_positions, f"扫描 {pos_desc}")

                print(f"  [{current_idx}/{total_positions}] {pos_desc}")

                # 移动球机
                if not ptz.goto_exact_position(float(pan), float(tilt), 1):
                    print(f"    移动球机失败, 跳过")
                    continue

                time.sleep(stabilize_time)

                # 抓拍球机画面
                ptz_frame = ptz_capture() if ptz_capture else None
                if ptz_frame is None:
                    print(f"    球机抓拍失败, 跳过")
                    continue

                # 获取当前全景帧并匹配
                cur_panorama = get_panorama_frame()
                if cur_panorama is None:
                    continue

                success, match_count, cx, cy = self.match_frames(ptz_frame, cur_panorama)

                if success:
                    h, w = cur_panorama.shape[:2]
                    x_ratio = cx / w
                    y_ratio = cy / h
                    print(f"    ✓ 匹配成功: {match_count}个特征点, 全景位置=({x_ratio:.3f}, {y_ratio:.3f})")
                    scan_results.append((float(pan), float(tilt), match_count, x_ratio, y_ratio))
                else:
                    print(f"    ✗ 匹配不足: {match_count}个特征点")

        if not scan_results:
            print("\n  未发现任何视野重叠位置!")
            return []

        print(f"\n  发现 {len(scan_results)} 个有重叠的扫描位置")

        # 3. 合并相邻位置为重叠区间
        overlap_ranges = self._merge_scan_results(
            scan_results,
            max_ranges=max_ranges,
            min_positions=min_positions_per_range
        )

        for i, r in enumerate(overlap_ranges):
            print(f"  重叠区间 {i+1}: pan=[{r.pan_start:.0f}°, {r.pan_end:.0f}°], "
                  f"tilt=[{r.tilt_start:.0f}°, {r.tilt_end:.0f}°], "
                  f"匹配点={r.match_count}")

        return overlap_ranges

    def _merge_scan_results(
        self,
        results: List[Tuple[float, float, int, float, float]],
        pan_tolerance: float = 25,
        tilt_tolerance: float = 20,
        max_ranges: int = 3,
        min_positions: int = 3
    ) -> List[OverlapRange]:
        """
        使用union-find连通分量聚类合并相邻扫描结果
        只保留最大的 max_ranges 个区间
        """
        if not results:
            return []

        n = len(results)

        # union-find
        parent = list(range(n))

        def find(x):
            while parent[x] != x:
                parent[x] = parent[parent[x]]
                x = parent[x]
            return x

        def union(a, b):
            ra, rb = find(a), find(b)
            if ra != rb:
                parent[ra] = rb

        # 判断两点是否相邻
        for i in range(n):
            for j in range(i + 1, n):
                pi, ti = results[i][0], results[i][1]
                pj, tj = results[j][0], results[j][1]
                if abs(pi - pj) <= pan_tolerance and abs(ti - tj) <= tilt_tolerance:
                    union(i, j)

        # 按连通分量分组
        groups: Dict[int, List[int]] = {}
        for i in range(n):
            root = find(i)
            if root not in groups:
                groups[root] = []
            groups[root].append(i)

        # 转换为OverlapRange，过滤太小的组
        ranges = []
        for indices in groups.values():
            if len(indices) < min_positions:
                continue
            group_data = [results[i] for i in indices]
            ranges.append(self._group_to_range(group_data))

        # 按match_count降序排序，只保留最大的 max_ranges 个
        ranges.sort(key=lambda r: r.match_count, reverse=True)
        ranges = ranges[:max_ranges]

        # 按pan_start排序输出
        ranges.sort(key=lambda r: r.pan_start)

        return ranges

    def _group_to_range(self, group: List[Tuple[float, float, int, float, float]]) -> OverlapRange:
        """将一组扫描结果转换为一个OverlapRange"""
        pans = [r[0] for r in group]
        tilts = [r[1] for r in group]
        match_counts = [r[2] for r in group]
        x_ratios = [r[3] for r in group]
        y_ratios = [r[4] for r in group]

        step = 5  # 在边缘各扩展5度
        return OverlapRange(
            pan_start=min(pans) - step,
            pan_end=max(pans) + step,
            tilt_start=min(tilts) - step,
            tilt_end=max(tilts) + step,
            match_count=max(match_counts),
            panorama_center_x=float(np.mean(x_ratios)),
            panorama_center_y=float(np.mean(y_ratios))
        )


class VisualCalibrationDetector:
    """
    视觉校准检测器
    通过运动检测和特征匹配定位球机在全景画面中的位置
    """

    def __init__(self):
        try:
            self.feature_detector = cv2.SIFT_create()
            self.feature_type = 'SIFT'
        except AttributeError:
            self.feature_detector = cv2.ORB_create(nfeatures=500)
            self.feature_type = 'ORB'

        self.matcher = cv2.BFMatcher(
            cv2.NORM_L2 if self.feature_type == 'SIFT' else cv2.NORM_HAMMING
        )

        self.use_motion_detection = True
        self.use_feature_matching = True

    def detect_by_motion(self, frames_before: np.ndarray,
                         frames_after: np.ndarray) -> Optional[Tuple[float, float]]:
        """通过运动检测定位球机指向位置"""
        if frames_before is None or frames_after is None:
            return None

        before_gray = cv2.cvtColor(frames_before, cv2.COLOR_BGR2GRAY) \
            if len(frames_before.shape) == 3 else frames_before
        after_gray = cv2.cvtColor(frames_after, cv2.COLOR_BGR2GRAY) \
            if len(frames_after.shape) == 3 else frames_after

        diff = cv2.absdiff(before_gray, after_gray)
        _, thresh = cv2.threshold(diff, 25, 255, cv2.THRESH_BINARY)

        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (15, 15))
        thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
        thresh = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel)

        contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if not contours:
            return None

        max_contour = max(contours, key=cv2.contourArea)
        area = cv2.contourArea(max_contour)
        if area < 500:
            return None

        M = cv2.moments(max_contour)
        if M["m00"] == 0:
            return None

        cx = M["m10"] / M["m00"]
        cy = M["m01"] / M["m00"]

        h, w = before_gray.shape
        print(f"      运动检测: 中心=({cx:.1f}, {cy:.1f}), 面积={area:.0f})")

        return (cx / w, cy / h)

    def detect_by_feature_match(self, panorama_frame: np.ndarray,
                                 ptz_frame: np.ndarray) -> Optional[Tuple[float, float]]:
        """通过特征匹配定位"""
        if panorama_frame is None or ptz_frame is None:
            return None

        try:
            pan_gray = cv2.cvtColor(panorama_frame, cv2.COLOR_BGR2GRAY) \
                if len(panorama_frame.shape) == 3 else panorama_frame
            ptz_gray = cv2.cvtColor(ptz_frame, cv2.COLOR_BGR2GRAY) \
                if len(ptz_frame.shape) == 3 else ptz_frame

            kp1, des1 = self.feature_detector.detectAndCompute(ptz_gray, None)
            kp2, des2 = self.feature_detector.detectAndCompute(pan_gray, None)

            if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4:
                return None

            matches = self.matcher.knnMatch(des1, des2, k=2)
            good_matches = []
            for match_pair in matches:
                if len(match_pair) == 2:
                    m, n = match_pair
                    if m.distance < 0.75 * n.distance:
                        good_matches.append(m)

            if len(good_matches) < 4:
                return None

            pan_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches])
            center_x = np.mean(pan_pts[:, 0])
            center_y = np.mean(pan_pts[:, 1])

            h, w = pan_gray.shape
            print(f"      特征匹配: 匹配点={len(good_matches)}, 中心=({center_x:.1f}, {center_y:.1f})")

            return (center_x / w, center_y / h)

        except Exception as e:
            print(f"      特征匹配错误: {e}")
            return None

    def detect_position(self, panorama_frame: np.ndarray,
                        frames_before: np.ndarray = None,
                        frames_after: np.ndarray = None,
                        ptz_frame: np.ndarray = None) -> Tuple[bool, float, float]:
        """综合检测球机在全景画面中的位置"""
        results = []

        if self.use_motion_detection and frames_before is not None and frames_after is not None:
            motion_result = self.detect_by_motion(frames_before, frames_after)
            if motion_result:
                results.append(('motion', motion_result, 0.4))

        if self.use_feature_matching and ptz_frame is not None:
            feature_result = self.detect_by_feature_match(panorama_frame, ptz_frame)
            if feature_result:
                results.append(('feature', feature_result, 0.6))

        if not results:
            return (False, 0.0, 0.0)

        total_weight = sum(r[2] for r in results)
        x_ratio = sum(r[1][0] * r[2] for r in results) / total_weight
        y_ratio = sum(r[1][1] * r[2] for r in results) / total_weight

        print(f"      融合结果: ({x_ratio:.3f}, {y_ratio:.3f})")
        return (True, x_ratio, y_ratio)


class CameraCalibrator:
    """
    相机校准器
    两阶段校准：先发现视野重叠区域，再在重叠区内校准
    """

    def __init__(self, ptz_camera: PTZCamera,
                 get_frame_func: Callable[[], np.ndarray],
                 detect_marker_func: Callable[[np.ndarray], Optional[Tuple[float, float]]] = None,
                 ptz_capture_func: Callable[[], Optional[np.ndarray]] = None):
        self.ptz = ptz_camera
        self.get_frame = get_frame_func
        self.detect_marker = detect_marker_func
        self.ptz_capture = ptz_capture_func

        self.visual_detector = VisualCalibrationDetector()
        self.overlap_discovery = OverlapDiscovery()

        self.state = CalibrationState.IDLE
        self.result: Optional[CalibrationResult] = None

        # 变换参数
        self.pan_offset = 0.0
        self.pan_scale_x = 1.0
        self.pan_scale_y = 0.0
        self.tilt_offset = 0.0
        self.tilt_scale_x = 0.0
        self.tilt_scale_y = 1.0

        # 校准配置
        self.stabilize_time = 2.0
        self.use_motion_detection = True
        self.use_feature_matching = True

        # 重叠发现配置
        self.overlap_pan_range = (0, 360)
        self.overlap_tilt_range = (-30, 30)
        self.overlap_pan_step = 20
        self.overlap_tilt_step = 15
        self.max_overlap_ranges = 3
        self.min_positions_per_range = 3

        # 回调
        self.on_progress: Optional[Callable[[int, int, str], None]] = None
        self.on_complete: Optional[Callable[[CalibrationResult], None]] = None

        # 发现的重叠区间
        self.overlap_ranges: List[OverlapRange] = []

    def calibrate(self, quick_mode: bool = True) -> CalibrationResult:
        """
        执行校准 - 两阶段流程
        
        阶段1: 视野重叠发现 - 扫描球机范围，找出与全景有重叠的角度区间
        阶段2: 精确校准 - 仅在重叠区间内生成校准点，逐一验证后拟合变换
        """
        self.state = CalibrationState.RUNNING

        # ===================== 阶段1: 视野重叠发现 =====================
        print(f"\n{'='*60}")
        print(f"阶段1: 视野重叠发现 - 确定球机与全景的重叠区域")
        print(f"{'='*60}")

        self.overlap_ranges = self.overlap_discovery.discover_overlap_ranges(
            ptz=self.ptz,
            get_panorama_frame=self.get_frame,
            ptz_capture=self.ptz_capture,
            pan_range=self.overlap_pan_range,
            tilt_range=self.overlap_tilt_range,
            pan_step=self.overlap_pan_step,
            tilt_step=self.overlap_tilt_step,
            stabilize_time=self.stabilize_time,
            on_progress=self.on_progress,
            max_ranges=self.max_overlap_ranges,
            min_positions_per_range=self.min_positions_per_range
        )

        if not self.overlap_ranges:
            self.state = CalibrationState.FAILED
            self.result = CalibrationResult(
                success=False,
                points=[],
                error_message="未发现球机与全景的视野重叠区域，无法校准。请检查两台摄像头的安装位置和朝向。"
            )
            print(f"\n校准失败: {self.result.error_message}")
            if self.on_complete:
                self.on_complete(self.result)
            return self.result

        print(f"\n发现 {len(self.overlap_ranges)} 个重叠区间")

        # 选择匹配点最多的区间用于校准
        best_range = max(self.overlap_ranges, key=lambda r: r.match_count)
        self.overlap_ranges = [best_range]

        print(f"选择最佳重叠区间: pan=[{best_range.pan_start:.0f}°, {best_range.pan_end:.0f}°], "
              f"tilt=[{best_range.tilt_start:.0f}°, {best_range.tilt_end:.0f}°], "
              f"匹配点={best_range.match_count}")
        print(f"进入阶段2校准")

        # ===================== 阶段2: 在重叠区内精确校准 =====================
        print(f"\n{'='*60}")
        print(f"阶段2: 精确校准 - 在重叠区间内采集校准点")
        print(f"{'='*60}")

        calib_points = self._generate_points_in_overlaps(quick_mode)

        valid_points = []
        total_points = len(calib_points)

        print(f"生成 {total_points} 个校准点（仅位于重叠区域内）")

        for idx, point in enumerate(calib_points):
            if self.on_progress:
                self.on_progress(idx + 1, total_points,
                                f"校准点 {idx + 1}/{total_points}: pan={point.pan:.1f}°, tilt={point.tilt:.1f}°")

            print(f"\n  校准点 {idx + 1}/{total_points}: pan={point.pan:.1f}°, tilt={point.tilt:.1f}°")

            # 步骤1: 获取移动前全景帧（用于运动检测）
            frames_before_list = []
            for _ in range(3):
                frame = self.get_frame()
                if frame is not None:
                    frames_before_list.append(frame)
                time.sleep(0.1)

            if not frames_before_list:
                print(f"    警告: 无法获取移动前的全景画面")
                continue

            frames_before = np.mean(frames_before_list, axis=0).astype(np.uint8)

            # 步骤2: 移动球机到目标位置
            print(f"    [2/4] 移动球机到目标位置...")
            if not self.ptz.goto_exact_position(point.pan, point.tilt, 1):
                print(f"    警告: 移动球机失败")
                continue

            time.sleep(self.stabilize_time)

            # 步骤3: 获取移动后全景帧和球机抓拍
            print(f"    [3/4] 获取移动后的帧...")
            frames_after_list = []
            for _ in range(3):
                frame = self.get_frame()
                if frame is not None:
                    frames_after_list.append(frame)
                time.sleep(0.1)

            if not frames_after_list:
                print(f"    警告: 无法获取移动后的全景画面")
                continue

            frames_after = np.mean(frames_after_list, axis=0).astype(np.uint8)
            panorama_frame = frames_after

            # 球机抓拍（关键：特征匹配需要球机画面）
            ptz_frame = None
            if self.use_feature_matching and self.ptz_capture:
                try:
                    ptz_frame = self.ptz_capture()
                    if ptz_frame is not None:
                        print(f"      球机抓拍成功: {ptz_frame.shape}")
                except Exception as e:
                    print(f"      球机抓拍失败: {e}")

            # 步骤4: 视觉检测 + 重叠验证
            print(f"    [4/4] 视觉检测与重叠验证...")

            # 优先使用特征匹配（最可靠的方法）
            if ptz_frame is not None and panorama_frame is not None:
                success, match_count, cx, cy = self.overlap_discovery.match_frames(ptz_frame, panorama_frame)
                if success:
                    h, w = panorama_frame.shape[:2]
                    point.x_ratio = cx / w
                    point.y_ratio = cy / h
                    point.detected = True
                    valid_points.append(point)
                    print(f"    ✓ 特征匹配验证通过: {match_count}个匹配点, "
                          f"全景位置=({point.x_ratio:.3f}, {point.y_ratio:.3f})")
                    continue
                else:
                    print(f"    ✗ 特征匹配不足({match_count}点), 尝试运动检测...")

            # 备选: 运动检测
            if self.use_motion_detection and frames_before is not None and frames_after is not None:
                motion_result = self.visual_detector.detect_by_motion(frames_before, frames_after)
                if motion_result:
                    point.x_ratio, point.y_ratio = motion_result
                    point.detected = True
                    valid_points.append(point)
                    print(f"    ✓ 运动检测定位: ({point.x_ratio:.3f}, {point.y_ratio:.3f})")
                    continue

            # 自定义标记检测
            if self.detect_marker:
                marker_pos = self.detect_marker(panorama_frame)
                if marker_pos:
                    point.x_ratio, point.y_ratio = marker_pos
                    point.detected = True
                    valid_points.append(point)
                    print(f"    ✓ 标记检测成功: ({point.x_ratio:.3f}, {point.y_ratio:.3f})")
                    continue

            # 所有方法均失败 - 跳过此点（不使用估算！）
            print(f"    ✗ 此校准点无法验证，跳过（不使用估算）")

        # ===================== 检查有效校准点 =====================
        min_valid = 4
        if len(valid_points) < min_valid:
            self.state = CalibrationState.FAILED
            self.result = CalibrationResult(
                success=False,
                points=valid_points,
                error_message=f"有效校准点不足 (需要至少{min_valid}个, 实际{len(valid_points)}个)。"
                              f"请检查球机与全景的视野重叠是否足够。"
            )
            print(f"\n校准失败: {self.result.error_message}")
            if self.on_complete:
                self.on_complete(self.result)
            return self.result

        # ===================== 计算变换参数 =====================
        success = self._calculate_transform(valid_points)

        if success:
            self.state = CalibrationState.SUCCESS
            rms_error = self._calculate_rms_error(valid_points)

            self.result = CalibrationResult(
                success=True,
                points=valid_points,
                rms_error=rms_error
            )
            print(f"\n{'='*60}")
            print(f"校准成功!")
            print(f"有效校准点: {len(valid_points)}")
            print(f"重叠区间数: {len(self.overlap_ranges)}")
            print(f"RMS误差: {rms_error:.4f}°")
            
            # 自动保存校准结果
            try:
                from config import CALIBRATION_CONFIG
                if CALIBRATION_CONFIG.get('auto_save', True):
                    filepath = CALIBRATION_CONFIG.get('calibration_file', 'calibration.json')
                    self.save_calibration(filepath)
            except Exception:
                pass
            
            # 校准完成后，将球机复位到初始位置
            self._reset_ptz_position()
            
            print(f"{'='*60}")
        else:
            self.state = CalibrationState.FAILED
            self.result = CalibrationResult(
                success=False,
                points=valid_points,
                error_message="变换参数计算失败"
            )
            print(f"校准失败: {self.result.error_message}")

        if self.on_complete:
            self.on_complete(self.result)

        return self.result

    def _reset_ptz_position(self):
        """校准完成后将球机复位到初始位置"""
        if self.ptz is None:
            return
        
        try:
            # 获取默认位置配置
            from config import PTZ_CONFIG
            default_pan = PTZ_CONFIG.get('default_pan', 0)
            default_tilt = PTZ_CONFIG.get('default_tilt', 0)
            default_zoom = PTZ_CONFIG.get('default_zoom', 1)
            
            print(f"[校准] 球机复位到位置: pan={default_pan}, tilt={default_tilt}, zoom={default_zoom}")
            self.ptz.goto_exact_position(default_pan, default_tilt, default_zoom)
            time.sleep(0.5)  # 等待球机到位
        except Exception as e:
            print(f"[校准] 球机复位失败: {e}")

    def _generate_points_in_overlaps(self, quick_mode: bool = True) -> List[CalibrationPoint]:
        """
        在发现的重叠区间内生成校准点
        只在球机和全景有视觉重叠的区域生成点
        """
        points = []

        if quick_mode:
            # 快速模式: 每个重叠区间内生成3-5个点
            for overlap in self.overlap_ranges:
                # 在区间中心生成点
                pan_center = (overlap.pan_start + overlap.pan_end) / 2
                tilt_center = (overlap.tilt_start + overlap.tilt_end) / 2
                pan_span = overlap.pan_end - overlap.pan_start
                tilt_span = overlap.tilt_end - overlap.tilt_start

                # 中心点
                points.append(CalibrationPoint(pan=pan_center, tilt=tilt_center, zoom=1.0))

                # 四角点（如果区间足够宽）
                if pan_span > 10:
                    points.append(CalibrationPoint(
                        pan=overlap.pan_start + pan_span * 0.25,
                        tilt=tilt_center, zoom=1.0))
                    points.append(CalibrationPoint(
                        pan=overlap.pan_start + pan_span * 0.75,
                        tilt=tilt_center, zoom=1.0))

                if tilt_span > 10:
                    points.append(CalibrationPoint(
                        pan=pan_center,
                        tilt=overlap.tilt_start + tilt_span * 0.3, zoom=1.0))
                    points.append(CalibrationPoint(
                        pan=pan_center,
                        tilt=overlap.tilt_start + tilt_span * 0.7, zoom=1.0))
        else:
            # 完整模式: 在每个重叠区间内均匀分布
            grid_size = 5
            for overlap in self.overlap_ranges:
                for i in range(grid_size):
                    for j in range(grid_size):
                        pan = overlap.pan_start + (overlap.pan_end - overlap.pan_start) * i / (grid_size - 1)
                        tilt = overlap.tilt_start + (overlap.tilt_end - overlap.tilt_start) * j / (grid_size - 1)
                        points.append(CalibrationPoint(pan=pan, tilt=tilt, zoom=1.0))

        return points

    def _calculate_transform(self, points: List[CalibrationPoint]) -> bool:
        """使用RANSAC + 最小二乘法拟合变换参数，剔除异常值"""
        try:
            if len(points) < 4:
                print(f"计算变换参数错误: 有效点不足({len(points)}个)")
                return False

            pan_values = np.array([p.pan for p in points])
            tilt_values = np.array([p.tilt for p in points])
            x_ratios = np.array([p.x_ratio for p in points])
            y_ratios = np.array([p.y_ratio for p in points])

            # RANSAC剔除异常值
            inlier_mask = self._ransac_filter(x_ratios, y_ratios, pan_values, tilt_values)

            inlier_count = np.sum(inlier_mask)
            if inlier_count < 4:
                print(f"RANSAC后有效点不足({inlier_count}个)，使用全部点")
                inlier_mask = np.ones(len(points), dtype=bool)
                inlier_count = np.sum(inlier_mask)  # 更新inlier_count
            else:
                print(f"RANSAC: {len(points)}个点中{inlier_count}个内点，剔除{len(points) - inlier_count}个异常值")

            # 用内点拟合
            A = np.ones((inlier_count, 3))
            A[:, 1] = x_ratios[inlier_mask]
            A[:, 2] = y_ratios[inlier_mask]

            pan_params, _, _, _ = np.linalg.lstsq(A, pan_values[inlier_mask], rcond=None)
            tilt_params, _, _, _ = np.linalg.lstsq(A, tilt_values[inlier_mask], rcond=None)

            self.pan_offset = pan_params[0]
            self.pan_scale_x = pan_params[1]
            self.pan_scale_y = pan_params[2]

            self.tilt_offset = tilt_params[0]
            self.tilt_scale_x = tilt_params[1]
            self.tilt_scale_y = tilt_params[2]

            print(f"变换参数:")
            print(f"  pan = {self.pan_offset:.2f} + {self.pan_scale_x:.2f}*x + {self.pan_scale_y:.2f}*y")
            print(f"  tilt = {self.tilt_offset:.2f} + {self.tilt_scale_x:.2f}*x + {self.tilt_scale_y:.2f}*y")

            return True

        except Exception as e:
            print(f"计算变换参数错误: {e}")
            return False

    def _ransac_filter(self, x: np.ndarray, y: np.ndarray,
                       pan: np.ndarray, tilt: np.ndarray,
                       max_iterations: int = 200, threshold: float = 15.0,
                       min_samples: int = 4) -> np.ndarray:
        """RANSAC剔除变换拟合中的异常值"""
        n = len(x)
        best_inliers = np.zeros(n, dtype=bool)
        best_inlier_count = 0

        rng = np.random.RandomState(42)

        for _ in range(max_iterations):
            # 随机选min_samples个点
            indices = rng.choice(n, min_samples, replace=False)

            # 用这些点拟合
            A = np.ones((min_samples, 3))
            A[:, 1] = x[indices]
            A[:, 2] = y[indices]

            try:
                pan_params, _, _, _ = np.linalg.lstsq(A, pan[indices], rcond=None)
                tilt_params, _, _, _ = np.linalg.lstsq(A, tilt[indices], rcond=None)
            except np.linalg.LinAlgError:
                continue

            # 计算所有点的误差
            pred_pan = pan_params[0] + pan_params[1] * x + pan_params[2] * y
            pred_tilt = tilt_params[0] + tilt_params[1] * x + tilt_params[2] * y

            errors = np.sqrt((pred_pan - pan) ** 2 + (pred_tilt - tilt) ** 2)

            inliers = errors < threshold
            inlier_count = np.sum(inliers)

            if inlier_count > best_inlier_count:
                best_inlier_count = inlier_count
                best_inliers = inliers

        if best_inlier_count == 0:
            return np.ones(n, dtype=bool)

        return best_inliers

    def _calculate_rms_error(self, points: List[CalibrationPoint]) -> float:
        """计算均方根误差"""
        total_error = 0.0

        for p in points:
            pred_pan, pred_tilt = self.transform(p.x_ratio, p.y_ratio)
            error = math.sqrt((pred_pan - p.pan) ** 2 + (pred_tilt - p.tilt) ** 2)
            total_error += error ** 2

        return math.sqrt(total_error / len(points))

    def transform(self, x_ratio: float, y_ratio: float) -> Tuple[float, float]:
        """将全景坐标转换为PTZ角度"""
        pan = self.pan_offset + self.pan_scale_x * x_ratio + self.pan_scale_y * y_ratio
        tilt = self.tilt_offset + self.tilt_scale_x * x_ratio + self.tilt_scale_y * y_ratio
        return (pan, tilt)

    def inverse_transform(self, pan: float, tilt: float) -> Tuple[float, float]:
        """将PTZ角度转换为全景坐标（使用矩阵求逆处理交叉项）"""
        # 变换矩阵: [pan]   = [pan_offset]   + [pan_scale_x  pan_scale_y] * [x]
        #          [tilt]   [tilt_offset]   [tilt_scale_x tilt_scale_y]   [y]
        # 
        # 逆变换: [x] = M^-1 * ([pan, tilt] - offset)
        
        # 构造变换矩阵
        M = np.array([
            [self.pan_scale_x, self.pan_scale_y],
            [self.tilt_scale_x, self.tilt_scale_y]
        ])
        
        # 检查矩阵是否可逆
        det = np.linalg.det(M)
        if abs(det) < 1e-10:
            # 矩阵接近奇异，回退到简化计算
            x_ratio = (pan - self.pan_offset) / self.pan_scale_x if abs(self.pan_scale_x) > 1e-10 else 0.5
            y_ratio = (tilt - self.tilt_offset) / self.tilt_scale_y if abs(self.tilt_scale_y) > 1e-10 else 0.5
        else:
            # 计算逆矩阵
            M_inv = np.linalg.inv(M)
            offset = np.array([pan - self.pan_offset, tilt - self.tilt_offset])
            result = M_inv @ offset
            x_ratio, y_ratio = result[0], result[1]
        
        
        return (max(0, min(1, x_ratio)), max(0, min(1, y_ratio)))

    def is_calibrated(self) -> bool:
        return self.state == CalibrationState.SUCCESS

    def get_state(self) -> CalibrationState:
        return self.state

    def get_result(self) -> Optional[CalibrationResult]:
        return self.result

    def get_overlap_ranges(self) -> List[OverlapRange]:
        """返回发现的重叠区间"""
        return self.overlap_ranges

    def save_calibration(self, filepath: str) -> bool:
        """保存校准结果"""
        if not self.is_calibrated():
            return False

        try:
            import json
            ptz_config = _get_ptz_config()
            data = {
                'pan_offset': self.pan_offset,
                'pan_scale_x': self.pan_scale_x,
                'pan_scale_y': self.pan_scale_y,
                'tilt_offset': self.tilt_offset,
                'tilt_scale_x': self.tilt_scale_x,
                'tilt_scale_y': self.tilt_scale_y,
                'rms_error': self.result.rms_error if self.result else 0,
                'overlap_ranges': [
                    {
                        'pan_start': r.pan_start,
                        'pan_end': r.pan_end,
                        'tilt_start': r.tilt_start,
                        'tilt_end': r.tilt_end,
                        'match_count': r.match_count
                    }
                    for r in self.overlap_ranges
                ],
                # 保存安装方向配置
                'mount_type': ptz_config.get('mount_type', 'wall'),
                'tilt_flip': ptz_config.get('tilt_flip', False),
                'pan_flip': ptz_config.get('pan_flip', False),
            }

            with open(filepath, 'w') as f:
                json.dump(data, f, indent=2)

            print(f"校准结果已保存: {filepath}")
            return True

        except Exception as e:
            print(f"保存校准结果失败: {e}")
            return False

    def load_calibration(self, filepath: str) -> bool:
        """加载校准结果"""
        try:
            import json

            with open(filepath, 'r') as f:
                data = json.load(f)

            self.pan_offset = data['pan_offset']
            self.pan_scale_x = data['pan_scale_x']
            self.pan_scale_y = data['pan_scale_y']
            self.tilt_offset = data['tilt_offset']
            self.tilt_scale_x = data['tilt_scale_x']
            self.tilt_scale_y = data['tilt_scale_y']

            # 加载重叠区间（如果有）
            if 'overlap_ranges' in data:
                self.overlap_ranges = [
                    OverlapRange(
                        pan_start=r['pan_start'],
                        pan_end=r['pan_end'],
                        tilt_start=r['tilt_start'],
                        tilt_end=r['tilt_end'],
                        match_count=r['match_count'],
                        panorama_center_x=0,
                        panorama_center_y=0
                    )
                    for r in data['overlap_ranges']
                ]

            self.state = CalibrationState.SUCCESS
            self.result = CalibrationResult(
                success=True,
                points=[],
                rms_error=data.get('rms_error', 0)
            )
            
            # 检查安装方向配置是否匹配
            ptz_config = _get_ptz_config()
            current_mount = ptz_config.get('mount_type', 'wall')
            saved_mount = data.get('mount_type', 'wall')
            if current_mount != saved_mount:
                print(f"警告: 当前安装类型({current_mount})与校准时的({saved_mount})不同，建议重新校准！")

            print(f"校准结果已加载: {filepath}")
            return True

        except FileNotFoundError:
            print(f"校准文件不存在: {filepath}")
            return False
        except Exception as e:
            print(f"加载校准结果失败: {e}")
            return False


class CalibrationManager:
    """校准管理器"""

    def __init__(self, calibrator: CameraCalibrator, calibration_file: str = None):
        self.calibrator = calibrator
        # 优先使用传入的路径，否则从配置读取，最后使用默认值
        if calibration_file:
            self.calibration_file = calibration_file
        else:
            try:
                from config import CALIBRATION_CONFIG
                self.calibration_file = CALIBRATION_CONFIG.get(
                    'calibration_file', 'calibration.json'
                )
            except ImportError:
                self.calibration_file = 'calibration.json'

    def auto_calibrate(self, force: bool = False, fallback_on_failure: bool = True) -> CalibrationResult:
        """
        自动校准
        
        Args:
            force: 是否强制重新校准（不加载已有数据）
            fallback_on_failure: 校准失败时是否回退使用已有数据
        
        Returns:
            校准结果
        """
        # 检查是否启用加载上次校准数据
        load_on_startup = True  # 默认启用
        try:
            from config import CALIBRATION_CONFIG
            load_on_startup = CALIBRATION_CONFIG.get('load_on_startup', True)
        except:
            pass
        
        # 如果不是强制校准，尝试加载已有数据
        if not force and load_on_startup:
            if self.calibrator.load_calibration(self.calibration_file):
                print("使用已有校准结果")
                return self.calibrator.get_result()

        # 执行新校准
        if force:
            print("强制重新校准（不使用已有数据）...")
        elif not load_on_startup:
            print("已禁用加载校准数据，开始新校准...")
        else:
            print("开始自动校准...")
            
        result = self.calibrator.calibrate(quick_mode=True)

        if result.success:
            self.calibrator.save_calibration(self.calibration_file)
        elif fallback_on_failure:
            # 校准失败，尝试回退使用已有数据
            print("校准失败，尝试回退使用已有校准数据...")
            if self.calibrator.load_calibration(self.calibration_file):
                print("已回退到已有校准数据")
                result = self.calibrator.get_result()

        return result

    def check_calibration(self) -> Tuple[bool, str]:
        """检查校准状态"""
        state = self.calibrator.get_state()

        if state == CalibrationState.SUCCESS:
            result = self.calibrator.get_result()
            overlaps = self.calibrator.get_overlap_ranges()
            overlap_info = f", {len(overlaps)}个重叠区间" if overlaps else ""
            return (True, f"校准有效, RMS误差: {result.rms_error:.4f}°{overlap_info}")
        elif state == CalibrationState.FAILED:
            return (False, "校准失败")
        elif state == CalibrationState.RUNNING:
            return (False, "校准进行中")
        else:
            return (False, "未校准")