def write_detection_results(result_dir, id_list, type_list, box2d_list, center_list, \
heading_cls_list, heading_res_list, \
size_cls_list, size_res_list, \
rot_angle_list, score_list):
''' Write frustum pointnets results to KITTI format label files. '''
if result_dir is None: return
results = {} # map from idx to list of strings, each string is a line (without \n)
for i in range(len(center_list)):
idx = id_list[i]
output_str = type_list[i] + " -1 -1 -10 "
box2d = box2d_list[i]
output_str += "%f %f %f %f " % (box2d[0],box2d[1],box2d[2],box2d[3])
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])
score = score_list[i]
output_str += "%f %f %f %f %f %f %f %f" % (h,w,l,tx,ty,tz,ry,score)
if idx not in results: results[idx] = []
results[idx].append(output_str)
# Write TXT files
if not os.path.exists(result_dir): os.mkdir(result_dir)
output_dir = os.path.join(result_dir, 'data')
if not os.path.exists(output_dir): os.mkdir(output_dir)
for idx in results:
pred_filename = os.path.join(output_dir, '%06d.txt'%(idx))
fout = open(pred_filename, 'w')
for line in results[idx]:
fout.write(line+'\n')
fout.close()
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 write_detection_results(result_dir, id_list, type_list, box2d_list, center_list, \
heading_cls_list, heading_res_list, \
size_cls_list, size_res_list, \
rot_angle_list, score_list):
''' Write frustum pointnets results to KITTI format label files. '''
if result_dir is None: return
results = {} # map from idx to list of strings, each string is a line (without \n)
for i in range(len(center_list)):
idx = id_list[i]
output_str = type_list[i] + " -1 -1 -10 "
box2d = box2d_list[i]
output_str += "%f %f %f %f " % (box2d[0],box2d[1],box2d[2],box2d[3])
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])
score = score_list[i]
output_str += "%f %f %f %f %f %f %f %f" % (h,w,l,tx,ty,tz,ry,score)
if idx not in results: results[idx] = []
results[idx].append(output_str)
# Write TXT files
if not os.path.exists(result_dir): os.mkdir(result_dir)
output_dir = os.path.join(result_dir, 'data')
if not os.path.exists(output_dir): os.mkdir(output_dir)
for idx in results:
pred_filename = os.path.join(output_dir, '%06d.txt'%(idx))
fout = open(pred_filename, 'w')
for line in results[idx]:
fout.write(line+'\n')
fout.close()
def write_detection_results_3dbat(result_dir, id_list, center_list, \
heading_cls_list, heading_res_list, \
size_cls_list, size_res_list, \
rot_angle_list, score_list):
orig_path = "/root/3D_BoundingBox_Annotation_Tool_3D_BAT/input/NuScenes/ONE/"
# copy files to 3d bbat
i=0
for idx in id_list:
#copy pcd file
copyfile("/root/frustum-pointnets_RSC/dataset/KITTI/object/training/velodyne/%06d.pcd" %idx ,orig_path+"/pointclouds/%06d.pcd" %i)
copyfile("/root/frustum-pointnets_RSC/dataset/KITTI/object/training/image_left/%06d.jpg" %idx ,orig_path+"/images/CAM_FRONT_LEFT/%06d.jpg" %i)
copyfile("/root/frustum-pointnets_RSC/dataset/KITTI/object/training/image_right/%06d.jpg" % idx,orig_path + "/images/CAM_FRONT/%06d.jpg" % i)
i=i+1
#copy image or create sym link zum image: watch out that you dont broke the links
# generate json annotation
annotation = []
for i in range(len(center_list)):
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])
x= tz
z=-ty
y=-tx
th= h
tw= l
tl= w
annotation.append([{"class":"Pedestrian","width":tw,"length":tl,"height":th,"x":x,"y":y,"z":z,"rotationY":ry,"trackId":1,"frameIdx":i}])
with open(orig_path+ "/annotations/NuScenes_ONE_annotations.txt", 'w') as outfile:
json.dump(annotation, outfile)
def write_detection_results_test(result_dir,split, id_list, center_list, \
heading_cls_list, heading_res_list, \
size_cls_list, size_res_list, \
rot_angle_list, segp_list,score_list):
''' Write frustum pointnets results to KITTI format label files. '''
result_dir = OUTPUT_FILE + split + "/" + result_dir + "/"
if not os.path.exists(result_dir):
os.makedirs(result_dir)
if result_dir is None: return
results = {
} # map from idx to list of strings, each string is a line (without \n)
for i in range(len(segp_list)):
if np.count_nonzero(segp_list[i] == 1) < 50:
continue
idx = id_list[i]
output_str = "Pedestrian -1 -1 -10 "
output_str += "0.0 0.0 0.0 0.0 "
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])
score = 0.0
output_str += "%f %f %f %f %f %f %f %f" % (h, w, l, tx, ty, tz, ry,
score_list[i])
if idx not in results: results[idx] = []
results[idx].append(output_str)
# Write TXT files
if not os.path.exists(result_dir): os.mkdir(result_dir)
output_dir = os.path.join(result_dir, 'data')
if not os.path.exists(output_dir): os.mkdir(output_dir)
for idx in results:
pred_filename = os.path.join(output_dir, '%06d.txt' % (idx))
fout = open(pred_filename, 'w')
for line in results[idx]:
fout.write(line + '\n')
fout.close()
def write_track_detection_results(result_dir, id_list, type_list, box2d_list, center_list,
heading_cls_list, heading_res_list,
size_cls_list, size_res_list,
rot_angle_list, score_list,dataset):
''' Write frustum pointnets results to KITTI format label files. '''
if result_dir is None: return
results = {} # map from idx to list of strings, each string is a line (without \n)
for i in range(len(center_list)):
idx = id_list[i]
output_str = type_list[i] + " -1 -1 -10.0 "
box2d = box2d_list[i]
output_str += "%f %f %f %f " % (box2d[0],box2d[1],box2d[2],box2d[3])
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])
score = score_list[i]
output_str += "%f %f %f %f %f %f %f %f" % (h,w,l,tx,ty,tz,ry,score)
world_id = dataset.world_id_list[idx]
frame_id = dataset.image_id_list[idx]
if world_id not in results: results[world_id]={}
if frame_id not in results[world_id]: results[world_id][frame_id]=[]
results[world_id][frame_id].append(output_str)
#if idx not in results: results[idx] = []
#results[idx].append(output_str)
drives = list(set(dataset.world_id_list))
drives.sort()
# Write TXT files
if not os.path.exists(result_dir): os.mkdir(result_dir)
output_dir = os.path.join(result_dir, 'data')
if not os.path.exists(output_dir): os.mkdir(output_dir)
i_common_folder_fr = 0 ## Frame ID to use while writing predictions to the common folder (the folder that contains all validation drives)
for i_dr,drive in enumerate(drives):
## Each drive will be written to separate folders
drive_path = os.path.join(output_dir,'%04d'%drive)
if not os.path.exists(drive_path): os.mkdir(drive_path)
drive_path = os.path.join(drive_path,'data')
if not os.path.exists(drive_path):
os.mkdir(drive_path)
else:
shutil.rmtree(drive_path)
os.mkdir(drive_path)
## In addition, all results of all validation drives will be written to the same folder for an evaluation at once
drive_path_same = os.path.join(output_dir,'%04d'%98)
if not os.path.exists(drive_path_same): os.mkdir(drive_path_same)
drive_path_same = os.path.join(drive_path_same,'data')
if not os.path.exists(drive_path_same):
os.mkdir(drive_path_same)
else:
if i_dr == 0:
shutil.rmtree(drive_path_same)
os.mkdir(drive_path_same)
## Write results to the drive folder
res_drive = results[drive]
for fr_id in res_drive.keys():
pred_filename = os.path.join(drive_path, '%06d.txt'%(fr_id))
fout = open(pred_filename, 'w')
for line in res_drive[fr_id]:
fout.write(line+'\n')
fout.close()
## Write results to the common folder
#IPython.embed()
for fr_id in res_drive.keys():
pred_filename = os.path.join(drive_path_same, '%06d.txt'%(i_common_folder_fr+fr_id))
fout = open(pred_filename, 'w')
for line in res_drive[fr_id]:
fout.write(line+'\n')
fout.close()
i_common_folder_fr += (dataset.drive_sizes[drive]+1)
def detect_batch(sess, end_points, point_clouds, feature_vec, rot_angle_list, prop_cls_labels):
sample_num = len(point_clouds)
logits = np.zeros((sample_num, NUM_OBJ_CLASSES))
centers = np.zeros((sample_num, 3))
heading_logits = np.zeros((sample_num, NUM_HEADING_BIN))
heading_residuals = np.zeros((sample_num, NUM_HEADING_BIN))
size_logits = np.zeros((sample_num, NUM_SIZE_CLUSTER))
size_residuals = np.zeros((sample_num, NUM_SIZE_CLUSTER, 3))
scores = np.zeros((sample_num,)) # 3D box score
points_num = np.zeros((sample_num,))
for i in range(math.floor(sample_num/batch_size)):
begin = i * batch_size
end = min((i + 1) * batch_size, sample_num)
feed_dict = {\
end_points['pointclouds_pl']: point_clouds[begin:end],
end_points['features_pl']: feature_vec[begin:end],
end_points['is_training_pl']: False}
batch_logits, batch_seg_logits, batch_centers, \
batch_heading_scores, batch_heading_residuals, \
batch_size_scores, batch_size_residuals = \
sess.run([end_points['cls_logits'], end_points['mask_logits'], end_points['center'],
end_points['heading_scores'], end_points['heading_residuals'],
end_points['size_scores'], end_points['size_residuals']],
feed_dict=feed_dict)
logits[begin:end,...] = batch_logits
centers[begin:end,...] = batch_centers
heading_logits[begin:end,...] = batch_heading_scores
heading_residuals[begin:end,...] = batch_heading_residuals
size_logits[begin:end,...] = batch_size_scores
size_residuals[begin:end,...] = batch_size_residuals
points_num[begin:end,...] = np.sum(np.equal(np.argmax(batch_seg_logits, 2), 1), axis=1)
# Compute scores
batch_cls_prob = np.max(softmax(batch_logits),1) # B,
batch_seg_prob = softmax(batch_seg_logits)[:,:,1] # BxN
batch_seg_mask = np.argmax(batch_seg_logits, 2) # BxN
mask_mean_prob = np.sum(batch_seg_prob * batch_seg_mask, 1) # B,
mask_mean_prob = mask_mean_prob / np.sum(batch_seg_mask,1) # B,
heading_prob = np.max(softmax(batch_heading_scores),1) # B
size_prob = np.max(softmax(batch_size_scores),1) # B,
batch_scores = (batch_cls_prob + mask_mean_prob + heading_prob + size_prob) / 4
# scores[begin:end] = batch_cls_prob
scores[begin:end] = batch_scores
# Finished computing scores
type_cls = np.argmax(logits, 1)
heading_cls = np.argmax(heading_logits, 1) # B
size_cls = np.argmax(size_logits, 1) # B
heading_res = np.array([heading_residuals[i,heading_cls[i]] \
for i in range(sample_num)])
size_res = np.vstack([size_residuals[i,size_cls[i],:] \
for i in range(sample_num)])
output = []
for i in range(sample_num):
# if type_cls[i] == g_type2onehotclass['NonObject'] or scores[i] < 0.5 or points_num[i] == 0:
# use ground as cls output
type_cls[i] = prop_cls_labels[i]
if type_cls[i] == g_type2onehotclass['NonObject']:
continue
h,w,l,tx,ty,tz,ry = provider.from_prediction_to_label_format(centers[i],
heading_cls[i], heading_res[i],
size_cls[i], size_res[i], rot_angle_list[i])
# FIXME: this offset should be fixed in get_pointnet_input
tx, ty, tz = rotate_pc_along_y(np.expand_dims(np.asarray([tx,ty,tz]), 0), -np.pi/2)[0]
ry += np.pi/2
obj_type = type_cls[i]
confidence = scores[i]
output.append([tx,ty,tz,l,w,h,ry,confidence,obj_type])
if len(output) == 0:
return output
# 2d nms on bev
nms_idxs = nms_on_bev(output, 0.01)
output = [output[i] for i in nms_idxs]
return output
def predict(self, velo_pts, image, calib):
detection = self.ssd_detector.predict(image)
vehicle_idx = np.where(detection['detection_classes'] == 1)
conf_idx = np.where(
detection['detection_scores'] >= self.ssd_threshold)
final_idx = np.intersect1d(vehicle_idx, conf_idx)
bbox = detection['detection_boxes'][final_idx]
detection_conf = detection['detection_scores'][final_idx]
rect_pts = np.zeros_like(velo_pts)
rect_pts[:, :3] = calib.project_velo_to_rect(velo_pts[:, :3].copy())
rect_pts[:, 3] = velo_pts[:, 3]
img_height, img_width, _ = image.shape
_, img_pts, in_img_mask = get_lidar_in_image_fov(
velo_pts[:, :3].copy(),
calib,
0,
0,
img_width,
img_height,
return_more=True)
frustum_examples = []
frustum_angles = []
scene = Scene([])
for box in bbox:
box = (box.reshape((2, 2)) * image.shape[:2]).astype(int)
(ul_y, ul_x), (lr_y, lr_x) = box
box_mask = (img_pts[:, 1] < lr_y) * (img_pts[:, 1] >= ul_y) * (
img_pts[:, 0] < lr_x) * (img_pts[:, 0] >= ul_x)
mask = in_img_mask & box_mask
rect_pts_masked = rect_pts[mask]
if len(rect_pts_masked):
box2d_center = np.array([(ul_x + lr_x) / 2.0,
(ul_y + ul_y) / 2.0])
uvdepth = np.zeros((1, 3))
uvdepth[0, :2], uvdepth[0, 2] = box2d_center, 10
box2d_center_rect = calib.project_image_to_rect(uvdepth)
frustum_angle = -1 * np.arctan2(box2d_center_rect[0, 2],
box2d_center_rect[0, 0])
frustum_angle += np.pi / 2.0
np.random.seed()
point_cloud = provider.rotate_pc_along_y(
rect_pts_masked.copy(), frustum_angle)
idx = np.random.choice(len(point_cloud),
size=self.frustum_num_pts,
replace=True)
point_cloud = point_cloud[idx]
frustum_angles.append(frustum_angle)
frustum_examples.append(point_cloud)
if len(frustum_examples):
one_hot_batch = np.array([
[1, 0, 0],
] * len(frustum_examples)).reshape(-1, 3)
predictions = self.frustum_pointnet(self.frustum_sess,
self.frustum_ops,
np.array(frustum_examples),
one_hot_batch,
self.frustum_batch_size)
_, centers, heading_cls, heading_res, size_cls, size_res, _ = predictions
for i, _ in enumerate(heading_cls):
h, w, l, tx, ty, tz, ry = provider.from_prediction_to_label_format(
centers[i], heading_cls[i], heading_res[i], size_cls[i],
size_res[i], frustum_angles[i])
detection = Detection(xyz=np.array((tx, ty, tz)),
angle=ry,
lwh=np.array((l, w, h)),
confidence=detection_conf[i])
scene.detections.append(detection)
return scene
