Skip to content

J
jfxmap_python
Commit 11e6dcab authored by oscar's avatar oscar
Browse files
Options

提交多线程代码

parent 23ea9874
Hide whitespace changes
Inline Side-by-side
Showing with 851 additions and 0 deletions
script/trans_and_export_data_multi.py 0 → 100644
View file @ 11e6dcab
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
from utils.getloc_coll import get_loc, get_world_loc,get_loc_from_origin
from easydict import EasyDict
import yaml
from jfxmap_lib.jfxmap import init_jfxmap, get_map_data
from utils.compute_yaw import compute_yaw,car_yaw_cal
from seg_ww_ground import ground_segmentation
import threading
def compute_pitch(A,B,C):
origin_direction = [0,0,1]
nor = [A,B,C]
angle = np.arccos(np.dot(origin_direction,nor) / np.linalg.norm(nor))
print(angle,angle* 180/ np.pi)
return angle
#绕z轴旋转：欧拉角到旋转矩阵
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 compute_rot(A,B,C):
origin_direction = np.array([0,0,1])
nor = np.array([A,B,C])
mod = np.linalg.norm(nor)
nor = nor / mod
angle = np.arccos(np.dot(nor,origin_direction))
print(angle,angle* 180/ np.pi)
p_rot = np.cross(nor,origin_direction)
p_rot = p_rot / np.linalg.norm(p_rot)
sin_ang = np.sin(angle)
cos_ang = np.cos(angle)
rotMat = np.zeros([4,4])
rotMat[0,0] = cos_ang + p_rot[0] * p_rot[0] * (1 - cos_ang)
rotMat[0,1] = p_rot[0] * p_rot[1] * ( 1 - cos_ang - p_rot[2] * sin_ang)
rotMat[0,2] = p_rot[1] * sin_ang + p_rot[0] * p_rot[2] * (1-cos_ang)
rotMat[1,0] = p_rot[2] * sin_ang + p_rot[0] * p_rot[1] * (1 - cos_ang)
rotMat[1,1] = cos_ang + p_rot[1] * p_rot[1] * (1 - cos_ang)
rotMat[1,2] = -p_rot[0] * sin_ang + p_rot[1] * p_rot[2] * ( 1 - cos_ang)
rotMat[2,0] = -p_rot[1] * sin_ang + p_rot[0] * p_rot[2] * (1 - cos_ang)
rotMat[2,1] = p_rot[0] * sin_ang + p_rot[1] * p_rot[2] * ( 1 - cos_ang)
rotMat[2,2] = cos_ang + p_rot[2] * p_rot[2] * (1 - cos_ang)
rotMat[3,3] = 1
return angle,rotMat
rotMat = {}
def gen_rotMat():
global rotMat
#生成N7_1这段数据的全局转换矩阵
A,B,C,D = 5.10151553e-04,1.30740918e-03,1.89664435e-01,9.81847968e-01
#A,B,C,D = -9.39865821e-04, 1.04783823e-03, 1.85715376e-01, 9.82602574e-01
ang,hori_rotMat = compute_rot(A,B,C)
print(ang,hori_rotMat)
#ext_theta = 0.5 * np.pi
ext_theta = 90 / 180.0 * np.pi
kitti_Rot = np.zeros([4,4])
kitti_Rot[:3,:3] = rotz(ext_theta)
kitti_Rot[2,3] = -1
kitti_Rot[3,3] = 1
print(kitti_Rot)
tmp = np.dot(kitti_Rot,hori_rotMat)
rotMat['N7_1'] = tmp
print(tmp)
inverse_tmp = np.linalg.inv(tmp)
print(inverse_tmp)
N2_1_mat = np.array([[0.05045908306549516, -0.997741728436212, 0.04433197799906369, 0.0],
[0.9959209468126726, 0.05359234819149573, 0.07259013648610471, 0.0], [-0.0748020630460102,
0.04048831377621809, 0.9963761076077744, 0.2859260540868016], [0.0, 0.0, 0.0,
1.0]])
rotMat['N2_1'] = N2_1_mat
N2_2_mat = np.array([[0.07510626206747571, -0.9946697513726462, 0.07064796601834904, 0.0],
[0.9968676257061473, 0.07313389683723709, -0.03010597868179836, 0.0], [0.024778745271707994,
0.07268781767035316, 0.997046887034447, -0.7000000000000002], [0.0, 0.0, 0.0,
1.0]])
rotMat['N2_2'] = N2_2_mat
N2_3_mat = np.array([[0.07568437565247627, -0.9968308870840116, -0.02449607800031248,
0.0], [0.9971317454228593, 0.07565174868629727, 0.002257252902847979, 0.0],
[-0.0003969282768393359, -0.024596655789107878, 0.9996973776958381, 0.9000000000000004],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N2_3'] = N2_3_mat
N2_4_mat = np.array([[0.07516802438907474, -0.996779910756179, 0.0279209172905782, 0.0],
[0.9961881280280445, 0.07630731297561882, 0.042265914901034, 0.0], [-0.044260385057339445,
0.024637441006419476, 0.9987161833149752, 0.09999999999999964], [0.0, 0.0, 0.0,
1.0]])
rotMat['N2_4'] = N2_4_mat
N3_1_mat = np.array([[0.10282893175566048, -0.9927983688656276, 0.06146226136206685, 0.0],
[0.9946815801416514, 0.10226437626202323, -0.01226994191701453, 0.0], [0.0058961784994428,
0.06239708427073043, 0.9980339868729993, 0.7844127162859094], [0.0, 0.0, 0.0,
1.0]])
rotMat['N3_1'] = N3_1_mat
N3_2_mat = np.array([[0.9995180204153945, 0.028485544660163925, -0.012341013341895022,
10.0], [-0.02812481342444661, 0.9991986491976914, 0.02847901528053492, 0.0],
[0.013142364122603362, -0.02811820027887063, 0.9995182064766736, 0.7000000000000002],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N3_2'] = N3_2_mat
N4_1_mat = np.array([[-0.444341735964156, -0.8947739313681952, 0.044045810518876544, 0.0],
[0.8953720752482885, -0.4419458502434798, 0.054705688169885476, 0.0], [-0.029483360492542885,
0.06374540921880009, 0.9975305781065533, -0.09999999999999964], [0.0, 0.0, 0.0,
1.0]])
rotMat['N4_1'] = N4_1_mat
N4_2_mat = np.array([[0.9988523295913587, 0.04671359708300838, -0.010576555085760117,
10.0], [-0.04648606911938285, 0.9987018755893003, 0.020823281975752502, 0.0],
[0.011535555805583356, -0.02030772124045653, 0.9997272265024476, 0.7999999999999998],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N4_2'] = N4_2_mat
N5_1_mat = np.array([[-0.08986377281989212, -0.9954705408424266, 0.031030704946254054,
0.0], [0.9958706079834863, -0.09021596031025624, -0.010139657780214443, 0.0],
[0.012893195460257717, 0.029991379097749554, 0.9994669993004465, 0.5747271756415682],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N5_1'] = N5_1_mat
N5_2_mat = np.array([[-0.09764111729780293, -0.9944741458664936, 0.038566636046517395,
0.0], [0.9945996324606676, -0.09887670911792698, -0.03154310547669095, 0.0],
[0.035182144930937416, 0.03527845797532399, 0.9987580523234553, 1.2727123965813156],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N5_2'] = N5_2_mat
N5_3_mat = np.array([[0.11376454710271973, -0.9928370794231558, 0.03649878826770976, 0.0],
[0.9918680257870178, 0.11560991762950971, 0.05321810187391049, 0.0], [-0.05705652674214411,
0.030147647805359817, 0.9979156638152981, -0.404451678835378], [0.0, 0.0, 0.0,
1.0]])
rotMat['N5_3'] = N5_3_mat
N6_1_mat = np.array([[0.9994973004805413, -0.03086596626934255, 0.007241440351907325,
10.0], [0.03082860898771179, 0.9995110809882194, 0.0052149640117460745, 0.0],
[-0.007398864777328993, -0.004989098918726587, 0.9999601820532585, 0.9000000000000004],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N6_1'] = N6_1_mat
N6_2_mat = np.array([[0.9988722862129534, -0.04214125427326351, -0.021869396973478106,
10.0], [0.0409985649537926, 0.99789214163878, -0.05030299521200135, 0.0], [
0.023943130694148426, 0.04934965393845363, 0.9984945358632253, 0.9000000000000004],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N6_2'] = N6_2_mat
N6_3_mat = np.array([[-0.04889424680162646, -0.9987854830493128, -0.006075481845301646,
0.0], [0.9987878973799329, -0.048858057728727665, -0.005968772264798985, 0.0],
[0.00566468684698101, -0.00635995636205657, 0.9999637304812602, -0.6553131698852468],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N6_3'] = N6_3_mat
N8_1_mat = np.array([[0.06742395876958965, -0.9977243948218384, 0.00020435565303055666,
0.0], [0.9976344970690704, 0.06741513105551401, -0.013439135482274529, 0.0],
[0.013394776652850061, 0.001109991965790168, 0.9999096698583608, 0.7999999999999998],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N8_1'] = N8_1_mat
N9_1_mat = np.array([[-0.19809481806349166, -0.9794499176397107, 0.03789857374567175,
0.0], [0.9799804151970427, -0.19712000934332657, 0.02796583177233762, 0.0],
[-0.01992056441529652, 0.04267974639159458, 0.998890189340813, 0.9980420980703562],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N9_1'] = N9_1_mat
N9_2_mat = np.array([[-0.03297378854159967, -0.9961526066188378, 0.08119552694397052,
0.0], [0.9993813319576447, -0.03186786767058145, 0.014879258875841538, 0.0],
[-0.01223448420563244, 0.0816359194020958, 0.9965871231656551, -0.11066447945547608],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N9_2'] = N9_2_mat
N10_1_mat = np.array([[-0.10138137931389381, -0.9920819078535333, 0.07413031794148582,
0.0], [0.9946640529021992, -0.10251230933181928, -0.011603805394292926, 0.0],
[0.019111195477614955, 0.07255835269044347, 0.9971810505932539, -0.09999999999999964],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N10_1'] = N10_1_mat
N10_2_mat = np.array([[0.051858864388487746, -0.9973430294431006, 0.05116189798663601,
0.0], [0.9980146776291314, 0.049924120800806875, -0.0383964243008054, 0.0],
[0.03574019335646457, 0.05305152008684534, 0.9979519902256409, 1.259040914953271],
[0.0, 0.0, 0.0, 1.0]])
rotMat['N10_2'] = N10_2_mat
gen_rotMat()
#根据quart和计算出的原始坐标系的corners_3d来
#计算转换到新虚拟坐标系下的heading和heading平面
def my_compute_heading_plane(quart,corners_3d):
rot = R.from_quat(quart)
euler = rot.as_euler('zxy', degrees=False) #航向角
#点的方向按照逆时针顺序给出，保证算出的法向量垂直于平面向外
six_planes = [[corners_3d[1],corners_3d[5],corners_3d[6],corners_3d[2]],\
[corners_3d[0],corners_3d[4],corners_3d[5],corners_3d[1]],\
[corners_3d[3],corners_3d[7],corners_3d[4],corners_3d[0]],\
[corners_3d[2],corners_3d[6],corners_3d[7],corners_3d[3]],\
[corners_3d[0],corners_3d[1],corners_3d[2],corners_3d[3]],\
[corners_3d[7],corners_3d[6],corners_3d[5],corners_3d[4]] ]
normals = []
for it in six_planes:
it = np.array(it)
nor = np.cross(it[1] - it[2],it[0] - it[1])
normals.append(nor)
origin_direction = [1,0,0]
ret_ind = -1
for ind,nor in enumerate(normals):
direction = np.arccos(np.dot(origin_direction,nor) / np.linalg.norm(nor))
if abs(direction - abs(euler[0])) < 0.5:
ret_ind = ind
#print('debug:',direction,ind)
break
try:
assert(ret_ind == 0)
except:
print("wrong annotation")
return False, [], []
#new_heading = origin_lidar_heading_to_new_virtual(euler[0])
new_heading = euler[0]
new_heading_plane = copy.deepcopy(six_planes[ret_ind])
'''
nhp_len = len(new_heading_plane)
for ii in range(nhp_len):
#TODO cur lidar -> virtual kitti lidar
new_heading_plane[ii][0] = new_heading_plane[ii][0] - 15.0
new_heading_plane[ii][1] = new_heading_plane[ii][1] - 7.0
'''
return True,new_heading, new_heading_plane
#根据标注的3d框的center、quart、size计算3d bbox的8个顶点
#此处计算的是原始lidar坐标系下的坐标
def my_compute_box_3d(center, quart, size):
# P' = T * R * S * p
#其中p为box3d局部坐标，P'为变换后的世界坐标，假设初始box3d局部坐标系与lidar世界坐标系重合
#标准的box边长为1，正方体
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
corners_3d = np.ones([4,8])
corners_3d[:3,:] = tmp
S = np.diag([size[0],size[1],size[2],1])
rot = R.from_quat(quart)
TR = rot.as_matrix()
Trans = np.zeros([4,4])
Trans[:3,:3] = TR
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)
return np.transpose(world_corners_3d[:3,:])
#读入原始lidar坐标系的标注信息
#将原始lidar坐标系转化为标准kitti坐标系
#转换方式：
# 所有corners_3d、center坐标按照“ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)”来转换
# 新的虚拟new_heading = np.pi + heading if -np.pi < heading < 0;
# 新的虚拟new_heading = heading - np.pi if 0 < heading < np.pi;
########
#保持不变的信息：
# category、size
def read_all_3d_bboxes(box3d,rotMat):
bboxes = []
for item in box3d:
center = [item['box3D']['translation']['x'],item['box3D']['translation']['y'],item['box3D']['translation']['z']]
center = np.array(center, np.float32)
center = np.transpose(np.dot(rotMat[:3,:3],np.transpose(center))) + rotMat[:3,3]
quart = [item['box3D']['rotation']['x'],item['box3D']['rotation']['y'],item['box3D']['rotation']['z'],item['box3D']['rotation']['w']]
origin_rot = R.from_quat(quart)
origin_rot_mat = origin_rot.as_matrix()
new_rot_mat = np.dot(rotMat[:3,:3],origin_rot_mat)
tmp = R.from_matrix(new_rot_mat)
quart = tmp.as_quat()
size = [item['box3D']['size']['x'],item['box3D']['size']['y'],item['box3D']['size']['z']]
size = np.array(size, np.float32)
if size[0] < 0.4 or size[1] < 0.4 or size[2] < 0.4:
continue
#计算原始lidar坐标系中的corners_3d
corners_3d = my_compute_box_3d(center, quart, size)
category = item['category']
assert_flag, heading, heading_plane = my_compute_heading_plane(quart,corners_3d)
if assert_flag == False:
return False, []
#以上为原始lidar坐标系的数据，下面要把bbox转成虚拟lidar坐标系的数据
#TODO cur lidar -> virtual kitti lidar
#new x = old x - 15 new y = old y - 7
#corners_3d[:,0] = corners_3d[:,0] - 15.0
#corners_3d[:,1] = corners_3d[:,1] - 7.0
#TODO cur lidar -> virtual kitti lidar
#center[0] = center[0] - 15.0
#center[1] = center[1] - 7.0
#特别注意，下面的heading和heading_plane是虚拟lidar坐标系下的heading
bboxes.append([category, corners_3d, heading, heading_plane, center, quart, size])
return True, bboxes
#按照文本来解析pcd点云文件
#解析过程中将原始lidar坐标系转换为新虚拟lidar坐标系
#转换方式：
# ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)
# intensity = intensity / 255.0
def parse_pandarmind_pcd(pandar_pcd_path,rotMat):
'''
pandar_fp = open(pandar_pcd_path,'r')
origin_data = []
pc = []
for line in pandar_fp.readlines()[11:]:
#TODO cur lidar -> virtual kitti lidar
x = float(line.split(' ')[1]) * -1 + 5 #将原点移到铁丝网后5米，这样可以把剔除掉的对象拉回来
y = float(line.split(' ')[0]) * 1 + 22 #将原点右移22米
z = float(line.split(' ')[2])
intensity = int(line.split(' ')[3])
origin_data.append([x,y,z,intensity/255.0])
pc.append([x,y,z])
origin_data = np.array(origin_data,dtype=np.float32)
pc = np.array(pc,dtype=np.float32)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pc)
return origin_data, pcd
'''
pcd = o3d.io.read_point_cloud(pandar_pcd_path)
converted_pcd = copy.deepcopy(pcd)
xyz = np.array(pcd.points)
# convert points to horizontal plane
xyz_h = copy.deepcopy(xyz)
xyz_h = np.transpose(np.dot(rotMat[:3,:3], np.transpose(xyz_h))) + rotMat[:3,3]
xyzi = np.zeros([xyz_h.shape[0],4])
xyzi[:,:3] = xyz_h[:,:]
converted_pcd.points = o3d.utility.Vector3dVector(xyzi[:,:3])
'''
xyzi[:,0] = xyz[:,0] - 15
xyzi[:,1] = xyz[:,1] - 7
xyzi[:,2] = xyz[:,2]
pcd.points = o3d.utility.Vector3dVector(xyzi[:,:3])
'''
return xyzi,pcd,converted_pcd
#根据corners_3d来生成open3d渲染要用的12条边
def gen_o3d_3dbboxes(corners_):
bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7], [7, 4], [0, 4], [1, 5], [2, 6], [3, 7]]
colors = [[1, 0, 0] for _ in range(len(bbox_lines))] #red
bbox = o3d.geometry.LineSet()
bbox.lines = o3d.utility.Vector2iVector(bbox_lines)
bbox.colors = o3d.utility.Vector3dVector(colors)
bbox.points = o3d.utility.Vector3dVector(corners_)
return bbox
def custom_draw_geometry_with_key_callback(pcd, 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)
def in_hull(p, hull):
"""
:param p: (N, K) test points
:param hull: (M, K) M corners of a box
:return (N) bool
"""
try:
if not isinstance(hull, Delaunay):
hull = Delaunay(hull)
flag = hull.find_simplex(p) >= 0
except scipy.spatial.qhull.QhullError:
print('Warning: not a hull %s' % str(hull))
flag = np.zeros(p.shape[0], dtype=np.bool)
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
def shadow_casting(points,bbox):
bbox_shp = bbox.shape
assert bbox_shp[0] == 8, " must have 8 points"
assert bbox_shp[1] == 3, " each point must have 3 coordinates"
far_points = bbox * 1.5
hull = np.concatenate((bbox,far_points),axis=0)
flag = in_hull(points,hull)
del_flag = in_hull(points,bbox)
flag = flag^del_flag
#print("X"*100, np.count_nonzero(flag))
#print(points[flag])
return flag
import numpy
class NumpyEncoder(json.JSONEncoder):
def default(self, obj):
if isinstance(obj, (numpy.int_, numpy.intc, numpy.intp, numpy.int8,
numpy.int16, numpy.int32, numpy.int64, numpy.uint8,
numpy.uint16, numpy.uint32, numpy.uint64)):
return int(obj)
elif isinstance(obj, (numpy.float_, numpy.float16, numpy.float32,numpy.float64)):
return float(obj)
elif isinstance(obj, (numpy.ndarray,)):
return obj.tolist()
return json.JSONEncoder.default(self, obj)
g_saveFileType = 1 #0为pcd文件，1是bin文件
g_showCloud = 0 #0不显示，1显示
g_pcd_info = {}
g_no_save_count = 0
g_save_count = 0
MAX_SAVE_FILE_NUM = 100
g_thread_lock = threading.Lock()
def Save_Cloud_File(cloud,json):
global g_saveFileType
global g_showCloud
global g_save_count
global MAX_SAVE_FILE_NUM
#去除boxx内的点
drawpointcloud = []
jsn_pcd = {}
jsn_pcd["labels_box3D"] = []
for bbox in cloud["boxes"]:
jsn_box = {}
jsn_box["box3D"] = {}
jsn_box["box3D"]["translation"] = {"x":bbox[4][0],"y":bbox[4][1],"z":bbox[4][2]}
jsn_box["box3D"]["size"] = {"x":bbox[6][0],"y":bbox[6][1],"z":bbox[6][2]}
jsn_box["box3D"]["rot_y"] = bbox[2]
jsn_box["category"] = bbox[0]
jsn_pcd["labels_box3D"].append(jsn_box)
flag_del = shadow_casting(cloud["np_pcd"],bbox[1])
#g_np_flag = g_np_flag & (~flag_del)
cloud["np_pcd"] = cloud["np_pcd"][~flag_del]
if g_showCloud == 1:
drawboxes = gen_o3d_3dbboxes(bbox[1])
drawpointcloud += [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]))
colors = [[0, 1, 0]]
heading_pcd.colors = o3d.utility.Vector3dVector(colors)
drawpointcloud += [heading_pcd]
g_thread_lock.acquire()
if g_save_count >= MAX_SAVE_FILE_NUM :
g_thread_lock.release()
return
cloud["pcdName"] = str(g_save_count) + cloud["pcdName"]
if g_saveFileType == 0 or g_showCloud == 1 :
pcd_save = o3d.geometry.PointCloud()
pcd_save.points = o3d.utility.Vector3dVector(cloud["np_pcd"])
if g_saveFileType == 0:
savefile = os.path.join(cloud["path"],cloud["pcdName"])
o3d.io.write_point_cloud(savefile,pcd_save)
if g_showCloud == 1:
drawpointcloud += [pcd_save]
if g_saveFileType == 1:
savefile = os.path.join(cloud["path"],cloud["pcdName"])
cloud["np_pcd"].tofile(savefile)
jsn_pcd["fileuri"] = "5-3/pcd/" + cloud["pcdName"]
json.append(jsn_pcd)
if g_showCloud == 1:
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
drawpointcloud += [axis_pcd]
custom_draw_geometry_with_key_callback(drawpointcloud, "")
g_save_count += 1
print("save file g_save_count = ",g_save_count," file = ",cloud["pcdName"])
g_thread_lock.release()
def Add_Cloud_box(save_cloud,add_idx,bbox,pickcloud,json):
save_cloud[add_idx]["boxes"].append(bbox)
delflag = in_hull(save_cloud[add_idx]["np_pcd"],bbox[1])
save_cloud[add_idx]["np_pcd"] = save_cloud[add_idx]["np_pcd"][~delflag]
save_cloud[add_idx]["np_pcd"] = np.vstack((save_cloud[add_idx]["np_pcd"],pickcloud))
if save_cloud[0]["isSave"] == 1:
if len(save_cloud[0]["boxes"]) >=10:
Save_Cloud_File(save_cloud[0],json)
save_cloud.pop(0)
def Check_Add_Cloud_box(save_cloud,bbox,pcd,pcdName,path,index):
global g_save_count
add_idx = -1
for cloud_idx,cloud in enumerate(save_cloud):
#查找可以添加的点云
interactive = 0
for box in cloud["boxes"]:
if abs(bbox[4][0] - box[4][0]) < (bbox[6][0] + box[6][0]) and abs(bbox[4][1] - box[4][1]) < (bbox[6][0] + box[6][0]):
interactive = 1
if interactive == 1 :
continue
else:
add_idx = cloud_idx
break;
if add_idx == -1 :
#需要保存数据了
if index == 1 and len(save_cloud) >= 50 and len(save_cloud[0]["boxes"]) >= 10 :
save_cloud[0]["isSave"] = 1
elif index == 1 and len(save_cloud) > 0 and len(save_cloud[0]["boxes"]) > 20 :
save_cloud[0]["isSave"] = 1
elif index == 1 and len(save_cloud) >= 100 :
save_cloud[0]["isSave"] = 1
if add_idx == -1:
new_cloud = {}
new_cloud["np_pcd"] = np.array(pcd.points)
new_cloud["boxes"] = []
new_cloud["isSave"] = 0
new_cloud["pcdName"] = pcdName
new_cloud["path"] = path
save_cloud.append(new_cloud)
add_idx = len(save_cloud) - 1;
return add_idx
Thread_NUM = 2
threads = []
threadID = 1
class pcdThread (threading.Thread):
def __init__(self, threadID, name,start,idx, _dirs,_dir_pcd_list,save_list,jsn_list, trans,kit2o):
threading.Thread.__init__(self)
self.threadID = threadID
self.name = name
self.begin = start
self.idx = idx
self._dirs = _dirs
self._dir_pcd_list = _dir_pcd_list
self.save_list = save_list
self.jsn_list = jsn_list
self.trans = trans
self.kit2ori = kit2o
def run(self):
global g_save_count
global MAX_SAVE_FILE_NUM
global g_saveFileType
print ("开启线程：" + self.name)
isStop = 0
index = self.begin
dir_pcd_list = self._dir_pcd_list
dirs = self._dirs
thread_idx = self.begin
save_cloud_list = self.save_list #保存点云的数组
json_list = self.jsn_list
while isStop == 0:
if g_save_count >= MAX_SAVE_FILE_NUM:
print("finish g_save_count = ",g_save_count)
break
isDeal = 0
for dir in dirs:
if len(dir_pcd_list[dir][0]) > index:
isDeal = 1
pcd_file = dir_pcd_list[dir][0][index]
anno = dir_pcd_list[dir][1]
child_dir = dir
if pcd_file not in anno:
print('the anno is null : ', pcd_file)
continue
# print(pcd_file)
boxx = anno[pcd_file]
bbox_flag,bboxes = read_all_3d_bboxes(boxx, rotMat[child_dir])
if bbox_flag == False or len(bboxes) < 1:
continue
xyzi,pcd,converted_pcd = parse_pandarmind_pcd(pcd_file, rotMat[child_dir])
xyz = np.array(converted_pcd.points)
if xyz.size == 0:
continue;
for bbox in bboxes:
# exportCenterBL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, generate_Trans)
lidar_loc_ex,out_BL_ex,exportCenterBL = get_loc([bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0],bbox[6][1],bbox[6][2],bbox[2]], 0, self.trans,self.kit2ori)
# print(exportCenterBL)
angle2 = car_yaw_cal(generate_car_yaw_cal_angle,bbox[2])
mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle2)
# print("call get ex data isInMap = ",mapInfoExport)
if mapInfoExport[0] != 1:
continue;
laneAngle = mapInfoExport[10];
detaAngel = laneAngle - angle2;
while detaAngel > 180:
detaAngel -= 360
while detaAngel < -180:
detaAngel += 360
# print("angle = ",angle2," laneAngle = ",laneAngle," detaAngel = ",detaAngel)
if bbox[0] != "pedestrian" and abs(detaAngel) > 30:
continue;
#获取到汽车点云符合条件，可以添加到新点云里
# f1.write('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n'%(exportCenterBL[0],exportCenterBL[1],bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0], bbox[6][1], bbox[6][2],angle2,laneAngle))
name = pcd_file.split("/")[-1]
if g_saveFileType == 1:
name = name[0:-3] + "bin"
ind = 0
if index > self.begin :
ind = 1
idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,name,generate_pcd_path, ind)
if idx == -1:
continue;
flag = in_hull(xyz,bbox[1])
pickcloud = xyz[flag]
bottom_center = (bbox[1][5,:] + bbox[1][7,:]) / 2.0
ground_z = (-m[3] - m[0] * bottom_center[0] - m[1] * bottom_center[1]) / m[2]
delta_z = ground_z - bottom_center[2]
dz = np.array([0,0,delta_z])
pickcloud = np.add(pickcloud,dz)
# print(pickcloud)
bbox[1] += dz
Add_Cloud_box(save_cloud_list,idx,bbox,pickcloud,json_list)
if isDeal == 1:
index += self.idx
if (index - self.begin) % 10*self.idx == 0:
print("while thread idx = ",self.begin," index = ",index)
if index >= 50:
break
else:
isStop = 1
print("finish all the data")
print ("退出线程：" + self.name)
if __name__ == '__main__':
origin_root_path = "/media/sf_shared/nodes/" #读取数据的总目录
generate_root_path = "/media/sf_shared/5-3/"
generate_car_yaw_cal_angle = 79.89299572540227
generate_pcd_path = os.path.join(generate_root_path,"pcd/")
if os.path.exists(generate_pcd_path) == False:
os.mkdir(generate_pcd_path)
if os.path.isdir(generate_pcd_path) == False:
os.remove(generate_pcd_path)
os.mkdir(generate_pcd_path)
generate_child_dir = "N5_3"
generate_cfg_path = generate_root_path + "config/5-3.yaml"
generate_cfg = read_yaml(generate_cfg_path)
generate_Trans=generate_cfg.TRACKING.TRANS
generate_Trans = np.array(generate_Trans)
generate_kitti2origin=generate_cfg.POINTPILLARS.KITTI2ORIGIN
generate_kitti2origin = np.array(generate_kitti2origin)
p_dir = "/home/oscar/ros/git/jfxmap_python/jfxmap/"
f_path = "./maps/qichecheng/mapconfig.json"
ret = init_jfxmap(p_dir,f_path)
print(ret)
base_file_path = os.path.join(generate_root_path, "base/1633640721.417269000.pcd")
g_xyzi,g_pcd,g_converted_pcd = parse_pandarmind_pcd(base_file_path, rotMat[generate_child_dir])
# 计算点云图的地面平面
# converted_points = np.array(g_converted_pcd.points)
# b = np.where( (converted_points[:,2] <= -5.0) & (converted_points[:,2] > -7) )
# cropped_cropped = converted_points[b]
# print(cropped_cropped.shape)
# seg,m = ground_segmentation(data=cropped_cropped[:,:3])
m = [ 1.11119401e-03, -8.66741252e-04, 1.60358817e-01, 9.87057781e-01]
save_cloud_list = [] #保存点云的数组
save_json = {}
save_json["annotations"] = []
save_json_list = []
dir_pcd_list = {}
dirs = os.listdir(origin_root_path)
for dir in dirs:
if os.path.isdir(origin_root_path + "/" + dir) == False:
continue
pcd_list = glob.glob(os.path.join(origin_root_path,dir)+'/pcd/*.pcd') #获取子目录下的所有pcd文件列表，其中每个元素都有完整的路径和文件名
dir_pcd_list[dir] = [pcd_list]
anno = {}
jsn_file = os.path.join(os.path.join(origin_root_path,dir), dir + ".json")
#print(jsn_file)
with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp:
jsn = json.load(fp)
annos = jsn["annotations"]
for an in annos:
fileuri = os.path.join(origin_root_path,dir)+ "/pcd/" + an['fileuri'].split("/")[-1]
anno[fileuri] = an['labels_box3D']
dir_pcd_list[dir].append(anno)
for i in range(Thread_NUM):
list = []
save_cloud_list.append(list)
jsn_list = []
save_json_list.append(jsn_list)
thread = pcdThread(threadID, "Thread"+ str(i) ,i,Thread_NUM,dirs,dir_pcd_list,save_cloud_list[i],save_json_list[i],generate_Trans,generate_kitti2origin)
thread.start()
threads.append(thread)
threadID += 1
for t in threads:
t.join()
for _ind,cloud_i in enumerate(save_cloud_list):
for cloud in cloud_i:
if len(cloud["boxes"]) >=10:
Save_Cloud_File(cloud,save_json_list[_ind])
for t_idx in save_json_list:
for jsn in t_idx:
save_json["annotations"].append(jsn)
jsn_path = os.path.join(generate_root_path,generate_child_dir + ".json")
with open(jsn_path,'w') as file_obj:
json.dump(save_json,file_obj,cls=NumpyEncoder)
# isStop = 0
# index = 0
# while isStop == 0:
# if g_save_count >= MAX_SAVE_FILE_NUM:
# print("finish g_save_count = ",g_save_count)
# break
# isDeal = 0
# for dir in dirs:
# if len(dir_pcd_list[dir][0]) > index:
# isDeal = 1
# pcd_file = dir_pcd_list[dir][0][index]
# anno = dir_pcd_list[dir][1]
# child_dir = dir
# if pcd_file not in anno:
# print('the anno is null : ', pcd_file)
# continue
# # print(pcd_file)
# boxx = anno[pcd_file]
# bbox_flag,bboxes = read_all_3d_bboxes(boxx, rotMat[child_dir])
# if bbox_flag == False or len(bboxes) < 1:
# continue
# xyzi,pcd,converted_pcd = parse_pandarmind_pcd(pcd_file, rotMat[child_dir])
# xyz = np.array(converted_pcd.points)
# if xyz.size == 0:
# continue;
# for bbox in bboxes:
# # exportCenterBL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, generate_Trans)
# lidar_loc_ex,out_BL_ex,exportCenterBL = get_loc([bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0],bbox[6][1],bbox[6][2],bbox[2]], 0, generate_Trans,generate_kitti2origin)
# # print(exportCenterBL)
# angle2 = car_yaw_cal(generate_car_yaw_cal_angle,bbox[2])
# mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle2)
# # print("call get ex data isInMap = ",mapInfoExport)
# if mapInfoExport[0] != 1:
# continue;
# laneAngle = mapInfoExport[10];
# detaAngel = laneAngle - angle2;
# while detaAngel > 180:
# detaAngel -= 360
# while detaAngel < -180:
# detaAngel += 360
# # print("angle = ",angle2," laneAngle = ",laneAngle," detaAngel = ",detaAngel)
# if bbox[0] != "pedestrian" and abs(detaAngel) > 30:
# continue;
# #获取到汽车点云符合条件，可以添加到新点云里
# # f1.write('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n'%(exportCenterBL[0],exportCenterBL[1],bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0], bbox[6][1], bbox[6][2],angle2,laneAngle))
# name = pcd_file.split("/")[-1]
# if g_saveFileType == 1:
# name = name[0:-3] + "bin"
# idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,name,generate_pcd_path,index)
# if idx == -1:
# continue;
# flag = in_hull(xyz,bbox[1])
# pickcloud = xyz[flag]
# bottom_center = (bbox[1][5,:] + bbox[1][7,:]) / 2.0
# ground_z = (-m[3] - m[0] * bottom_center[0] - m[1] * bottom_center[1]) / m[2]
# delta_z = ground_z - bottom_center[2]
# dz = np.array([0,0,delta_z])
# pickcloud = np.add(pickcloud,dz)
# # print(pickcloud)
# bbox[1] += dz
# Add_Cloud_box(save_cloud_list,idx,bbox,pickcloud,save_json)
# if isDeal == 1:
# index += 1
# if index % 10 == 0:
# print("while index = ",index)
# # if index >= 10:
# # break
# else:
# isStop = 1
# print("finish all the data")
# for cloud in save_cloud_list:
# Save_Cloud_File(cloud,save_json)
# jsn_path = os.path.join(generate_root_path,generate_child_dir + ".json")
# with open(jsn_path,'w') as file_obj:
# json.dump(save_json,file_obj,cls=NumpyEncoder)
# # for file, num in g_pcd_info.items():
# # print("file = ",file,", box num = ",num)
# print("no save file num = ",g_no_save_count)
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