提交坐标数据求解

7-1.yaml

+CAMERA_LAYOUT: [0,1,2]
+CAMERA_LAYOUT: [0, 1, 2] #-1代表没有此方向相机
+X_ROT_ANGLE: 1 # 0代表x轴不旋转，仅微调；1代表x轴顺时针旋转90度；2代表顺时针旋转180度
+POLICY_MASK: [1, 1, 1]    # 1代表这个方向加远处策略，-1代表不加策略
+CAMERA_SERIAL_NO: [21185406, 21185390, 21177824] #相机序列号，-1代表没有
+DRIVE_DIRECTION: [0, 0, 0] # 0为双向，1为单向朝左，2为单向朝右，-1代表没有
+USE_BLIND: 1 # -1 means no blind area is used
+
+POINTPILLARS:
+    MAX_NUM_PILLARS: 20000
+    MAX_NUM_POINTS_PER_PILLAR: 32
+    NUM_CLASS: 5
+    SCORE_THRESHOLD: 0.3
+    NMS_OVERLAP_THRESHOLD: 0.5
+    PFE_OUT_SIZE: 64
+    NUM_THREADS: 64
+    BATCH_SIZE: 1
+
+    PILLAR_SIZES: [0.16, 0.16, 6]
+    GRID_SIZES: [512, 1024, 1]
+    PC_T_RANGE: [11.0,-7.0,150.0,26.0,14.0,26.0,-20.0,150.0,-150.0]
+    PC_RANGE: [0, -81.92, -7.0, 81.92, 81.92, -1.0]
+
+    PFE_ONNX_FILE: /home/nvidia/catkin_ws/src/jfxrosperceiver/pointpillars/build/models/4.0_N7_2/PillarVFE_JF_ep100_20000_4.0_N7_2.onnx
+    RPN_ONNX_FILE: /home/nvidia/catkin_ws/src/jfxrosperceiver/pointpillars/build/models/4.0_N7_2/RPNWithHead_JF_ep100_4.0_N7_2.onnx
+
+
+    IMAGE_SIZES: [[2048, 2448], 
+                  [2048, 2448],
+                  [2048, 2448]]
+
+    IMG_CONTOURS: [[[1488,305],[2184,305],[2391,545],[930,545]],
+                   [[173,563],[686,565],[1239,800],[397,795]],
+                   [[1672,471],[2339,451],[2016,695],[1158,698]]]
+        
+    PC_POLICY_ROTY: [[3.1252,-0.025],[-1.5707,1.5707],[1.5707,-1.5707]]
+    PC_POLICY_AREA: [[[930,545],[1794,545],[1842,446],[1794,368],[1827,305],[1488,305],
+                      [1794,545],[2391,545],[2184,305],[1827,305],[1794,368],[1842,446]],
+                     [[273,563],[437,565],[676,753],[731,800],[397,795],[374,748],
+                      [437,565],[686,565],[1022,719],[1239,800],[731,800],[676,753]],
+                     [[1672,471],[1947,461],[1758,620],[1665,692],[1158,698],[1332,623],
+                      [1947,461],[2139,451],[2067,599],[2016,695],[1665,692],[1758,620]]]
+
+    ORIGIN2KITTI: [[-1.42763000e-05, -9.99984084e-01,  5.64201068e-03,  0.00000000e+00],
+                   [9.99987195e-01,  1.42763000e-05,  5.06064089e-03,  0.00000000e+00 ],
+                   [-5.06064089e-03,  5.64201068e-03,  9.99971278e-01, -1.00000000e+00],
+                   [0.     ,     0.     ,     0.    ,      1  ]]
+
+    KITTI2ORIGIN:  [[-1.42763000e-05,  9.99987195e-01, -5.06064089e-03, -5.06064089e-03],
+                    [-9.99984083e-01,  1.42763000e-05,  5.64201068e-03,  5.64201068e-03],
+                    [5.64201068e-03,  5.06064089e-03,  9.99971279e-01,  9.99971279e-01],
+                    [0.        ,  0.        ,  0.        ,  1]]
+
+    GROUND_Z_VALUE: [-6.2,-6.4,-6.2]
+
+    MINMAX_DEPTH: [82.0,150.0]
+
+    LIDAR_CAM_RS_0: [[-0.995936, -0.0897805,  0.0070823, -0.0132271],
+                     [-0.0173271, 0.11385,  -0.993347, -0.239199],
+                     [0.0883769,  -0.989433,  -0.114943, 0.131749],
+                     [0,         0,           0,         1]]
+
+    LIDAR_CAM_RS_1: [[-0.155891,  -0.987287,  0.0310081, 0.0516705],
+                     [-0.0836811, -0.0180789,  -0.996329, -0.194909],
+                     [0.984223,  -0.157914, -0.0797989,  -0.0143181],
+                     [0,         0,           0,         1]]
+
+    LIDAR_CAM_RS_2: [[-0.130709,   0.990636, -0.0394256, -0.0604812],
+                     [0.11214, -0.0247386,  -0.993384, -0.199602],
+                     [-0.985058,  -0.134266,  -0.107857, 0.530719],
+                     [0,         0,           0,         1]] 
+
+    CAM_INTRINSICS_0: [[7350., 0., 1.1735000000000000e+03], 
+                       [0., 7.3648000000000002e+03,9.9913459999999998e+02], 
+                       [0., 0., 1.0]]
+
+    CAM_INTRINSICS_1: [[7.2934429305973763e+03, 0., 1.1825517251573970e+03], 
+                       [0.,7.3044951009030074e+03, 9.8152916404361952e+02], 
+                       [0., 0., 1.0]]
+
+    CAM_INTRINSICS_2: [[7.5527649384820270e+03, 0., 1.2284353420623622e+03],
+                       [0.,7.5662248499282468e+03, 1.0190673853747635e+03],
+                       [0., 0., 1.0]]
+
+
+TRACKING:
+    LOG_FOLDER: /home/nvidia/catkin_ws_N7_1/src/jfxrosperceiver/tracking_logs/
+    TIMING: True
+
+    TRANS: [[ 6.61568761e-01, -7.49847829e-01 , 7.42486399e-03 , 4.06130966e+07],
+            [ 7.49874949e-01,  6.61577821e-01 ,-1.51043758e-03 , 3.46271626e+06],
+            [-3.77952680e-03,  6.56697899e-03 , 9.99971211e-01 , 1.87295623e+01],
+            [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00 , 1.00000000e+00]]
+
+    BLIND_IN: [0.6771730200742623, 9.954512691674598, -10.771220036350993, 11.174161744271133]
+    BLIND_OUT: [1.4383846400866778, 7.686821481042271, -15.282747463293354, -9.032991078132698]
+    BLIND_OUT_2: []

