Skip to content

C
catkin_ws
Commit ab6f81e3 authored by wangqibing's avatar wangqibing
Browse files
Options

cs add scripts

parent bb9ce2ca
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 603 additions and 1 deletion
README.md
View file @ ab6f81e3
# catkin_ws
常熟二期总的工程仓库
scripts/gaussian.py 0 → 100755
View file @ ab6f81e3
import math
import numpy as np
import wgs84
from rotations import degree2rad, rad2degree
ZoneOffset = 1e6
ZoneWidth = 3
EastOffset = 500000.0
def Convert_v2(inLonLat): # BL --> XY
Zone = (int)((inLonLat[1] + 1.5) / ZoneWidth)
L0 = Zone * ZoneWidth
dL = inLonLat[1] - L0
dLRadian = degree2rad(dL)
latRadian = degree2rad(inLonLat[0])
X = wgs84.GetX(latRadian)
N = wgs84.GetN(latRadian)
t = math.tan(latRadian)
t2 = t * t
sinB = math.sin(latRadian)
cosB = math.cos(latRadian)
ita2 = wgs84.ep2 * cosB * cosB
temp = dLRadian * cosB
temp2 = temp * temp
yPart1 = 0.5 * N * temp * temp * t
yPart2 = N * t * (5 - t2 + 9 * ita2 + 4 * ita2 * ita2) * temp2 * temp2 / 24
yPart3 = temp2 * temp2 * temp2 * N * t * (61 - 58 * t2 + t2 * t2) / 720
y = X + yPart1 + yPart2 + yPart3
xPart1 = N * temp
xPart2 = (1 - t2 + ita2) * N * temp * temp2 / 6
xPart3 = (5 - 18 * t2 + t2 * t2 + 14 * ita2 - 58 * ita2 * t2) * N * temp * temp2 * temp2 / 120
x = Zone * ZoneOffset + EastOffset + xPart1 + xPart2 + xPart3
projPt = np.array([x, y])
return projPt
def Inverse_v2(inXY): # XY --> BL
Zone = (int)(inXY[0] / ZoneOffset)
L0 = Zone * ZoneWidth
L0_radian = degree2rad(L0)
X0 = Zone * ZoneOffset + EastOffset
Y0 = 0
x = inXY[0] - X0
y = inXY[1] - Y0
Br = wgs84.GetLatByX(y)#
cosB = math.cos(Br)
secB = 1 / cosB
ita2 = wgs84.ep2 * cosB * cosB
t = math.tan(Br)
t2 = t * t
V = wgs84.GetV(Br)
V2 = V * V
N = wgs84.c / V
M = N/V/V
D = x/N
tmp3 = (1 + 2 * t2 + ita2) * D * D * D / 6
tmp5 = (5 + 28 * t2 + 24 * t2 * t2 + 6 * ita2 + 8 * ita2 * t2) * D * D * D * D * D / 120
l = secB * (D - tmp3 + tmp5)
L_radian = L0_radian + l
tmp2 = D * D / 2
tmp4 = (5 + 3 * t2 + ita2 - 9 * ita2 * t2) * D * D * D * D / 24
tmp6 = (61 + 90 * t2 + 45 * t2 * t2) * D * D * D * D * D * D / 720
B_radian = Br - t * V2 * (tmp2 - tmp4 + tmp6)
B = rad2degree(B_radian)
L = rad2degree(L_radian)
outLonLat = np.array([B, L])
return outLonLat
scripts/gen_all_configs.py 0 → 100755
View file @ ab6f81e3
This diff is collapsed.
scripts/rotations.py 0 → 100755
View file @ ab6f81e3
This diff is collapsed.
scripts/scripts.md 0 → 100644
View file @ ab6f81e3
This directory consists of the generating scripts of the configuration files, models, binary executable files and maps.
scripts/templates/shell/jrun 0 → 100755
View file @ ab6f81e3
This diff is collapsed.
scripts/templates/yamls/entry.yaml 0 → 100644
View file @ ab6f81e3
LIDAR_TYPE: hesai
LIDAR_ROS_YAML: yamls/fix_nodes_yaml/all_lidars.yaml
CAM_TYPE: flir
CAM_ROS_YAML: yamls/fix_nodes_yaml/flir_ros.yaml
LIDAR_MODEL_TYPE: 1
LIDAR_MODEL_YAML: yamls/fix_nodes_yaml/pointpillars.yaml
#LIDAR_MODEL_TYPE: 2
#LIDAR_MODEL_YAML: yamls/fix_nodes_yaml/cp_4stage.yaml
#LIDAR_MODEL_TYPE: 3
#LIDAR_MODEL_YAML: yamls/fix_nodes_yaml/cia_ssd.yaml
CAM_MODEL_TYPE: yolov5
CAM_MODEL_YAML: yamls/fix_nodes_yaml/yolov5.yaml
TRACKING_TYPE: tracking_v1
FUSION_TYPE: lift_2d_to_3d
FUSION_YAML: yamls/fix_nodes_yaml/lift_2d_to_3d.yaml
TRACKING_YAML: yamls/fix_nodes_yaml/tracking_v1.yaml
EVENT_TYPE: Qichecheng
EVENT_YAML: yamls/fix_nodes_yaml/Qichecheng_event.yaml
PUBLISH_TYPE: shanghai
PUBLISH_YAML: yamls/fix_nodes_yaml/all_publish.yaml
NODE_NAME: node17
ALL_NODES_YAML_DIR: yamls/custom_nodes_yaml/
scripts/total_yamls/total_cam_models.yaml 0 → 100644
View file @ ab6f81e3
node1:
yolov5m_enginepath: /models/yolov5/yolov5s_dy_230131.engine
node2:
yolov5m_enginepath: /models/yolov5/yolov5s_dy_230131.engine
scripts/total_yamls/total_cameras.yaml 0 → 100644
View file @ ab6f81e3
node1:
GlobalConfig:
acquisition_frame_rate: 10
utc_leap_nanoseconds: 37000000000
CameraParams:
- idx: 0
url: 10.56.108.124
location_lon: 121.188479
location_lat: 31.275802
location_ele: 0
raw_topic: /camera_array/cam0/image_raw
cam_trans: [[ 1.51042207e-02, -1.50427234e-02, -1.13173363e+01],
[-7.78807026e-04, -1.45158646e-01, 1.13531205e+02],
[ 1.27580184e-05, 2.20449713e-03, -7.25591444e-01]]
cam_intrinsics: [[3.13188623,-1.69726816,3006.77076161],
[0.15893756,0.21158008,290.67111259],
[0.00054013,0.00059379,1. ]]
cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
- idx: 1
url: 10.56.109.127
location_lon: 121.1877493
location_lat: 31.2757621
location_ele: 0
raw_topic: /camera_array/cam1/image_raw
cam_trans: [[ -1.64915971 , -4.53892942 ,-4759.1907378 ],
[ 0.65469646 , -0.308533, 731.68354325],
[ 0.00106312, -0.00075274, 1. ]]
cam_intrinsics: [[ -1.64915971, -4.53892942, -4759.1907378 ],
[ 0.65469646, -0.308533, 731.68354325],
[ 0.00106312, -0.00075274, 1. ]]
cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
- idx: 2
url: 10.56.109.128
location_lon: 121.1876840
location_lat: 31.2756782
location_ele: 0
raw_topic: /camera_array/cam2/image_raw
cam_trans: [[-8.18108420e-04 , 1.09704652e-01, -1.16081430e+02],
[ 1.74596191e-02 , 1.00373102e-02 ,-1.67333506e+01],
[ 6.03293984e-05 , 8.99020631e-04, 2.53539016e-02]]
cam_intrinsics: [[-3.17881853,-4.97343073,-7034.53070758],
[0.23636193,0.14470063,495.80985215],
[0.00090617,-0.00047155,1.]]
cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
LogConfig:
log_name: node1
log_enable: false
log_path: /workspace/log/
log_to_stderr: false
also_log_to_stderr: false
stderr_threshold: 0
log_buf_secs: 0
max_log_size: 100
stop_logging_if_full_disk: true
log_prefix: true
log_clean_days: 0
log_color: true
node2:
GlobalConfig:
acquisition_frame_rate: 10
utc_leap_nanoseconds: 37000000000
CameraParams:
- idx: 0
url: 10.56.100.131
location_lon: 121.160087
location_lat: 31.282610
location_ele: 0
raw_topic: /camera_array/cam0/image_raw
cam_trans: [[1.72715738e-02, 2.95514778e-01, -2.19812153e+02],
[-7.34889367e-03, -3.16259208e-01, 1.38078122e+02],
[ 1.97048960e-04, 2.58610293e-03, -1.32415680e+00]]
cam_intrinsics: [[4.3004e+03, 0.0, 968.6373,],
[0.0, 4.3064e+03, 688.2004,],
[0.0, 0.0, 1.0,]]
cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
- idx: 1
url: 10.56.100.129
location_lon: 121.160464
location_lat: 31.281601
location_ele: 0
raw_topic: /camera_array/cam1/image_raw
cam_trans: [[1.53258448e-02, -9.78250085e-02, 1.58912911e+01],
[3.65285051e-03, 1.73733978e-01, 4.15968464e+00],
[-4.07695972e-05, -2.54866135e-03, 9.52422567e-01]]
cam_intrinsics: [[4.3004e+03, 0.0, 968.6373,],
[0.0, 4.3064e+03, 688.2004,],
[0.0, 0.0, 1.0,]]
cam_dist: [-0.1387, 3.5558, 0.0065, 9.1060e-04, 0.2381]
LogConfig:
log_name: node2
log_enable: false
log_path: /workspace/log/
log_to_stderr: false
also_log_to_stderr: false
stderr_threshold: 0
log_buf_secs: 0
max_log_size: 100
stop_logging_if_full_disk: true
log_prefix: true
log_clean_days: 0
log_color: true
scripts/total_yamls/total_common.yaml 0 → 100644
View file @ ab6f81e3
node1:
MEC_V4_IP: 10.56.109.1
MEC_V6_IP: 2408:860c:5:b01:106:101:a01:1
MEC_NAME: JFX_MEC_1
MEC_LOCATION_LON: 121.188618
MEC_LOCATION_LAT: 31.27368
MEC_LOCATION_ELE: 0
CROSS_NAME: boyuanlu-anhonglu
SCENE_TYPE: 0 # 0表示开放区域(城市道路); 1表示半封闭区域(高速高架,桥梁隧道); 2表示封闭限定区域(园区,机场,停车场); 3为预留
node2:
MEC_V4_IP: 10.56.100.1
MEC_V6_IP: 2408:860c:5:b01:106:101:101:1
MEC_NAME: JFX_MEC_2
MEC_LOCATION_LON: 121.160798
MEC_LOCATION_LAT: 31.282089
MEC_LOCATION_ELE: 0
CROSS_NAME: boyuanlu-moyunanlu
SCENE_TYPE: 0 # 0表示开放区域(城市道路); 1表示半封闭区域(高速高架,桥梁隧道); 2表示封闭限定区域(园区,机场,停车场); 3为预留
scripts/total_yamls/total_event.yaml 0 → 100644
View file @ ab6f81e3
node1:
road_cross_index: 1
node2:
road_cross_index: 2
scripts/total_yamls/total_fusion.yaml 0 → 100644
View file @ ab6f81e3
node1:
match_color: 0
node2:
match_color: 0
scripts/total_yamls/total_lidars.yaml 0 → 100644
View file @ ab6f81e3
node1:
LIDAR_TOPIC_LIST: [L0,L1,L2]
LIDAR_MAIN_NODE_INDEX: 0
LIDAR_JOIN_NODE: 7
IS_PUBLISH_POINT: 0
IS_NORMALIZATION: 0
IS_PACKET_ALL_POINT: 0
IS_PACKET_OBJ_POINT: 0
IS_PUBLISH_MARK: 1
L0: # West
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense0
LIDAR_LON: 121.188468
LIDAR_LAT: 31.275081
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets19
LIDAR_IP: 172.16.20.19
LIDAR_PORT: 26699
LIDAR_INDEX: 0
LIDAR_DELAY: 0
LIDAR_SELF: [[0.896619498730, -0.442748308182, -0.006880199537, -259.632781982422],
[0.442592650652, 0.896565437317, -0.016799459234, -549.550964355469],
[0.013606481254, 0.012017597444, 0.999835252762, 16.488615036011],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
L1: # South
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense1
LIDAR_LON: 121.188479
LIDAR_LAT: 31.275802
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets18
LIDAR_IP: 172.16.20.18
LIDAR_PORT: 16699
LIDAR_INDEX: 1
LIDAR_DELAY: 0
LIDAR_SELF: [[-0.607184648514, -0.788420617580, -0.098588868976, -209.208343505859],
[0.778907299042, -0.615131080151, 0.122137434781, -585.203369140625],
[-0.156940743327, -0.002631605603, 0.987604498863, 17.272777557373],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
L2: # North
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense2
LIDAR_LON: 121.188583
LIDAR_LAT: 31.275165
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets20
LIDAR_IP: 172.16.20.20
LIDAR_PORT: 36699
LIDAR_INDEX: 2
LIDAR_DELAY: 0
LIDAR_SELF: [[0.573620438576, 0.809384703636, 0.125920116901, -274.959991455078],
[-0.789917349815, 0.587276041508, -0.176457703114, -519.970703125000],
[-0.216772064567, 0.001753266319, 0.976220667362, 17.266538619995],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
node2:
LIDAR_TOPIC_LIST: [L0,L1,L2,L3]
LIDAR_MAIN_NODE_INDEX: 0
LIDAR_JOIN_NODE: 15
IS_PUBLISH_POINT: 0
IS_NORMALIZATION: 0
IS_PACKET_ALL_POINT: 0
IS_PACKET_OBJ_POINT: 0
IS_PUBLISH_MARK: 1
L0: # South
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense0
LIDAR_LON: 121.160835
LIDAR_LAT: 31.282184
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets21
LIDAR_IP: 172.16.22.21
LIDAR_PORT: 46699
LIDAR_INDEX: 0
LIDAR_DELAY: 0
LIDAR_SELF: [[0.997278869152, -0.072646379471, -0.012538959272, -3892.811279296875],
[0.072265379131, 0.996976613998, -0.028550757095, 218.303405761719],
[0.014575158246, 0.027566935867, 0.999513566494, 2.135483741760],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
L1: # West
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense1
LIDAR_LON: 121.160820
LIDAR_LAT: 31.282287
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets20
LIDAR_IP: 172.16.22.20
LIDAR_PORT: 36699
LIDAR_INDEX: 1
LIDAR_DELAY: 0
LIDAR_SELF: [[-0.050201263279, -0.048703737557, -0.997550964355, -3888.115966796875],
[0.043598838151, 0.997751116753, -0.050907615572, 199.882598876953],
[0.997787058353, -0.046047694981, -0.047964952886, 4.731142520905],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
L2: # South
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense2
LIDAR_LON: 121.160299
LIDAR_LAT: 31.281535
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets19
LIDAR_IP: 172.16.22.19
LIDAR_PORT: 26699
LIDAR_INDEX: 2
LIDAR_DELAY: 0
LIDAR_SELF: [[-0.063920862973, 0.985661029816, -0.156161770225, -3932.408203125000],
[0.036017764360, 0.158658415079, 0.986676275730, 115.666076660156],
[0.997304737568, 0.057444605976, -0.045642886311, 4.675023555756],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
L3: # East
LIDAR_TYPE: 2
LIDAR_NAME: robsense
FRAME_ID: robsense3
LIDAR_LON: 121.160464
LIDAR_LAT: 31.281601
LIDAR_ELE: 0
LIDAR_TOPIC: /rslidar_packets18
LIDAR_IP: 172.16.22.18
LIDAR_PORT: 16699
LIDAR_INDEX: 3
LIDAR_DELAY: 0
LIDAR_SELF: [[-0.013388182037, 0.976763427258, -0.213901326060, -3942.002685546875],
[0.039395920932, 0.214269667864, 0.975979804993, 114.107917785645],
[0.999133944511, 0.004639756866, -0.041349183768, 4.373412609100],
[0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000]]
scripts/total_yamls/total_pointcloud_detectors.yaml 0 → 100644
View file @ ab6f81e3
node1:
MAX_POINTS_ALLOWED: 400000
MAX_NUM_VOXELS: 150000
MAX_ACTIVE: 400000
MAX_NUM_POINTS_PER_VOXEL: 12
NUM_CLASS: 5
SCORE_THRESHOLD: 0.4
NMS_OVERLAP_THRESHOLD: 0.1
NUM_THREADS: 64
BATCH_SIZE: 1
VOXEL_SIZES: [0.1, 0.1, 0.125] # cp voxel
GRID_SIZES: [3072, 3072, 40] # xyz
PC_RANGE: [-153.6, -153.6,-7,153.6,153.6, -2]
DS_RATIO: 8 # ratio of GRID_SIZES to BEV_FEATURE_SIZE
BEV_FEATURE_SIZE: [384, 384, 256] # x y c
RPN_ONNX_FILE: unify_32_roads_1226.onnx
CUTCONV_WEIGHTS: unify_32_roads_1226.weights
ORIGIN2KITTI: [[0.050645,-0.000283,0.998717,-81.275970,],
[-0.011183,-0.999937,0.000283,7.052648,],
[0.998654,-0.011183,-0.050645,0.800000,],
[0.000000,0.000000,0.000000,1.000000,],
]
KITTI2ORIGIN: [[0.050645,-0.011183,0.998654,3.396167,],
[-0.000284,-0.999938,-0.011183,7.038095,],
[0.998716,0.000284,-0.050645,81.210156,],
[0.000000,0.000000,0.000000,1.000000,],
]
INFER_THRESHOLD:
CLASS_0: 0.4 # truck
CLASS_1: 0.4 # car
CLASS_2: 0.4 # other_v
CLASS_3: 0.4 # cyclist
CLASS_4: 0.4 # pedestrain
node2:
MAX_POINTS_ALLOWED: 400000
MAX_NUM_VOXELS: 150000
MAX_ACTIVE: 400000
MAX_NUM_POINTS_PER_VOXEL: 12
NUM_CLASS: 5
SCORE_THRESHOLD: 0.4
NMS_OVERLAP_THRESHOLD: 0.1
NUM_THREADS: 64
BATCH_SIZE: 1
VOXEL_SIZES: [0.1, 0.1, 0.125] # cp voxel
GRID_SIZES: [3072, 3072, 40] # xyz
PC_RANGE: [-153.6, -153.6,-7,153.6,153.6, -2]
DS_RATIO: 8 # ratio of GRID_SIZES to BEV_FEATURE_SIZE
BEV_FEATURE_SIZE: [384, 384, 256] # x y c
RPN_ONNX_FILE: unify_32_roads_1226.onnx
CUTCONV_WEIGHTS: unify_32_roads_1226.weights
ORIGIN2KITTI: [[0.999907,-0.000186,-0.013612,63.010513,],
[-0.000186,0.999629,-0.027250,12.516913,],
[0.013612,0.027250,0.999536,-1.600000,],
[0.000000,0.000000,0.000000,1.000000,],
]
KITTI2ORIGIN: [[0.999907,-0.000186,0.013612,-62.980574,],
[-0.000186,0.999629,0.027250,-12.456976,],
[-0.013612,-0.027250,0.999536,2.798023,],
[0.000000,0.000000,0.000000,1.000000,],
]
INFER_THRESHOLD:
CLASS_0: 0.4 # truck
CLASS_1: 0.4 # car
CLASS_2: 0.4 # other_v
CLASS_3: 0.4 # cyclist
CLASS_4: 0.4 # pedestrain
scripts/total_yamls/total_track.yaml 0 → 100644
View file @ ab6f81e3
node1:
kf_gpu: 0
node2:
kf_gpu: 0
scripts/wgs84.py 0 → 100755
View file @ ab6f81e3
import math
import numpy as np
from rotations import degree2rad, rad2degree
a = 6378137.0
f = 0.003352810664
b = 6356752.3142499477
c = 6399593.6257536924
e2 = 0.006694379988651241
ep2 = 0.0067394967407662064
m0 = 6335439.3273023246
m2 = 63617.757378010137
m4 = 532.35180239277622
m6 = 4.1577261283373916
m8 = 0.031312573415813519
a0 = 6367449.1457686741
a2 = 32077.017223574985
a4 = 67.330398573595
a6 = 0.13188597734903185
a8 = 0.00024462947981104312
#class Wgs84:
def GetN(radB):
cosB = math.cos(radB)
V = math.sqrt(1 + ep2 * cosB * cosB)
N = c / V
return N
def GetRT_ECEF2ENU(BLH):
BLH0 = np.array([BLH[0], BLH[1], 0.])
translation = BLH2ECEF(BLH0);
B = degree2rad(BLH[0])
L = degree2rad(BLH[1])
sinL = np.sin(L)
cosL = np.cos(L)
sinB = np.sin(B)
cosB = np.cos(B)
rot = np.array([[-sinL, cosL, 0.], [-sinB * cosL, -sinB * sinL, cosB], [cosB * cosL, cosB * sinL, sinB]])
return rot, np.dot(rot, -translation)
def GetX(radB):
sinB = math.sin(radB)
cosB = math.cos(radB)
al = a0 * radB
sc = sinB * cosB
ss = sinB * sinB
X = al - sc *((a2 - a4 + a6) + (2 * a4 - a6 * 16 / 3) * ss + a6 * 16 * ss * ss /3)
return X
def GetLatByX(X):
Bfi0 = X / a0
Bfi1 = 0
num = 1
minAngle = math.pi * 1e-9
menus_minAngle = 0 - minAngle
while (num == 1):
num = 0
sinB = math.sin(Bfi0)
sinB2 = sinB * sinB
cosB = math.cos(Bfi0)
FBfi = 0 - sinB * cosB *((a2 - a4 + a6) + sinB2 * (2 * a4 - 16 * a6 / 3) + sinB2 * sinB2 * a6 * 16 / 3)
Bfi1 = (X - FBfi) / a0
deltaB = Bfi1 - Bfi0
if deltaB < menus_minAngle or deltaB > minAngle:
num = 1
Bfi0 = Bfi1
Bf = Bfi1
return Bf
def GetV(lat):
cosB = math.cos(lat)
V = math.sqrt(1 + ep2 * cosB * cosB)
return V
def BLH2ECEF(ptBLH):
cosB = math.cos(degree2rad(ptBLH[0]))
sinB = math.sin(degree2rad(ptBLH[0]))
cosL = math.cos(degree2rad(ptBLH[1]))
sinL = math.sin(degree2rad(ptBLH[1]))
N = GetN(degree2rad(ptBLH[0]))
X = (N + ptBLH[2]) * cosB * cosL
Y = (N + ptBLH[2]) * cosB * sinL
Z = (N * (1 - e2) + ptBLH[2]) * sinB
ptXYZ = np.array([X, Y, Z])
return ptXYZ
def ECEF2BLH(ptXYZ):
L_radian = math.atan(ptXYZ[1] / ptXYZ[0])
if L_radian < 0:
L_radian += math.pi
L = rad2degree(L_radian)
sqrtXY = math.sqrt(ptXYZ[0] * ptXYZ[0] + ptXYZ[1] * ptXYZ[1])
t0 = ptXYZ[2] / sqrtXY
p = c * e2 / sqrtXY
k = 1+ep2
ti = t0
ti1 = 0
loop = 0
while loop < 10000:
loop += 1
ti1 = t0 + p * ti / math.sqrt( k + ti * ti)
if math.fabs(ti1 - ti) < 1e-12:
break
ti = ti1
B_radian = math.atan(ti1)
N = GetN(B_radian)
H = sqrtXY / math.cos(B_radian) - N
B = rad2degree(B_radian)
geopoint = np.array([B, L, H])
return geopoint
def GetR_ENU2ECEF(radianL, radianB):
sinB = math.sin(radianB)
cosB = math.cos(radianB)
sinL = math.sin(radianL)
cosL = math.cos(radianL)
R4 = np.array([[-sinL, -sinB * cosL, cosB * cosL],
[cosL, -sinB * sinL, cosB * sinL],
[0, cosB, sinB]])
return R4
def GetAngleDif(distance):
angleRafian = distance / a
return rad2degree(angleRafian)
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment