在无人机航拍应用中，一个核心的需求是将图像上的某个点与现实世界中的地理位置精确对应起来。无论是目标跟踪、地图测绘还是农情监测，理解图像像素与其对应的经纬度（GPS坐标）之间的关系至关重要。本文将详细介绍如何实现单个像素坐标到GPS坐标的双向转换，并提供基于Python的实现思路。
核心问题
- 像素坐标 -> GPS坐标： 已知图像上一个点的像素坐标 (u, v)，以及拍摄时无人机的状态（位置、姿态、相机参数），如何计算该像素点对应的地面GPS经纬度 (Lon, Lat)？
- GPS坐标 -> 像素坐标： 已知地面上一个目标的GPS经纬度 (Lon, Lat)，以及拍摄时无人机的状态，如何反算出该目标出现在图像中的像素坐标 (u, v)？
所需信息（前提条件）
- 图像参数： 图像宽度 img_width (像素)，图像高度 img_height (像素)。
- 相机内参：
- 传感器尺寸：宽度 sensor_width (毫米)，高度 sensor_height (毫米)。
- 相机焦距 focal_length (毫米)。
- 无人机状态信息：
- 相机位置：经度 camera_lon (度)，纬度 camera_lat (度)，相对于海平面的高度或起飞点的高度 flight_height (米)。
- 相机姿态：偏航角 yaw_angle (度)，俯仰角 pitch_angle (度)，滚转角 roll_angle (度)。
坐标系约定（非常重要！）
为了保证计算的正确性，我们必须明确并统一坐标系的定义。以下是一种常见的约定，你的具体实现可能需要根据实际情况调整：
- 图像坐标系 (u, v)：
- 原点：图像左上角。
- u轴：水平向右。
- v轴：垂直向下。
- 单位：像素。
- 相机坐标系 (Xc, Yc, Zc)：
- 原点：相机光心。
- Xc轴：指向相机传感器的右方。
- Yc轴：指向相机传感器的下方（与图像v轴同向）。
- Zc轴：指向相机前方（光轴方向）。
- 单位：米。
- 注意：也有约定Yc轴向上的，这会影响内参矩阵 fy 和后续计算中的符号。本文档和示例代码采用Yc向下的约定。
- 世界/导航坐标系 (Xw, Yw, Zw)：
- 通常使用 NED (North-East-Down) 坐标系。
- 原点：无人机（相机）位置在地面上的投影点。
- Xw轴：指向正北 (North)。
- Yw轴：指向正东 (East)。
- Zw轴：指向地心 (Down)。
- 单位：米。
- 地理坐标系 (Lat, Lon)：
- 使用WGS84标准。
- 单位：度。
第一部分：像素坐标 (u, v) -> GPS坐标 (Lon, Lat)
这个过程模拟了光线从地面点穿过相机光心最终成像在传感器像素上的逆过程。
步骤：
- 计算相机内参矩阵 (K)：
将焦距从毫米转换为米：focal_m = focal_length / 1000。
将传感器尺寸从毫米转换为米：
- sensor_width_m = sensor_width / 1000,
- sensor_height_m = sensor_height / 1000。
计算以像素为单位的焦距：
- fx = focal_m / sensor_width_m * img_width
- fy = focal_m / sensor_height_m * img_height
计算主点（通常是图像中心）：
- cx = img_width / 2
- cy = img_height / 2
构建内参矩阵 K:
K = np.array([ [fx, 0, cx], [0, fy, cy], [0, 0, 1] ])计算 K 的逆矩阵 K_inv = np.linalg.inv(K)。
- 像素坐标转相机坐标系方向向量：
- 将像素坐标 (u, v) 转换为齐次坐标 [u, v, 1]。
- 使用内参矩阵的逆 K_inv 将像素坐标转换到相机坐标系下的一个方向向量：
- camera_coords_homogeneous = K_inv @ np.array([u, v, 1])
- 这个向量表示从相机光心指向该像素方向的三维射线。为了后续计算，我们通常将其归一化或设定一个深度。由于我们采用 Zc 轴向前的约定，可以将 Zc 分量设为 1（或者 -1，取决于后续旋转定义，这里设为 1 方便理解）：
- Xc = camera_coords_homogeneous[0]
- Yc = camera_coords_homogeneous[1]
- Zc = 1.0 # 方向向量，Z分量设为1
计算旋转矩阵 ( R )：
将无人机姿态角（Yaw, Pitch, Roll）从度转换为弧度。
根据 世界坐标系 (NED) 到 相机坐标系 的转换关系，构建旋转矩阵。这通常是一个组合旋转，例如按 ZYX 的顺序（先绕Z轴偏航，再绕新的Y轴俯仰，最后绕新的X轴滚转 - 这是内旋顺序；或者按 ZYX 外旋，先滚转，再俯仰，最后偏航）。一个常见的 NED 到 相机 (X右Y下Z前) 的旋转矩阵构建方式（外旋 ZYX，对应 Roll, Pitch, Yaw）：
# 角度转弧度 yaw_rad, pitch_rad, roll_rad = np.radians(yaw_angle), np.radians(pitch_angle), np.radians(roll_angle) # 基本旋转矩阵 Rx = np.array([[1, 0, 0], [0, np.cos(roll_rad), -np.sin(roll_rad)], [0, np.sin(roll_rad), np.cos(roll_rad)]]) # Roll 绕 X (North) Ry = np.array([[np.cos(pitch_rad), 0, np.sin(pitch_rad)], [0, 1, 0], [-np.sin(pitch_rad), 0, np.cos(pitch_rad)]]) # Pitch 绕 Y (East) Rz = np.array([[np.cos(yaw_rad), -np.sin(yaw_rad), 0], [np.sin(yaw_rad), np.cos(yaw_rad), 0], [0, 0, 1]]) # Yaw 绕 Z (Down) # 从 NED 到 机体坐标系的旋转矩阵 (注意顺序和内外旋定义) # 如果姿态角是相对于NED的，常见的组合是 R = Rz @ Ry @ Rx <