script/test.py

+import os
+import open3d as o3d
+import json
+import glob
+import numpy as np
+import math
+import random
+import shutil as sh
+import copy
+from scipy.spatial.transform import Rotation as R
+import cv2
+from scipy.spatial import Delaunay
+import scipy
+
+
+if __name__ == '__main__':
+
+    file_path = "D:/pointcloud1.pcd"
+    pcd = o3d.io.read_point_cloud(file_path)
+    def save_pcd(vis):
+    #io.write_point_cloud(file_path, pcd, write_ascii=True)
+    #print(file_path)
+        return False
+    key_to_callback = {}
+    key_to_callback[ord("S")] = save_pcd
+    o3d.visualization.draw_geometries_with_key_callbacks([pcd], key_to_callback)
\ No newline at end of file

script/trans_data.py

@@ -11,6 +11,10 @@ from scipy.spatial.transform import Rotation as R
 import cv2
 from scipy.spatial import Delaunay
 import scipy
+from utils.getloc_coll import get_loc, get_world_loc
+from easydict import EasyDict
+import yaml
+
 
 
 def compute_pitch(A,B,C):
@@ -408,8 +412,32 @@ def in_hull(p, hull):
 
     return flag
 
+def merge_new_config(config, new_config):
+    for key, val in new_config.items():
+        if not isinstance(val, dict):
+            config[key] = val
+            continue
+        if key not in config:
+            config[key] = EasyDict()
+        merge_new_config(config[key], val)
+    return config
+
+def read_yaml(path):
+    with open(path,  'r',encoding='utf-8') as f:
+        new_config = yaml.load(f, Loader=yaml.FullLoader)
+    config = EasyDict()
+    merge_new_config(config=config, new_config=new_config)
+    return config
+
 if __name__ == '__main__':
 
+    cfg_path = "D:/work/git_workspace/jfxmap_python/7-1.yaml"
+    cfg = read_yaml(cfg_path)
+    Trans=cfg.TRACKING.TRANS
+    Trans = np.array(Trans)
+    kitti2origin=cfg.POINTPILLARS.KITTI2ORIGIN
+    kitti2origin = np.array(kitti2origin)
+
     root_path = "D:/work/git_workspace/jfxmap_python/script/data_aug_samples/"
     child_dir = "N10_1"
     #读标注的json文件
