Transformation de la pose de la caméra dans le système de coordonnées fixe au sol ECEF et le système de coordonnées ENU

Dans le domaine des sciences de la terre et de la navigation, le système de coordonnées terrestre centré sur la terre (ECEF, Earth-Centered, Earth-Fixed) et le système de coordonnées nord-est du ciel (ENU, East-North-Up) sont généralement utilisés pour décrire le position et attitude sur la terre. Comme indiqué ci-dessous:

Coordonnées terrestres centrées sur la Terre ecef et système de coordonnées ENU du ciel nord-est

Dans le processus de photogrammétrie oblique, la transformation de ces deux systèmes de coordonnées est généralement impliquée et la pose de la caméra est tournée.Si vous avez déjà la position Xecef_cam de la caméra dans le système de coordonnées terrestre centré (ECEF) et la matrice de rotation de pose de la caméra R, vous pouvez les convertir dans le système de coordonnées nord-est du ciel (ENU) en suivant les étapes suivantes :

1. Calculez les coordonnées de la position de la caméra dans le système de coordonnées ENU :

Tout d'abord, la position de la caméra est transformée du système de coordonnées ECEF au système de coordonnées ENU. Le système de coordonnées ENU est un système de coordonnées local relatif au point de référence, il est donc nécessaire de fournir une position du point de référence, qui est généralement représentée par la hauteur de latitude et de longitude des coordonnées géographiques. Supposons que vous ayez un point de référence avec les coordonnées ECEF Xecef_ref. Ensuite, la coordonnée Xenu_cam de la position de la caméra dans le système de coordonnées ENU peut être calculée par : dXecef = Xecef_cam - Xecef_ref

Xenu_cam = R_ref.T * dXecef

Ici, dXecef est la différence entre la position de la caméra et les coordonnées ECEF du point de référence, R_ref.T est la transposée de la matrice de rotation du point de référence, R_ref peut être obtenu en calculant la latitude et la longitude du point de référence, la la forme spécifique est la suivante

# 参考点的经纬度
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. Calculez la pose de la caméra dans le système de coordonnées ENU :

Dans le système de coordonnées ENU, la représentation de la pose de la caméra est généralement représentée par une matrice de direction en cosinus (DCM, Direction Cosine Matrix). DCM peut être calculé par :

DCM_fra = R * R_ref.T

Ici, R_ref.T est la transposée de la matrice de rotation du point de référence, ibid. Grâce à ces deux étapes, vous pouvez convertir la position et l'attitude de la caméra du système de coordonnées ECEF au système de coordonnées ENU. N'oubliez pas de vous assurer que vous utilisez les mêmes unités de coordonnées et la même représentation de pose. Il convient de noter que le système de coordonnées ENU est un système de coordonnées local, de sorte que le résultat de la transformation dépend du choix du point de référence.

Ci-dessous un exemple de code 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')

résultat de l'opération

​

# position et pose correctes

enu_x =-5.25326294611772

enu_y =-88.1162361244917

enu_z =91.8780135626621

enu_matrix = np.tableau([[0.994999772929298,-0.0988252823926444,-0.0144573659655384],[-0.067174196729575,-0.555035783385474,-0.8291097 07128924],[0.0739126454971252,0.825935132272315,-0.55889898739748]])

Comme vous pouvez le voir, la conversion est correcte