Skip to content

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

提交更新

parent 90d7dd3f
Hide whitespace changes
Inline Side-by-side
Showing with 49 additions and 16 deletions
script/trans_data.py
View file @ b04bd511
......@@ -431,6 +431,17 @@ def read_yaml(path):
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)
#print("X"*100, np.count_nonzero(flag))
#print(points[flag])
return flag
if __name__ == '__main__':
......@@ -467,6 +478,16 @@ 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]
# 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]
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:
......@@ -486,7 +507,7 @@ if __name__ == '__main__':
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\n')
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))
......@@ -500,18 +521,12 @@ if __name__ == '__main__':
print(xyzi)
print(pcd)
print(converted_pcd)
b = np.where( (converted_pcd[:,2] <= -3.5) & (converted_pcd[:,2] > -7) )
cropped_cropped = converted_pcd[b]
print(cropped_cropped.shape)
seg,m = ground_segmentation(data=cropped_cropped[:,:3])
exportDrawCloud = []
xyz = np.array(converted_pcd.points)
savecloud = o3d.geometry.PointCloud()
savecloud += g_converted_pcd
drawpointcloud = []
merge_geos = [converted_pcd]
exportDrawCloud = [g_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)
......@@ -519,27 +534,32 @@ if __name__ == '__main__':
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)
f1.write('%f,%f,%f,%f,%f,%f,%f,%f,%f\n'%(out_center_BL[0],out_center_BL[1],bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0], bbox[6][1], bbox[6][2],angle))
# 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)
angle = car_yaw_cal(79.89299572540227,bbox[2])
mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle)
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 - angle;
detaAngel = laneAngle - angle2;
while detaAngel > 180:
detaAngel -= 360
while detaAngel < -180:
detaAngel += 360
print("angle = ",angle," laneAngle = ",laneAngle," detaAngel = ",detaAngel)
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]
dz = np.array([0,0,0])
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)
......@@ -550,11 +570,20 @@ if __name__ == '__main__':
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]
#去除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 += [drawboxes]
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]))
......@@ -565,6 +594,10 @@ if __name__ == '__main__':
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:
......@@ -576,7 +609,7 @@ if __name__ == '__main__':
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(merge_geos, file_path)
# custom_draw_geometry_with_key_callback(drawpointcloud, file_path)
custom_draw_geometry_with_key_callback(exportDrawCloud, file_path)
......
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