@@ -429,6 +457,7 @@ if __name__ == '__main__':
           print(fileuri_path,"  ",fileuri)
           anno[fileuri_path] = an['labels_box3D']
 
+    num = 1
     for file_path,boxx in anno.items():
       # print(file_path)
       # print(boxx)
@@ -444,19 +473,29 @@ if __name__ == '__main__':
       print(converted_pcd)
 
       xyz = np.array(pcd.points)
-
+      savecloud = o3d.geometry.PointCloud()
       drawpointcloud = []
       merge_geos = [pcd]
       for bbox in bboxes:
+        lidar_loc, out_BL, out_center_BL = get_loc([bbox[4][0], bbox[4][1], bbox[4][2], bbox[6][0], bbox[6][1], bbox[6][2], bbox[2]], 0, Trans, kitti2origin)
+        print(out_center_BL)
+        
         flag = in_hull(xyz,bbox[1])
+        pickcloud = xyz[flag]
+        dz = np.array([0,0,10])
+        addz = np.add(pickcloud,dz)
+        print(pickcloud)
+        print(addz)
         point_cloud = o3d.geometry.PointCloud()
         color_ = [[0, 1, 1]]
         point_cloud.colors = o3d.utility.Vector3dVector(color_)
-        point_cloud.points = o3d.utility.Vector3dVector(xyz[flag])
+        point_cloud.points = o3d.utility.Vector3dVector(addz)
         drawpointcloud += [point_cloud]
-        drawpointcloud += [gen_o3d_3dbboxes(bbox[1])]
+        savecloud += point_cloud
+        drawboxes = gen_o3d_3dbboxes(bbox[1])
+        drawpointcloud += [drawboxes]
         #渲染bbox
-        merge_geos += [gen_o3d_3dbboxes(bbox[1])]
+        merge_geos += [drawboxes]
         heading_point = (bbox[3][0] + bbox[3][2]) / 2
         heading_pcd = o3d.geometry.PointCloud()
         heading_pcd.points = o3d.utility.Vector3dVector(np.array([heading_point]))
@@ -464,12 +503,17 @@ if __name__ == '__main__':
         heading_pcd.colors = o3d.utility.Vector3dVector(colors)
         #渲染车辆形式方向的点
         merge_geos += [heading_pcd]
+        drawpointcloud += [heading_pcd]
+        savecloud += heading_pcd
       
       if vis:
         axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
         merge_geos += [axis_pcd]
+        drawpointcloud += [axis_pcd]
         #pcd.paint_uniform_color([1.00, 0, 0])
-        
+        savefile = "D:/pointcloud" + str(num) + ".pcd"
+        o3d.io.write_point_cloud(savefile,savecloud)
+        num += 1
         # custom_draw_geometry_with_key_callback(merge_geos, file_path)
         custom_draw_geometry_with_key_callback(drawpointcloud, file_path)
         

script/utils/compute_yaw.py

