提交坐标数据求解

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/rotations.py

+# Utitlity file with functions for handling rotations
+#
+# Author: Trevor Ablett
+# University of Toronto Institute for Aerospace Studies
+
+import numpy as np
+from numpy import dot, inner, array, linalg
+from numba import jit
+
+def skew_symmetric(v):
+    return np.array(
+        [[0, -v[2], v[1]],
+         [v[2], 0, -v[0]],
+         [-v[1], v[0], 0]]
+    )
+
+def wraps2pi_pi(rad):
+    while rad > np.pi:
+        rad -= 2 * np.pi
+    while rad < - np.pi:
+        rad += 2 * np.pi
+    return rad
+
+class Quaternion():
+    def __init__(self, w=1., x=0., y=0., z=0., axis_angle=None, euler=None):
+        """
+        Allow initialization either with explicit wxyz, axis angle, or euler XYZ (RPY) angles.
+
+        :param w: w (real) of a quaternion.
+        :param x: x (i) of a quaternion.
+        :param y: y (j) of a quaternion.
+        :param z: z (k) of a quaternion.
+        :param axis_angle: Set of three values from axis-angle representation, as list or [3,] or [3,1] np.ndarray.
+                           See C2M5L2 Slide 7 for details.
+        :param euler: Set of three angles from euler XYZ rotating frame angles.
+        """
+        if axis_angle is None and euler is None:
+            self.w = w
+            self.x = x
+            self.y = y
+            self.z = z
+        elif euler is not None and axis_angle is not None:
+            raise AttributeError("Only one of axis_angle and euler can be specified.")
+        elif axis_angle is not None:
+            if not (type(axis_angle) == list or type(axis_angle) == np.ndarray) or len(axis_angle) != 3:
+                raise ValueError("axis_angle must be list or np.ndarray with length 3.")
+            axis_angle = np.array(axis_angle)
+            norm = np.linalg.norm(axis_angle)
+            self.w = np.cos(norm / 2)
+            if norm < 1e-50:  # to avoid instabilities and nans
+                self.x = 0
+                self.y = 0
+                self.z = 0
+            else:
+                imag = axis_angle / norm * np.sin(norm / 2)
+                self.x = imag[0].item()
+                self.y = imag[1].item()
+                self.z = imag[2].item()
+        else:
+            roll = euler[0]
+            pitch = euler[1]
+            yaw = euler[2]
+
+            cy = np.cos(yaw * 0.5)
+            sy = np.sin(yaw * 0.5)
+            cr = np.cos(roll * 0.5)
+            sr = np.sin(roll * 0.5)
+            cp = np.cos(pitch * 0.5)
+            sp = np.sin(pitch * 0.5)
+
+            # static frame
+            self.w = cr * cp * cy + sr * sp * sy
+            self.x = sr * cp * cy - cr * sp * sy
+            self.y = cr * sp * cy + sr * cp * sy
+            self.z = cr * cp * sy - sr * sp * cy
+
+            # rotating frame
+            # self.w = cr * cp * cy - sr * sp * sy
+            # self.x = cr * sp * sy + sr * cp * cy
+            # self.y = cr * sp * cy - sr * cp * sy
+            # self.z = cr * cp * sy + sr * sp * cy
+
+    def __repr__(self):
+        return "Quaternion (wxyz): [%2.5f, %2.5f, %2.5f, %2.5f]" % (self.w, self.x, self.y, self.z)
+
+    def to_mat(self):
+        v = np.array([self.x, self.y, self.z]).reshape(3,1)
+        return (self.w ** 2 - np.dot(v.T, v)) * np.eye(3) + \
+               2 * np.dot(v, v.T) + 2 * self.w * skew_symmetric(v)
+
+    def to_euler(self):
+        """ Return as xyz (roll pitch yaw) Euler angles, with a static frame """
+        roll = np.arctan2(2 * (self.w * self.x + self.y * self.z), 1 - 2 * (self.x**2 + self.y**2))
+        pitch = np.arcsin(2 * (self.w * self.y - self.z * self.x))
+        yaw = np.arctan2(2 * (self.w * self.z + self.x * self.y), 1 - 2 * (self.y**2 + self.z**2))
+        return np.array([roll, pitch, yaw])
+
+    def to_numpy(self):
+        """ Return numpy wxyz representation. """
+        return np.array([self.w, self.x, self.y, self.z])
+
+    def normalize(self):
+        """ Return a normalized version of this quaternion to ensure that it is valid. """
+        norm = np.linalg.norm([self.w, self.x, self.y, self.z])
+        return Quaternion(self.w / norm, self.x / norm, self.y / norm, self.z / norm)
+
+    def to_reverse(self):
+        return Quaternion(self.w, -self.x, -self.y, -self.z)
+
+    def quat_outer_mult(self, q, out='np'):
+        """
+        Give output of multiplying this quaternion by another quaternion.
+        See equation from C2M5L2 Slide 7 for details.
+
+        :param q: The quaternion to multiply by, either a Quaternion or 4x1 ndarray.
+        :param out: Output type, either np or Quaternion.
+        :return: Returns quaternion of desired type
+        """
+
+        v = np.array([self.x, self.y, self.z]).reshape(3, 1)
+        sum_term = np.zeros([4,4])
+        sum_term[0,1:] = -v[:,0]
+        sum_term[1:, 0] = v[:,0]
+        sum_term[1:, 1:] = -skew_symmetric(v)
+        sigma = self.w * np.eye(4) + sum_term
+
+        if type(q).__name__ == "Quaternion":
+            quat_np = np.dot(sigma, q.to_numpy())
+        else:
+            quat_np = np.dot(sigma, q)
+
+        if out == 'np':
+            return quat_np
+        elif out == 'Quaternion':
+            quat_obj = Quaternion(quat_np[0], quat_np[1], quat_np[2], quat_np[3])
+            return quat_obj
+
+
+def euler2quat(eulers):
+  eulers = array(eulers)
+  if len(eulers.shape) > 1:
+    output_shape = (-1,4)
+  else:
+    output_shape = (4,)
+  eulers = np.atleast_2d(eulers)
+  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
+
+  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
+       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
+  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
+
+  quats = array([q0, q1, q2, q3]).T
+  for i in xrange(len(quats)):
+    if quats[i,0] < 0:
+      quats[i] = -quats[i]
+  return quats.reshape(output_shape)
+
+
+def quat2euler(quats):
+  quats = array(quats)
+  if len(quats.shape) > 1:
+    output_shape = (-1,3)
+  else:
+    output_shape = (3,)
+  quats = np.atleast_2d(quats)
+  q0, q1, q2, q3 = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
+
+  gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
+  theta = np.arcsin(2 * (q0 * q2 - q3 * q1))
+  psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
+
+  eulers = array([wraps2pi_pi(gamma), wraps2pi_pi(theta), wraps2pi_pi(psi)]).T
+  return eulers.reshape(output_shape)
+
+
+def quat2rot(quats):
+  quats = array(quats)
+  input_shape = quats.shape
+  quats = np.atleast_2d(quats)
+  Rs = np.zeros((quats.shape[0], 3, 3))
+  q0 = quats[:, 0]
+  q1 = quats[:, 1]
+  q2 = quats[:, 2]
+  q3 = quats[:, 3]
+  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
+  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
+  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
+  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
+  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
+  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
+  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
+  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
+  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
+
+  if len(input_shape) < 2:
+    return Rs[0]
+  else:
+    return Rs
+
+
+def rot2quat(rots):
+  input_shape = rots.shape
+  if len(input_shape) < 3:
+    rots = array([rots])
+  K3 = np.empty((len(rots), 4, 4))
+  K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0
+  K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0
+  K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0
+  K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0
+  K3[:, 1, 0] = K3[:, 0, 1]
+  K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0
+  K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0
+  K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0
+  K3[:, 2, 0] = K3[:, 0, 2]
+  K3[:, 2, 1] = K3[:, 1, 2]
+  K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0
+  K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0
+  K3[:, 3, 0] = K3[:, 0, 3]
+  K3[:, 3, 1] = K3[:, 1, 3]
+  K3[:, 3, 2] = K3[:, 2, 3]
+  K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0
+  q = np.empty((len(rots), 4))
+  for i in xrange(len(rots)):
+    _, eigvecs = linalg.eigh(K3[i].T)
+    eigvecs = eigvecs[:,3:]
+    q[i, 0] = eigvecs[-1]
+    q[i, 1:] = -eigvecs[:-1].flatten()
+    if q[i, 0] < 0:
+      q[i] = -q[i]
+
+  if len(input_shape) < 3:
+    return q[0]
+  else:
+    return q
+
+
+def euler2rot(eulers):
+  return quat2rot(euler2quat(eulers))
+
+
+def rot2euler(rots):
+  return quat2euler(rot2quat(rots))
+
+
+def gpseuler2rot_in_cptframe(gps_euler_degrees):
+  # R2 defines: XYZ right/forward/up -> ENU
+  yaw = degree2rad(gps_euler_degrees[2])
+  pitch = degree2rad(gps_euler_degrees[1])
+  roll = degree2rad(gps_euler_degrees[0])
+
+  sinA = np.sin(yaw);
+  cosA = np.cos(yaw);
+  matA = np.array([[cosA, sinA, 0], [-sinA, cosA, 0], [0, 0, 1]])
+
+  sinP = np.sin(pitch)
+  cosP = np.cos(pitch)
+  matP = np.array([[1, 0, 0], [0, cosP, -sinP], [0, sinP, cosP]])
+
+  sinR = np.sin(roll)
+  cosR = np.cos(roll)
+  matR = np.array([[cosR, 0, sinR], [0, 1, 0], [-sinR, 0, cosR]])
+  return matA.dot(matP.dot(matR))
+
+
+def gpseuler2rot(gps_euler_degrees):
+  # R1 defines: XYZ forward/left/up -> XYZ right/forward/up
+  R1 = np.mat([[0, -1, 0],
+               [1, 0, 0],
+               [0, 0, 1]])
+  R2 = gpseuler2rot_in_cptframe(gps_euler_degrees)
+  # R2 * R1 * XYZ = ENU
+  return R2.dot(R1)
+
+
+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
+
+@jit
+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 quat_product(q, r):
+  t = np.zeros(4)
+  t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3]
+  t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2]
+  t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1]
+  t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0]
+  return 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/rotations.py

