概述
本文介绍如何根据无人机的位置、姿态及相机参数，将影像中的像素坐标转换为地理坐标。涉及获取相机内外方位元素并进行坐标转换。
一、认识坐标系
从像素坐标系到大地坐标系的转换过程中，涉及多个坐标系，包括：像素坐标系、图像坐标系、相机坐标系、机体坐标系、ENU 坐标系、大地坐标系。以下简略介绍各坐标系及其联系。
（1）像素坐标系
像素坐标系通常以 o-u-v 表示，原点 o 位于图像右上角，u 轴水平指向右，v 轴竖直指向下，以像素（pixel）为单位。
（2）图像坐标系
图像坐标系和像素坐标系在同一个平面上，原点是相机光中与成像平面的交点，通常为成像中心，以毫米（mm）为单位。从下图不难看出，图像坐标系和像素坐标系之间由平移、缩放关系，转换较为简便。
（3）相机坐标系
相机坐标系以光心为原点，x、y 轴与像素坐标系下的 u 轴和 v 轴平行，以主光轴为 z 轴，光心距离像素平面的距离为焦距 f。实际地物点通过光心在成像面上成像，两者存在等比关系。
（4）机体坐标系
机体坐标系坐标原点与相机坐标系原点相同，该坐标系有的遵循右手法则，有的遵循左手法则，转换过程中确定某一种即可。这里以左手法则为例，x 轴指向飞机前进方向，y 轴指向前进方向右侧，z 轴垂直于机身平面向上。机体坐标系与相机坐标系之间的关系依靠安装矩阵联系。
（5）ENU 坐标系
ENU 坐标系即'东—北—天'坐标系（East—North—Up），坐标原点与机体坐标系原点相同，x 轴指向正东，y 轴指向正北，Z 轴垂直于水平面向上。因此 ENU 坐标系与机体坐标系的差别在于轴的朝向不同，两者之间的关系靠飞机的姿态角（航偏、俯仰、横滚）联系。
（6）ECEF 坐标系
ECEF 坐标系（Earth-Centered Earth-Fixed）是一种笛卡尔坐标系，原点为地球质心，x 轴指向本初子午线与赤道的交点，z 轴与地轴平行指向北极点，y 轴垂直于 xoz 面构成右手坐标系。ECEF 与 ENU 是依靠旋转矩阵联系。
以上就是在转换过程中所涉及的坐标系，最后的 ECEF 坐标系是以（X，Y，Z）表示地面点的位置，但通常我们使用（大地经度，大地纬度，大地高）来表示，因此最后需要转换一下。
二、坐标系之间的转换
（1）像素坐标系向相机坐标系转换
已知条件：像素坐标系下的某一点的坐标（u，v），分辨率（dx，dy），图像大小（width，height，以像素为单位），焦距（f），深度 z。
像素坐标系可以先转为图像坐标系，进而转为相机坐标系，也可以直接转为相机坐标系，这里采用后者。具体推导过程可参考相关摄影测量资料，主要是构建相机内参矩阵 K:
其中：fx=f/dx，fy=f/dy，cx=width/2，cy=height/2，则像素坐标系下的坐标在相机坐标系下的坐标为:
其中：Z 为深度，即场景点到相机光心沿光轴的距离。代码实现如下：
def pixel_to_camera(u0,v0,width,height,dx,dy,f,z):
''' u0, v0: 像素坐标（以像素为单位）
width,height：图像大小（以像素为单位）
dx, dy: 每个像素的物理尺寸（mm/像素）
f:相机焦距
z:深度
返回值为相机坐标系中的位置（单位 mm）
'''
fx = f/dx
fy = f/dy
cx = width/2#cx, cy: 图像中心（主点坐标，像素为单位，通常为图像宽/2，高/2）
cy = height/2
K = np.array([[fx,0,cx], [0,fy,cy], [0,0,1]])
uv1 = np.array([[u0], [v0], [1]])
xy1 = np.linalg.inv(K) @ uv1
xyz = xy1*z
return xyz
注意：需统一单位。
（2）相机坐标系向机体坐标系转换
当相机坐标系原点与机体坐标系原点在同一位置时，只需要将相机坐标左乘旋转矩阵即可，该旋转矩阵也许会在技术文档中给出。举个简单的例子：
def camera_to_UAV(a,xyz_camera):
''' a:相机旋转角度，R_cb 以绕 x 轴旋转为例
xyz_camera:相机坐标系下坐标
'''
theta = np.deg2rad(a)
R_cb = np.array([[1,0,0], [0,np.cos(theta),-np.sin(theta)], [0,np.sin(theta),np.cos(theta)]])
xyz_UAV = R_cb @ xyz_camera
return xyz_UAV
（3）机体坐标系向 ENU 坐标系转换
该过程就需要无人机的姿态角了，首先构建旋转矩阵：
def euler_to_rotation_matrix(yaw,pitch,roll):
''' yaw:航偏角 pitch:俯仰角 roll:横滚角
'''
yaw = np.deg2rad(yaw)
pitch = np.deg2rad(pitch)
roll = np.deg2rad(roll)
R_z = np.array([[np.cos(yaw),-np.sin(yaw),0], [np.sin(yaw),np.cos(yaw),0], [0,0,1] ])
R_y = np.array([[np.cos(pitch),0,np.sin(pitch)], [0,1,0], [-np.sin(pitch),0,np.cos(pitch)] ])
R_x = np.array([[1,0,0], [0,np.cos(roll),-np.sin(roll)], [0,np.sin(roll),np.cos(roll)] ])
R = R_z @ R_y @ R_x
return R
然后需要确定 ENU 坐标系的坐标原点，通常以无人机的位置为 ENU 坐标系原点，这样只需要左乘旋转矩阵即可完成转换：
def UVA_to_ENU(yaw,pitch,roll,UAV_coor):
''' yaw:航偏角 pitch:俯仰角 roll:横滚角
UAV_coor:机体坐标系下的坐标
'''
R = euler_to_rotation_matrix(yaw,pitch,roll)
X_ENU = R @ UAV_coor
return X_ENU
（4）ENU 坐标系 ECEF 坐标系转换
这里是已知 ENU 坐标原点的坐标，通常为 WGS84 下的坐标，以经度、纬度、大地高表示，需要先转为 ECEF 表示的坐标，然后再进行旋转和平移：
def ENU_to_ECEF(lat0,lon0,h0,ENU_coor):
''' lat0,lon0,h0:ENU 坐标系原点的经纬度和高度
ENU_coor:ENU 坐标系下的坐标
'''
wgs84_to_ecef = Transformer.from_crs('epsg:4979', 'epsg:4978',always_xy=True)
x0,y0,z0 = wgs84_to_ecef.transform(lon0, lat0, h0)
lat0_rad = np.radians(lat0)
lon0_rad = np.radians(lon0)
s_lat = np.sin(lat0_rad)
s_lon = np.sin(lon0_rad)
c_lat = np.cos(lat0_rad)
c_lon = np.cos(lon0_rad)
R = np.array([[-s_lon,c_lon,0], [-s_lat*c_lon,-s_lat*s_lon,c_lat], [c_lat*c_lon,c_lat*s_lon,s_lat]])
ECEF = R.T @ ENU_coor + np.array([[x0],[y0],[z0]])
return ECEF
（5）坐标格式转换
最后为了易读，还是要转为经度、纬度、高度格式：
def ECEF_to_lla(ECEF_coor):
ecef_to_wgs84 = Transformer.from_crs('epsg:4978', 'epsg:4979',always_xy=True)
lon,lat,h = ecef_to_wgs84.transform(ECEF_coor[0],ECEF_coor[1],ECEF_coor[2])
return np.array([[lon],[lat],[h]])
转换流程结束。