+#coding:utf-8
+
+import sys
+import math
+#sys.path.append("./venv/Lib")
+import gaussian
+import numpy as np
+
+def wgs84_angle_cal(lat1,lon1,lat2,lon2):
+	p1 = np.array([lat1, lon1])
+	p11 = gaussian.Convert_v2(p1)
+	#print(gaussian.Convert_v2(p1))
+
+	p2 = np.array([lat2, lon2])
+	p22 = gaussian.Convert_v2(p2)
+	#print(gaussian.Convert_v2(p2))
+
+	x = abs(p22[0] - p11[0])
+	y = abs(p22[1] - p11[1])
+
+	#print(math.sqrt(x * x + y * y))
+	tmp_angle = math.atan(y / x) * 57.29578
+
+	#print(tmp_angle)
+
+	if p22[0] > p11[0] and p22[1] > p11[1]:
+		angle = 90 - tmp_angle
+	elif p22[0] > p11[0] and p22[1] < p11[1]:
+		angle = 90 + tmp_angle
+	elif p22[0] < p11[0] and p22[1] > p11[1]:
+		angle = 270 + tmp_angle
+	elif p22[0] < p11[0] and p22[1] < p11[1]:
+		angle = 270 - tmp_angle
+	elif p22[0] == p11[0] and p22[1] < p11[1]:
+		angle = 180
+	elif p22[0] < p11[0] and p22[1] == p11[1]:
+		angle = 270
+	elif p22[0] > p11[0] and p22[1] == p11[1]:
+		angle = 90
+	else:
+		angle = 0
+	return angle
+
+def xy_angle_cal(x1,y1,x2,y2):
+
+	x = abs(x2 - x1)
+	y = abs(y2 - y1)
+
+	#print(math.sqrt(x * x + y * y))
+	tmp_angle = math.atan(y / x) * 57.29578
+
+	#print(tmp_angle)
+
+	if x2 > x1 and y2 > y1:
+		angle = 90 - tmp_angle
+	elif x2 > x1 and y2 < y1:
+		angle = 90 + tmp_angle
+	elif x2 < x1 and y2 > y1:
+		angle = 270 + tmp_angle
+	elif x2 < x1 and y2 < y1:
+		angle = 270 - tmp_angle
+	elif x2 == x1 and y2 < y1:
+		angle = 180
+	elif x2 < x1 and y2 == y1:
+		angle = 270
+	elif x2 > x1 and y2 == y1:
+		angle = 90
+	else:
+		angle = 0
+	return angle
+
+def car_yaw_cal(base_angle,car_yaw):
+    tmp_angle = 0.0
+    car_angle = car_yaw / math.pi * 180.0
+    if car_angle >= 0:
+	tmp_angle = base_angle - car_angle
+    else:
+	tmp_angle = base_angle + abs(car_angle)
+
+    return (tmp_angle+360.0)%360.0
+
+def compute_yaw(car_yaw_in_lidar):
+    '''
+  lat1 = 31.2808039
+  lon1 = 121.1876594
+  p1 = np.array([lat1, lon1])
+  p11 = gaussian.Convert_v2(p1)
+
+  lidar_x = p11[0]
+  lidar_y = p11[1]
+
+  R = [[6.61568761e-01, -7.49847829e-01,  7.42486399e-03], 
+       [7.49874949e-01,  6.61577821e-01, -1.51043758e-03],
+	[-3.77952680e-03,  6.56697899e-03,  9.99971211e-01]]
+
+    #取x轴两个点坐标用来标定lidar的x轴的绝对航向
+  tmp_loc1 = [30,0,0]
+  tmp_loc2 = [50,0,0]
+
+  tmp_xyz1 = np.dot(R,tmp_loc1)
+  tmp_xyz2 = np.dot(R, tmp_loc2)
+    '''
+    #计算lidar x轴的绝对航向
+    #lidar_x_angle = (xy_angle_cal(tmp_xyz1[0],tmp_xyz1[1],tmp_xyz2[0],tmp_xyz2[1]) + 90.0)%360.0
+    lidar_x_angle = 268.35201485622395
+
+    #print("ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ the lidar_x_angle is:",lidar_x_angle)
+    #将局部坐标系的航向，转换成绝对航向
+    yaw = car_yaw_cal(lidar_x_angle,car_yaw_in_lidar)
+    #print("the car yaw is:",yaw)
+    return yaw
+
+if __name__ == "__main__":
+    lat1 = 31.2840452
+    lon1 = 121.1597165
+    p1 = np.array([lat1, lon1])
+    p11 = gaussian.Convert_v2(p1)
+
+    lidar_x = p11[0]
+    lidar_y = p11[1]
+
+    R = [[-0.330806583166,  -0.941249847412,  0.067936882377], 
+       [0.943696200848,  -0.329787790775,  0.026026830077],
+	[-0.002092991956,  0.072721630335,  0.997349977493]]
+
+    #取x轴两个点坐标用来标定lidar的x轴的绝对航向
+    tmp_loc1 = [30,0,0]
+    tmp_loc2 = [50,0,0]
+
+    tmp_xyz1 = np.dot(R,tmp_loc1)
+    tmp_xyz2 = np.dot(R, tmp_loc2)
+    #计算lidar x轴的绝对航向
+    lidar_x_angle = (xy_angle_cal(tmp_xyz1[0],tmp_xyz1[1],tmp_xyz2[0],tmp_xyz2[1]) + 90.0)%360.0
+    print(lidar_x_angle)

script/utils/gaussian.py

+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
+

script/utils/getloc_coll.py

