64 lines
2.5 KiB
Python
64 lines
2.5 KiB
Python
import numpy as np
|
||
from pyproj import Transformer
|
||
|
||
def lidar_2d_to_wgs84(lidar_points_xy, origin_lon, origin_lat, rotation_angle_deg, fixed_z=0):
|
||
"""
|
||
将激光雷达的2D坐标(X, Y)转换为WGS84地理坐标(纬度, 经度)。
|
||
|
||
参数:
|
||
- lidar_points_xy: (N, 2) 的 NumPy 数组，表示 N 个激光雷达点的 x, y 坐标。
|
||
- origin_lon: 激光雷达坐标系原点的经度。
|
||
- origin_lat: 激光雷达坐标系原点的纬度。
|
||
- rotation_angle_deg: 激光雷达坐标系相对于地理北向的旋转角度（以度为单位）。
|
||
- fixed_z: 所有点的固定高度，默认为0。
|
||
|
||
返回:
|
||
- latitudes: (N,) 的 NumPy 数组，表示转换后的纬度。
|
||
- longitudes: (N,) 的 NumPy 数组，表示转换后的经度。
|
||
"""
|
||
|
||
# 将角度从度转换为弧度
|
||
rotation_angle_rad = np.radians(rotation_angle_deg)
|
||
|
||
# 创建旋转矩阵
|
||
rotation_matrix = np.array([
|
||
[np.cos(rotation_angle_rad), -np.sin(rotation_angle_rad)],
|
||
[np.sin(rotation_angle_rad), np.cos(rotation_angle_rad)]
|
||
])
|
||
|
||
# 对每个点应用旋转
|
||
rotated_points = np.dot(lidar_points_xy, rotation_matrix.T)
|
||
|
||
# 添加固定的高度
|
||
earth_points = np.column_stack((rotated_points, np.full(rotated_points.shape[0], fixed_z)))
|
||
|
||
# 使用 UTM 投影进行转换，首先找到对应的 UTM 区域
|
||
transformer_to_utm = Transformer.from_crs("EPSG:4326", "+proj=utm +zone=33 +ellps=WGS84 +datum=WGS84 +units=m +no_defs")
|
||
utm_x_origin, utm_y_origin = transformer_to_utm.transform(origin_lat, origin_lon)
|
||
|
||
# 将激光雷达坐标转换为 UTM 坐标
|
||
utm_points = earth_points.copy()
|
||
utm_points[:, 0] += utm_x_origin
|
||
utm_points[:, 1] += utm_y_origin
|
||
|
||
# 再将 UTM 坐标转换回 WGS84
|
||
transformer_to_wgs84 = Transformer.from_crs("+proj=utm +zone=33 +ellps=WGS84 +datum=WGS84 +units=m +no_defs", "EPSG:4326")
|
||
latitudes, longitudes = transformer_to_wgs84.transform(utm_points[:, 0], utm_points[:, 1])
|
||
|
||
return latitudes, longitudes
|
||
|
||
# 示例：一些激光雷达点云数据 (X, Y)
|
||
lidar_points_xy = np.array([[100, 200], [300, 400]])
|
||
|
||
# 示例参数
|
||
origin_lon = 12.3456 # 激光雷达坐标系原点的经度
|
||
origin_lat = 54.3210 # 激光雷达坐标系原点的纬度
|
||
rotation_angle_deg = 0 # 激光雷达坐标系相对于地理北向的旋转角度（度）
|
||
|
||
# 执行转换
|
||
latitudes, longitudes = lidar_2d_to_wgs84(lidar_points_xy, origin_lon, origin_lat, rotation_angle_deg)
|
||
|
||
print("Latitude:", latitudes)
|
||
print("Longitude:", longitudes)
|
||
|