import numpy as np
import smpy.utils as smpy_utils
import ditu.topbind as tb
from scipy.spatial.transform import Rotation as R
import o3d_hdmap.open3d as o3d
geojson_path = '/data/mpcv_lspo_download_data/prod/PLEF35196-2021-11-10-17-48-39/mapping_results/ground_pole_geojson_LC.json'
geojson_data = smpy_utils.read_json(geojson_path)
anchor_lla_ = tb.utils.ecef2lla( np.array(geojson_data['extrinsics'][0]['center']))
lla = [111.64994725780178,30.73907846113732,131.25631082151085]
enu = tb.utils.lla2enu_cheap([lla], anchor_lla=anchor_lla_)
print(enu)
[[ 966.10732692 -5813.22954404 -59.09057418]]
上述方法操作简单,但是lla2enu_cheap函数存在损失,导致结果不够准确。需要无损的lla2enu转换,则使用如下方法进行
坐标转换:通过旋转矩阵乘以(lla2ecef - 锚点(ecef))的方式,将lla转换成enu坐标
import numpy as np
import smpy.utils as smpy_utils
import ditu.topbind as tb
from scipy.spatial.transform import Rot