+import numpy as np
+import math
+import cv2
+
+
+ZoneOffset = 1e6
+ZoneWidth = 3
+EastOffset = 500000.0
+wgs84_ep2 = 0.0067394967407662064
+wgs84_c = 6399593.6257536924
+
+a0 = 6367449.1457686741
+a2 = 32077.017223574985
+a4 = 67.330398573595
+a6 = 0.13188597734903185
+
+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 + wgs84_ep2 * cosB * cosB)
+    return V
+
+def degree2rad(angle=None, list=None):
+  if not (list is None):
+    newlist = np.zeros(len(list))
+    i = 0
+    for angle in list:
+      newlist[i] = angle * np.pi / 180.0
+      i += 1
+    return newlist
+  else:
+    return angle * np.pi / 180.0
+
+def rad2degree(rad=None, list=None):
+  if not (list is None):
+    newlist = np.zeros(len(list))
+    i = 0
+    for rad in list:
+      newlist[i] = rad * 180.0 / np.pi
+      i += 1
+    return newlist
+  else:
+    return rad * 180.0 / np.pi
+
+
+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 = GetLatByX(y)#
+
+
+    cosB = math.cos(Br)
+    secB = 1 / cosB
+    ita2 = wgs84_ep2 * cosB * cosB
+    t = math.tan(Br)
+    t2 = t * t
+    V = 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
+
+def rotz(t):
+  """Rotation about the z-axis."""
+  c = np.cos(t)
+  s = np.sin(t)
+  return np.array([[c, -s,  0],
+                    [s,  c,  0],
+                    [0,  0,  1]])
+
+def my_compute_box_3d_jit(corners_3d, center, heading, size, kitti2origin):
+  # P' = T * R * S * p
+
+  '''
+  x_corners = np.array([-1,1,1,-1,-1,1,1,-1])
+  y_corners = np.array([1,1,-1,-1,1,1,-1,-1])
+  z_corners = np.array([1,1,1,1,-1,-1,-1,-1])
+
+  tmp = np.vstack((x_corners, y_corners, z_corners)) / 2.0
+  corners_3d = np.ones([4,8])
+  corners_3d[:3,:] = tmp
+  '''
+
+  S = np.diag([size[0],size[1],size[2],1])
+  rot = rotz(heading)
+  Trans = np.zeros([4,4])
+  Trans[:3,:3] = rot
+  Trans[0,3] = center[0]
+  Trans[1,3] = center[1]
+  Trans[2,3] = center[2]
+
+  tmp4x4 = np.dot(S,corners_3d)
+  world_corners_3d = np.dot(Trans,tmp4x4)
+
+  '''
+  kitti2origin = [[-1.42763000e-05,  9.99987195e-01, -5.06064089e-03, -5.06064089e-03],
+                  [-9.99984083e-01,  1.42763000e-05,  5.64201068e-03,  5.64201068e-03],
+                  [5.64201068e-03,  5.06064089e-03,  9.99971279e-01,  9.99971279e-01],
+                  [ 0        ,  0  ,        0  ,        1        ]]
+  '''
+
+  world_corners_3d = np.dot(kitti2origin,world_corners_3d)
+
+  return np.transpose(world_corners_3d[:3,:])
+
+
+
+
+def get_loc(boxes_3d, car_type, Trans, kitti2origin):
+  '''
+  Trans = np.array([[ 6.61568761e-01, -7.49847829e-01,  7.42486399e-03,  4.06130966e+07],
+                    [ 7.49874949e-01,  6.61577821e-01, -1.51043758e-03,  3.46271626e+06],
+                    [-3.77952680e-03,  6.56697899e-03,  9.99971211e-01,  1.87295623e+01],
+                    [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])
+  '''
+
+  corners_3d_in = np.array(
+          [[ -0.5 , 0.5 , 0.5 , -0.5 , -0.5 , 0.5 , 0.5 , -0.5 , ],
+            [ 0.5 , 0.5 , -0.5 , -0.5 , 0.5 , 0.5 , -0.5 , -0.5 , ],
+            [ 0.5 , 0.5 , 0.5 , 0.5 , -0.5 , -0.5 , -0.5 , -0.5 , ],
+            [ 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , ]]
+          )
+  #corners_3d = my_compute_box_3d(boxes_3d[:3],boxes_3d[6],boxes_3d[3:6])
+  corners_3d = my_compute_box_3d_jit(corners_3d_in, boxes_3d[:3],boxes_3d[6],boxes_3d[3:6], kitti2origin)
+
+  #adjust bbox
+  #new_corners_3d = adjust_bbox(boxes_3d,corners_3d)
+
+  head_pnt = (corners_3d[1] + corners_3d[2]) / 2.0
+  tail_pnt = (corners_3d[0] + corners_3d[3]) / 2.0
+  inter_len = 0.413
+  total_len = 4.6
+  '''
+  if car_type == 3:
+    inter_len = 1.35
+    total_len = 12.42
+  if car_type == 5:
+    inter_len = 2.25
+    total_len = 4.5
+  '''
+  lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0*total_len) + head_pnt
+
+  tmp_loc = np.ones([4])
+  tmp_loc[:3] = lidar_loc[:3]
+  world_loc = np.dot(Trans,tmp_loc)
+
+  out_BL = Inverse_v2(world_loc[:2])
+
+  #center point transfer
+  tmp_center_loc = np.ones([4])
+  tmp_center_loc[:3] = (corners_3d[0] + corners_3d[6]) / 2.0
+  world_center_loc = np.dot(Trans,tmp_center_loc)
+
+  out_center_BL = Inverse_v2(world_center_loc[:2])
+
+  return lidar_loc, out_BL, out_center_BL
+
+
+def get_world_loc(boxes_3d, Trans, kitti2origin):
+  '''
+  Trans = np.array([[ 6.61568761e-01, -7.49847829e-01,  7.42486399e-03,  4.06130966e+07],
+                    [ 7.49874949e-01,  6.61577821e-01, -1.51043758e-03,  3.46271626e+06],
+                    [-3.77952680e-03,  6.56697899e-03,  9.99971211e-01,  1.87295623e+01],
+                    [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])
+  '''
+
+  corners_3d_in = np.array(
+          [[ -0.5 , 0.5 , 0.5 , -0.5 , -0.5 , 0.5 , 0.5 , -0.5 , ],
+            [ 0.5 , 0.5 , -0.5 , -0.5 , 0.5 , 0.5 , -0.5 , -0.5 , ],
+            [ 0.5 , 0.5 , 0.5 , 0.5 , -0.5 , -0.5 , -0.5 , -0.5 , ],
+            [ 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , ]]
+          )
+  #corners_3d = my_compute_box_3d(boxes_3d[:3],boxes_3d[6],boxes_3d[3:6])
+  corners_3d = my_compute_box_3d_jit(corners_3d_in, boxes_3d[:3],boxes_3d[6],boxes_3d[3:6], kitti2origin)
+
+  #center point transfer
+  center_loc = np.ones([4])
+  center_loc[:3] = (corners_3d[0] + corners_3d[6]) / 2.0
+  world_center_loc = np.dot(Trans, center_loc)
+
+  return world_center_loc[:2]
+
+
+def get_camera_world_loc(u, v, cam_trans, cam_intrinsics, cam_dist):
+
+    #undistort (u, v)
+    pts = np.array([u, v])
+    undistort_pts = cv2.undistortPoints(pts, cam_intrinsics, cam_dist, None, cam_intrinsics)
+    u, v =  undistort_pts[0][0][0], undistort_pts[0][0][1]
+    
+    #(u, v) to (y, x)
+    loc_camera = np.array([u, v, 1])
+    xy = np.dot(cam_trans, loc_camera.T)
+    xy = xy / xy[2]
+
+    rot_xy = [[math.cos(math.pi / 90), math.sin(math.pi / 90)], [-math.sin(math.pi / 90), math.cos(math.pi / 90)]]
+    xy = np.dot(rot_xy, xy[:2])
+
+    return xy[1], -xy[0] - 4.5
+

