Skip to content

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

提交坐标数据求解

parent 150f3ee8
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 1228 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
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 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
This diff is collapsed.
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
This diff is collapsed.
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