def _calculate_num_points_in_gt(data_path, infos, relative_path, remove_outside=True, num_features=4):
for info in infos:
if relative_path:
v_path = str(pathlib.Path(data_path) / info["velodyne_path"])
else:
v_path = info["velodyne_path"]
points_v = np.fromfile(
v_path, dtype=np.float32, count=-1).reshape([-1, num_features])
rect = info['calib/R0_rect']
Trv2c = info['calib/Tr_velo_to_cam']
P2 = info['calib/P2']
if remove_outside:
points_v = box_np_ops.remove_outside_points(points_v, rect, Trv2c, P2,
info["img_shape"])
# points_v = points_v[points_v[:, 0] > 0]
annos = info['annos']
num_obj = len([n for n in annos['name'] if n != 'DontCare'])
# annos = kitti.filter_kitti_anno(annos, ['DontCare'])
dims = annos['dimensions'][:num_obj]
loc = annos['location'][:num_obj]
rots = annos['rotation_y'][:num_obj]
gt_boxes_camera = np.concatenate(
[loc, dims, rots[..., np.newaxis]], axis=1)
gt_boxes_lidar = box_np_ops.box_camera_to_lidar(
gt_boxes_camera, rect, Trv2c)
indices = box_np_ops.points_in_rbbox(points_v[:, :3], gt_boxes_lidar)
num_points_in_gt = indices.sum(0)
num_ignored = len(annos['dimensions']) - num_obj
num_points_in_gt = np.concatenate(
[num_points_in_gt, -np.ones([num_ignored])])
annos["num_points_in_gt"] = num_points_in_gt.astype(np.int32)
def _create_reduced_point_cloud(data_path,
info_path,
save_path=None,
back=False):
with open(info_path, 'rb') as f:
kitti_infos = pickle.load(f)
for info in prog_bar(kitti_infos):
v_path = info['velodyne_path']
v_path = pathlib.Path(data_path) / v_path
points_v = np.fromfile(
str(v_path), dtype=np.float32, count=-1).reshape([-1, 4])
rect = info['calib/R0_rect']
P2 = info['calib/P2']
Trv2c = info['calib/Tr_velo_to_cam']
# first remove z < 0 points
# keep = points_v[:, -1] > 0
# points_v = points_v[keep]
# then remove outside.
if back:
points_v[:, 0] = -points_v[:, 0]
points_v = box_np_ops.remove_outside_points(points_v, rect, Trv2c, P2,
info["img_shape"])
if save_path is None:
save_filename = v_path.parent.parent / (v_path.parent.stem + "_reduced") / v_path.name
# save_filename = str(v_path) + '_reduced'
if back:
save_filename += "_back"
else:
save_filename = str(pathlib.Path(save_path) / v_path.name)
if back:
save_filename += "_back"
with open(save_filename, 'w') as f:
points_v.tofile(f)
def prep_pointcloud(input_dict,
root_path,
voxel_generator,
target_assigner,
db_sampler=None,
max_voxels=20000,
class_names=['Car'],
remove_outside_points=False,
training=True,
create_targets=True,
shuffle_points=False,
reduce_valid_area=False,
remove_unknown=False,
gt_rotation_noise=[-np.pi / 3, np.pi / 3],
gt_loc_noise_std=[1.0, 1.0, 1.0],
global_rotation_noise=[-np.pi / 4, np.pi / 4],
global_scaling_noise=[0.95, 1.05],
global_loc_noise_std=(0.2, 0.2, 0.2),
global_random_rot_range=[0.78, 2.35],
generate_bev=False,
without_reflectivity=False,
num_point_features=4,
anchor_area_threshold=1,
gt_points_drop=0.0,
gt_drop_max_keep=10,
remove_points_after_sample=True,
anchor_cache=None,
remove_environment=False,
random_crop=False,
reference_detections=None,
add_rgb_to_points=False,
lidar_input=False,
unlabeled_db_sampler=None,
out_size_factor=2,
min_gt_point_dict=None,
bev_only=False,
use_group_id=False,
out_dtype=np.float32):
"""convert point cloud to voxels, create targets if ground truths
exists.
"""
points = input_dict["points"]
if training:
gt_boxes = input_dict["gt_boxes"]
gt_names = input_dict["gt_names"]
difficulty = input_dict["difficulty"]
group_ids = None
if use_group_id and "group_ids" in input_dict:
group_ids = input_dict["group_ids"]
rect = input_dict["rect"]
Trv2c = input_dict["Trv2c"]
P2 = input_dict["P2"]
unlabeled_training = unlabeled_db_sampler is not None
image_idx = input_dict["image_idx"]
if reference_detections is not None:
C, R, T = box_np_ops.projection_matrix_to_CRT_kitti(P2)
frustums = box_np_ops.get_frustum_v2(reference_detections, C)
frustums -= T
# frustums = np.linalg.inv(R) @ frustums.T
frustums = np.einsum('ij, akj->aki', np.linalg.inv(R), frustums)
frustums = box_np_ops.camera_to_lidar(frustums, rect, Trv2c)
surfaces = box_np_ops.corner_to_surfaces_3d_jit(frustums)
masks = points_in_convex_polygon_3d_jit(points, surfaces)
points = points[masks.any(-1)]
if remove_outside_points and not lidar_input:
image_shape = input_dict["image_shape"]
points = box_np_ops.remove_outside_points(points, rect, Trv2c, P2,
image_shape)
if remove_environment is True and training:
selected = kitti.keep_arrays_by_name(gt_names, class_names)
gt_boxes = gt_boxes[selected]
gt_names = gt_names[selected]
difficulty = difficulty[selected]
if group_ids is not None:
group_ids = group_ids[selected]
points = prep.remove_points_outside_boxes(points, gt_boxes)
if training:
# print(gt_names)
selected = kitti.drop_arrays_by_name(gt_names, ["DontCare"])
gt_boxes = gt_boxes[selected]
gt_names = gt_names[selected]
difficulty = difficulty[selected]
if group_ids is not None:
group_ids = group_ids[selected]
gt_boxes = box_np_ops.box_camera_to_lidar(gt_boxes, rect, Trv2c)
if remove_unknown:
remove_mask = difficulty == -1
"""
gt_boxes_remove = gt_boxes[remove_mask]
gt_boxes_remove[:, 3:6] += 0.25
points = prep.remove_points_in_boxes(points, gt_boxes_remove)
"""
keep_mask = np.logical_not(remove_mask)
gt_boxes = gt_boxes[keep_mask]
gt_names = gt_names[keep_mask]
difficulty = difficulty[keep_mask]
if group_ids is not None:
group_ids = group_ids[keep_mask]
gt_boxes_mask = np.array([n in class_names for n in gt_names],
dtype=np.bool_)
if db_sampler is not None:
sampled_dict = db_sampler.sample_all(root_path,
gt_boxes,
gt_names,
num_point_features,
random_crop,
gt_group_ids=group_ids,
rect=rect,
Trv2c=Trv2c,
P2=P2)
if sampled_dict is not None:
sampled_gt_names = sampled_dict["gt_names"]
sampled_gt_boxes = sampled_dict["gt_boxes"]
sampled_points = sampled_dict["points"]
sampled_gt_masks = sampled_dict["gt_masks"]
# gt_names = gt_names[gt_boxes_mask].tolist()
gt_names = np.concatenate([gt_names, sampled_gt_names], axis=0)
# gt_names += [s["name"] for s in sampled]
gt_boxes = np.concatenate([gt_boxes, sampled_gt_boxes])
gt_boxes_mask = np.concatenate(
[gt_boxes_mask, sampled_gt_masks], axis=0)
if group_ids is not None:
sampled_group_ids = sampled_dict["group_ids"]
group_ids = np.concatenate([group_ids, sampled_group_ids])
if remove_points_after_sample:
points = prep.remove_points_in_boxes(
points, sampled_gt_boxes)
points = np.concatenate([sampled_points, points], axis=0)
# unlabeled_mask = np.zeros((gt_boxes.shape[0], ), dtype=np.bool_)
if without_reflectivity:
used_point_axes = list(range(num_point_features))
used_point_axes.pop(3)
points = points[:, used_point_axes]
pc_range = voxel_generator.point_cloud_range
if bev_only: # set z and h to limits
gt_boxes[:, 2] = pc_range[2]
gt_boxes[:, 5] = pc_range[5] - pc_range[2]
prep.noise_per_object_v3_(
gt_boxes,
points,
gt_boxes_mask,
rotation_perturb=gt_rotation_noise,
center_noise_std=gt_loc_noise_std,
global_random_rot_range=global_random_rot_range,
group_ids=group_ids,
num_try=100)
# should remove unrelated objects after noise per object
gt_boxes = gt_boxes[gt_boxes_mask]
gt_names = gt_names[gt_boxes_mask]
if group_ids is not None:
group_ids = group_ids[gt_boxes_mask]
gt_classes = np.array([class_names.index(n) + 1 for n in gt_names],
dtype=np.int32)
gt_boxes, points = prep.random_flip(gt_boxes, points)
gt_boxes, points = prep.global_rotation(gt_boxes,
points,
rotation=global_rotation_noise)
gt_boxes, points = prep.global_scaling_v2(gt_boxes, points,
*global_scaling_noise)
# Global translation
gt_boxes, points = prep.global_translate(gt_boxes, points,
global_loc_noise_std)
bv_range = voxel_generator.point_cloud_range[[0, 1, 3, 4]]
mask = prep.filter_gt_box_outside_range(gt_boxes, bv_range)
gt_boxes = gt_boxes[mask]
gt_classes = gt_classes[mask]
if group_ids is not None:
group_ids = group_ids[mask]
# limit rad to [-pi, pi]
gt_boxes[:, 6] = box_np_ops.limit_period(gt_boxes[:, 6],
offset=0.5,
period=2 * np.pi)
if shuffle_points:
# shuffle is a little slow.
np.random.shuffle(points)
# [0, -40, -3, 70.4, 40, 1]
voxel_size = voxel_generator.voxel_size
pc_range = voxel_generator.point_cloud_range
grid_size = voxel_generator.grid_size
# [352, 400]
voxels, coordinates, num_points = voxel_generator.generate(
points, max_voxels)
#print('coordinates',coordinates.shape)
example = {
'voxels': voxels,
'num_points': num_points,
'coordinates': coordinates,
"num_voxels": np.array([voxels.shape[0]], dtype=np.int64)
}
example.update({
'rect': rect,
'Trv2c': Trv2c,
'P2': P2,
})
# if not lidar_input:
feature_map_size = grid_size[:2] // out_size_factor
feature_map_size = [*feature_map_size, 1][::-1]
if anchor_cache is not None:
anchors = anchor_cache["anchors"]
anchors_bv = anchor_cache["anchors_bv"]
matched_thresholds = anchor_cache["matched_thresholds"]
unmatched_thresholds = anchor_cache["unmatched_thresholds"]
else:
ret = target_assigner.generate_anchors(feature_map_size)
anchors = ret["anchors"]
anchors = anchors.reshape([-1, 7])
matched_thresholds = ret["matched_thresholds"]
unmatched_thresholds = ret["unmatched_thresholds"]
anchors_bv = box_np_ops.rbbox2d_to_near_bbox(anchors[:,
[0, 1, 3, 4, 6]])
example["anchors"] = anchors
# print("debug", anchors.shape, matched_thresholds.shape)
# anchors_bv = anchors_bv.reshape([-1, 4])
anchors_mask = None
if anchor_area_threshold >= 0:
coors = coordinates
dense_voxel_map = box_np_ops.sparse_sum_for_anchors_mask(
coors, tuple(grid_size[::-1][1:]))
dense_voxel_map = dense_voxel_map.cumsum(0)
dense_voxel_map = dense_voxel_map.cumsum(1)
anchors_area = box_np_ops.fused_get_anchors_area(
dense_voxel_map, anchors_bv, voxel_size, pc_range, grid_size)
anchors_mask = anchors_area > anchor_area_threshold
# example['anchors_mask'] = anchors_mask.astype(np.uint8)
example['anchors_mask'] = anchors_mask
if generate_bev:
bev_vxsize = voxel_size.copy()
bev_vxsize[:2] /= 2
bev_vxsize[2] *= 2
bev_map = points_to_bev(points, bev_vxsize, pc_range,
without_reflectivity)
example["bev_map"] = bev_map
if not training:
return example
if create_targets:
targets_dict = target_assigner.assign(
anchors,
gt_boxes,
anchors_mask,
gt_classes=gt_classes,
matched_thresholds=matched_thresholds,
unmatched_thresholds=unmatched_thresholds)
example.update({
'labels': targets_dict['labels'],
'reg_targets': targets_dict['bbox_targets'],
'reg_weights': targets_dict['bbox_outside_weights'],
})
return example
def create_groundtruth_database(data_path,
info_path=None,
used_classes=None,
database_save_path=None,
db_info_save_path=None,
relative_path=True,
lidar_only=False,
bev_only=False,
coors_range=None):
root_path = pathlib.Path(data_path)
if info_path is None:
info_path = root_path / 'kitti_infos_train.pkl'
if database_save_path is None:
database_save_path = root_path / 'gt_database'
else:
database_save_path = pathlib.Path(database_save_path)
if db_info_save_path is None:
db_info_save_path = root_path / "kitti_dbinfos_train.pkl"
database_save_path.mkdir(parents=True, exist_ok=True)
with open(info_path, 'rb') as f:
kitti_infos = pickle.load(f)
all_db_infos = {}
if used_classes is None:
used_classes = list(kitti.get_classes())
used_classes.pop(used_classes.index('DontCare'))
for name in used_classes:
all_db_infos[name] = []
group_counter = 0
for info in prog_bar(kitti_infos):
velodyne_path = info['velodyne_path']
if relative_path:
# velodyne_path = str(root_path / velodyne_path) + "_reduced"
velodyne_path = str(root_path / velodyne_path)
num_features = 4
if 'pointcloud_num_features' in info:
num_features = info['pointcloud_num_features']
points = np.fromfile(
velodyne_path, dtype=np.float32, count=-1).reshape([-1, num_features])
image_idx = info["image_idx"]
rect = info['calib/R0_rect']
P2 = info['calib/P2']
Trv2c = info['calib/Tr_velo_to_cam']
if not lidar_only:
points = box_np_ops.remove_outside_points(points, rect, Trv2c, P2,
info["img_shape"])
annos = info["annos"]
names = annos["name"]
bboxes = annos["bbox"]
difficulty = annos["difficulty"]
gt_idxes = annos["index"]
num_obj = np.sum(annos["index"] >= 0)
rbbox_cam = kitti.anno_to_rbboxes(annos)[:num_obj]
rbbox_lidar = box_np_ops.box_camera_to_lidar(rbbox_cam, rect, Trv2c)
if bev_only: # set z and h to limits
assert coors_range is not None
rbbox_lidar[:, 2] = coors_range[2]
rbbox_lidar[:, 5] = coors_range[5] - coors_range[2]
group_dict = {}
group_ids = np.full([bboxes.shape[0]], -1, dtype=np.int64)
if "group_ids" in annos:
group_ids = annos["group_ids"]
else:
group_ids = np.arange(bboxes.shape[0], dtype=np.int64)
point_indices = box_np_ops.points_in_rbbox(points, rbbox_lidar)
for i in range(num_obj):
filename = f"{image_idx}_{names[i]}_{gt_idxes[i]}.bin"
filepath = database_save_path / filename
gt_points = points[point_indices[:, i]]
gt_points[:, :3] -= rbbox_lidar[i, :3]
with open(filepath, 'w') as f:
gt_points.tofile(f)
if names[i] in used_classes:
if relative_path:
db_path = str(database_save_path.stem + "/" + filename)
else:
db_path = str(filepath)
db_info = {
"name": names[i],
"path": db_path,
"image_idx": image_idx,
"gt_idx": gt_idxes[i],
"box3d_lidar": rbbox_lidar[i],
"num_points_in_gt": gt_points.shape[0],
"difficulty": difficulty[i],
# "group_id": -1,
# "bbox": bboxes[i],
}
local_group_id = group_ids[i]
# if local_group_id >= 0:
if local_group_id not in group_dict:
group_dict[local_group_id] = group_counter
group_counter += 1
db_info["group_id"] = group_dict[local_group_id]
if "score" in annos:
db_info["score"] = annos["score"][i]
all_db_infos[names[i]].append(db_info)
for k, v in all_db_infos.items():
print(f"load {len(v)} {k} database infos")
with open(db_info_save_path, 'wb') as f:
pickle.dump(all_db_infos, f)