script/utils/http_utils.py

+# -*- coding:utf-8 -*-
+import hashlib
+import json
+import traceback
+import logging
+import threading
+import requests
+
+
+def upload_thread(msgs, ipc_code, url):
+    threading.Thread(target=upload_data, args=[msg, ipc_code, url]).start()
+
+def upload_data(msgs, ipc_code, url):
+    request_data = {
+        "devNo": ipc_code,
+        "mecNo": ipc_code,
+        "objs": []
+    }
+    for msg in msgs:
+        request_data["objs"].append({
+	    "heading": msg.rot_y,
+	    "id": msg.obj_id,
+	    "lat": msg.Lat,
+	    "lon": msg.Long,
+	    "name": msg.name,
+	    "speed": node["speed"],
+	    "time": get_current_millisecond(),
+	    "type": node["obj_type"]
+        })
+    request_json_pure(url, request_data, http_timeout=1)
+
+
+def request_json_pure(http_url, data, http_timeout=10):
+    """
+    http json 协议传输 无校验
+
+    :param http_url:
+    :param data:
+    :param http_timeout:
+    :return:
+    """
+    header_data = {
+        "Content-Type": "application/json",
+        'Connection': 'keep-alive'
+    }
+    data_json = json.dumps(data, ensure_ascii=False, separators=(',', ':'))
+    print(data_json)
+    s = requests.session()
+    response = s.post(http_url, data=data_json, headers=header_data, timeout=http_timeout)
+    result = response.text
+    print(result)
+    response.close()
+    result = json.loads(result)
+    return result

