def show_image_with_boxes(img, objects, calib, show3d=True):
''' 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
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, calib.P)
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
Image.fromarray(img1).show()
if show3d:
Image.fromarray(img2).show()
def show_lidar_with_boxes(pc_velo,
objects,
calib,
img_fov=False,
img_width=None,
img_height=None,
fig=None):
''' Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) '''
if not fig:
fig = mlab.figure(figure="KITTI_POINT_CLOUD",
bgcolor=(0, 0, 0),
fgcolor=None,
engine=None,
size=(1250, 550))
if img_fov:
pc_velo = get_lidar_in_image_fov(pc_velo, calib, 0, 0, img_width,
img_height)
draw_lidar(pc_velo, fig1=fig)
for obj in objects:
if obj.type == 'DontCare': continue
# Draw 3d bounding box
box3d_pts_2d, box3d_pts_3d = kitti_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 = kitti_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,
color=(0, 1, 1),
line_width=2,
draw_text=False)
mlab.plot3d([x1, x2], [y1, y2], [z1, z2],
color=(0.5, 0.5, 0.5),
tube_radius=None,
line_width=1,
figure=fig)
mlab.view(distance=90)
def show_image_with_boxes(img, objects, calib, show3d=False):
''' Show image with 2D bounding boxes '''
img2 = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type == 'DontCare': continue
#cv2.rectangle(img2, (int(obj.xmin),int(obj.ymin)),
# (int(obj.xmax),int(obj.ymax)), (0,255,0), 2)
box3d_pts_2d, box3d_pts_3d = kitti_utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is not None:
img2 = kitti_utils.draw_projected_box3d(img2, box3d_pts_2d,
cnf.colors[obj.cls_id])
if show3d:
cv2.imshow("img", img2)
return img2
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 mayavi_vis.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 predictions_to_kitti_format(img_detections, calib, img_shape_2d, img_size, RGB_Map=None):
predictions = np.zeros([50, 7], dtype=np.float32)
count = 0
for detections in img_detections:
if detections is None:
continue
# Rescale boxes to original image
for x, y, w, l, im, re, conf, cls_conf, cls_pred in detections:
yaw = np.arctan2(im, re)
predictions[count, :] = cls_pred, x/img_size, y/img_size, w/img_size, l/img_size, im, re
count += 1
predictions = bev_utils.inverse_yolo_target(predictions, cnf.boundary)
if predictions.shape[0]:
predictions[:, 1:] = aug_utils.lidar_to_camera_box(predictions[:, 1:], calib.V2C, calib.R0, calib.P)
objects_new = []
corners3d = []
for index, l in enumerate(predictions):
str = "Pedestrian"
if l[0] == 0:str="Car"
elif l[0] == 1:str="Pedestrian"
elif l[0] == 2: str="Cyclist"
else:str = "DontCare"
line = '%s -1 -1 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0' % str
obj = kitti_utils.Object3d(line)
obj.t = l[1:4]
obj.h,obj.w,obj.l = l[4:7]
obj.ry = np.arctan2(math.sin(l[7]), math.cos(l[7]))
_, corners_3d = kitti_utils.compute_box_3d(obj, calib.P)
corners3d.append(corners_3d)
objects_new.append(obj)
if len(corners3d) > 0:
corners3d = np.array(corners3d)
img_boxes, _ = calib.corners3d_to_img_boxes(corners3d)
img_boxes[:, 0] = np.clip(img_boxes[:, 0], 0, img_shape_2d[1] - 1)
img_boxes[:, 1] = np.clip(img_boxes[:, 1], 0, img_shape_2d[0] - 1)
img_boxes[:, 2] = np.clip(img_boxes[:, 2], 0, img_shape_2d[1] - 1)
img_boxes[:, 3] = np.clip(img_boxes[:, 3], 0, img_shape_2d[0] - 1)
img_boxes_w = img_boxes[:, 2] - img_boxes[:, 0]
img_boxes_h = img_boxes[:, 3] - img_boxes[:, 1]
box_valid_mask = np.logical_and(img_boxes_w < img_shape_2d[1] * 0.8, img_boxes_h < img_shape_2d[0] * 0.8)
for i, obj in enumerate(objects_new):
x, z, ry = obj.t[0], obj.t[2], obj.ry
beta = np.arctan2(z, x)
alpha = -np.sign(beta) * np.pi / 2 + beta + ry
obj.alpha = alpha
obj.box2d = img_boxes[i, :]
if RGB_Map is not None:
labels, noObjectLabels = kitti_utils.read_labels_for_bevbox(objects_new)
if not noObjectLabels:
labels[:, 1:] = aug_utils.camera_to_lidar_box(labels[:, 1:], calib.V2C, calib.R0, calib.P) # convert rect cam to velo cord
target = bev_utils.build_yolo_target(labels)
utils.draw_box_in_bev(RGB_Map, target)
return objects_new
def demo():
import mayavi.mlab as mlab
from mayavi_vis.viz_util import draw_lidar, draw_lidar_simple, draw_gt_boxes3d
dataset = kitti_object(os.path.join(ROOT_DIR, 'Data/KITTI/object'))
data_idx = 692
# Load data from dataset
objects = dataset.get_label_objects(data_idx)
objects[0].print_object()
img = dataset.get_image(data_idx)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
img_height, img_width, img_channel = img.shape
print(('Image shape: ', img.shape))
pc_velo = dataset.get_lidar(data_idx)[:,0:3]
calib = dataset.get_calibration(data_idx)
## Draw lidar in rect camera coord
#print(' -------- LiDAR points in rect camera coordination --------')
#pc_rect = calib.project_velo_to_rect(pc_velo)
#fig = draw_lidar_simple(pc_rect)
#raw_input()
# Draw 2d and 3d boxes on image
print(' -------- 2D/3D bounding boxes in images --------')
show_image_with_boxes(img, objects, calib)
raw_input()
# Show all LiDAR points. Draw 3d box in LiDAR point cloud
print(' -------- LiDAR points and 3D boxes in velodyne coordinate --------')
#show_lidar_with_boxes(pc_velo, objects, calib)
#raw_input()
show_lidar_with_boxes(pc_velo, objects, calib, True, img_width, img_height)
raw_input()
# Visualize LiDAR points on images
print(' -------- LiDAR points projected to image plane --------')
show_lidar_on_image(pc_velo, img, calib, img_width, img_height)
raw_input()
# Show LiDAR points that are in the 3d box
print(' -------- LiDAR points in a 3D bounding box --------')
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(objects[0], calib.P)
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
box3droi_pc_velo, _ = extract_pc_in_box3d(pc_velo, box3d_pts_3d_velo)
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))
draw_lidar(box3droi_pc_velo, fig=fig)
draw_gt_boxes3d([box3d_pts_3d_velo], fig=fig)
mlab.show(1)
raw_input()
# UVDepth Image and its backprojection to point clouds
print(' -------- LiDAR points in a frustum from a 2D box --------')
imgfov_pc_velo, pts_2d, fov_inds = get_lidar_in_image_fov(pc_velo,
calib, 0, 0, img_width, img_height, True)
imgfov_pts_2d = pts_2d[fov_inds,:]
imgfov_pc_rect = calib.project_velo_to_rect(imgfov_pc_velo)
cameraUVDepth = np.zeros_like(imgfov_pc_rect)
cameraUVDepth[:,0:2] = imgfov_pts_2d
cameraUVDepth[:,2] = imgfov_pc_rect[:,2]
# Show that the points are exactly the same
backprojected_pc_velo = calib.project_image_to_velo(cameraUVDepth)
print(imgfov_pc_velo[0:20])
print(backprojected_pc_velo[0:20])
fig = mlab.figure(figure=None, bgcolor=(0,0,0),
fgcolor=None, engine=None, size=(1000, 500))
draw_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[0].xmin, objects[0].ymin, objects[0].xmax, objects[0].ymax
boxfov_pc_velo = \
get_lidar_in_image_fov(pc_velo, calib, 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))
draw_lidar(boxfov_pc_velo, fig=fig)
mlab.show(1)
raw_input()
def extract_frustum_data(idx_filename, split, 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 = kitti_object(os.path.join(ROOT_DIR,'Data/KITTI/object'), 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
all_cnt = 0
for data_idx in data_idx_list:
print('------------- ', data_idx)
calib = dataset.get_calibration(data_idx) # 3 by 4 matrix
objects = dataset.get_label_objects(data_idx)
pc_velo = dataset.get_lidar(data_idx)
pc_rect = np.zeros_like(pc_velo)
pc_rect[:,0:3] = calib.project_velo_to_rect(pc_velo[:,0:3])
pc_rect[:,3] = pc_velo[:,3]
img = dataset.get_image(data_idx)
img_height, img_width, img_channel = img.shape
_, pc_image_coord, img_fov_inds = get_lidar_in_image_fov(pc_velo[:,0:3],
calib, 0, 0, img_width, img_height, True)
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_rect[box_fov_inds,:]
# 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_rect = calib.project_image_to_rect(uvdepth)
frustum_angle = -1 * np.arctan2(box2d_center_rect[0,2],
box2d_center_rect[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, calib.P)
_,inds = extract_pc_in_box3d(pc_in_box_fov, box3d_pts_3d)
label = np.zeros((pc_in_box_fov.shape[0]))
label[inds] = 1
# Get 3D BOX heading
heading_angle = obj.ry
# Get 3D BOX size
box3d_size = np.array([obj.l, obj.w, obj.h])
# 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)))
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()