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 tqdm(kitti_infos):
pc_info = info["point_cloud"]
image_info = info["image"]
calib = info["calib"]
v_path = pc_info["velodyne_path"]
v_path = Path(data_path) / v_path
points_v = np.fromfile(str(v_path), dtype=np.float32,
count=-1).reshape([-1, 4])
rect = calib["R0_rect"]
P2 = calib["P2"]
Trv2c = 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,
image_info["image_shape"])
if save_path is None:
Path.mkdir(v_path.parent.parent /
(v_path.parent.stem + "_reduced"))
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(Path(save_path) / v_path.name)
if back:
save_filename += "_back"
with open(save_filename, "w") as f:
points_v.tofile(f)
def _calculate_num_points_in_gt(data_path,
infos,
relative_path,
remove_outside=True,
num_features=4):
for info in infos:
pc_info = info["point_cloud"]
image_info = info["image"]
calib = info["calib"]
if relative_path:
v_path = str(Path(data_path) / pc_info["velodyne_path"])
else:
v_path = pc_info["velodyne_path"]
points_v = np.fromfile(v_path, dtype=np.float32,
count=-1).reshape([-1, num_features])
rect = calib["R0_rect"]
Trv2c = calib["Tr_velo_to_cam"]
P2 = calib["P2"]
if remove_outside:
points_v = box_np_ops.remove_outside_points(
points_v, rect, Trv2c, P2, image_info["image_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 prep_pointcloud_rpn(
input_dict,
root_path,
task_class_names=[],
prep_cfg=None,
db_sampler=None,
remove_outside_points=False,
training=True,
num_point_features=4,
random_crop=False,
reference_detections=None,
out_dtype=np.float32,
min_points_in_gt=-1,
logger=None,
):
"""
convert point cloud to voxels, create targets if ground truths exists.
input_dict format: dataset.get_sensor_data format
"""
assert prep_cfg is not None
remove_environment = prep_cfg.REMOVE_UNKOWN_EXAMPLES
if training:
remove_unknown = prep_cfg.REMOVE_UNKOWN_EXAMPLES
gt_rotation_noise = prep_cfg.GT_ROT_NOISE
gt_loc_noise_std = prep_cfg.GT_LOC_NOISE
global_rotation_noise = prep_cfg.GLOBAL_ROT_NOISE
global_scaling_noise = prep_cfg.GLOBAL_SCALE_NOISE
global_random_rot_range = prep_cfg.GLOBAL_ROT_PER_OBJ_RANGE
global_translate_noise_std = prep_cfg.GLOBAL_TRANS_NOISE
gt_points_drop = prep_cfg.GT_DROP_PERCENTAGE
gt_drop_max_keep = prep_cfg.GT_DROP_MAX_KEEP_POINTS
remove_points_after_sample = prep_cfg.REMOVE_POINTS_AFTER_SAMPLE
class_names = list(itertools.chain(*task_class_names))
# points_only = input_dict["lidar"]["points"]
# times = input_dict["lidar"]["times"]
# points = np.hstack([points_only, times])
points = input_dict["lidar"]["points"]
if training:
anno_dict = input_dict["lidar"]["annotations"]
gt_dict = {
"gt_boxes": anno_dict["boxes"],
"gt_names": np.array(anno_dict["names"]).reshape(-1),
}
if "difficulty" not in anno_dict:
difficulty = np.zeros([anno_dict["boxes"].shape[0]],
dtype=np.int32)
gt_dict["difficulty"] = difficulty
else:
gt_dict["difficulty"] = anno_dict["difficulty"]
# if use_group_id and "group_ids" in anno_dict:
# group_ids = anno_dict["group_ids"]
# gt_dict["group_ids"] = group_ids
calib = None
if "calib" in input_dict:
calib = input_dict["calib"]
if reference_detections is not None:
assert calib is not None and "image" in input_dict
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.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:
assert calib is not None
image_shape = input_dict["image"]["image_shape"]
points = box_np_ops.remove_outside_points(points, calib["rect"],
calib["Trv2c"], calib["P2"],
image_shape)
if remove_environment is True and training:
selected = kitti.keep_arrays_by_name(gt_names, target_assigner.classes)
_dict_select(gt_dict, selected)
masks = box_np_ops.points_in_rbbox(points, gt_dict["gt_boxes"])
points = points[masks.any(-1)]
if training:
selected = kitti.drop_arrays_by_name(gt_dict["gt_names"],
["DontCare", "ignore"])
_dict_select(gt_dict, selected)
if remove_unknown:
remove_mask = gt_dict["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)
_dict_select(gt_dict, keep_mask)
gt_dict.pop("difficulty")
gt_boxes_mask = np.array(
[n in class_names for n in gt_dict["gt_names"]], dtype=np.bool_)
# db_sampler = None
if db_sampler is not None:
group_ids = None
# if "group_ids" in gt_dict:
# group_ids = gt_dict["group_ids"]
sampled_dict = db_sampler.sample_all(
root_path,
gt_dict["gt_boxes"],
gt_dict["gt_names"],
num_point_features,
random_crop,
gt_group_ids=group_ids,
calib=calib,
)
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_dict["gt_names"] = np.concatenate(
[gt_dict["gt_names"], sampled_gt_names], axis=0)
gt_dict["gt_boxes"] = np.concatenate(
[gt_dict["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"]
# gt_dict["group_ids"] = np.concatenate(
# [gt_dict["group_ids"], sampled_group_ids])
if remove_points_after_sample:
masks = box_np_ops.points_in_rbbox(points,
sampled_gt_boxes)
points = points[np.logical_not(masks.any(-1))]
points = np.concatenate([sampled_points, points], axis=0)
# group_ids = None
# if "group_ids" in gt_dict:
# group_ids = gt_dict["group_ids"]
prep.noise_per_object_v3_(
gt_dict["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=None,
num_try=100,
)
# should remove unrelated objects after noise per object
# for k, v in gt_dict.items():
# print(k, v.shape)
_dict_select(gt_dict, gt_boxes_mask)
gt_classes = np.array(
[class_names.index(n) + 1 for n in gt_dict["gt_names"]],
dtype=np.int32)
gt_dict["gt_classes"] = gt_classes
gt_dict["gt_boxes"], points = prep.random_flip(gt_dict["gt_boxes"],
points)
gt_dict["gt_boxes"], points = prep.global_rotation(
gt_dict["gt_boxes"], points, rotation=global_rotation_noise)
gt_dict["gt_boxes"], points = prep.global_scaling_v2(
gt_dict["gt_boxes"], points, *global_scaling_noise)
prep.global_translate_(gt_dict["gt_boxes"], points,
global_translate_noise_std)
task_masks = []
flag = 0
for class_name in task_class_names:
task_masks.append([
np.where(gt_dict["gt_classes"] == class_name.index(i) + 1 +
flag) for i in class_name
])
flag += len(class_name)
task_boxes = []
task_classes = []
task_names = []
flag2 = 0
for idx, mask in enumerate(task_masks):
task_box = []
task_class = []
task_name = []
for m in mask:
task_box.append(gt_dict["gt_boxes"][m])
task_class.append(gt_dict["gt_classes"][m] - flag2)
task_name.append(gt_dict["gt_names"][m])
task_boxes.append(np.concatenate(task_box, axis=0))
task_classes.append(np.concatenate(task_class))
task_names.append(np.concatenate(task_name))
flag2 += len(mask)
for task_box in task_boxes:
# limit rad to [-pi, pi]
task_box[:, -1] = box_np_ops.limit_period(task_box[:, -1],
offset=0.5,
period=2 * np.pi)
# print(gt_dict.keys())
gt_dict["gt_classes"] = task_classes
gt_dict["gt_names"] = task_names
gt_dict["gt_boxes"] = task_boxes
example = {
"pts_input": points,
"pts_rect": None,
"pts_features": None,
"gt_boxes3d": gt_dict["gt_boxes"],
"rpn_cls_label": [],
"rpn_reg_label": [],
}
if calib is not None:
example["calib"] = calib
return example
def prep_sequence_pointcloud(
input_dict,
root_path,
voxel_generator,
target_assigners,
prep_cfg=None,
db_sampler=None,
remove_outside_points=False,
training=True,
create_targets=True,
num_point_features=4,
anchor_cache=None,
random_crop=False,
reference_detections=None,
out_size_factor=2,
out_dtype=np.float32,
min_points_in_gt=-1,
logger=None,
):
"""
convert point cloud to voxels, create targets if ground truths exists.
input_dict format: dataset.get_sensor_data format
"""
assert prep_cfg is not None
remove_environment = prep_cfg.REMOVE_ENVIRONMENT
max_voxels = prep_cfg.MAX_VOXELS_NUM
shuffle_points = prep_cfg.SHUFFLE
anchor_area_threshold = prep_cfg.ANCHOR_AREA_THRES
if training:
remove_unknown = prep_cfg.REMOVE_UNKOWN_EXAMPLES
gt_rotation_noise = prep_cfg.GT_ROT_NOISE
gt_loc_noise_std = prep_cfg.GT_LOC_NOISE
global_rotation_noise = prep_cfg.GLOBAL_ROT_NOISE
global_scaling_noise = prep_cfg.GLOBAL_SCALE_NOISE
global_random_rot_range = prep_cfg.GLOBAL_ROT_PER_OBJ_RANGE
global_translate_noise_std = prep_cfg.GLOBAL_TRANS_NOISE
gt_points_drop = prep_cfg.GT_DROP_PERCENTAGE
gt_drop_max_keep = prep_cfg.GT_DROP_MAX_KEEP_POINTS
remove_points_after_sample = prep_cfg.REMOVE_POINTS_AFTER_SAMPLE
min_points_in_gt = prep_cfg.get("MIN_POINTS_IN_GT", -1)
task_class_names = [
target_assigner.classes for target_assigner in target_assigners
]
class_names = list(itertools.chain(*task_class_names))
# points_only = input_dict["lidar"]["points"]
# times = input_dict["lidar"]["times"]
# points = np.hstack([points_only, times])
try:
points = input_dict["current_frame"]["lidar"]["combined"]
except Exception:
points = input_dict["current_frame"]["lidar"]["points"]
keyframe_points = input_dict["keyframe"]["lidar"]["combined"]
if training:
anno_dict = input_dict["current_frame"]["lidar"]["annotations"]
gt_dict = {
"gt_boxes": anno_dict["boxes"],
"gt_names": np.array(anno_dict["names"]).reshape(-1),
}
if "difficulty" not in anno_dict:
difficulty = np.zeros([anno_dict["boxes"].shape[0]],
dtype=np.int32)
gt_dict["difficulty"] = difficulty
else:
gt_dict["difficulty"] = anno_dict["difficulty"]
# if use_group_id and "group_ids" in anno_dict:
# group_ids = anno_dict["group_ids"]
# gt_dict["group_ids"] = group_ids
calib = None
if "calib" in input_dict:
calib = input_dict["current_frame"]["calib"]
if reference_detections is not None:
assert calib is not None and "image" in input_dict["current_frame"]
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.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:
assert calib is not None
image_shape = input_dict["current_frame"]["image"]["image_shape"]
points = box_np_ops.remove_outside_points(points, calib["rect"],
calib["Trv2c"], calib["P2"],
image_shape)
if remove_environment is True and training:
selected = kitti.keep_arrays_by_name(gt_names, target_assigner.classes)
_dict_select(gt_dict, selected)
masks = box_np_ops.points_in_rbbox(points, gt_dict["gt_boxes"])
points = points[masks.any(-1)]
if training:
# boxes_lidar = gt_dict["gt_boxes"]
# cv2.imshow('pre-noise', bev_map)
selected = kitti.drop_arrays_by_name(gt_dict["gt_names"],
["DontCare", "ignore"])
_dict_select(gt_dict, selected)
if remove_unknown:
remove_mask = gt_dict["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)
_dict_select(gt_dict, keep_mask)
gt_dict.pop("difficulty")
if min_points_in_gt > 0:
# points_count_rbbox takes 10ms with 10 sweeps nuscenes data
point_counts = box_np_ops.points_count_rbbox(
points, gt_dict["gt_boxes"])
mask = point_counts >= min_points_in_gt
_dict_select(gt_dict, mask)
gt_boxes_mask = np.array(
[n in class_names for n in gt_dict["gt_names"]], dtype=np.bool_)
# db_sampler = None
if db_sampler is not None:
group_ids = None
# if "group_ids" in gt_dict:
# group_ids = gt_dict["group_ids"]
sampled_dict = db_sampler.sample_all(
root_path,
gt_dict["gt_boxes"],
gt_dict["gt_names"],
num_point_features,
random_crop,
gt_group_ids=group_ids,
calib=calib,
)
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_dict["gt_names"] = np.concatenate(
[gt_dict["gt_names"], sampled_gt_names], axis=0)
gt_dict["gt_boxes"] = np.concatenate(
[gt_dict["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"]
# gt_dict["group_ids"] = np.concatenate(
# [gt_dict["group_ids"], sampled_group_ids])
if remove_points_after_sample:
masks = box_np_ops.points_in_rbbox(points,
sampled_gt_boxes)
points = points[np.logical_not(masks.any(-1))]
points = np.concatenate([sampled_points, points], axis=0)
pc_range = voxel_generator.point_cloud_range
# group_ids = None
# if "group_ids" in gt_dict:
# group_ids = gt_dict["group_ids"]
# prep.noise_per_object_v3_(
# gt_dict["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
# for k, v in gt_dict.items():
# print(k, v.shape)
_dict_select(gt_dict, gt_boxes_mask)
gt_classes = np.array(
[class_names.index(n) + 1 for n in gt_dict["gt_names"]],
dtype=np.int32)
gt_dict["gt_classes"] = gt_classes
# concatenate
points_current = points.shape[0]
points_keyframe = keyframe_points.shape[0]
points = np.concatenate((points, keyframe_points), axis=0)
# data aug
gt_dict["gt_boxes"], points = prep.random_flip(gt_dict["gt_boxes"],
points)
gt_dict["gt_boxes"], points = prep.global_rotation(
gt_dict["gt_boxes"], points, rotation=global_rotation_noise)
gt_dict["gt_boxes"], points = prep.global_scaling_v2(
gt_dict["gt_boxes"], points, *global_scaling_noise)
prep.global_translate_(gt_dict["gt_boxes"], points,
global_translate_noise_std)
# slice
points_keyframe = points[points_current:, :]
points = points[:points_current, :]
bv_range = voxel_generator.point_cloud_range[[0, 1, 3, 4]]
mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range)
_dict_select(gt_dict, mask)
task_masks = []
flag = 0
for class_name in task_class_names:
task_masks.append([
np.where(gt_dict["gt_classes"] == class_name.index(i) + 1 +
flag) for i in class_name
])
flag += len(class_name)
task_boxes = []
task_classes = []
task_names = []
flag2 = 0
for idx, mask in enumerate(task_masks):
task_box = []
task_class = []
task_name = []
for m in mask:
task_box.append(gt_dict["gt_boxes"][m])
task_class.append(gt_dict["gt_classes"][m] - flag2)
task_name.append(gt_dict["gt_names"][m])
task_boxes.append(np.concatenate(task_box, axis=0))
task_classes.append(np.concatenate(task_class))
task_names.append(np.concatenate(task_name))
flag2 += len(mask)
for task_box in task_boxes:
# limit rad to [-pi, pi]
task_box[:, -1] = box_np_ops.limit_period(task_box[:, -1],
offset=0.5,
period=2 * np.pi)
# print(gt_dict.keys())
gt_dict["gt_classes"] = task_classes
gt_dict["gt_names"] = task_names
gt_dict["gt_boxes"] = task_boxes
# 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]
# points = points[:int(points.shape[0] * 0.1), :]
voxels, coordinates, num_points = voxel_generator.generate(
points, max_voxels)
# res = voxel_generator.generate(
# points, max_voxels)
# voxels = res["voxels"]
# coordinates = res["coordinates"]
# num_points = res["num_points_per_voxel"]
num_voxels = np.array([voxels.shape[0]], dtype=np.int64)
# key frame voxel
keyframe_info = voxel_generator.generate(keyframe_points, max_voxels)
keyframe_info = keyframe_voxels, keyframe_coordinates, keyframe_num_points
keyframe_num_voxels = np.array([keyframe_voxels.shape[0]], dtype=np.int64)
example = {
"voxels": voxels,
"num_points": num_points,
"points": points,
"coordinates": coordinates,
"num_voxels": num_voxels,
}
example_keyframe = {
"voxels": keyframe_voxels,
"num_points": keyframe_num_points,
"points": keyframe_points,
"coordinates": keyframe_coordinates,
"num_voxels": keyframe_num_voxels,
}
if training:
example["gt_boxes"] = gt_dict["gt_boxes"]
if calib is not None:
example["calib"] = calib
feature_map_size = grid_size[:2] // out_size_factor
feature_map_size = [*feature_map_size, 1][::-1]
if anchor_cache is not None:
anchorss = anchor_cache["anchors"]
anchors_bvs = anchor_cache["anchors_bv"]
anchors_dicts = anchor_cache["anchors_dict"]
else:
rets = [
target_assigner.generate_anchors(feature_map_size)
for target_assigner in target_assigners
]
anchorss = [ret["anchors"].reshape([-1, 7]) for ret in rets]
anchors_dicts = [
target_assigner.generate_anchors_dict(feature_map_size)
for target_assigner in target_assigners
]
anchors_bvs = [
box_np_ops.rbbox2d_to_near_bbox(anchors[:, [0, 1, 3, 4, 6]])
for anchors in anchorss
]
example["anchors"] = anchorss
if anchor_area_threshold >= 0:
example["anchors_mask"] = []
for idx, anchors_bv in enumerate(anchors_bvs):
anchors_mask = None
# slow with high resolution. recommend disable this forever.
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"].append(anchors_mask)
example_sequences = {}
example_sequences["current_frame"] = example
example_sequences["keyframe"] = example_keyframe
if not training:
return example_sequences
# voxel_labels = box_np_ops.assign_label_to_voxel(gt_boxes, coordinates,
# voxel_size, coors_range)
"""
example.update({
'gt_boxes': gt_boxes.astype(out_dtype),
'num_gt': np.array([gt_boxes.shape[0]]),
# 'voxel_labels': voxel_labels,
})
"""
if create_targets:
targets_dicts = []
for idx, target_assigner in enumerate(target_assigners):
if "anchors_mask" in example:
anchors_mask = example["anchors_mask"][idx]
else:
anchors_mask = None
targets_dict = target_assigner.assign_v2(
anchors_dicts[idx],
gt_dict["gt_boxes"][idx],
anchors_mask,
gt_classes=gt_dict["gt_classes"][idx],
gt_names=gt_dict["gt_names"][idx],
)
targets_dicts.append(targets_dict)
example_sequences["current_frame"].update({
"labels":
[targets_dict["labels"] for targets_dict in targets_dicts],
"reg_targets":
[targets_dict["bbox_targets"] for targets_dict in targets_dicts],
"reg_weights": [
targets_dict["bbox_outside_weights"]
for targets_dict in targets_dicts
],
})
return example_sequences
