前言
最近老板让我实现：已知无人机的位置、姿态、相机参数，获取无人机所摄照片中某一地物的地理坐标，虽然本科学过摄影测量，可无奈当时没好好学，现在大脑一片空白·······但直觉告诉我：就是获取相机内外方位元素，然后进行坐标转换即可。
（如有错误，还请批评指正QAQ）
一、认识坐标系
拜读了一些大佬的博客，发现从像素坐标系到大地坐标系的转换过程中，涉及到好多坐标系，如：像素坐标系、图像坐标系、相机坐标系、机体坐标系、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。
像素坐标系可以先转为图像坐标系，进而转为相机坐标系，也可以直接转为相机坐标系，这里采用后者。具体推导过程可以参考无人机——像素坐标系转世界坐标系（NED），主要是构建相机内参矩阵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]])至此，转换完成！