相机的位姿在地固坐标系ECEF和ENU坐标系的转换

在地球科学和导航领域,通常使用地心地固坐标系(ECEF,Earth-Centered, Earth-Fixed)和东北天坐标系(ENU,East-North-Up)来描述地球上的位置和姿态。如下图所示:

相机的位姿在地固坐标系ECEF和ENU坐标系的转换_第1张图片

​地心地固坐标ecef和东北天ENU坐标系

在倾斜摄影测量过程中,通常涉及这两个坐标系的转换,将相机的位姿互转,如果你已经有了相机在地心地固坐标系(ECEF)中的位置 Xecef_cam 和相机的姿态旋转矩阵 R,你可以通过以下步骤将它们转换到东北天坐标系(ENU):

1、计算相机位置在ENU坐标系中的坐标:

首先,将相机位置从ECEF坐标系转换为ENU坐标系。ENU坐标系是相对于参考点的局部坐标系,所以需要提供一个参考点的位置,通常使用地理坐标经纬高来表示。假设你有一个参考点的ECEF坐标为 Xecef_ref。那么,相机位置在ENU坐标系中的坐标 Xenu_cam 可以通过以下方式计算: dXecef = Xecef_cam - Xecef_ref

Xenu_cam = R_ref.T * dXecef

这里,dXecef 是相机位置相对于参考点的ECEF坐标的差值,R_ref.T 是参考点的旋转矩阵的转置,R_ref可以通过计算参考点的经纬度得到,具体形式如下

# 参考点的经纬度
lon = 114.676720
lat = 37.746420
alt = 0  # 参考点的海拔高度(单位:米)
rclat, rclng = np.radians(lat), np.radians(lon) 
rot_ECEF2ENUV = np.array([[-math.sin(rclng),                math.cos(rclng),                              0],
                              [-math.sin(rclat)*math.cos(rclng), -math.sin(rclat)*math.sin(rclng), math.cos(rclat)],
                              [math.cos(rclat)*math.cos(rclng),  math.cos(rclat)*math.sin(rclng),  math.sin(rclat)]])

2、计算相机的ENU坐标系中的姿态:

在ENU坐标系中,相机的姿态表示通常使用方向余弦矩阵(DCM,Direction Cosine Matrix)来表示。DCM可以通过以下方式计算:

DCM_enu = R * R_ref.T

这里,R_ref.T 是参考点的旋转矩阵的转置,同上。 通过这两个步骤,你就可以将相机的位置和姿态从ECEF坐标系转换到ENU坐标系中了。记得要确保使用相同的坐标单位和姿态表示方式。值得注意的是,ENU坐标系是一个局部坐标系,所以转换结果依赖于参考点的选择。

下面是一个Python代码示例

import numpy as np
import math

def geodetic_to_ecef(lon, lat, alt):
    a = 6378137.0  # 地球的半长轴(赤道半径,单位:米)
    f = 1 / 298.257223563  # 扁率
    
    lon_rad = np.radians(lon)
    lat_rad = np.radians(lat)
    
    N = a / np.sqrt(1 - (f * (2 - f)) * np.sin(lat_rad)**2)
    
    x = (N + alt) * np.cos(lat_rad) * np.cos(lon_rad)
    y = (N + alt) * np.cos(lat_rad) * np.sin(lon_rad)
    z = (N * (1 - f)**2 + alt) * np.sin(lat_rad)
    
    return np.array([x, y, z])


#已知的地固坐标系下的位置和姿态
ecef_x= -2108290.78524083 
ecef_y= 4588675.69211609 
ecef_z= 3883213.009044
#R
ecef_matrix = np.array([[-0.924619168850922, -0.37082528979597, -0.0869942356778073],
                             [0.192895998529247, -0.258938143907446, -0.946436564900771],
                             [0.328436487535772, -0.891874229966031, 0.310949885958594]])

# 为了验证的enu坐标系下的正确位置和姿态
enu_x = -5.25326294611772 
enu_y = -88.1162361244917
enu_z = 91.8780135626621			
enu_matrix = np.array([[0.994999772929298, -0.0988252823926444, -0.0144573659655384],
                           [-0.067174196729575, -0.555035783385474,-0.829109707128924],
                           [0.0739126454971252, 0.825935132272315, -0.55889898739748]])

# 参考点的经纬度
lon = 114.676720
lat = 37.746420
alt = 0  # 参考点的海拔高度(单位:米)
rclat, rclng = np.radians(lat), np.radians(lon) 
#R_ref
rot_ECEF2ENUV = np.array([[-math.sin(rclng),                math.cos(rclng),                              0],
                              [-math.sin(rclat)*math.cos(rclng), -math.sin(rclat)*math.sin(rclng), math.cos(rclat)],
                              [math.cos(rclat)*math.cos(rclng),  math.cos(rclat)*math.sin(rclng),  math.sin(rclat)]])

# 计算参考点的ECEF坐标
Xecef_ref = geodetic_to_ecef(lon, lat, alt)
print('参考点位置-地固坐标系')
print(Xecef_ref)
print('\n')

# 相机位置在ECEF坐标系中
Xecef_cam = np.array([ecef_x, ecef_y, ecef_z])  

# 计算相机位置在ENU坐标系中的坐标
dXecef = Xecef_cam - Xecef_ref
print('dXecef')
print(dXecef)
print('\n')

#Xenu_cam = np.dot(ecef_matrix.T, dXecef)
Xenu_cam = np.dot(dXecef, rot_ECEF2ENUV.T)
print('相机在ENU坐标中的位置')
print(Xenu_cam)
print('\n')


# 计算相机的ENU坐标系中的姿态
DCM_enu = np.dot(ecef_matrix, rot_ECEF2ENUV.T)
print('相机在ENU坐标中姿态')
print(DCM_enu)
print('\n')

运行结果

相机的位姿在地固坐标系ECEF和ENU坐标系的转换_第2张图片

# 正确的位置和姿态

enu_x =-5.25326294611772

enu_y =-88.1162361244917

enu_z =91.8780135626621

enu_matrix = np.array([[0.994999772929298,-0.0988252823926444,-0.0144573659655384],[-0.067174196729575,-0.555035783385474,-0.829109707128924],[0.0739126454971252,0.825935132272315,-0.55889898739748]])

可以看到,转换正确

你可能感兴趣的:(SFM,计算机视觉,python)