video-location/pixel_to_lonlat.py

# -*- coding: utf-8 -*-
"""
最终功能模块：从像素坐标到经纬度的转换

核心功能:
- pixel_to_lonlat: 输入像素坐标(u, v)和摄像头状态，输出对应的经纬度。
"""
import numpy as np
from scipy.spatial.transform import Rotation as R

# --- 核心函数 ---

def pixel_to_lonlat(u, v, camera_params, ptz_params):
    """
    正向投影：将像素坐标(u, v)转换为经纬度。

    Args:
        u (int): 像素横坐标
        v (int): 像素纵坐标
        camera_params (dict): 摄像头的固定物理参数 (通过标定获得)
        ptz_params (dict): 摄像头当前的云台参数

    Returns:
        tuple: (经度, 纬度) 或 (None, None) 如果计算失败
    """
    # --- 1. 解包所有已知参数 ---
    res_w, res_h = int(camera_params['resolution_w']), int(camera_params['resolution_h'])
    cam_lon, cam_lat = float(camera_params['cam_lon']), float(camera_params['cam_lat'])
    cam_height, hfov =float( camera_params['height']), float(camera_params['hfov'])
    north_p = float(camera_params['north_p_value'])
    p, t = float(ptz_params['pan']), float(ptz_params['tilt'])

    # --- 2. 相机内参模型：将像素(u,v)转换为相机坐标系下的方向向量 ---
    # (相机坐标系定义: x向右, y向下, z向前)
    f_x = res_w / (2 * np.tan(np.radians(hfov) / 2))
    f_y = f_x  # 假设像素是正方形
    cx, cy = res_w / 2, res_h / 2

    # 计算方向向量并归一化
    vec_cam = np.array([(u - cx) / f_x, (v - cy) / f_y, 1.0])
    vec_cam /= np.linalg.norm(vec_cam)

    # --- 3. 旋转变换：将相机坐标系下的向量旋转到世界坐标系 ---
    # (世界坐标系定义: ENU, 即 x向东, y向北, z向上)
    r_base = R.from_euler('x', -90, degrees=True) # 基础旋转：将(相机前)对准(世界北)
    p_corrected = p - north_p                     # 校正P值，使其以正北为0度
    r_pan = R.from_euler('z', p_corrected, degrees=True)  # 方位角旋转 (绕世界Z轴)
    r_tilt = R.from_euler('x', -t, degrees=True)          # 俯仰角旋转 (绕世界X轴)

    # 合成总旋转
    total_rotation = r_pan * r_tilt * r_base
    vec_enu = total_rotation.apply(vec_cam)

    # --- 4. 几何解算：计算光线与地面的交点 ---
    # 如果光线向量的z分量是正数或零，说明光线朝向天空或水平，无法与地面相交
    if vec_enu[2] >= -1e-9:
        return None, None

    # 摄像头在世界坐标系中的位置
    cam_pos_enu = np.array([0, 0, cam_height])

    # 求解射线参数 s
    s = -cam_height / vec_enu[2]

    # 计算交点坐标 (米)
    intersection_enu = cam_pos_enu + s * vec_enu

    # --- 5. 坐标转换：将米制坐标转回经纬度 ---
    EARTH_RADIUS = 6378137.0
    delta_lat_rad = intersection_enu[1] / EARTH_RADIUS
    delta_lon_rad = intersection_enu[0] / (EARTH_RADIUS * np.cos(np.radians(cam_lat)))

    lon = cam_lon + np.degrees(delta_lon_rad)
    lat = cam_lat + np.degrees(delta_lat_rad)

    return {"lon":lon, "lat":lat}

# ==============================================================================
# 使用示例
# ==============================================================================
if __name__ == '__main__':
    # --- 1. 定义您最终标定好的、精确的摄像头参数 ---
    # 这些是固定的物理参数，您应该保存下来
    CAMERA_PARAMS = {
        "height": 28.0000,
        "hfov": 61.0000,
        "resolution_w": 1280,
        "resolution_h": 720,
        "cam_lon": 112.893799,
        "cam_lat": 28.221701,
        "north_p_value": 39.0
    }

    # --- 2. 定义摄像头当前的云台状态 ---
    # 在您的实际应用中，这些值应该是从摄像头API实时读取的
    CURRENT_PTZ_PARAMS = {
        "pan": 271.9,
        "tilt": 26.2
    }

    # --- 3. 指定您想要查询的屏幕像素坐标 ---
    pixel_u, pixel_v = 91, 276 # 使用您标定过的“黄金数据点”之一作为测试

    print(f"--- 正在将像素坐标 ({pixel_u}, {pixel_v}) 转换为经纬度 ---")

    # --- 4. 调用核心函数进行计算 ---
    predicted_lon, predicted_lat = pixel_to_lonlat(pixel_u, pixel_v, CAMERA_PARAMS, CURRENT_PTZ_PARAMS)

    # --- 5. 打印结果 ---
    if predicted_lon is not None:
        print("\n计算成功！")
        print(f"  预测经度: {predicted_lon:.8f}")
        print(f"  预测纬度: {predicted_lat:.8f}")

        # 与标定时的真实值对比 (可选，用于验证)
        true_lon, true_lat = 112.89461289520455, 28.221594983922852
        print("\n--- 与标定时的真实值对比 ---")
        print(f"  真实经度: {true_lon:.8f}")
        print(f"  真实纬度: {true_lat:.8f}")
    else:
        print("\n计算失败（可能该点指向天空）。")