无人机影像的像素坐标转大地坐标

前言

最近老板让我实现：已知无人机的位置、姿态、相机参数，获取无人机所摄照片中某一地物的地理坐标，虽然本科学过摄影测量，可无奈当时没好好学，现在大脑一片空白·······但直觉告诉我：就是获取相机内外方位元素，然后进行坐标转换即可。

（如有错误，还请批评指正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]])

至此，转换完成！