def show_lidar_with_boxes(pc_velo, objects, calib, img_fov=False, img_width=None,
img_height=None, objects_pred=None):
''' Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) '''
if 'mlab' not in sys.modules: import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(('All point num: ', pc_velo.shape[0]))
fig = mlab.figure(figure=None, bgcolor=(0,0,0),
fgcolor=None, engine=None, size=(1000, 500))
if img_fov:
pc_velo = get_lidar_in_image_fov(pc_velo, calib, 0, 0,
img_width, img_height)
print(('FOV point num: ', pc_velo.shape[0]))
draw_lidar(pc_velo, fig=fig)
color=(0,1,0)
for obj in objects:
if obj.type=='DontCare':continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1,y1,z1 = ori3d_pts_3d_velo[0,:]
x2,y2,z2 = ori3d_pts_3d_velo[1,:]
mlab.plot3d([x1, x2], [y1, y2], [z1,z2], color=color,
tube_radius=None, line_width=1, figure=fig)
if objects_pred is not None:
color=(1,0,0)
for obj in objects_pred:
if obj.type=='DontCare':continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1,y1,z1 = ori3d_pts_3d_velo[0,:]
x2,y2,z2 = ori3d_pts_3d_velo[1,:]
mlab.plot3d([x1, x2], [y1, y2], [z1,z2], color=color,
tube_radius=None, line_width=1, figure=fig)
mlab.show(1)
def show_lidar_with_boxes(pc_velo, objects, calib,
img_fov=False, img_width=None, img_height=None):
''' Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) '''
if 'mlab' not in sys.modules: import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(('All point num: ', pc_velo.shape[0]))
fig = mlab.figure(figure=None, bgcolor=(0,0,0),
fgcolor=None, engine=None, size=(1000, 500))
if img_fov:
pc_velo = get_lidar_in_image_fov(pc_velo, calib, 0, 0,
img_width, img_height)
print(('FOV point num: ', pc_velo.shape[0]))
draw_lidar(pc_velo, fig=fig)
for obj in objects:
if obj.type=='DontCare':continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1,y1,z1 = ori3d_pts_3d_velo[0,:]
x2,y2,z2 = ori3d_pts_3d_velo[1,:]
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig)
mlab.plot3d([x1, x2], [y1, y2], [z1,z2], color=(0.5,0.5,0.5),
tube_radius=None, line_width=1, figure=fig)
mlab.show(1)
def show_lidar_with_results(pc_velo, results, calib,
img_fov=False, img_width=None, img_height=None):
''' Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) '''
if 'mlab' not in sys.modules: import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(('All point num: ', pc_velo.shape[0]))
fig = mlab.figure(figure=None, bgcolor=None,
fgcolor=None, engine=None, size=(1000, 500))
if img_fov:
pc_velo = get_lidar_in_image_fov(pc_velo, calib, 0, 0,
img_width, img_height)
print(('FOV point num: ', pc_velo.shape[0]))
#pc_rect = np.zeros_like(pc_velo)
#print(pc_rect.shape)
#pc_rect[:,0:3] = calib.project_velo_to_rect(pc_velo[:,0:3])
#print(pc_rect.shape)
#print(pc_velo.shape)
#pc_rect[:,3] = pc_velo[:,3]
draw_lidar(pc_velo, fig=fig,pts_scale=0.1,pts_mode='sphere')
for obj in results:
if obj.type=='DontCare':continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(obj, calib.P)
#ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1,y1,z1 = ori3d_pts_3d[0,:]
x2,y2,z2 = ori3d_pts_3d[1,:]
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=(1,0,0))
mlab.plot3d([x1, x2], [y1, y2], [z1,z2], color=(0.5,0.5,0.5),
tube_radius=None, line_width=1, figure=fig)
mlab.show(1)
def show_lidar_with_boxes(
pc_velo,
objects,
calib,
img_fov=False,
img_width=None,
img_height=None,
objects_pred=None,
depth=None,
cam_img=None,
):
""" Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) """
if "mlab" not in sys.modules:
import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(("All point num: ", pc_velo.shape[0]))
fig = mlab.figure(figure=None,
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1000, 500))
if img_fov:
pc_velo = get_lidar_in_image_fov(pc_velo[:, 0:3], calib, 0, 0,
img_width, img_height)
print(("FOV point num: ", pc_velo.shape[0]))
print("pc_velo", pc_velo.shape)
draw_lidar(pc_velo, fig=fig)
# pc_velo=pc_velo[:,0:3]
color = (0, 1, 0)
for obj in objects:
if obj.type == "Car":
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw depth
if depth is not None:
# import pdb; pdb.set_trace()
depth_pt3d = depth_region_pt3d(depth, obj)
depth_UVDepth = np.zeros_like(depth_pt3d)
depth_UVDepth[:, 0] = depth_pt3d[:, 1]
depth_UVDepth[:, 1] = depth_pt3d[:, 0]
depth_UVDepth[:, 2] = depth_pt3d[:, 2]
print("depth_pt3d:", depth_UVDepth)
dep_pc_velo = calib.project_image_to_velo(depth_UVDepth)
print("dep_pc_velo:", dep_pc_velo)
draw_lidar(dep_pc_velo, fig=fig, pts_color=(0, 0, 255))
#
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(
obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
if objects_pred is not None:
color = (1, 0, 0)
for obj in objects_pred:
if obj.type != "Car":
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(
obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
def show_lidar_with_depth(pc_velo,
objects,
calib,
fig,
img_fov=False,
img_width=None,
img_height=None,
objects_pred=None,
depth=None,
cam_img=None,
data_idx=0,
constraint_box=False,
pc_label=False,
save=False,
fov_restrict=False):
""" Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) """
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(("All point num: ", pc_velo.shape[0]))
if img_fov:
pc_velo_index = get_lidar_index_in_image_fov(pc_velo[:, :3], calib, 0,
0, img_width, img_height)
pc_velo = pc_velo[pc_velo_index, :]
print(("FOV point num: ", pc_velo.shape))
pc_velo = pc_velo[pc_velo[:, 0] < 70.4]
if fov_restrict:
x_range = [0, 70.4]
y_range = [-40, 40]
z_range = [-1, 3]
pts_x, pts_y, pts_z = pc_velo[:, 0], pc_velo[:, 1], pc_velo[:, 2]
range_flag = (pts_x >= x_range[0]) & (pts_x <= x_range[1]) \
& (pts_y >= y_range[0]) & (pts_y <= y_range[1]) \
& (pts_z >= z_range[0]) & (pts_z <= z_range[1])
pc_velo = pc_velo[range_flag, :]
print("pc_velo", pc_velo.shape)
draw_lidar(pc_velo, fig=fig, pc_label=pc_label, pts_scale=1.0)
color = (0, 1, 0)
for obj in objects:
if obj.type != "Car" or obj.t[2] > 70.4:
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
color_list = [(0, 1, 0), (0, 0, 1)]
draw_gt_boxes3d([box3d_pts_3d_velo],
fig=fig,
color=color,
label=obj.type,
color_list=color_list,
draw_text=False)
if objects_pred is not None:
color = (1, 0, 0)
for obj in objects_pred:
if obj.type != "Car":
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(
obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
if (args.save_img):
mlab.savefig(args.save_dir + 'lidar/' + str(data_idx).zfill(6) +
'.png')
mlab.clf()
else:
mlab.show(1)
def show_lidar_with_depth(
pc_velo,
objects,
calib,
fig,
img_fov=False,
img_width=None,
img_height=None,
objects_pred=None,
depth=None,
cam_img=None,
constraint_box=False,
pc_label=False,
save=False,
):
""" Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) """
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(("All point num: ", pc_velo.shape[0]))
if img_fov:
pc_velo_index = get_lidar_index_in_image_fov(
pc_velo[:, :3], calib, 0, 0, img_width, img_height
)
pc_velo = pc_velo[pc_velo_index, :]
print(("FOV point num: ", pc_velo.shape))
print("pc_velo", pc_velo.shape)
draw_lidar(pc_velo, fig=fig, pc_label=pc_label)
# Draw depth
if depth is not None:
depth_pc_velo = calib.project_depth_to_velo(depth, constraint_box)
indensity = np.ones((depth_pc_velo.shape[0], 1)) * 0.5
depth_pc_velo = np.hstack((depth_pc_velo, indensity))
print("depth_pc_velo:", depth_pc_velo.shape)
print("depth_pc_velo:", type(depth_pc_velo))
print(depth_pc_velo[:5])
draw_lidar(depth_pc_velo, fig=fig, pts_color=(1, 1, 1))
if save:
data_idx = 0
vely_dir = "data/obj/training/depth_pc"
save_filename = os.path.join(vely_dir, "%06d.bin" % (data_idx))
print(save_filename)
# np.save(save_filename+".npy", np.array(depth_pc_velo))
depth_pc_velo = depth_pc_velo.astype(np.float32)
depth_pc_velo.tofile(save_filename)
color = (0, 1, 0)
for obj in objects:
if obj.type == "DontCare":
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
if objects_pred is not None:
color = (1, 0, 0)
for obj in objects_pred:
if obj.type == "DontCare":
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
print("box3d_pts_3d_velo:")
print(box3d_pts_3d_velo)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig, color=color)
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
mlab.show(1)
def show_lidar_with_boxes(pc_velo,
objects,
calib,
img_fov=False,
img_width=None,
img_height=None,
objects_pred=None,
depth=None,
cam_img=None,
data_idx=None,
pseudo_lidar=None):
""" Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) """
if "mlab" not in sys.modules:
import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_lidar, draw_gt_boxes3d
print(("All point num: ", pc_velo.shape[0]))
fig = mlab.figure(
figure=None,
# bgcolor=(1, 1, 1), # black background
bgcolor=(0, 0, 0), # white background
fgcolor=None,
engine=None,
size=(1000, 500)
# figure=None
)
if img_fov: # filter pcl out of fov
pc_velo = get_lidar_in_image_fov(pc_velo[:, 0:3], calib, 0, 0,
img_width, img_height)
print(pc_velo.shape)
print(("FOV point num: ", pc_velo.shape[0]))
print("pc_velo", pc_velo.shape)
draw_lidar(pc_velo, fig=fig, pts_color=(0, 1, 0))
# show_box_lidar(objects, calib, data_idx, fig)
car_obj = []
color = (1, 0, 0)
for obj in objects:
if obj.type == "DontCare":
print("############## Dont care gt: t: {}, (h: {}, l: {}, w: {})".
format(obj.t, obj.h, obj.l, obj.w))
continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
if obj.type == "Car":
car_obj.append(box3d_pts_3d)
draw_gt_boxes3d([box3d_pts_3d_velo],
fig=fig,
color=color,
data_idx=data_idx,
type=obj.type,
occlu=obj.occlusion)
# Draw depth from ego-vehicle to objects
if depth is not None:
# import pdb; pdb.set_trace()
depth_pt3d = depth_region_pt3d(depth, obj)
depth_UVDepth = np.zeros_like(depth_pt3d)
depth_UVDepth[:, 0] = depth_pt3d[:, 1]
depth_UVDepth[:, 1] = depth_pt3d[:, 0]
depth_UVDepth[:, 2] = depth_pt3d[:, 2]
print("depth_pt3d:", depth_UVDepth)
dep_pc_velo = calib.project_image_to_velo(depth_UVDepth)
print("dep_pc_velo:", dep_pc_velo)
draw_lidar(dep_pc_velo, fig=fig, pts_color=(1, 1, 1))
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(obj, calib.P)
ori3d_pts_3d_velo = calib.project_rect_to_velo(ori3d_pts_3d)
x1, y1, z1 = ori3d_pts_3d_velo[0, :]
x2, y2, z2 = ori3d_pts_3d_velo[1, :]
mlab.plot3d(
[x1, x2],
[y1, y2],
[z1, z2],
color=color,
tube_radius=None,
line_width=1,
figure=fig,
)
mlab.show()
