def get_proposal_from_label(self, label, calib):
'''construct proposal from label'''
_, corners_3d = utils.compute_box_3d(label, calib.P)
bev_box = corners_3d[:4, [0, 2]]
box_l = label.l
box_w = label.w
box_h = label.h
# Rotate to nearest multiple of 90 degrees
box_ry = label.ry
half_pi = np.pi / 2
box_ry = np.abs(np.round(box_ry / half_pi) * half_pi)
cos_ry = np.abs(np.cos(box_ry))
sin_ry = np.abs(np.sin(box_ry))
w = box_l * cos_ry + box_w * sin_ry
l = box_w * cos_ry + box_l * sin_ry
h = box_h
prop_obj = ProposalObject(
list(label.t) + [l, h, w, box_ry], 1, label.type, None)
_, corners_prop = utils.compute_box_3d(prop_obj, calib.P)
bev_box_prop = corners_prop[:4, [0, 2]]
prop_poly = Polygon(bev_box_prop)
gt_poly = Polygon(bev_box)
intersection = prop_poly.intersection(gt_poly)
iou = intersection.area / (prop_poly.area + gt_poly.area -
intersection.area)
return prop_obj, iou
def show_image_with_boxes(img, calib, objects=[], objects_pred=[]):
""" Show image with 2D bounding boxes """
img_2d = np.copy(img) # for 2d bbox
img_3d = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type not in care_types:
continue
cv2.rectangle(
img_2d,
(int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)),
(0, 255, 0),
2,
)
box3d_pts_2d, _ = utils.compute_box_3d(obj, calib.P)
img_3d = utils.draw_projected_box3d(img_3d, box3d_pts_2d)
for obj in objects_pred:
if obj.type not in care_types:
continue
cv2.rectangle(
img_2d,
(int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)),
(0, 0, 255),
2,
)
box3d_pts_2d, _ = utils.compute_box_3d(obj, calib.P)
img_3d = utils.draw_projected_box3d(img_3d,
box3d_pts_2d,
color=(0, 0, 255))
return img_2d, img_3d
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--kitti_path', help='Path to Kitti Object Data')
parser.add_argument('--split', help='Which split to calculate, train/val')
args = parser.parse_args()
BASE_DIR = os.path.dirname(os.path.abspath(__file__))
if args.split == 'train':
idx_filename = os.path.join(BASE_DIR, 'image_sets/train.txt')
elif args.split == 'val':
idx_filename = os.path.join(BASE_DIR, 'image_sets/val.txt')
elif args.split == 'trainval':
idx_filename = os.path.join(BASE_DIR, 'image_sets/trainval.txt')
else:
raise Exception('unknown split %s' % args.split)
type_whitelist = ['Car', 'Pedestrian', 'Cyclist']
dataset = kitti_object_avod(args.kitti_path, 'training')
data_idx_list = [int(line.rstrip()) for line in open(idx_filename)]
total_obj = defaultdict(int)
recall_num = defaultdict(int)
for i, data_idx in enumerate(data_idx_list):
print('------------- ', data_idx)
calib = dataset.get_calibration(data_idx) # 3 by 4 matrix
# ground truth
objects = dataset.get_label_objects(data_idx)
proposals = dataset.get_proposals(data_idx, rpn_score_threshold=0.1, nms_iou_thres=0.8)
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]
props_bev = []
for prop in proposals:
_, prop_corners_3d = utils.compute_box_3d(prop, calib.P)
props_bev.append(Polygon(prop_corners_3d[:4, [0,2]]))
objects = filter(lambda obj: obj.type in type_whitelist, objects)
for obj in objects:
# 0 - easy, 1 - medium, 2 - hard
if obj.difficulty not in [0, 1]:
continue
gt_image_2d, gt_corners_3d = utils.compute_box_3d(obj, calib.P)
if gt_image_2d is None:
print('skip proposal behind camera')
continue
gt_bev = Polygon(gt_corners_3d[:4, [0,2]])
if is_recall(gt_bev, props_bev):
recall_num[obj.type] += 1
total_obj[obj.type] += 1
if i % 5 == 0:
print_statics(recall_num, total_obj, type_whitelist)
print_statics(recall_num, total_obj, type_whitelist)
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 run_on_tracking_image(img, objects, calib):
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
for obj in objects:
track_id = int(obj.track)
color = create_unique_color_uchar(track_id)
rectangle(img1,
obj.xmin,
obj.ymin,
obj.xmax,
obj.ymax,
color,
label=str(track_id))
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is None: continue
img2 = utils.draw_projected_box3d(img2,
box3d_pts_2d,
color,
thickness=2)
left_top = np.amin(box3d_pts_2d, axis=0)
put_track_id(img2,
left_top[0],
left_top[1],
str(track_id),
color,
thickness=2)
# img2 = overlay_class_names(img2, objects, calib)
return img1, img2
def to_detection_objects(id_list, type_list, center_list, \
heading_cls_list, heading_res_list, \
size_cls_list, size_res_list, \
rot_angle_list, score_list, prob_list, proposal_score_list):
objects = {}
for i in range(len(center_list)):
if type_list[i] == 'NonObject':
continue
idx = id_list[i]
#score = score_list[i] + np.log(proposal_score_list[i])
score = score_list[i]
h, w, l, tx, ty, tz, ry = provider.from_prediction_to_label_format(
center_list[i], heading_cls_list[i], heading_res_list[i],
size_cls_list[i], size_res_list[i], rot_angle_list[i])
obj = DetectObject(h, w, l, tx, ty, tz, ry, idx, type_list[i], score)
# cal 2d box from 3d box
calib = get_calibration(idx)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is None:
continue
x1 = np.amin(box3d_pts_2d, axis=0)[0]
y1 = np.amin(box3d_pts_2d, axis=0)[1]
x2 = np.amax(box3d_pts_2d, axis=0)[0]
y2 = np.amax(box3d_pts_2d, axis=0)[1]
obj.box_2d = [x1, y1, x2, y2]
obj.box_3d = box3d_pts_3d
obj.probs = prob_list[i]
obj.proposal_score = proposal_score_list[i]
if idx not in objects:
objects[idx] = []
objects[idx].append(obj)
return objects
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
img3 = 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)
# project
box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
#box3d_pts_32d = utils.box3d_to_rgb_box00(box3d_pts_3d_velo)
box3d_pts_32d = calib.project_velo_to_image(box3d_pts_3d_velo)
img3 = utils.draw_projected_box3d(img3, box3d_pts_32d)
#print("img1:", img1.shape)
Image.fromarray(img1).show()
print("img3:", img3.shape)
Image.fromarray(img3).show()
show3d = False
if show3d:
print("img2:", img2.shape)
Image.fromarray(img2).show()
def show_image_with_boxes(img, objects, calib, show3d=True):
"""
Show image with 2D bounding boxes
:param img: image loaded
:param objects:
:param calib:
:param show3d:
:return:
"""
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
for obj in objects: # process each object in an image
if obj.type == 'DontCare': continue # remove 'DontCare' class
# draw 2d bbox
cv2.rectangle(img1, (int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)), (0, 255, 0), 2)
# calculate 3d bbox for left color cam from P: P2
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
print("********************, ", obj.type)
# print("********************, ", box3d_pts_2d.shape)
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
# Image.fromarray(img1).show()
cv2.imshow('2D bounding box in image',
cv2.cvtColor(img1, cv2.COLOR_BGR2RGB))
if show3d:
# Image.fromarray(img2).show()
cv2.imshow('3D bounding box in image',
cv2.cvtColor(img2, cv2.COLOR_BGR2RGB))
cv2.waitKey(0)
cv2.destroyAllWindows()
def dataset_viz(root_dir, args):
dataset = kitti_object(root_dir)
print(root_dir)
print(len(dataset))
for data_idx in range(len(dataset)):
# Load data from dataset
objects = dataset.get_label_objects(data_idx)
pc_velo = dataset.get_lidar(data_idx)[:, 0:4]
calib = dataset.get_calibration(data_idx)
path = "./out/" + str(data_idx) + '.txt'
f = open(path, 'w+')
#compute bounding box
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)
#box3d_pts_3d_velo = np.array(box3d_pts_3d_velo )
box3d_pts_3d_velo = np.reshape(box3d_pts_3d_velo, (1, -1))
# print(obj.type)
# print(box3d_pts_3d_velo[0][0])
# print(type(box3d_pts_3d_velo))
f.write("%s " % obj.type)
[
f.write("%4.3f " % (box3d_pts_3d_velo[0][i]))
for i in range(len(box3d_pts_3d_velo[0]))
]
f.write("\n")
f.close()
def show_image_with_boxes(img, objects, calib, show3d=True, depth=None):
""" Show image with 2D bounding boxes """
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
img3 = 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)
# project
# box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
# box3d_pts_32d = utils.box3d_to_rgb_box00(box3d_pts_3d_velo)
# box3d_pts_32d = calib.project_velo_to_image(box3d_pts_3d_velo)
# img3 = utils.draw_projected_box3d(img3, box3d_pts_32d)
# print("img1:", img1.shape)
cv2.imshow("2dbox", img1)
# print("img3:",img3.shape)
# Image.fromarray(img3).show()
show3d = True
if show3d:
# print("img2:",img2.shape)
cv2.imshow("3dbox", img2)
if depth is not None:
cv2.imshow("depth", depth)
def show_image_with_boxes(img,
data_idx,
objects,
calib,
show3d=True,
depth=None):
""" Show image with 2D bounding boxes """
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
img3 = 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)
# project
# box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
# box3d_pts_32d = utils.box3d_to_rgb_box00(box3d_pts_3d_velo)
# box3d_pts_32d = calib.project_velo_to_image(box3d_pts_3d_velo)
# img3 = utils.draw_projected_box3d(img3, box3d_pts_32d)
# print("img1:", img1.shape)
# cv2.imshow("2dbox", img1)
cv2.imwrite('results/%s_image_with_boxes.png' % data_idx, img2)
def show_image_with_pred_boxes(img,
objects,
calib,
data_idx,
savepath,
show3d=True,
depth=None):
""" Show image with 2D bounding boxes """
#img1 = cv2.imread(savepath + "/" + str(data_idx) + ".png")
#img2 = cv2.imread(savepath + "/" + str(data_idx) + ".png")
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, _ = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is not None:
img2 = utils.draw_projected_box3d(img2,
box3d_pts_2d,
color=(0, 0, 255))
show3d = True
#cv2.imwrite(savepath + "/" + str(data_idx) + "2d.png", img1)
if show3d:
cv2.imwrite(savepath + "/" + str(data_idx) + "_pred.png", img2)
if depth is not None:
cv2.imwrite("imgs/depth" + ".png", depth)
return img1, img2
def show_image_with_boxes_results(img, objects,results, calib, show3d=False):
''' 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)
for obj in results:
if obj.type=='DontCare':continue
cv2.rectangle(img1, (int(obj.xmin),int(obj.ymin)),
(int(obj.xmax),int(obj.ymax)), (255,0,0), 2)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
#print()
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
Image.fromarray(img1).show()
if show3d:
Image.fromarray(img2).show()
def overlay_class_names(img, objects, calib):
template = "{}: {:.2f}"
for obj in objects:
if obj.type == 'DontCare': continue
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
x, y = box3d_pts_2d[5, 0], box3d_pts_2d[5, 1]
s = template.format(obj.type, obj.score)
cv2.putText(img, s, (int(x), int(y - 5)), cv2.FONT_HERSHEY_SIMPLEX, .5,
(255, 255, 255), 1)
return img
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 viz_sample(self, sample):
import mayavi.mlab as mlab
from viz_util import draw_lidar, draw_lidar_simple, draw_gt_boxes3d
pc_rect = sample['pointcloud']
proposal = ProposalObject(sample['proposal_box'])
proposal.ry = 0
proposal.t = np.zeros((3,))
_, prop_box = utils.compute_box_3d(proposal, sample['calib'])
fig = draw_lidar(pc_rect)
mask = pc_rect[:,5] == 1
fig = draw_lidar(pc_rect[mask], fig=fig, pts_color=(1, 1, 1))
fig = draw_gt_boxes3d([prop_box], fig, draw_text=False, color=(1, 1, 1))
raw_input()
def show_image_with_boxes(img, objects, calib, show3d=True, depth=None):
""" Show image with 2D bounding boxes """
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
#img3 = np.copy(img) # for 3d bbox
global i
print(len(objects))
for obj in objects:
print(obj.type)
if obj.type == "DontCare":
continue
cv2.rectangle(
img1,
(int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)),
(0, 255, 0),
2,
)
# if (obj.type == "car" or obj.type == "Van" or obj.type == "Truck") and (obj.alpha >0 and obj.alpha < 1.57):
if (obj.type == "Car" or obj.type == "Van" or obj.type == "Truck"):
# print("###################")
box3d_pts_2d, _ = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is None:
continue
img2, img_extract, coord = utils.draw_projected_box3d(img2, box3d_pts_2d)
if img_extract is not None and img_extract.shape[0] >= 60 and img_extract.shape[1] >= 40:
# print(img_extract.shape)
# print("writing img and bbox")
cv2.imwrite('./result/{}.jpg'.format(i), img_extract)
np.savetxt('./result/' + str(i), coord)
i += 1
# project
# box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
# box3d_pts_32d = utils.box3d_to_rgb_box00(box3d_pts_3d_velo)
# box3d_pts_32d = calib.project_velo_to_image(box3d_pts_3d_velo)
# img3 = utils.draw_projected_box3d(img3, box3d_pts_32d)
# print("img1:", img1.shape)
# cv2.imshow("2dbox", img1)
# print("img3:",img3.shape)
# Image.fromarray(img3).show()
show3d = True
# if show3d:
# # print("img2:",img2.shape)
# cv2.imshow("3dbox", img2)
# if depth is not None:
# cv2.imshow("depth", depth)
return img1, img2
def group_overlaps(detections, calib, iou_thres=0.01):
bev_boxes = map(lambda obj: utils.compute_box_3d(obj, calib.P)[1][:4, [0,2]], detections)
groups = []
candidates = range(len(detections))
while len(candidates) > 0:
idx = candidates[0]
group = [idx]
for i in candidates[1:]:
if get_iou(bev_boxes[idx], bev_boxes[i]) >= iou_thres:
group.append(i)
for j in group:
candidates.remove(j)
groups.append(map(lambda i: detections[i], group))
return groups
def stat_lidar_with_boxes(pc_velo, objects, calib):
''' Show all LiDAR points.
Draw 3d box in LiDAR point cloud (in velo coord system) '''
#print(('All 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
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)
v_l, v_w, v_h, _ = get_velo_whl(box3d_pts_3d_velo, pc_velo)
print("%.4f %.4f %.4f %s" % (v_w, v_h, v_l, obj.type))
def run_on_opencv_image(img, objects, calib):
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type not in color_map: continue
cv2.rectangle(img1, (int(obj.xmin), int(obj.ymin)),
(int(obj.xmax), int(obj.ymax)), color_map[obj.type], 2)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is None: continue
img2 = utils.draw_projected_box3d(img2,
box3d_pts_2d,
color_map[obj.type],
thickness=2)
# img2 = overlay_class_names(img2, objects, calib)
return img1, img2
def get_proposal_from_label(self, label, calib, roi_features):
'''construct proposal from label'''
_, corners_3d = utils.compute_box_3d(label, calib.P)
# wrap ground truth with box parallel to axis
bev_box = corners_3d[:4, [0,2]]
xmax = bev_box[:, 0].max(axis=0)
ymax = bev_box[:, 1].max(axis=0)
xmin = bev_box[:, 0].min(axis=0)
ymin = bev_box[:, 1].min(axis=0)
l = xmax - xmin
w = ymax - ymin
h = label.h
prop_obj = ProposalObject(list(label.t) + [l, w, h, 0.0], 1, label.type, roi_features)
_, corners_prop = utils.compute_box_3d(prop_obj, calib.P)
bev_box_prop = corners_prop[:4, [0,2]]
prop_poly = Polygon(bev_box_prop)
gt_poly = Polygon(bev_box)
intersection = prop_poly.intersection(gt_poly)
iou = intersection.area / (prop_poly.area + gt_poly.area - intersection.area)
# this iou maybe lower, force to use this for regression
if iou < 0.65:
iou = 0.65
return prop_obj, iou
def process_proposal(prop):
b2d,prop_box_3d = utils.compute_box_3d(prop, calib.P)
prop_box_xy = prop_box_3d[:4, [0,2]]
max_idx, max_iou = find_match_label(prop_box_xy, gt_boxes_xy)
sample = self.get_sample(pc_rect, image, img_seg_map, calib, prop, max_iou, max_idx, objects)
# print(max_iou)
if not sample:
return -1
if sample['class'] == 0:
show_boxes.append(prop_box_3d)
negative_samples.append(sample)
else:
positive_samples.append(sample)
show_boxes.append(prop_box_3d)
recall[max_idx] = 1
return sample['class']
def save_image_with_boxes(img, objects, calib, path_to_save):
""" Show image with 2D bounding boxes """
if objects is None:
print('no objects')
return 0
img1 = np.copy(img) # for 3d bbox
for obj in objects:
if obj.type == "DontCare":
continue
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is None:
continue
img1 = utils.draw_projected_box3d(img1, box3d_pts_2d)
cv2.imwrite(path_to_save, img1)
def draw_boxes(objects, calib, plot_axes):
all_corners = []
for obj in objects:
if hasattr(obj, 'type_label'):
obj.obj_type = obj.type_label
else:
obj.obj_type = obj.type
if not hasattr(obj, 'truncation'):
obj.truncation = 0
if not hasattr(obj, 'occlusion'):
obj.occlusion = 0
if not hasattr(obj, 'score'):
obj.score = 1
if obj.obj_type not in type_whitelist:
continue
vis_utils.draw_box_3d(plot_axes,
obj,
calib.P,
show_orientation=False,
color_table=['r', 'y', 'r', 'w'],
line_width=2,
double_line=False)
box3d_pts_2d, corners = utils.compute_box_3d(obj, calib.P)
if box3d_pts_2d is None:
continue
all_corners.append(corners)
# draw text info
x1 = np.amin(box3d_pts_2d, axis=0)[0]
y1 = np.amin(box3d_pts_2d, axis=0)[1]
x2 = np.amax(box3d_pts_2d, axis=0)[0]
y2 = np.amax(box3d_pts_2d, axis=0)[1]
text_x = (x1 + x2) / 2
text_y = y1
text = "{}\n{:.2f}".format(obj.obj_type, obj.score)
plot_axes.text(text_x,
text_y - 4,
text,
verticalalignment='bottom',
horizontalalignment='center',
color=BOX_COLOUR_SCHEME[obj.obj_type],
fontsize=10,
fontweight='bold',
path_effects=[
patheffects.withStroke(linewidth=2,
foreground='black')
])
return all_corners
def print_box3d_statistics(idx_filename,
type_whitelist=['Car', 'Pedestrian', 'Cyclist'],
split='train'):
''' Collect and dump 3D bounding box statistics '''
dataset = kitti_object(os.path.join(ROOT_DIR, 'dataset/KITTI/object'))
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_rect = calib.project_velo_to_rect(pc_velo[:, 0:3])
objects = dataset.get_label_objects(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], calib.P)
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 return_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)
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_lidar_with_depth(data_idx,
pc_velo,
objects,
calib,
fig,
objects_pred=None,
depth=None,
constraint_box=False,
pc_label=False,
save=False):
print(("All point num: ", pc_velo.shape[0]))
print("pc_velo", pc_velo.shape)
draw_lidar(pc_velo, fig=fig, pc_label=pc_label)
color = (0, 1, 0)
for obj in objects:
if obj.type == "DontCare":
continue
obj.score = norm_score(obj.score)
if obj.score < threshold:
continue
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_box3d(box3d_pts_3d_velo, fig, obj)
mlab.view(
azimuth=180,
elevation=65,
# focalpoint=[12.0909996 , -1.04700089, -2.03249991],
focalpoint=[0, 0, 0],
distance=40.0,
figure=fig)
# MAKE SURE THAT FLAG IS TRUE
mlab.savefig(filename=path.join(output_dir, 'pc_%.3d.png' % data_idx),
figure=fig)
# MAKE SURE THAT FLAG IS FALSE
mlab.show(stop=False)
def get_proposals(self, idx, rpn_score_threshold=0.1, nms_iou_thres=0.3):
assert (idx < self.num_samples)
proposals_file_path = os.path.join(self.proposal_dir,
'%06d.txt' % (idx))
roi_file_path = os.path.join(self.proposal_dir, '%06d_roi.txt' % (idx))
proposals_and_scores = np.loadtxt(proposals_file_path)
keep_idxs = np.arange(0, len(proposals_and_scores))
proposal_boxes_3d = proposals_and_scores[:, 0:7]
proposal_scores = proposals_and_scores[:, 7]
# Apply score mask to proposals
score_mask = proposal_scores > rpn_score_threshold
# 3D box in the format [x, y, z, l, w, h, ry]
proposal_boxes_3d = proposal_boxes_3d[score_mask]
keep_idxs = keep_idxs[score_mask]
proposal_objs = \
[ProposalObject(box_3d) for box_3d in proposal_boxes_3d]
boxes = []
box_scores = []
calib = self.get_calibration(idx)
for obj in proposal_objs:
_, corners = utils.compute_box_3d(obj, calib.P)
# corners_velo = calib.project_rect_to_velo(corners)
# boxes.append(corners_velo)
boxes.append(corners)
box_scores.append(obj.score)
#bev_boxes = list(map(lambda bs: [np.amin(bs[0],axis=0)[0], np.amin(bs[0], axis=0)[2], np.amax(bs[0], axis=0)[0], np.amax(bs[0], axis=0)[2], bs[1]], zip(boxes, box_scores)))
#bev_boxes = np.array(bev_boxes)
# print('before nms: {0}'.format(len(bev_boxes)))
#nms_idxs = non_max_suppression(bev_boxes, nms_iou_thres)
# print('after nms: {0}'.format(len(nms_idxs)))
# boxes = [boxes[i] for i in nms_idxs]
#keep_idxs = keep_idxs[nms_idxs]
proposals_roi_features = self.np_read_lines(roi_file_path, keep_idxs)
proposal_scores = proposal_scores[keep_idxs]
# proposal_objs = [proposal_objs[i] for i in nms_idxs]
for obj, score, feat in zip(proposal_objs, proposal_scores,
proposals_roi_features):
obj.score = score
obj.roi_features = feat
return proposal_objs
def show_image_with_results(img, results, calib, show3d=True):
''' Show image with 2D bounding boxes '''
img1 = np.copy(img) # for 2d bbox
img2 = np.copy(img) # for 3d bbox
# i = 0;
for obj in results:
if obj.type=='DontCare':continue
cv2.rectangle(img1, (int(obj.xmin),int(obj.ymin)),
(int(obj.xmax),int(obj.ymax)), (255,0,0), 2)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(img1,str(obj.score),(int(obj.xmin),int(obj.ymin)), font, 2,(255,255,255),2,cv2.LINE_AA)
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P)
#print(box3d_pts_2d)
#print(box3d_pts_3d)
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
# i = i + 1
Image.fromarray(img1).show()
if show3d:
Image.fromarray(img2).show()
def get_proposal_from_label(label, calib, type_list):
'''
construct proposal from label
'''
_, corners_3d = utils.compute_box_3d(label, calib.P)
# wrap ground truth with box parallel to axis
bev_box = corners_3d[:4, [0, 2]]
xmax = bev_box[:, 0].max(axis=0)
ymax = bev_box[:, 1].max(axis=0)
xmin = bev_box[:, 0].min(axis=0)
ymin = bev_box[:, 1].min(axis=0)
l = xmax - xmin
w = ymax - ymin
h = label.h
# TODO: fake roi_features
roi_features = np.ones(7 * 7 * 32 * 2) * type_list.index(label.type)
return ProposalObject(
list(label.t) + [l, w, h, 0.0], 1, label.type, roi_features)
def show_image_with_boxes_right(img_right, objects, calib, show3d=True):
''' Show image with 2D bounding boxes '''
img1 = np.copy(img_right) # for 2d bbox
img2 = np.copy(img_right) # for 3d bbox
for obj in objects:
if obj.type == 'DontCare': continue
box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P3)
img2 = utils.draw_projected_box3d(img2, box3d_pts_2d)
xcord = box3d_pts_2d[:, 0]
ycord = box3d_pts_2d[:, 1]
cv2.rectangle(img1, (int(min(xcord)), int(min(ycord))),
(int(max(xcord)), int(max(ycord))), (0, 255, 0), 2)
Image.fromarray(img1).show(title="Left Image 2D")
if show3d:
Image.fromarray(img2).show(title="Left Image 3D")
def demo():
import mayavi.mlab as mlab
from viz_util import draw_lidar, draw_lidar_simple, draw_gt_boxes3d
dataset = kitti_object(os.path.join(ROOT_DIR, 'dataset/KITTI/object'))
data_idx = 0
# 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,'dataset/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()
