Skip to content

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

提交修改

parent 43350160
Hide whitespace changes
Inline Side-by-side
Showing with 143 additions and 145 deletions
script/trans_and_export_data.py
View file @ ff7752cf
......@@ -442,34 +442,87 @@ def shadow_casting(points,bbox):
#print(points[flag])
return flag
def Save_Cloud_File(cloud):
#去除boxx内的点
drawpointcloud = []
for bbox in cloud.boxes:
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]
drawboxesorgin = 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]
pcd_save = o3d.geometry.PointCloud()
pcd_save.points = o3d.utility.Vector3dVector(cloud.np_pcd)
drawpointcloud += [pcd_save]
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
drawpointcloud += [axis_pcd]
savefile = os.path.join(cloud.path,cloud.pcdName)
o3d.io.write_point_cloud(savefile,pcd_save)
custom_draw_geometry_with_key_callback(drawpointcloud, savefile)
def Add_Cloud_box(save_cloud,add_idx,bbox,pickcloud):
save_cloud[add_idx].boxes.append(bbox)
save_cloud[add_idx].np_pcd += pickcloud
if save_cloud[0].isSave == 1:
Save_Cloud_File(save_cloud[0])
save_cloud.pop(0)
def Check_Add_Cloud_box(save_cloud,bbox,pcd,child_dir,pcdName,path):
add_idx = -1
for cloud_idx,cloud in enumerate(save_cloud):
#查找可以添加的点云
add_idx = cloud_idx
break;
if add_idx == -1 and len(save_cloud) > 50:
#需要保存数据了
save_cloud[0].isSave = 1
elif 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.path = path
save_cloud.append(new_cloud)
add_idx = 0;
return add_idx
if __name__ == '__main__':
origin_root_path = "/home/oscar/ros/git/jfxmap_python/script/data/10-1/"
origin_root_path = "/media/sf_shared/nodes/" #读取数据的总目录
generate_root_path = "/home/oscar/ros/git/jfxmap_python/script/data/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)
save_root_path = "/home/oscar/ros/git/jfxmap_python/script/"
origin_child_dir = "N10_1"
generate_child_dir = "N5_3"
origin_cfg_path = origin_root_path + "config/10-1.yaml"
origin_cfg = read_yaml(origin_cfg_path)
origin_Trans=origin_cfg.TRACKING.TRANS
origin_Trans = np.array(origin_Trans)
origin_kitti2origin=origin_cfg.POINTPILLARS.KITTI2ORIGIN
origin_kitti2origin = np.array(origin_kitti2origin)
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)
#读标注的json文件
anno = {}
saveName = {}
jsn_file = os.path.join(origin_root_path, "origin/" + origin_child_dir + ".json")
print(jsn_file)
generate_kitti2origin = np.array(generate_kitti2origin)
p_dir = "/home/oscar/ros/git/jfxmap_python/jfxmap/"
f_path = "./maps/qichecheng/mapconfig.json"
......@@ -478,6 +531,7 @@ if __name__ == '__main__':
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]
......@@ -486,132 +540,77 @@ if __name__ == '__main__':
# seg,m = ground_segmentation(data=cropped_cropped[:,:3])
m = [ 1.11119401e-03, -8.66741252e-04, 1.60358817e-01, 9.87057781e-01]
g_np_cloud = np.array(g_converted_pcd.points)
#g_np_flag = np.ones([g_np_cloud.shape[0]], dtype=bool)
vis = True
with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp:
jsn = json.load(fp)
annos = jsn["annotations"]
for an in annos:
fileuri = "origin/pcds/" + an['fileuri'].split("/")[-1]
fileuri_path = os.path.join(origin_root_path,fileuri)
# print(fileuri_path)
if os.path.isfile(fileuri_path):
print(fileuri_path," ",fileuri)
anno[fileuri_path] = an['labels_box3D']
pcdname = an['fileuri'].split("/")[-1]
saveName[fileuri_path] = pcdname[0:pcdname.find('.pcd')]
num = 1
for file_path,boxx in anno.items():
svaeFile = save_root_path + saveName[file_path] + ".csv"
f1 = open(svaeFile, 'w')
f1.write('lat,lon,x,y,z,l,w,h,rot_y,laneAngle\n')
# print(file_path)
# print(boxx)
# bbox_flag,bboxes = read_all_3d_bboxes(boxx, np.identity(4))
bbox_flag,bboxes = read_all_3d_bboxes(boxx, rotMat[origin_child_dir])
if bbox_flag == False or len(bboxes) < 1:
continue
# print(bbox_flag)
# print(bboxes)
# 地面转成水平面，地面点的z值几乎差不多
xyzi,pcd,converted_pcd = parse_pandarmind_pcd(file_path, rotMat[origin_child_dir])
print(xyzi)
print(pcd)
print(converted_pcd)
exportDrawCloud = []
xyz = np.array(converted_pcd.points)
savecloud = o3d.geometry.PointCloud()
drawpointcloud = []
merge_geos = [converted_pcd]
for bbox in bboxes:
# out_center_BL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, origin_Trans)
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, origin_Trans,origin_kitti2origin)
print(out_center_BL)
angle = car_yaw_cal(131.419987,bbox[2])
mapInfo = get_map_data(out_center_BL[0],out_center_BL[1],angle)
print("call get data isInMap = ",mapInfo)
# 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(79.89299572540227,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))
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])
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(addz)
drawpointcloud += [point_cloud]
exportDrawCloud += [point_cloud]
savecloud += point_cloud
drawboxesorgin = gen_o3d_3dbboxes(bbox[1])
bbox[1] += dz
drawboxes = gen_o3d_3dbboxes(bbox[1])
drawpointcloud += [drawboxes]
exportDrawCloud += [drawboxes]
save_cloud_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文件
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))
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:
Save_Cloud_File(cloud)
#去除boxx内的点
flag_del = shadow_casting(g_np_cloud,bbox[1])
#g_np_flag = g_np_flag & (~flag_del)
g_np_cloud = g_np_cloud[~flag_del]
#tmp = g_converted_pcd.select_by_index(flag_del)
#g_converted_pcd = tmp
#渲染bbox
merge_geos += [drawboxesorgin]
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)
#渲染车辆形式方向的点
merge_geos += [heading_pcd]
drawpointcloud += [heading_pcd]
exportDrawCloud += [heading_pcd]
# savecloud += heading_pcd
pcd_g_np = o3d.geometry.PointCloud()
pcd_g_np.points = o3d.utility.Vector3dVector(g_np_cloud)
savecloud += pcd_g_np
exportDrawCloud += [pcd_g_np]
f1.close()
if vis:
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
merge_geos += [axis_pcd]
drawpointcloud += [axis_pcd]
exportDrawCloud += [axis_pcd]
#pcd.paint_uniform_color([1.00, 0, 0])
savefile = "/home/oscar/ros/git/jfxmap_python/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)
custom_draw_geometry_with_key_callback(exportDrawCloud, file_path)
# pcd_list = glob.glob("D:/work/git_workspace/jfxmap_python/script/data_aug_samples/10-1_pcds/*.pcd")
# print(pcd_list)
\ No newline at end of file
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