def print_npts_statistics(idx_filename,
type_whitelist=['Car'],
split='v1.0-mini'):
dataset = nuscenes2kitti_object(os.path.join(ROOT_DIR,
'dataset/nuScenes2KITTI'),
split=split,
sensor_list=['CAM_FRONT'])
x = [
0, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 600, 700, 800, 900,
1000, 10000
]
x = np.array(x).astype(np.int32)
y = np.zeros(x.shape).astype(np.int32)
data_idx_list = [int(line.rstrip()) for line in open(idx_filename)]
for data_idx in tqdm(data_idx_list):
calib = dataset.get_calibration(data_idx) # 3 by 4 matrix
pc_velo = dataset.get_lidar(data_idx)
pc_global = calib.project_lidar_to_global(pc_velo[:, 0:3].T)
pc_rect = calib.project_global_to_cam(pc_global, sensor).T
objects = dataset.get_label_objects(sensor, data_idx)
for obj_idx in range(len(objects)):
obj = objects[obj_idx]
if obj.type not in type_whitelist: continue
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
objects[obj_idx], getattr(calib, sensor))
pts_in_box3d, _ = extract_pc_in_box3d(pc_rect, box3d_pts_3d)
npts = len(pts_in_box3d)
for k in range(1, len(x) - 1):
if npts < x[k]:
y[k - 1] += 1
break
np.savetxt('kitti_npts_info', (x, y))
def show_lidar_with_boxes(pc_velo,
objects,
calib,
sensor,
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
#from viz_util import draw_lidar
from viz_util import draw_gt_boxes3d
view = getattr(calib, sensor)
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, sensor, 0, 0,
img_width, img_height)
print(('FOV point num: ', pc_velo.shape[0]))
#draw_lidar(pc_velo, fig=fig)
#fig = draw_nusc_lidar(pc_velo,pts_scale=0.1,pts_mode='sphere')
fig = utils.draw_nusc_lidar(pc_velo)
obj_mean = np.array([0.0, 0.0, 0.0])
obj_count = 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, np.eye(4)) #(8,2),(8,3)
box3d_pts_3d_global = calib.project_cam_to_global(
box3d_pts_3d.T, sensor) # (3,8)
box3d_pts_3d_velo = calib.project_global_to_lidar(
box3d_pts_3d_global).T #(8,3)
# box3d_pts_3d_velo = box3d_pts_3d
# Draw heading arrow
ori3d_pts_2d, ori3d_pts_3d = utils.compute_orientation_3d(
obj, np.eye(4)) #(2,2),(2,3)
ori3d_pts_3d_global = calib.project_cam_to_global(
ori3d_pts_3d.T, sensor) #(3,2)
ori3d_pts_3d_velo = calib.project_global_to_lidar(
ori3d_pts_3d_global).T #(2,3)
ori3d_pts_3d_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)
obj_mean += np.sum(box3d_pts_3d_velo, axis=0)
obj_count += 1
obj_mean = obj_mean / obj_count
print("mean:", obj_mean)
mlab.show(1)
def return_image_with_preds(img, objects, calib, show3d=True):
''' Show image with 2D bounding boxes '''
sensor = 'CAM_FRONT'
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type == 'DontCare': continue
cv2.rectangle(img1, (int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)), (0, 255, 0), 2)
cv2.putText(img1,
"%.2f" % (obj.score),
org=(int(obj.xmin), int(obj.ymin)),
fontFace=cv2.FONT_HERSHEY_TRIPLEX,
fontScale=0.5,
color=(0, 0, 255))
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
obj, getattr(calib, sensor))
if np.sum(box3d_pts_2d == None) != 1:
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
# Image.fromarray(img1).show()
if show3d:
# Image.fromarray(img2).show()
return img1, img2
else:
return img1
def show_image_with_boxes(img,
objects,
calib,
sensor,
show3d=True,
linewidth=2,
colors=((0, 0, 255), (255, 0, 0), (155, 155, 155))):
''' Show image with 2D bounding boxes '''
view = getattr(calib, sensor)
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
type2color = {'Pedestrian': 0, 'Car': 1, 'Cyclist': 2}
for obj in objects:
if obj.type == 'DontCare': continue
if obj.type not in type2color.keys(): continue
c = type2color[obj.type]
cv2.rectangle(img1, (int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)), colors[c][::-1], 2)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, view)
#img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
cv2.line(img2, (int(prev[0]), int(prev[1])),
(int(corner[0]), int(corner[1])), color, linewidth)
prev = corner
corners_2d = box3d_pts_2d.T #(2,8)
# Draw the sides
for i in range(4):
cv2.line(
img2, (int(corners_2d.T[i][0]), int(corners_2d.T[i][1])),
(int(corners_2d.T[i + 4][0]), int(corners_2d.T[i + 4][1])),
colors[c][::-1], linewidth)
# Draw front (first 4 corners) and rear (last 4 corners) rectangles(3d)/lines(2d)
draw_rect(corners_2d.T[:4], colors[c][::-1])
draw_rect(corners_2d.T[4:], colors[c][::-1])
# Draw line indicating the front
center_bottom_forward = np.mean(corners_2d.T[0:2], axis=0)
center_bottom = np.mean(corners_2d.T[[0, 1, 2, 3]], axis=0)
# center_bottom_forward = np.mean(corners_2d.T[2:4], axis=0)
# center_bottom = np.mean(corners_2d.T[[2, 3, 7, 6]], axis=0)
cv2.line(
img2, (int(center_bottom[0]), int(center_bottom[1])),
(int(center_bottom_forward[0]), int(center_bottom_forward[1])),
colors[c][::-1], linewidth)
Image.fromarray(img1).show()
if show3d:
Image.fromarray(img2).show()
def print_box3d_statistics(idx_filename,
type_whitelist=['Car', 'Pedestrian', 'Cyclist'],
split='v1.0-mini',
sensor='CAM_FRONT'):
''' Collect and dump 3D bounding box statistics '''
dataset = nuscenes2kitti_object(os.path.join(ROOT_DIR,
'dataset/nuScenes2KITTI'),
split,
sensor_list=['CAM_FRONT'])
dimension_list = []
type_list = []
ry_list = []
mean_t_list = []
mean_t_by_center_list = []
data_idx_list = [int(line.rstrip()) for line in open(idx_filename)]
for data_idx in tqdm(data_idx_list):
calib = dataset.get_calibration(data_idx) # 3 by 4 matrix
pc_velo = dataset.get_lidar(data_idx)
pc_global = calib.project_lidar_to_global(pc_velo[:, 0:3].T)
pc_rect = calib.project_global_to_cam(pc_global, sensor).T
objects = dataset.get_label_objects(sensor, data_idx)
for obj_idx in range(len(objects)):
obj = objects[obj_idx]
if obj.type not in type_whitelist: continue
dimension_list.append(np.array([obj.l, obj.w, obj.h]))
type_list.append(obj.type)
ry_list.append(obj.ry)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
objects[obj_idx], getattr(calib, sensor))
pts_in_box3d, _ = extract_pc_in_box3d(pc_rect, box3d_pts_3d)
if len(pts_in_box3d) == 0: continue
mean_t_list.append(pts_in_box3d.mean(0))
pts_in_box3d -= obj.t
mean_t_by_center_list.append(pts_in_box3d.mean(0))
dimensions = np.array(dimension_list)
mts = np.array(mean_t_list)
rys = np.array(ry_list)
mtbcs = np.array(mean_t_by_center_list)
md = dimensions.mean(0)
mmt = mts.mean(0)
mry = rys.mean()
mmtbcs = mtbcs.mean(0)
print('mean points in 3d box: (%.1f,%.1f,%.1f)' % (mmt[0], mmt[1], mmt[2]))
print('mean points related to box center: (%.1f,%.1f,%.1f)' %
(mmtbcs[0], mmtbcs[1], mmtbcs[2]))
print('mean size: (%.1f,%.1f,%.1f)' % (md[0], md[1], md[2]))
print('mean ry: (%.2f)' % (mry))
"""
def render_lidar_bev(pc_velo, objects, calib, sensor, saveto=None):
_, ax = plt.subplots(1, 1, figsize=(9, 9))
print('pc_velo.shape:', pc_velo.shape)
ax.scatter(pc_velo[:, 0], pc_velo[:, 1], c=pc_velo[:, 2], s=0.2)
ax.set_xlim(-40, 40)
ax.set_ylim(-20, 70)
linewidth = 2
colors = ('b', 'r', 'k')
for obj in objects:
if obj.type == 'DontCare': continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
obj, np.eye(4)) #(8,2),(8,3)
box3d_pts_3d_global = calib.project_cam_to_global(
box3d_pts_3d.T, sensor) # (3,8)
box3d_pts_3d_velo = calib.project_global_to_lidar(
box3d_pts_3d_global) #(3,8)
corners = box3d_pts_3d_velo #(3,8)
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
ax.plot([prev[0], corner[0]], [prev[1], corner[1]],
color=color,
linewidth=linewidth)
prev = corner
# Draw the sides
for i in range(4):
ax.plot([corners.T[i][0], corners.T[i + 4][0]],
[corners.T[i][1], corners.T[i + 4][1]],
color=colors[2],
linewidth=linewidth)
# Draw front (first 4 corners) and rear (last 4 corners) rectangles(3d)/lines(2d)
draw_rect(corners.T[:4], colors[0])
draw_rect(corners.T[4:], colors[1])
# Draw line indicating the front
center_bottom_forward = np.mean(corners.T[2:4], axis=0)
center_bottom = np.mean(corners.T[[2, 3, 7, 6]], axis=0)
ax.plot([center_bottom[0], center_bottom_forward[0]],
[center_bottom[1], center_bottom_forward[1]],
color=colors[0],
linewidth=linewidth)
if saveto:
plt.savefig(saveto)
else:
plt.show()
def return_image_with_boxes(img, objects, calib, show3d=True):
''' Show image with 2D bounding boxes '''
sensor = 'CAM_FRONT'
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type == 'DontCare': continue
if obj.type != 'Car': continue
cv2.rectangle(img1, (int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)), (0, 255, 0), 2)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
obj, getattr(calib, sensor))
if np.sum(box3d_pts_2d == None) != 1:
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
# Image.fromarray(img1).show()
if show3d:
# Image.fromarray(img2).show()
return img1, img2
else:
return img1
def render_objects(img,
objects,
view=np.eye(4),
colors=((0, 0, 255), (255, 0, 0), (155, 155, 155)),
linewidth=2):
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
cv2.line(img, (int(prev[0]), int(prev[1])),
(int(corner[0]), int(corner[1])),
color,
linewidth=linewidth)
prev = corner
for obj in objects:
if obj.type == 'DontCare': continue
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, view)
# Draw the sides
for i in range(4):
cv2.line(
img, (int(corners_2d.T[i][0]), int(corners_2d.T[i][1])),
(int(corners_2d.T[i + 4][0]), int(corners_2d.T[i + 4][1])),
colors[c][::-1], linewidth)
# Draw front (first 4 corners) and rear (last 4 corners) rectangles(3d)/lines(2d)
draw_rect(corners_2d.T[:4], colors[c][::-1])
draw_rect(corners_2d.T[4:], colors[c][::-1])
corners_2d = box3d_pts_2d
# Draw line indicating the front
center_bottom_forward = np.mean(corners_2d.T[0:2], axis=0)
center_bottom = np.mean(corners_2d.T[[0, 1, 2, 3]], axis=0)
# center_bottom_forward = np.mean(corners_2d.T[2:4], axis=0)
# center_bottom = np.mean(corners_2d.T[[2, 3, 7, 6]], axis=0)
cv2.line(
img, (int(center_bottom[0]), int(center_bottom[1])),
(int(center_bottom_forward[0]), int(center_bottom_forward[1])),
colors[0][::-1], linewidth)
def print_npts_statistics_v2(idx_filename,
type_whitelist=['Car'],
split='train',
sensor='CAM_FRONT'):
dataset = nuscenes2kitti_object(os.path.join(ROOT_DIR,
'dataset/nuScenes2KITTI'),
split=split,
sensor_list=['CAM_FRONT'])
N = 10000
x = np.zeros(N)
data_idx_list = [int(line.rstrip()) for line in open(idx_filename)]
for data_idx in tqdm(data_idx_list):
calib = dataset.get_calibration(data_idx) # 3 by 4 matrix
pc_velo = dataset.get_lidar(data_idx)
pc_global = calib.project_lidar_to_global(pc_velo[:, 0:3].T)
pc_rect = calib.project_global_to_cam(pc_global, sensor).T
objects = dataset.get_label_objects(sensor, data_idx)
for obj_idx in range(len(objects)):
obj = objects[obj_idx]
if obj.type not in type_whitelist: continue
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
objects[obj_idx], getattr(calib, sensor))
pts_in_box3d, _ = extract_pc_in_box3d(pc_rect, box3d_pts_3d)
npts = len(pts_in_box3d)
if npts >= 10000: continue
x[npts] += 1
tot = np.sum(x)
prefix = np.zeros(N)
prefix[0] = x[0]
now = 0.1
step = 0.1
y = []
for i in range(1, N):
prefix[i] = prefix[i - 1] + x[i]
if prefix[i] > now * tot:
y.append(i)
now += step
print(y)
np.savetxt('kitti_npts_info_v2', (y), fmt='%d')
def extract_frustum_data(idx_filename,
split,
sensor,
output_filename,
viz=False,
perturb_box2d=False,
augmentX=1,
type_whitelist=['Car']):
''' Extract point clouds and corresponding annotations in frustums
defined generated from 2D bounding boxes
Lidar points and 3d boxes are in *rect camera* coord system
(as that in 3d box label files)
Input:
idx_filename: string, each line of the file is a sample ID
split: string, either trianing or testing
output_filename: string, the name for output .pickle file
viz: bool, whether to visualize extracted data
perturb_box2d: bool, whether to perturb the box2d
(used for data augmentation in train set)
augmentX: scalar, how many augmentations to have for each 2D box.
type_whitelist: a list of strings, object types we are interested in.
Output:
None (will write a .pickle file to the disk)
'''
dataset = nuscenes2kitti_object(
os.path.join(ROOT_DIR, 'dataset/nuScenes2KITTI'), split)
data_idx_list = [int(line.rstrip()) for line in open(idx_filename)]
id_list = [] # int number
box2d_list = [] # [xmin,ymin,xmax,ymax]
box3d_list = [] # (8,3) array in rect camera coord
input_list = [] # channel number = 4, xyz,intensity in rect camera coord
label_list = [] # 1 for roi object, 0 for clutter
type_list = [] # string e.g. Car
heading_list = [] # ry (along y-axis in rect camera coord) radius of
# (cont.) clockwise angle from positive x axis in velo coord.
box3d_size_list = [] # array of l,w,h
frustum_angle_list = [] # angle of 2d box center from pos x-axis
pos_cnt = 0.0
all_cnt = 0.0
time_get_fov = 0.0
for data_idx in data_idx_list:
print('------------- ', data_idx)
calib = dataset.get_calibration(data_idx)
objects = dataset.get_label_objects(sensor, data_idx)
pc_velo = dataset.get_lidar(data_idx)
pc_cam = np.zeros_like(pc_velo)
pc_global = calib.project_lidar_to_global(pc_velo.T[0:3, :])
pc_cam[:, 0:3] = calib.project_global_to_cam(pc_global, sensor).T
pc_cam[:, 3] = pc_velo[:, 3]
img = dataset.get_image(sensor, data_idx)
img_height, img_width, img_channel = img.shape
time1 = time.perf_counter()
_, pc_image_coord, img_fov_inds = \
get_lidar_in_image_fov(pc_velo[:, 0:3],calib, sensor,
0, 0, img_width, img_height, True)
time_get_fov += (time.perf_counter() - time1)
for obj_idx in range(len(objects)):
if objects[obj_idx].type not in type_whitelist: continue
# 2D BOX: Get pts rect backprojected
box2d = objects[obj_idx].box2d
for _ in range(augmentX):
# Augment data by box2d perturbation
if perturb_box2d:
xmin, ymin, xmax, ymax = random_shift_box2d(box2d)
#print(box2d)
#print(xmin, ymin, xmax, ymax)
else:
xmin, ymin, xmax, ymax = box2d
box_fov_inds = (pc_image_coord[:, 0] < xmax) & \
(pc_image_coord[:, 0] >= xmin) & \
(pc_image_coord[:, 1] < ymax) & \
(pc_image_coord[:, 1] >= ymin)
box_fov_inds = box_fov_inds & img_fov_inds
pc_in_box_fov = pc_cam[box_fov_inds, :] # (1607, 4)
# Get frustum angle (according to center pixel in 2D BOX)
box2d_center = np.array([(xmin + xmax) / 2.0,
(ymin + ymax) / 2.0])
uvdepth = np.zeros((1, 3))
uvdepth[0, 0:2] = box2d_center
uvdepth[0, 2] = 20 # some random depth
box2d_center_cam = calib.project_image_to_cam(uvdepth, sensor)
#box2d_center_rect = calib.project_image_to_rect(uvdepth.T).T
frustum_angle = -1 * np.arctan2(box2d_center_cam[0, 2],
box2d_center_cam[0, 0])
# 3D BOX: Get pts velo in 3d box
obj = objects[obj_idx]
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
obj, getattr(calib, sensor)) # (8, 2)(8, 3)
_, inds = extract_pc_in_box3d(pc_in_box_fov,
box3d_pts_3d) # (375, 4)(1607,)
label = np.zeros((pc_in_box_fov.shape[0])) # (1607,)
label[inds] = 1
# Get 3D BOX heading
heading_angle = obj.ry # 0.01
# Get 3D BOX size
box3d_size = np.array([obj.l, obj.w,
obj.h]) # array([1.2 , 0.48, 1.89])
# Reject too far away object or object without points
if ymax - ymin < 25 or np.sum(label) == 0:
continue
id_list.append(data_idx)
box2d_list.append(np.array([xmin, ymin, xmax, ymax]))
box3d_list.append(box3d_pts_3d)
input_list.append(pc_in_box_fov)
label_list.append(label)
type_list.append(objects[obj_idx].type)
heading_list.append(heading_angle)
box3d_size_list.append(box3d_size)
frustum_angle_list.append(frustum_angle)
# collect statistics
pos_cnt += np.sum(label)
all_cnt += pc_in_box_fov.shape[0]
print('Average pos ratio: %f' % (pos_cnt / float(all_cnt)))
print('Average npoints: %f' % (float(all_cnt) / len(id_list)))
print('Average time of get_lidar_in_image_fov: %.2fms' %
(time_get_fov * 1000 / len(id_list)))
with open(output_filename, 'wb') as fp:
pickle.dump(id_list, fp)
pickle.dump(box2d_list, fp)
pickle.dump(box3d_list, fp)
pickle.dump(input_list, fp)
pickle.dump(label_list, fp)
pickle.dump(type_list, fp)
pickle.dump(heading_list, fp)
pickle.dump(box3d_size_list, fp)
pickle.dump(frustum_angle_list, fp)
if viz:
import mayavi.mlab as mlab
for i in range(10):
p1 = input_list[i]
seg = label_list[i]
fig = mlab.figure(figure=None,
bgcolor=(0.4, 0.4, 0.4),
fgcolor=None,
engine=None,
size=(500, 500))
mlab.points3d(p1[:, 0],
p1[:, 1],
p1[:, 2],
seg,
mode='point',
colormap='gnuplot',
scale_factor=1,
figure=fig)
fig = mlab.figure(figure=None,
bgcolor=(0.4, 0.4, 0.4),
fgcolor=None,
engine=None,
size=(500, 500))
mlab.points3d(p1[:, 2],
-p1[:, 0],
-p1[:, 1],
seg,
mode='point',
colormap='gnuplot',
scale_factor=1,
figure=fig)
raw_input()
def demo(data_idx=0, obj_idx=-1):
sensor = 'CAM_FRONT'
import mayavi.mlab as mlab
from viz_util import draw_lidar_simple, draw_gt_boxes3d
dataset = nuscenes2kitti_object(
os.path.join(ROOT_DIR, 'dataset/nuScenes2KITTI'))
# Load data from dataset
objects = dataset.get_label_objects(
sensor, data_idx) # objects = [Object3d(line) for line in lines]
for i, obj in enumerate(objects):
print('obj %d' % (i))
objects[obj_idx].print_object()
calib = dataset.get_calibration(
data_idx) # utils.Calibration(calib_filename)
box2d = objects[obj_idx].box2d
xmin, ymin, xmax, ymax = box2d
box2d_center = np.array([(xmin + xmax) / 2.0, (ymin + ymax) / 2.0])
uvdepth = np.zeros((1, 3))
uvdepth[0, 0:2] = box2d_center
uvdepth[0, 2] = 20 # some random depth
#box2d_center_rect = calib.project_image_to_rect(uvdepth)
#frustum_angle = -1 * np.arctan2(box2d_center_rect[0, 2],
# box2d_center_rect[0, 0])
#print('frustum_angle:', frustum_angle)
img = dataset.get_image(sensor, data_idx) # (370, 1224, 3)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
img_height, img_width, img_channel = img.shape
print(('Image shape: ', img.shape))
print(dataset.get_lidar(data_idx).shape)
pc_velo = dataset.get_lidar(data_idx)[:, 0:3] # (115384, 3)
calib = dataset.get_calibration(
data_idx) # utils.Calibration(calib_filename)
# 1.Draw lidar with boxes in LIDAR_TOP coord
print(' -------- LiDAR points in LIDAR_TOP coordination --------')
print('pc_velo.shape:', pc_velo.shape)
print('pc_velo[:10,:]:', pc_velo[:10, :])
##view = np.eye(4)
##pc_velo[:, :3] = utils.view_points(pc_velo[:, :3].T, view, normalize=False).T
##pc_rect = calib.project_velo_to_rect(pc_velo)
#fig = draw_lidar_simple(pc_velo)
show_lidar_with_boxes(pc_velo, objects, calib, sensor, False, img_width,
img_height)
raw_input()
# 2.Draw frustum lidar with boxes in LIDAR_TOP coord
print(
' -------- LiDAR points and 3D boxes in velodyne coordinate --------')
#show_lidar_with_boxes(pc_velo, objects, calib)
show_lidar_with_boxes(pc_velo.copy(), objects, calib, sensor, True,
img_width, img_height)
raw_input()
# 3.Draw 2d and 3d boxes on CAM_FRONT image
print(' -------- 2D/3D bounding boxes in images --------')
show_image_with_boxes(img, objects, calib, sensor)
raw_input()
print(
' -------- render LiDAR points (and 3D boxes) in LIDAR_TOP coordinate --------'
)
render_lidar_bev(pc_velo, objects, calib, sensor)
raw_input()
# Visualize LiDAR points on images
print(' -------- LiDAR points projected to image plane --------')
show_lidar_on_image(pc_velo, img.copy(), calib, sensor, img_width,
img_height) #pc_velo:(n,3)
raw_input()
# Show LiDAR points that are in the 3d box
print(' -------- LiDAR points in a 3D bounding box --------')
for obj_idx, obj in enumerate(objects):
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
objects[obj_idx], np.eye(4))
box3d_pts_3d_global = calib.project_cam_to_global(
box3d_pts_3d.T, sensor) # (3,8)
box3d_pts_3d_velo = calib.project_global_to_lidar(
box3d_pts_3d_global) # (3,8)
box3droi_pc_velo, _ = extract_pc_in_box3d(pc_velo, box3d_pts_3d_velo.T)
print(('Number of points in 3d box: ', box3droi_pc_velo.shape[0]))
fig = mlab.figure(figure=None,
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1000, 500))
utils.draw_nusc_lidar(box3droi_pc_velo, fig=fig)
draw_gt_boxes3d([box3d_pts_3d_velo.T], fig=fig)
mlab.show(1)
raw_input()
# UVDepth Image and its backprojection to point clouds
print(' -------- LiDAR points in a frustum --------')
imgfov_pc_velo, pts_2d, fov_inds = get_lidar_in_image_fov(
pc_velo, calib, sensor, 0, 0, img_width, img_height, True)
imgfov_pts_2d = pts_2d[fov_inds, :] #(n, 3)
imgfov_pc_global = calib.project_lidar_to_global(imgfov_pc_velo.T)
imgfov_pc_cam = calib.project_global_to_cam(imgfov_pc_global,
sensor) #(3,n)
#cameraUVDepth = utils.view_points(imgfov_pc_cam[:3, :], getattr(calib,sensor), normalize=True)#(3,3067)
#cameraUVDepth = cameraUVDepth#(3067, 3)
#ipdb.set_trace()
#cameraUVDepth = np.zeros_like(imgfov_pc_cam)
#cameraUVDepth[:,0:2] = imgfov_pts_2d[:, 0:2]
#cameraUVDepth[:,2] = imgfov_pc_cam[:,2]
# miss intrisic
# cameraUVDepth = imgfov_pc_cam
# backprojected_pc_cam = cameraUVDepth
#consider intrinsic
print('imgfov_pc_cam.shape:', imgfov_pc_cam.shape)
print('imgfov_pc_cam[:,0:5].T:\n', imgfov_pc_cam[:, 0:5].T)
cameraUVDepth = calib.project_cam_to_image(imgfov_pc_cam, sensor) #(3,n)
cameraUVDepth[2, :] = imgfov_pc_cam[2, :]
print('cameraUVDepth.shape:', cameraUVDepth.shape)
print('cameraUVDepth[:,0:5].T:\n', cameraUVDepth[:, 0:5].T)
backprojected_pc_cam = calib.project_image_to_cam(cameraUVDepth,
sensor) #(3,n)
print('backprojected_pc_cam.shape:', backprojected_pc_cam.shape)
print('backprojected_pc_cam[:,0:5].T\n:', backprojected_pc_cam[:, 0:5].T)
print('error:')
print(np.mean(backprojected_pc_cam - imgfov_pc_cam, axis=1))
# Show that the points are exactly the same
backprojected_pc_global = calib.project_cam_to_global(
backprojected_pc_cam, sensor) #(3,n)
backprojected_pc_velo = calib.project_global_to_lidar(
backprojected_pc_global).T #(n,3)
print('imgfov_pc_velo.shape:', imgfov_pc_velo.shape)
print(imgfov_pc_velo[0:5, :])
print('backprojected_pc_velo.shape:', backprojected_pc_velo.shape)
print(backprojected_pc_velo[0:5, :])
print('error:')
print(np.mean(backprojected_pc_velo - imgfov_pc_velo, axis=0))
fig = mlab.figure(figure=None,
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1000, 500))
utils.draw_nusc_lidar(backprojected_pc_velo, fig=fig)
raw_input()
# Only display those points that fall into 2d box
print(' -------- LiDAR points in a frustum from a 2D box --------')
xmin,ymin,xmax,ymax = \
objects[obj_idx].xmin, objects[obj_idx].ymin, objects[obj_idx].xmax, objects[obj_idx].ymax
boxfov_pc_velo = \
get_lidar_in_image_fov(pc_velo, calib, sensor, xmin, ymin, xmax, ymax)
print(('2d box FOV point num: ', boxfov_pc_velo.shape[0]))
fig = mlab.figure(figure=None,
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1000, 500))
utils.draw_nusc_lidar(boxfov_pc_velo, fig=fig)
mlab.show(1)
raw_input()
def vis_pred(split='training',
sensor_list=['CAM_FRONT'],
type_whitelist=['Car'],
vis_pred_path=None):
import mayavi.mlab as mlab
from viz_util import draw_lidar_simple # , draw_gt_boxes3d
dataset = nuscenes2kitti_object(os.path.join(ROOT_DIR,
'dataset/nuScenes2KITTI'),
split=split)
type2color = {}
for i, x in enumerate(type_whitelist):
type2color[x] = i
print('type_whitlist:', type_whitelist)
print('Sensor_list:', sensor_list)
linewidth = 2
colors = ((0, 0, 255), (255, 0, 0), (155, 155, 155))
print('linewidth={}'.format(linewidth))
'''
-v1.0-mini
-calib
-image_CAM_FRONT
-image_CAM_...
...
-label_CAM_FRONT
-label_CAM_...
...
-calib
_LIDAR_TOP
-vis
-vis2d_CAM_FRONT
-vis2d_CAM_...
...
-vis3d_CAM_FRONT
-vis3d_CAM_...
...
'''
for present_sensor in sensor_list:
save2ddir = os.path.join(ROOT_DIR, 'dataset/nuScenes2KITTI', split,
'vis_pred', 'vis2d_' + present_sensor)
save3ddir = os.path.join(ROOT_DIR, 'dataset/nuScenes2KITTI', split,
'vis_pred', 'vis3d_' + present_sensor)
if os.path.isdir(save2ddir) == True:
print('previous save2ddir found. deleting...')
shutil.rmtree(save2ddir)
os.makedirs(save2ddir)
if os.path.isdir(save3ddir) == True:
print('previous save3ddir found. deleting...')
shutil.rmtree(save3ddir)
os.makedirs(save3ddir)
print('Saving images with 2d boxes to {}...'.format(save2ddir))
print('Saving images with 3d boxes to {}...'.format(save3ddir))
filename_list = glob.glob(os.path.join(vis_pred_path, "*.txt"))
for label_filename in tqdm(filename_list):
# Load data from dataset
data_idx = int(label_filename[-10:-4])
objects = utils.read_label(label_filename)
# objects[0].print_object()
img = dataset.get_image(present_sensor, data_idx)
# img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
# print(('Image shape: ', img.shape))
pc_velo = dataset.get_lidar(data_idx)[:, 0:3]
calib = dataset.get_calibration(data_idx)
''' Show image with 2D bounding boxes '''
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type == 'DontCare': continue
# if obj.type not in type2color.keys(): continue
# c = type2color[obj.type]
c = 0
cv2.rectangle(img1, (int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)), colors[c][::-1],
2)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(
obj, getattr(calib, present_sensor))
# img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
def draw_rect(selected_corners, color):
prev = selected_corners[-1]
for corner in selected_corners:
cv2.line(img2, (int(prev[0]), int(prev[1])),
(int(corner[0]), int(corner[1])), color,
linewidth)
prev = corner
corners_2d = box3d_pts_2d # (8,2)
# Draw the sides
for i in range(4):
cv2.line(
img2, (int(corners_2d[i][0]), int(corners_2d[i][1])),
(int(corners_2d[i + 4][0]), int(corners_2d[i + 4][1])),
colors[c][::-1], linewidth)
# Draw front (first 4 corners) and rear (last 4 corners) rectangles(3d)/lines(2d)
draw_rect(corners_2d[:4], colors[c][::-1])
draw_rect(corners_2d[4:], colors[c][::-1])
# Draw line indicating the front
center_bottom_forward = np.mean(corners_2d[0:2], axis=0)
center_bottom = np.mean(corners_2d[[0, 1, 2, 3]], axis=0)
# center_bottom_forward = np.mean(corners_2d.T[2:4], axis=0)
# center_bottom = np.mean(corners_2d.T[[2, 3, 7, 6]], axis=0)
cv2.line(img2, (int(center_bottom[0]), int(center_bottom[1])),
(int(center_bottom_forward[0]),
int(center_bottom_forward[1])), colors[c][::-1],
linewidth)
cv2.imwrite(
os.path.join(save2ddir,
str(data_idx).zfill(6) + '.jpg'), img1)
cv2.imwrite(
os.path.join(save3ddir,
str(data_idx).zfill(6) + '.jpg'), img2)
def demo_object(data_idx=11, object_idx=0):
import mayavi.mlab as mlab
from viz_util import draw_lidar, draw_lidar_simple, draw_gt_boxes3d
def draw_3d_object(pc, color=None):
''' Draw lidar points. simplest set up. '''
fig = mlab.figure(figure=None,
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1600, 1000))
if color is None:
color = (pc[:, 2] - np.min(pc[:, 2])) / (np.max(pc[:, 2]) -
np.min(pc[:, 2]))
# draw points
#nodes = mlab.points3d(pc[:, 0], pc[:, 1], pc[:, 2], colormap='gnuplot', scale_factor=0.04,
# figure=fig)
#nodes.mlab_source.dataset.point_data.scalars = color
pts = mlab.pipeline.scalar_scatter(pc[:, 0], pc[:, 1], pc[:, 2])
pts.add_attribute(color, 'colors')
pts.data.point_data.set_active_scalars('colors')
g = mlab.pipeline.glyph(pts)
g.glyph.glyph.scale_factor = 0.05 # set scaling for all the points
g.glyph.scale_mode = 'data_scaling_off' # make all the points same size
# draw origin
mlab.points3d(0,
0,
0,
color=(1, 1, 1),
mode='sphere',
scale_factor=0.2)
# draw axis
axes = np.array([
[2., 0., 0., 0.],
[0., 2., 0., 0.],
[0., 0., 2., 0.],
],
dtype=np.float64)
mlab.plot3d([0, axes[0, 0]], [0, axes[0, 1]], [0, axes[0, 2]],
color=(1, 0, 0),
tube_radius=None,
figure=fig)
mlab.plot3d([0, axes[1, 0]], [0, axes[1, 1]], [0, axes[1, 2]],
color=(0, 1, 0),
tube_radius=None,
figure=fig)
mlab.plot3d([0, axes[2, 0]], [0, axes[2, 1]], [0, axes[2, 2]],
color=(0, 0, 1),
tube_radius=None,
figure=fig)
mlab.view(azimuth=180,
elevation=70,
focalpoint=[12.0909996, -1.04700089, -2.03249991],
distance=62.0,
figure=fig)
return fig
dataset = nuscenes2kitti_object(
os.path.join(ROOT_DIR, 'dataset/nuScenes2KITTI'))
sensor = 'CAM_FRONT'
objects = dataset.get_label_objects(sensor, data_idx)
obj = objects[object_idx]
obj.print_object()
calib = dataset.get_calibration(
data_idx) #utils.Calibration(calib_filename)
box2d = obj.box2d
xmin, ymin, xmax, ymax = box2d
cx, cy = (xmin + xmax) / 2, (ymin + ymax) / 2
w, l = xmax - xmin, ymax - ymin
# box3d
x, y, z = obj.t
# show 3d
pc_velo = dataset.get_lidar(data_idx)[:, 0:3]
pc_global = calib.project_lidar_to_global(pc_velo.T)
pc_rect = calib.project_global_to_cam(pc_global, sensor).T
pc_norm = pc_rect - obj.t
keep = []
for i in range(len(pc_norm)):
if np.sum(pc_norm[i]**2) < 4:
keep.append(i)
pc_keep = pc_norm[keep, :]
pc_keep[:, 1] *= -1
pc_keep = pc_keep[:, [0, 2, 1]]
print("num_points:%d" % (pc_keep.shape[0]))
fig = draw_3d_object(pc_keep)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(objects[object_idx],
getattr(calib, sensor))
box3d_pts_3d -= obj.t
box3d_pts_3d[:, 1] *= -1
box3d_pts_3d = box3d_pts_3d[:, [0, 2, 1]]
draw_gt_boxes3d([box3d_pts_3d], fig=fig, draw_text=False)
input()
