Commit cbebde16 authored by oscar's avatar oscar

提交遍历修改

parent e32879e5
......@@ -480,13 +480,13 @@ def Add_Cloud_box(save_cloud,add_idx,bbox,pickcloud):
Save_Cloud_File(save_cloud[0])
save_cloud.pop(0)
def Check_Add_Cloud_box(save_cloud,bbox,pcd,child_dir,pcdName,path):
def Check_Add_Cloud_box(save_cloud,bbox,pcd,pcdName,path,index):
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])/2 and abs(bbox[4][1] - box[4][1]) < (bbox[6][0] + box[6][0])/2:
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
......@@ -495,17 +495,16 @@ def Check_Add_Cloud_box(save_cloud,bbox,pcd,child_dir,pcdName,path):
break;
if add_idx == -1 :
#需要保存数据了
if len(save_cloud) >= 50 and len(save_cloud[0]["boxes"]) >= 10 :
if index > 1 and len(save_cloud) >= 100 and len(save_cloud[0]["boxes"]) >= 10 :
save_cloud[0]["isSave"] = 1
elif len(save_cloud) > 0 and len(save_cloud[0]["boxes"]) >20 :
elif index > 1 and len(save_cloud) > 0 and len(save_cloud[0]["boxes"]) > 50 :
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["child_dir"] = child_dir
new_cloud["pcdName"] = pcdName
new_cloud["pcdName"] = str(len(save_cloud)) + pcdName
new_cloud["path"] = path
save_cloud.append(new_cloud)
add_idx = len(save_cloud) - 1;
......@@ -556,73 +555,152 @@ if __name__ == '__main__':
save_cloud_list = [] #保存点云的数组
dir_pcd_list = {}
dirs = os.listdir(origin_root_path)
for dir in dirs:
if os.path.isdir(origin_root_path + "/" + dir) == False:
continue
child_dir = dir
pcd_list = glob.glob(os.path.join(origin_root_path,child_dir)+'/pcd/*.pcd') #获取子目录下的所有pcd文件列表,其中每个元素都有完整的路径和文件名
#print(pcd_list)
#读标注的json文件
pcd_list = glob.glob(os.path.join(origin_root_path,dir)+'/pcd/*.pcd') #获取子目录下的所有pcd文件列表,其中每个元素都有完整的路径和文件名
dir_pcd_list[dir] = [pcd_list]
anno = {}
saveName = {}
jsn_file = os.path.join(os.path.join(origin_root_path,child_dir), child_dir + ".json")
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,child_dir)+ "/pcd/" + an['fileuri'].split("/")[-1]
fileuri = os.path.join(origin_root_path,dir)+ "/pcd/" + an['fileuri'].split("/")[-1]
anno[fileuri] = an['labels_box3D']
dir_pcd_list[dir].append(anno)
isStop = 0
index = 0
while isStop == 0:
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)
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 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))
idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,pcd_file.split("/")[-1],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)
if isDeal == 1:
index += 1
else:
isStop = 1
# for dir in dirs:
# if os.path.isdir(origin_root_path + "/" + dir) == False:
# continue
# child_dir = dir
# pcd_list = glob.glob(os.path.join(origin_root_path,child_dir)+'/pcd/*.pcd') #获取子目录下的所有pcd文件列表,其中每个元素都有完整的路径和文件名
# #print(pcd_list)
# #读标注的json文件
# anno = {}
# saveName = {}
# jsn_file = os.path.join(os.path.join(origin_root_path,child_dir), child_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,child_dir)+ "/pcd/" + an['fileuri'].split("/")[-1]
# anno[fileuri] = an['labels_box3D']
for pcd_ind,pcd_file in enumerate(pcd_list):
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)
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 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))
# for pcd_ind,pcd_file in enumerate(pcd_list):
# 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)
# 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 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))
idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,child_dir,pcd_file.split("/")[-1],generate_pcd_path)
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)
# idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,child_dir,pcd_file.split("/")[-1],generate_pcd_path)
# 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)
for cloud in save_cloud_list:
......
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