+# Utitlity file with functions for handling rotations
+#
+# Author: Trevor Ablett
+# University of Toronto Institute for Aerospace Studies
+
+import numpy as np
+from numpy import dot, inner, array, linalg
+
+def skew_symmetric(v):
+    return np.array(
+        [[0, -v[2], v[1]],
+         [v[2], 0, -v[0]],
+         [-v[1], v[0], 0]]
+    )
+
+def wraps2pi_pi(rad):
+    while rad > np.pi:
+        rad -= 2 * np.pi
+    while rad < - np.pi:
+        rad += 2 * np.pi
+    return rad
+
+class Quaternion():
+    def __init__(self, w=1., x=0., y=0., z=0., axis_angle=None, euler=None):
+        """
+        Allow initialization either with explicit wxyz, axis angle, or euler XYZ (RPY) angles.
+
+        :param w: w (real) of a quaternion.
+        :param x: x (i) of a quaternion.
+        :param y: y (j) of a quaternion.
+        :param z: z (k) of a quaternion.
+        :param axis_angle: Set of three values from axis-angle representation, as list or [3,] or [3,1] np.ndarray.
+                           See C2M5L2 Slide 7 for details.
+        :param euler: Set of three angles from euler XYZ rotating frame angles.
+        """
+        if axis_angle is None and euler is None:
+            self.w = w
+            self.x = x
+            self.y = y
+            self.z = z
+        elif euler is not None and axis_angle is not None:
+            raise AttributeError("Only one of axis_angle and euler can be specified.")
+        elif axis_angle is not None:
+            if not (type(axis_angle) == list or type(axis_angle) == np.ndarray) or len(axis_angle) != 3:
+                raise ValueError("axis_angle must be list or np.ndarray with length 3.")
+            axis_angle = np.array(axis_angle)
+            norm = np.linalg.norm(axis_angle)
+            self.w = np.cos(norm / 2)
+            if norm < 1e-50:  # to avoid instabilities and nans
+                self.x = 0
+                self.y = 0
+                self.z = 0
+            else:
+                imag = axis_angle / norm * np.sin(norm / 2)
+                self.x = imag[0].item()
+                self.y = imag[1].item()
+                self.z = imag[2].item()
+        else:
+            roll = euler[0]
+            pitch = euler[1]
+            yaw = euler[2]
+
+            cy = np.cos(yaw * 0.5)
+            sy = np.sin(yaw * 0.5)
+            cr = np.cos(roll * 0.5)
+            sr = np.sin(roll * 0.5)
+            cp = np.cos(pitch * 0.5)
+            sp = np.sin(pitch * 0.5)
+
+            # static frame
+            self.w = cr * cp * cy + sr * sp * sy
+            self.x = sr * cp * cy - cr * sp * sy
+            self.y = cr * sp * cy + sr * cp * sy
+            self.z = cr * cp * sy - sr * sp * cy
+
+            # rotating frame
+            # self.w = cr * cp * cy - sr * sp * sy
+            # self.x = cr * sp * sy + sr * cp * cy
+            # self.y = cr * sp * cy - sr * cp * sy
+            # self.z = cr * cp * sy + sr * sp * cy
+
+    def __repr__(self):
+        return "Quaternion (wxyz): [%2.5f, %2.5f, %2.5f, %2.5f]" % (self.w, self.x, self.y, self.z)
+
+    def to_mat(self):
+        v = np.array([self.x, self.y, self.z]).reshape(3,1)
+        return (self.w ** 2 - np.dot(v.T, v)) * np.eye(3) + \
+               2 * np.dot(v, v.T) + 2 * self.w * skew_symmetric(v)
+
+    def to_euler(self):
+        """ Return as xyz (roll pitch yaw) Euler angles, with a static frame """
+        roll = np.arctan2(2 * (self.w * self.x + self.y * self.z), 1 - 2 * (self.x**2 + self.y**2))
+        pitch = np.arcsin(2 * (self.w * self.y - self.z * self.x))
+        yaw = np.arctan2(2 * (self.w * self.z + self.x * self.y), 1 - 2 * (self.y**2 + self.z**2))
+        return np.array([roll, pitch, yaw])
+
+    def to_numpy(self):
+        """ Return numpy wxyz representation. """
+        return np.array([self.w, self.x, self.y, self.z])
+
+    def normalize(self):
+        """ Return a normalized version of this quaternion to ensure that it is valid. """
+        norm = np.linalg.norm([self.w, self.x, self.y, self.z])
+        return Quaternion(self.w / norm, self.x / norm, self.y / norm, self.z / norm)
+
+    def to_reverse(self):
+        return Quaternion(self.w, -self.x, -self.y, -self.z)
+
+    def quat_outer_mult(self, q, out='np'):
+        """
+        Give output of multiplying this quaternion by another quaternion.
+        See equation from C2M5L2 Slide 7 for details.
+
+        :param q: The quaternion to multiply by, either a Quaternion or 4x1 ndarray.
+        :param out: Output type, either np or Quaternion.
+        :return: Returns quaternion of desired type
+        """
+
+        v = np.array([self.x, self.y, self.z]).reshape(3, 1)
+        sum_term = np.zeros([4,4])
+        sum_term[0,1:] = -v[:,0]
+        sum_term[1:, 0] = v[:,0]
+        sum_term[1:, 1:] = -skew_symmetric(v)
+        sigma = self.w * np.eye(4) + sum_term
+
+        if type(q).__name__ == "Quaternion":
+            quat_np = np.dot(sigma, q.to_numpy())
+        else:
+            quat_np = np.dot(sigma, q)
+
+        if out == 'np':
+            return quat_np
+        elif out == 'Quaternion':
+            quat_obj = Quaternion(quat_np[0], quat_np[1], quat_np[2], quat_np[3])
+            return quat_obj
+
+
+def euler2quat(eulers):
+  eulers = array(eulers)
+  if len(eulers.shape) > 1:
+    output_shape = (-1,4)
+  else:
+    output_shape = (4,)
+  eulers = np.atleast_2d(eulers)
+  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
+
+  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
+       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
+  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
+
+  quats = array([q0, q1, q2, q3]).T
+  for i in xrange(len(quats)):
+    if quats[i,0] < 0:
+      quats[i] = -quats[i]
+  return quats.reshape(output_shape)
+
+
+def quat2euler(quats):
+  quats = array(quats)
+  if len(quats.shape) > 1:
+    output_shape = (-1,3)
+  else:
+    output_shape = (3,)
+  quats = np.atleast_2d(quats)
+  q0, q1, q2, q3 = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
+
+  gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
+  theta = np.arcsin(2 * (q0 * q2 - q3 * q1))
+  psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
+
+  eulers = array([wraps2pi_pi(gamma), wraps2pi_pi(theta), wraps2pi_pi(psi)]).T
+  return eulers.reshape(output_shape)
+
+
+def quat2rot(quats):
+  quats = array(quats)
+  input_shape = quats.shape
+  quats = np.atleast_2d(quats)
+  Rs = np.zeros((quats.shape[0], 3, 3))
+  q0 = quats[:, 0]
+  q1 = quats[:, 1]
+  q2 = quats[:, 2]
+  q3 = quats[:, 3]
+  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
+  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
+  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
+  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
+  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
+  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
+  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
+  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
+  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
+
+  if len(input_shape) < 2:
+    return Rs[0]
+  else:
+    return Rs
+
+
+def rot2quat(rots):
+  input_shape = rots.shape
+  if len(input_shape) < 3:
+    rots = array([rots])
+  K3 = np.empty((len(rots), 4, 4))
+  K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0
+  K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0
+  K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0
+  K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0
+  K3[:, 1, 0] = K3[:, 0, 1]
+  K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0
+  K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0
+  K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0
+  K3[:, 2, 0] = K3[:, 0, 2]
+  K3[:, 2, 1] = K3[:, 1, 2]
+  K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0
+  K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0
+  K3[:, 3, 0] = K3[:, 0, 3]
+  K3[:, 3, 1] = K3[:, 1, 3]
+  K3[:, 3, 2] = K3[:, 2, 3]
+  K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0
+  q = np.empty((len(rots), 4))
+  for i in xrange(len(rots)):
+    _, eigvecs = linalg.eigh(K3[i].T)
+    eigvecs = eigvecs[:,3:]
+    q[i, 0] = eigvecs[-1]
+    q[i, 1:] = -eigvecs[:-1].flatten()
+    if q[i, 0] < 0:
+      q[i] = -q[i]
+
+  if len(input_shape) < 3:
+    return q[0]
+  else:
+    return q
+
+
+def euler2rot(eulers):
+  return quat2rot(euler2quat(eulers))
+
+
+def rot2euler(rots):
+  return quat2euler(rot2quat(rots))
+
+
+def gpseuler2rot_in_cptframe(gps_euler_degrees):
+  # R2 defines: XYZ right/forward/up -> ENU
+  yaw = degree2rad(gps_euler_degrees[2])
+  pitch = degree2rad(gps_euler_degrees[1])
+  roll = degree2rad(gps_euler_degrees[0])
+
+  sinA = np.sin(yaw);
+  cosA = np.cos(yaw);
+  matA = np.array([[cosA, sinA, 0], [-sinA, cosA, 0], [0, 0, 1]])
+
+  sinP = np.sin(pitch)
+  cosP = np.cos(pitch)
+  matP = np.array([[1, 0, 0], [0, cosP, -sinP], [0, sinP, cosP]])
+
+  sinR = np.sin(roll)
+  cosR = np.cos(roll)
+  matR = np.array([[cosR, 0, sinR], [0, 1, 0], [-sinR, 0, cosR]])
+  return matA.dot(matP.dot(matR))
+
+
+def gpseuler2rot(gps_euler_degrees):
+  # R1 defines: XYZ forward/left/up -> XYZ right/forward/up
+  R1 = np.mat([[0, -1, 0],
+               [1, 0, 0],
+               [0, 0, 1]])
+  R2 = gpseuler2rot_in_cptframe(gps_euler_degrees)
+  # R2 * R1 * XYZ = ENU
+  return R2.dot(R1)
+
+
+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 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 quat_product(q, r):
+  t = np.zeros(4)
+  t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3]
+  t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2]
+  t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1]
+  t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0]
+  return t
+

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