script/utils/not_using/demo_ww.py

+import numpy as np
+from gaussian import Convert_v2, Inverse_v2
+from numba import jit
+
+
+
+def my_compute_box_3d(center, heading, size):
+  # P' = T * R * S * p
+  x_corners = [-1,1,1,-1,-1,1,1,-1]
+  y_corners = [1,1,-1,-1,1,1,-1,-1]
+  z_corners = [1,1,1,1,-1,-1,-1,-1]
+
+  tmp = np.vstack([x_corners, y_corners, z_corners]) / 2.0
+  corners_3d = np.ones([4,8])
+  corners_3d[:3,:] = tmp
+
+  S = np.diag([size[0],size[1],size[2],1])
+  rot = rotz(heading)
+  Trans = np.zeros([4,4])
+  Trans[:3,:3] = rot
+  Trans[0,3] = center[0]
+  Trans[1,3] = center[1]
+  Trans[2,3] = center[2]
+  #Trans[2,3] = center[2] - size[2]/2
+
+  tmp4x4 = np.dot(S,corners_3d)
+  world_corners_3d = np.dot(Trans,tmp4x4)
+  
+  """
+  PREVIOUS:
+  kitti2origin = [[-0.001081   , 0.99956106, -0.02960616 ,-0.02960616],
+                  [-0.99733779 , 0.001081   , 0.07291202  ,0.07291202],
+                  [ 0.07291202 , 0.02960616 , 0.99689885 , 0.99689885],
+                  [ 0.         , 0.         , 0.         , 1.        ]]
+
+  """                  
+  kitti2origin = [[-0.0821106 ,  0.99618338 ,-0.02960616, -0.02960616],
+                 [-0.9941427  ,-0.07977548,  0.07291202 , 0.07291202],
+                 [ 0.0702719  , 0.0354196 ,  0.99689885,  0.99689885],
+                 [ 0        ,  0  ,        0  ,        1        ]]
+      
+        
+  kitti2origin = np.array(kitti2origin)
+
+  world_corners_3d = np.dot(kitti2origin,world_corners_3d)
+
+  return np.transpose(world_corners_3d[:3,:])
+
+
+@jit
+def my_compute_box_3d_jit(corners_3d, center, heading, size):
+  # P' = T * R * S * p
+
+  '''
+  x_corners = np.array([-1,1,1,-1,-1,1,1,-1])
+  y_corners = np.array([1,1,-1,-1,1,1,-1,-1])
+  z_corners = np.array([1,1,1,1,-1,-1,-1,-1])
+
+  tmp = np.vstack((x_corners, y_corners, z_corners)) / 2.0
+  corners_3d = np.ones([4,8])
+  corners_3d[:3,:] = tmp
+  '''
+
+  S = np.diag([size[0],size[1],size[2],1])
+  rot = rotz(heading)
+  Trans = np.zeros([4,4])
+  Trans[:3,:3] = rot
+  Trans[0,3] = center[0]
+  Trans[1,3] = center[1]
+  Trans[2,3] = center[2]
+
+  tmp4x4 = np.dot(S,corners_3d)
+  world_corners_3d = np.dot(Trans,tmp4x4)
+
+  kitti2origin = np.array([[-0.0821106 ,  0.99618338 ,-0.02960616, -0.02960616],
+                 [-0.9941427  ,-0.07977548,  0.07291202 , 0.07291202],
+                 [ 0.0702719  , 0.0354196 ,  0.99689885,  0.99689885],
+                 [ 0        ,  0  ,        0  ,        1        ]])
+
+
+  world_corners_3d = np.dot(kitti2origin,world_corners_3d)
+
+  return np.transpose(world_corners_3d[:3,:])
+
+
+@jit
+def rotz(t):
+  """Rotation about the z-axis."""
+  c = np.cos(t)
+  s = np.sin(t)
+  return np.array([[c, -s,  0],
+                    [s,  c,  0],
+                    [0,  0,  1]])
+
+
+def get_T():
+  h = 19.34
+  rot = [[-0.330806583166, -0.941249847412, 0.067936882377],
+         [0.943696200848, -0.329787790775, 0.026026830077],
+         [-0.002092991956, 0.072721630335 ,0.997349977493]]
+  rot = np.array(rot)
+  BL = [31.2840452,121.1597165]
+  XY = Convert_v2(BL) 
+  # print(XY)
+  Trans = np.zeros([4,4])
+  Trans[:3,:3] = rot
+  Trans[0,3] = XY[0]
+  Trans[1,3] = XY[1]
+  Trans[2,3] = h
+  Trans[3,3] = 1
+
+  return Trans
+
+
+def get_loc(boxes_3d, car_type):
+  Trans = np.array([[-3.30806583e-01, -9.41249847e-01 , 6.79368824e-02 , 4.06104317e+07],
+                     [ 9.43696201e-01, -3.29787791e-01 , 2.60268301e-02 , 3.46304736e+06],
+                     [-2.09299196e-03,  7.27216303e-02 , 9.97349977e-01 , 1.93400000e+01],
+                     [ 0.00000000e+00,  0.00000000e+00 , 0.00000000e+00 , 1.00000000e+00]])
+    
+
+
+  corners_3d_in = np.array(
+          [[ -0.5 , 0.5 , 0.5 , -0.5 , -0.5 , 0.5 , 0.5 , -0.5 , ],
+            [ 0.5 , 0.5 , -0.5 , -0.5 , 0.5 , 0.5 , -0.5 , -0.5 , ],
+            [ 0.5 , 0.5 , 0.5 , 0.5 , -0.5 , -0.5 , -0.5 , -0.5 , ],
+            [ 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , ]]
+          )
+  #corners_3d = my_compute_box_3d(boxes_3d[:3],boxes_3d[6],boxes_3d[3:6])
+  corners_3d = my_compute_box_3d_jit(corners_3d_in, boxes_3d[:3],boxes_3d[6],boxes_3d[3:6])
+
+  #adjust bbox
+  #new_corners_3d = adjust_bbox(boxes_3d,corners_3d)
+
+  head_pnt = (corners_3d[1] + corners_3d[2]) / 2.0
+  tail_pnt = (corners_3d[0] + corners_3d[3]) / 2.0
+  inter_len = 1.5
+  total_len = 6.0
+  if car_type == 3:
+    inter_len = 1.35
+    total_len = 12.42
+  if car_type == 5:
+    inter_len = 2.25
+    total_len = 4.5
+  if car_type == 1:
+    inter_len = 2.3
+    total_len = 4.6
+  lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0*total_len) + head_pnt
+  #print(head_pnt, lidar_loc, tail_pnt)
+
+  #Trans = get_T()
+  tmp_loc = np.ones([4])
+  tmp_loc[:3] = lidar_loc[:3]
+  world_loc = np.dot(Trans,tmp_loc)
+  #print(world_loc)
+
+  out_BL = Inverse_v2(world_loc[:2])
+
+  #center point transfer
+#  Trans = get_T()
+  tmp_center_loc = np.ones([4])
+  tmp_center_loc[:3] = boxes_3d[:3]
+  world_center_loc = np.dot(Trans,tmp_center_loc)
+
+  out_center_BL = Inverse_v2(world_center_loc[:2])
+  
+  return lidar_loc, out_BL, out_center_BL

script/utils/not_using/gaussian.py

+import math
+import numpy as np
+import wgs84
+from rotations import degree2rad, rad2degree
+from numba import jit
+
+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
+
+@jit
+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
+

script/utils/not_using/loc_compute.py

+import numpy as np
+from gaussian import Convert_v2, Inverse_v2
+def get_T():
+  h = 19.34
+  rot = [[-0.330806583166, -0.941249847412, 0.067936882377],
+         [0.943696200848, -0.329787790775, 0.026026830077],
+         [-0.002092991956, 0.072721630335 ,0.997349977493]]
+  rot = np.array(rot)
+  BL = [31.2840452,121.1597165]
+  XY = Convert_v2(BL)
+  # print(XY)
+  Trans = np.zeros([4,4])
+  Trans[:3,:3] = rot
+  Trans[0,3] = XY[0]
+  Trans[1,3] = XY[1]
+  Trans[2,3] = h
+  Trans[3,3] = 1
+
+  return Trans
+
+
+print(get_T())

script/utils/not_using/wgs84.py

+import math
+import numpy as np
+from rotations import degree2rad, rad2degree
+from numba import jit
+
+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 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
+
+@jit
+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
+
+@jit
+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)
+

script/utils/wgs84.py

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