Transformation of the camera's pose in the ground-fixed coordinate system ECEF and the ENU coordinate system

In the field of earth science and navigation, the earth-centered earth-fixed coordinate system (ECEF, Earth-Centered, Earth-Fixed) and the northeast sky coordinate system (ENU, East-North-Up) are usually used to describe the position and attitude on the earth. As shown below:

​Earth-centered earth-fixed coordinates ecef and northeast sky ENU coordinate system

In the process of oblique photogrammetry, the transformation of these two coordinate systems is usually involved, and the pose of the camera is rotated. If you already have the position Xecef_cam of the camera in the earth-centered earth-fixed coordinate system (ECEF) and the pose rotation matrix of the camera R, you can convert them to the northeast sky coordinate system (ENU) by following steps:

1. Calculate the coordinates of the camera position in the ENU coordinate system:

First, the camera position is transformed from the ECEF coordinate system to the ENU coordinate system. The ENU coordinate system is a local coordinate system relative to the reference point, so it is necessary to provide a position of the reference point, which is usually represented by the latitude and longitude height of the geographic coordinates. Suppose you have a reference point with ECEF coordinates Xecef_ref. Then, the coordinate Xenu_cam of the camera position in the ENU coordinate system can be calculated by: dXecef = Xecef_cam - Xecef_ref

Xenu_cam = R_ref.T * dXecef

Here, dXecef is the difference between the camera position and the ECEF coordinates of the reference point, R_ref.T is the transpose of the rotation matrix of the reference point, R_ref can be obtained by calculating the latitude and longitude of the reference point, the specific form is as follows

# 参考点的经纬度
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. Calculate the pose of the camera in the ENU coordinate system:

In the ENU coordinate system, the pose representation of the camera is usually represented by a Direction Cosine Matrix (DCM, Direction Cosine Matrix). DCM can be calculated by:

DCM_enu = R * R_ref.T

Here, R_ref.T is the transpose of the rotation matrix of the reference point, ibid. Through these two steps, you can convert the camera's position and attitude from the ECEF coordinate system to the ENU coordinate system. Remember to make sure you use the same coordinate units and pose representation. It is worth noting that the ENU coordinate system is a local coordinate system, so the transformation result depends on the choice of reference point.

Below is a Python code example

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')

operation result

​

# correct position and pose

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]])

As you can see, the conversion is correct