Skip to content

J
jfxmap_python
Commit 940abac7 authored by oscar's avatar oscar
Browse files
Options

提交坐标数据求解

parent 150f3ee8
Hide whitespace changes
Inline Side-by-side
Showing with 1848 additions and 5 deletions
7-1.yaml 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
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
View file @ 940abac7
...@@ -11,6 +11,10 @@ from scipy.spatial.transform import Rotation as R ...@@ -11,6 +11,10 @@ from scipy.spatial.transform import Rotation as R
import cv2 import cv2
from scipy.spatial import Delaunay from scipy.spatial import Delaunay
import scipy import scipy
from utils.getloc_coll import get_loc, get_world_loc
from easydict import EasyDict
import yaml
def compute_pitch(A,B,C): def compute_pitch(A,B,C):
...@@ -408,8 +412,32 @@ def in_hull(p, hull): ...@@ -408,8 +412,32 @@ def in_hull(p, hull):
return flag 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__': 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/" root_path = "D:/work/git_workspace/jfxmap_python/script/data_aug_samples/"
child_dir = "N10_1" child_dir = "N10_1"
#读标注的json文件 #读标注的json文件
...@@ -429,6 +457,7 @@ if __name__ == '__main__': ...@@ -429,6 +457,7 @@ if __name__ == '__main__':
print(fileuri_path," ",fileuri) print(fileuri_path," ",fileuri)
anno[fileuri_path] = an['labels_box3D'] anno[fileuri_path] = an['labels_box3D']
num = 1
for file_path,boxx in anno.items(): for file_path,boxx in anno.items():
# print(file_path) # print(file_path)
# print(boxx) # print(boxx)
...@@ -444,19 +473,29 @@ if __name__ == '__main__': ...@@ -444,19 +473,29 @@ if __name__ == '__main__':
print(converted_pcd) print(converted_pcd)
xyz = np.array(pcd.points) xyz = np.array(pcd.points)
savecloud = o3d.geometry.PointCloud()
drawpointcloud = [] drawpointcloud = []
merge_geos = [pcd] merge_geos = [pcd]
for bbox in bboxes: 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]) 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() point_cloud = o3d.geometry.PointCloud()
color_ = [[0, 1, 1]] color_ = [[0, 1, 1]]
point_cloud.colors = o3d.utility.Vector3dVector(color_) 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 += [point_cloud]
drawpointcloud += [gen_o3d_3dbboxes(bbox[1])] savecloud += point_cloud
drawboxes = gen_o3d_3dbboxes(bbox[1])
drawpointcloud += [drawboxes]
#渲染bbox #渲染bbox
merge_geos += [gen_o3d_3dbboxes(bbox[1])] merge_geos += [drawboxes]
heading_point = (bbox[3][0] + bbox[3][2]) / 2 heading_point = (bbox[3][0] + bbox[3][2]) / 2
heading_pcd = o3d.geometry.PointCloud() heading_pcd = o3d.geometry.PointCloud()
heading_pcd.points = o3d.utility.Vector3dVector(np.array([heading_point])) heading_pcd.points = o3d.utility.Vector3dVector(np.array([heading_point]))
...@@ -464,12 +503,17 @@ if __name__ == '__main__': ...@@ -464,12 +503,17 @@ if __name__ == '__main__':
heading_pcd.colors = o3d.utility.Vector3dVector(colors) heading_pcd.colors = o3d.utility.Vector3dVector(colors)
#渲染车辆形式方向的点 #渲染车辆形式方向的点
merge_geos += [heading_pcd] merge_geos += [heading_pcd]
drawpointcloud += [heading_pcd]
savecloud += heading_pcd
if vis: if vis:
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0]) axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
merge_geos += [axis_pcd] merge_geos += [axis_pcd]
drawpointcloud += [axis_pcd]
#pcd.paint_uniform_color([1.00, 0, 0]) #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(merge_geos, file_path)
custom_draw_geometry_with_key_callback(drawpointcloud, file_path) custom_draw_geometry_with_key_callback(drawpointcloud, file_path)
......
script/utils/compute_yaw.py 0 → 100644
View file @ 940abac7
#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 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
# -*- 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 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
# 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 0 → 100644
View file @ 940abac7
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 0 → 100644
View file @ 940abac7
# 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 0 → 100644
View file @ 940abac7
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)
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment