def test_remove_points_in_boxes():
points = np.array([[68.1370, 3.3580, 2.5160, 0.0000],
[67.6970, 3.5500, 2.5010, 0.0000],
[67.6490, 3.7600, 2.5000, 0.0000],
[66.4140, 3.9010, 2.4590, 0.0000],
[66.0120, 4.0850, 2.4460, 0.0000],
[65.8340, 4.1780, 2.4400, 0.0000],
[65.8410, 4.3860, 2.4400, 0.0000],
[65.7450, 4.5870, 2.4380, 0.0000],
[65.5510, 4.7800, 2.4320, 0.0000],
[65.4860, 4.9820, 2.4300, 0.0000]])
boxes = np.array(
[[30.0285, 10.5110, -1.5304, 0.5100, 0.8700, 1.6000, 1.6400],
[7.8369, 1.6053, -1.5605, 0.5800, 1.2300, 1.8200, -3.1000],
[10.8740, -1.0827, -1.3310, 0.6000, 0.5200, 1.7100, 1.3500],
[14.9783, 2.2466, -1.4950, 0.6100, 0.7300, 1.5300, -1.9200],
[11.0656, 0.6195, -1.5202, 0.6600, 1.0100, 1.7600, -1.4600],
[10.5994, -7.9049, -1.4980, 0.5300, 1.9600, 1.6800, 1.5600],
[28.7068, -8.8244, -1.1485, 0.6500, 1.7900, 1.7500, 3.1200],
[20.2630, 5.1947, -1.4799, 0.7300, 1.7600, 1.7300, 1.5100],
[18.2496, 3.1887, -1.6109, 0.5600, 1.6800, 1.7100, 1.5600],
[7.7396, -4.3245, -1.5801, 0.5600, 1.7900, 1.8000, -0.8300]])
points = LiDARPoints(points, points_dim=4)
points = ObjectSample.remove_points_in_boxes(points, boxes)
assert points.tensor.numpy().shape == (10, 4)
def test_object_noise():
np.random.seed(0)
object_noise = ObjectNoise()
points = np.fromfile(
'./tests/data/kitti/training/velodyne_reduced/000000.bin',
np.float32).reshape(-1, 4)
annos = mmcv.load('./tests/data/kitti/kitti_infos_train.pkl')
info = annos[0]
rect = info['calib']['R0_rect'].astype(np.float32)
Trv2c = info['calib']['Tr_velo_to_cam'].astype(np.float32)
annos = info['annos']
loc = annos['location']
dims = annos['dimensions']
rots = annos['rotation_y']
gt_bboxes_3d = np.concatenate([loc, dims, rots[..., np.newaxis]],
axis=1).astype(np.float32)
gt_bboxes_3d = CameraInstance3DBoxes(gt_bboxes_3d).convert_to(
Box3DMode.LIDAR, np.linalg.inv(rect @ Trv2c))
points = LiDARPoints(points, points_dim=4)
input_dict = dict(points=points, gt_bboxes_3d=gt_bboxes_3d)
input_dict = object_noise(input_dict)
points = input_dict['points']
gt_bboxes_3d = input_dict['gt_bboxes_3d'].tensor
expected_gt_bboxes_3d = torch.tensor(
[[9.1724, -1.7559, -1.3550, 0.4800, 1.2000, 1.8900, 0.0505]])
repr_str = repr(object_noise)
expected_repr_str = 'ObjectNoise(num_try=100, ' \
'translation_std=[0.25, 0.25, 0.25], ' \
'global_rot_range=[0.0, 0.0], ' \
'rot_range=[-0.15707963267, 0.15707963267])'
assert repr_str == expected_repr_str
assert points.tensor.numpy().shape == (800, 4)
assert torch.allclose(gt_bboxes_3d, expected_gt_bboxes_3d, 1e-3)
def test_points_sample():
np.random.seed(0)
points = np.fromfile(
'./tests/data/kitti/training/velodyne_reduced/000000.bin',
np.float32).reshape(-1, 4)
annos = mmcv.load('./tests/data/kitti/kitti_infos_train.pkl')
info = annos[0]
rect = torch.tensor(info['calib']['R0_rect'].astype(np.float32))
Trv2c = torch.tensor(info['calib']['Tr_velo_to_cam'].astype(np.float32))
points = LiDARPoints(points.copy(),
points_dim=4).convert_to(Coord3DMode.CAM,
rect @ Trv2c)
num_points = 20
sample_range = 40
input_dict = dict(points=points.clone())
point_sample = PointSample(num_points=num_points,
sample_range=sample_range)
sampled_pts = point_sample(input_dict)['points']
select_idx = np.array([
622, 146, 231, 444, 504, 533, 80, 401, 379, 2, 707, 562, 176, 491, 496,
464, 15, 590, 194, 449
])
expected_pts = points.tensor.numpy()[select_idx]
assert np.allclose(sampled_pts.tensor.numpy(), expected_pts)
repr_str = repr(point_sample)
expected_repr_str = f'PointSample(num_points={num_points}, ' \
f'sample_range={sample_range}, ' \
'replace=False)'
assert repr_str == expected_repr_str
# test when number of far points are larger than number of sampled points
np.random.seed(0)
point_sample = PointSample(num_points=2, sample_range=sample_range)
input_dict = dict(points=points.clone())
sampled_pts = point_sample(input_dict)['points']
select_idx = np.array([449, 444])
expected_pts = points.tensor.numpy()[select_idx]
assert np.allclose(sampled_pts.tensor.numpy(), expected_pts)
def test_random_flip_3d():
random_flip_3d = RandomFlip3D(flip_ratio_bev_horizontal=1.0,
flip_ratio_bev_vertical=1.0)
points = np.array([[22.7035, 9.3901, -0.2848, 0.0000],
[21.9826, 9.1766, -0.2698, 0.0000],
[21.4329, 9.0209, -0.2578, 0.0000],
[21.3068, 9.0205, -0.2558, 0.0000],
[21.3400, 9.1305, -0.2578, 0.0000],
[21.3291, 9.2099, -0.2588, 0.0000],
[21.2759, 9.2599, -0.2578, 0.0000],
[21.2686, 9.2982, -0.2588, 0.0000],
[21.2334, 9.3607, -0.2588, 0.0000],
[21.2179, 9.4372, -0.2598, 0.0000]])
bbox3d_fields = ['gt_bboxes_3d']
img_fields = []
box_type_3d = LiDARInstance3DBoxes
gt_bboxes_3d = LiDARInstance3DBoxes(
torch.tensor(
[[38.9229, 18.4417, -1.1459, 0.7100, 1.7600, 1.8600, -2.2652],
[12.7768, 0.5795, -2.2682, 0.5700, 0.9900, 1.7200, -2.5029],
[12.7557, 2.2996, -1.4869, 0.6100, 1.1100, 1.9000, -1.9390],
[10.6677, 0.8064, -1.5435, 0.7900, 0.9600, 1.7900, 1.0856],
[5.0903, 5.1004, -1.2694, 0.7100, 1.7000, 1.8300, -1.9136]]))
points = LiDARPoints(points, points_dim=4)
input_dict = dict(points=points,
bbox3d_fields=bbox3d_fields,
box_type_3d=box_type_3d,
img_fields=img_fields,
gt_bboxes_3d=gt_bboxes_3d)
input_dict = random_flip_3d(input_dict)
points = input_dict['points'].tensor.numpy()
gt_bboxes_3d = input_dict['gt_bboxes_3d'].tensor
expected_points = np.array([[22.7035, -9.3901, -0.2848, 0.0000],
[21.9826, -9.1766, -0.2698, 0.0000],
[21.4329, -9.0209, -0.2578, 0.0000],
[21.3068, -9.0205, -0.2558, 0.0000],
[21.3400, -9.1305, -0.2578, 0.0000],
[21.3291, -9.2099, -0.2588, 0.0000],
[21.2759, -9.2599, -0.2578, 0.0000],
[21.2686, -9.2982, -0.2588, 0.0000],
[21.2334, -9.3607, -0.2588, 0.0000],
[21.2179, -9.4372, -0.2598, 0.0000]])
expected_gt_bboxes_3d = torch.tensor(
[[38.9229, -18.4417, -1.1459, 0.7100, 1.7600, 1.8600, 5.4068],
[12.7768, -0.5795, -2.2682, 0.5700, 0.9900, 1.7200, 5.6445],
[12.7557, -2.2996, -1.4869, 0.6100, 1.1100, 1.9000, 5.0806],
[10.6677, -0.8064, -1.5435, 0.7900, 0.9600, 1.7900, 2.0560],
[5.0903, -5.1004, -1.2694, 0.7100, 1.7000, 1.8300, 5.0552]])
repr_str = repr(random_flip_3d)
expected_repr_str = 'RandomFlip3D(sync_2d=True,' \
' flip_ratio_bev_vertical=1.0)'
assert np.allclose(points, expected_points)
assert torch.allclose(gt_bboxes_3d, expected_gt_bboxes_3d)
assert repr_str == expected_repr_str
def test_background_points_filter():
np.random.seed(0)
background_points_filter = BackgroundPointsFilter((0.5, 2.0, 0.5))
points = np.fromfile(
'./tests/data/kitti/training/velodyne_reduced/000000.bin',
np.float32).reshape(-1, 4)
orig_points = points.copy()
annos = mmcv.load('./tests/data/kitti/kitti_infos_train.pkl')
info = annos[0]
rect = info['calib']['R0_rect'].astype(np.float32)
Trv2c = info['calib']['Tr_velo_to_cam'].astype(np.float32)
annos = info['annos']
loc = annos['location']
dims = annos['dimensions']
rots = annos['rotation_y']
gt_bboxes_3d = np.concatenate([loc, dims, rots[..., np.newaxis]],
axis=1).astype(np.float32)
gt_bboxes_3d = CameraInstance3DBoxes(gt_bboxes_3d).convert_to(
Box3DMode.LIDAR, np.linalg.inv(rect @ Trv2c))
extra_points = gt_bboxes_3d.corners.reshape(8, 3)[[1, 2, 5, 6], :]
extra_points[:, 2] += 0.1
extra_points = torch.cat([extra_points, extra_points.new_zeros(4, 1)], 1)
points = np.concatenate([points, extra_points.numpy()], 0)
points = LiDARPoints(points, points_dim=4)
input_dict = dict(points=points, gt_bboxes_3d=gt_bboxes_3d)
origin_gt_bboxes_3d = gt_bboxes_3d.clone()
input_dict = background_points_filter(input_dict)
points = input_dict['points'].tensor.numpy()
repr_str = repr(background_points_filter)
expected_repr_str = 'BackgroundPointsFilter(bbox_enlarge_range=' \
'[[0.5, 2.0, 0.5]])'
assert repr_str == expected_repr_str
assert points.shape == (800, 4)
assert np.equal(orig_points, points).all()
assert np.equal(input_dict['gt_bboxes_3d'].tensor.numpy(),
origin_gt_bboxes_3d.tensor.numpy()).all()
# test single float config
BackgroundPointsFilter(0.5)
# The length of bbox_enlarge_range should be 3
with pytest.raises(AssertionError):
BackgroundPointsFilter((0.5, 2.0))
def test_load_points_from_multi_sweeps():
load_points_from_multi_sweeps = LoadPointsFromMultiSweeps()
sweep = dict(
data_path='./tests/data/nuscenes/sweeps/LIDAR_TOP/'
'n008-2018-09-18-12-07-26-0400__LIDAR_TOP__1537287083900561.pcd.bin',
timestamp=1537290014899034,
sensor2lidar_translation=[-0.02344713, -3.88266051, -0.17151584],
sensor2lidar_rotation=np.array(
[[9.99979347e-01, 3.99870769e-04, 6.41441690e-03],
[-4.42034222e-04, 9.99978299e-01, 6.57316197e-03],
[-6.41164929e-03, -6.57586161e-03, 9.99957824e-01]]))
points = LiDARPoints(np.array([[1., 2., 3., 4., 5.], [1., 2., 3., 4., 5.],
[1., 2., 3., 4., 5.]]),
points_dim=5)
results = dict(points=points, timestamp=1537290014899034, sweeps=[sweep])
results = load_points_from_multi_sweeps(results)
points = results['points'].tensor.numpy()
repr_str = repr(load_points_from_multi_sweeps)
expected_repr_str = 'LoadPointsFromMultiSweeps(sweeps_num=10)'
assert repr_str == expected_repr_str
assert points.shape == (403, 4)
def test_voxel_based_point_filter():
np.random.seed(0)
cur_sweep_cfg = dict(voxel_size=[0.1, 0.1, 0.1],
point_cloud_range=[-50, -50, -4, 50, 50, 2],
max_num_points=1,
max_voxels=1024)
prev_sweep_cfg = dict(voxel_size=[0.1, 0.1, 0.1],
point_cloud_range=[-50, -50, -4, 50, 50, 2],
max_num_points=1,
max_voxels=1024)
voxel_based_points_filter = VoxelBasedPointSampler(cur_sweep_cfg,
prev_sweep_cfg,
time_dim=3)
points = np.stack([
np.random.rand(4096) * 120 - 60,
np.random.rand(4096) * 120 - 60,
np.random.rand(4096) * 10 - 6
],
axis=-1)
input_time = np.concatenate([np.zeros([2048, 1]), np.ones([2048, 1])], 0)
input_points = np.concatenate([points, input_time], 1)
input_points = LiDARPoints(input_points, points_dim=4)
input_dict = dict(points=input_points,
pts_mask_fields=[],
pts_seg_fields=[])
input_dict = voxel_based_points_filter(input_dict)
points = input_dict['points']
repr_str = repr(voxel_based_points_filter)
expected_repr_str = """VoxelBasedPointSampler(
num_cur_sweep=1024,
num_prev_sweep=1024,
time_dim=3,
cur_voxel_generator=
VoxelGenerator(voxel_size=[0.1 0.1 0.1],
point_cloud_range=[-50.0, -50.0, -4.0, 50.0, 50.0, 2.0],
max_num_points=1,
max_voxels=1024,
grid_size=[1000, 1000, 60]),
prev_voxel_generator=
VoxelGenerator(voxel_size=[0.1 0.1 0.1],
point_cloud_range=[-50.0, -50.0, -4.0, 50.0, 50.0, 2.0],
max_num_points=1,
max_voxels=1024,
grid_size=[1000, 1000, 60]))"""
assert repr_str == expected_repr_str
assert points.shape == (2048, 4)
assert (points.tensor[:, :3].min(0)[0].numpy() <
cur_sweep_cfg['point_cloud_range'][0:3]).sum() == 0
assert (points.tensor[:, :3].max(0)[0].numpy() >
cur_sweep_cfg['point_cloud_range'][3:6]).sum() == 0
# Test instance mask and semantic mask
input_dict = dict(points=input_points)
input_dict['pts_instance_mask'] = np.random.randint(0, 10, [4096])
input_dict['pts_semantic_mask'] = np.random.randint(0, 6, [4096])
input_dict['pts_mask_fields'] = ['pts_instance_mask']
input_dict['pts_seg_fields'] = ['pts_semantic_mask']
input_dict = voxel_based_points_filter(input_dict)
pts_instance_mask = input_dict['pts_instance_mask']
pts_semantic_mask = input_dict['pts_semantic_mask']
assert pts_instance_mask.shape == (2048, )
assert pts_semantic_mask.shape == (2048, )
assert pts_instance_mask.max() < 10
assert pts_instance_mask.min() >= 0
assert pts_semantic_mask.max() < 6
assert pts_semantic_mask.min() >= 0
def test_object_sample():
db_sampler = mmcv.ConfigDict({
'data_root': './tests/data/kitti/',
'info_path': './tests/data/kitti/kitti_dbinfos_train.pkl',
'rate': 1.0,
'prepare': {
'filter_by_difficulty': [-1],
'filter_by_min_points': {
'Pedestrian': 10
}
},
'classes': ['Pedestrian', 'Cyclist', 'Car'],
'sample_groups': {
'Pedestrian': 6
}
})
np.random.seed(0)
object_sample = ObjectSample(db_sampler)
points = np.fromfile(
'./tests/data/kitti/training/velodyne_reduced/000000.bin',
np.float32).reshape(-1, 4)
annos = mmcv.load('./tests/data/kitti/kitti_infos_train.pkl')
info = annos[0]
rect = info['calib']['R0_rect'].astype(np.float32)
Trv2c = info['calib']['Tr_velo_to_cam'].astype(np.float32)
annos = info['annos']
loc = annos['location']
dims = annos['dimensions']
rots = annos['rotation_y']
gt_names = annos['name']
gt_bboxes_3d = np.concatenate([loc, dims, rots[..., np.newaxis]],
axis=1).astype(np.float32)
gt_bboxes_3d = CameraInstance3DBoxes(gt_bboxes_3d).convert_to(
Box3DMode.LIDAR, np.linalg.inv(rect @ Trv2c))
CLASSES = ('Pedestrian', 'Cyclist', 'Car')
gt_labels = []
for cat in gt_names:
if cat in CLASSES:
gt_labels.append(CLASSES.index(cat))
else:
gt_labels.append(-1)
gt_labels = np.array(gt_labels, dtype=np.long)
points = LiDARPoints(points, points_dim=4)
input_dict = dict(points=points,
gt_bboxes_3d=gt_bboxes_3d,
gt_labels_3d=gt_labels)
input_dict = object_sample(input_dict)
points = input_dict['points']
gt_bboxes_3d = input_dict['gt_bboxes_3d']
gt_labels_3d = input_dict['gt_labels_3d']
repr_str = repr(object_sample)
expected_repr_str = 'ObjectSample sample_2d=False, ' \
'data_root=./tests/data/kitti/, ' \
'info_path=./tests/data/kitti/kitti' \
'_dbinfos_train.pkl, rate=1.0, ' \
'prepare={\'filter_by_difficulty\': [-1], ' \
'\'filter_by_min_points\': {\'Pedestrian\': 10}}, ' \
'classes=[\'Pedestrian\', \'Cyclist\', \'Car\'], ' \
'sample_groups={\'Pedestrian\': 6}'
assert repr_str == expected_repr_str
assert points.tensor.numpy().shape == (800, 4)
assert gt_bboxes_3d.tensor.shape == (1, 7)
assert np.all(gt_labels_3d == [0])
def test_points_conversion():
"""Test the conversion of points between different modes."""
points_np = np.array([[
-5.24223238e+00, 4.00209696e+01, 2.97570381e-01, 0.6666, 0.1956,
0.4974, 0.9409
],
[
-2.66751588e+01, 5.59499564e+00, -9.14345860e-01,
0.1502, 0.3707, 0.1086, 0.6297
],
[
-5.80979675e+00, 3.54092357e+01, 2.00889888e-01,
0.6565, 0.6248, 0.6954, 0.2538
],
[
-3.13086877e+01, 1.09007628e+00, -1.94612112e-01,
0.2803, 0.0258, 0.4896, 0.3269
]],
dtype=np.float32)
# test CAM to LIDAR and DEPTH
cam_points = CameraPoints(points_np,
points_dim=7,
attribute_dims=dict(color=[3, 4, 5], height=6))
convert_lidar_points = cam_points.convert_to(Coord3DMode.LIDAR)
expected_tensor = torch.tensor([[
2.9757e-01, 5.2422e+00, -4.0021e+01, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-9.1435e-01, 2.6675e+01, -5.5950e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
2.0089e-01, 5.8098e+00, -3.5409e+01,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-1.9461e-01, 3.1309e+01, -1.0901e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
lidar_point_tensor = Coord3DMode.convert_point(cam_points.tensor,
Coord3DMode.CAM,
Coord3DMode.LIDAR)
assert torch.allclose(expected_tensor, convert_lidar_points.tensor, 1e-4)
assert torch.allclose(lidar_point_tensor, convert_lidar_points.tensor,
1e-4)
convert_depth_points = cam_points.convert_to(Coord3DMode.DEPTH)
expected_tensor = torch.tensor([[
-5.2422e+00, 2.9757e-01, -4.0021e+01, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.6675e+01, -9.1435e-01, -5.5950e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
-5.8098e+00, 2.0089e-01, -3.5409e+01,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-3.1309e+01, -1.9461e-01, -1.0901e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
depth_point_tensor = Coord3DMode.convert_point(cam_points.tensor,
Coord3DMode.CAM,
Coord3DMode.DEPTH)
assert torch.allclose(expected_tensor, convert_depth_points.tensor, 1e-4)
assert torch.allclose(depth_point_tensor, convert_depth_points.tensor,
1e-4)
# test LIDAR to CAM and DEPTH
lidar_points = LiDARPoints(points_np,
points_dim=7,
attribute_dims=dict(color=[3, 4, 5], height=6))
convert_cam_points = lidar_points.convert_to(Coord3DMode.CAM)
expected_tensor = torch.tensor([[
-4.0021e+01, -2.9757e-01, -5.2422e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-5.5950e+00, 9.1435e-01, -2.6675e+01,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
-3.5409e+01, -2.0089e-01, -5.8098e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-1.0901e+00, 1.9461e-01, -3.1309e+01,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
cam_point_tensor = Coord3DMode.convert_point(lidar_points.tensor,
Coord3DMode.LIDAR,
Coord3DMode.CAM)
assert torch.allclose(expected_tensor, convert_cam_points.tensor, 1e-4)
assert torch.allclose(cam_point_tensor, convert_cam_points.tensor, 1e-4)
convert_depth_points = lidar_points.convert_to(Coord3DMode.DEPTH)
expected_tensor = torch.tensor([[
-4.0021e+01, -5.2422e+00, 2.9757e-01, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-5.5950e+00, -2.6675e+01, -9.1435e-01,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
-3.5409e+01, -5.8098e+00, 2.0089e-01,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-1.0901e+00, -3.1309e+01, -1.9461e-01,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
depth_point_tensor = Coord3DMode.convert_point(lidar_points.tensor,
Coord3DMode.LIDAR,
Coord3DMode.DEPTH)
assert torch.allclose(expected_tensor, convert_depth_points.tensor, 1e-4)
assert torch.allclose(depth_point_tensor, convert_depth_points.tensor,
1e-4)
# test DEPTH to CAM and LIDAR
depth_points = DepthPoints(points_np,
points_dim=7,
attribute_dims=dict(color=[3, 4, 5], height=6))
convert_cam_points = depth_points.convert_to(Coord3DMode.CAM)
expected_tensor = torch.tensor([[
-5.2422e+00, -2.9757e-01, 4.0021e+01, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.6675e+01, 9.1435e-01, 5.5950e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
-5.8098e+00, -2.0089e-01, 3.5409e+01,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-3.1309e+01, 1.9461e-01, 1.0901e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
cam_point_tensor = Coord3DMode.convert_point(depth_points.tensor,
Coord3DMode.DEPTH,
Coord3DMode.CAM)
assert torch.allclose(expected_tensor, convert_cam_points.tensor, 1e-4)
assert torch.allclose(cam_point_tensor, convert_cam_points.tensor, 1e-4)
rt_mat_provided = torch.tensor([[0.99789, -0.012698, -0.063678],
[-0.012698, 0.92359, -0.38316],
[0.063678, 0.38316, 0.92148]])
depth_points_new = torch.cat([
depth_points.tensor[:, :3] @ rt_mat_provided.t(),
depth_points.tensor[:, 3:]
],
dim=1)
mat = rt_mat_provided.new_tensor([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
rt_mat_provided = mat @ rt_mat_provided.transpose(1, 0)
cam_point_tensor_new = Coord3DMode.convert_point(depth_points_new,
Coord3DMode.DEPTH,
Coord3DMode.CAM,
rt_mat=rt_mat_provided)
assert torch.allclose(expected_tensor, cam_point_tensor_new, 1e-4)
convert_lidar_points = depth_points.convert_to(Coord3DMode.LIDAR)
expected_tensor = torch.tensor([[
4.0021e+01, 5.2422e+00, 2.9757e-01, 6.6660e-01, 1.9560e-01, 4.9740e-01,
9.4090e-01
],
[
5.5950e+00, 2.6675e+01, -9.1435e-01,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
3.5409e+01, 5.8098e+00, 2.0089e-01,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
1.0901e+00, 3.1309e+01, -1.9461e-01,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
lidar_point_tensor = Coord3DMode.convert_point(depth_points.tensor,
Coord3DMode.DEPTH,
Coord3DMode.LIDAR)
assert torch.allclose(lidar_point_tensor, convert_lidar_points.tensor,
1e-4)
assert torch.allclose(lidar_point_tensor, convert_lidar_points.tensor,
1e-4)
def test_lidar_points():
# test empty initialization
empty_boxes = []
points = LiDARPoints(empty_boxes)
assert points.tensor.shape[0] == 0
assert points.tensor.shape[1] == 3
# Test init with origin
points_np = np.array([[-5.24223238e+00, 4.00209696e+01, 2.97570381e-01],
[-2.66751588e+01, 5.59499564e+00, -9.14345860e-01],
[-5.80979675e+00, 3.54092357e+01, 2.00889888e-01],
[-3.13086877e+01, 1.09007628e+00, -1.94612112e-01]],
dtype=np.float32)
lidar_points = LiDARPoints(points_np, points_dim=3)
assert lidar_points.tensor.shape[0] == 4
# Test init with color and height
points_np = np.array([[
-5.24223238e+00, 4.00209696e+01, 2.97570381e-01, 0.6666, 0.1956,
0.4974, 0.9409
],
[
-2.66751588e+01, 5.59499564e+00, -9.14345860e-01,
0.1502, 0.3707, 0.1086, 0.6297
],
[
-5.80979675e+00, 3.54092357e+01, 2.00889888e-01,
0.6565, 0.6248, 0.6954, 0.2538
],
[
-3.13086877e+01, 1.09007628e+00, -1.94612112e-01,
0.2803, 0.0258, 0.4896, 0.3269
]],
dtype=np.float32)
lidar_points = LiDARPoints(
points_np,
points_dim=7,
attribute_dims=dict(color=[3, 4, 5], height=6))
expected_tensor = torch.tensor([[
-5.24223238e+00, 4.00209696e+01, 2.97570381e-01, 0.6666, 0.1956,
0.4974, 0.9409
],
[
-2.66751588e+01, 5.59499564e+00,
-9.14345860e-01, 0.1502, 0.3707,
0.1086, 0.6297
],
[
-5.80979675e+00, 3.54092357e+01,
2.00889888e-01, 0.6565, 0.6248, 0.6954,
0.2538
],
[
-3.13086877e+01, 1.09007628e+00,
-1.94612112e-01, 0.2803, 0.0258,
0.4896, 0.3269
]])
assert torch.allclose(expected_tensor, lidar_points.tensor)
assert torch.allclose(expected_tensor[:, :3], lidar_points.coord)
assert torch.allclose(expected_tensor[:, 3:6], lidar_points.color)
assert torch.allclose(expected_tensor[:, 6], lidar_points.height)
# test points clone
new_lidar_points = lidar_points.clone()
assert torch.allclose(new_lidar_points.tensor, lidar_points.tensor)
# test points shuffle
new_lidar_points.shuffle()
assert new_lidar_points.tensor.shape == torch.Size([4, 7])
# test points rotation
rot_mat = torch.tensor([[0.93629336, -0.27509585, 0.21835066],
[0.28962948, 0.95642509, -0.03695701],
[-0.19866933, 0.0978434, 0.97517033]])
lidar_points.rotate(rot_mat)
expected_tensor = torch.tensor([[
6.6239e+00, 3.9748e+01, -2.3335e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.3174e+01, 1.2600e+01, -6.9230e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
4.7760e+00, 3.5484e+01, -2.3813e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-2.8960e+01, 9.6364e+00, -7.0663e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
assert torch.allclose(expected_tensor, lidar_points.tensor, 1e-3)
new_lidar_points = lidar_points.clone()
new_lidar_points.rotate(0.1, axis=2)
expected_tensor = torch.tensor([[
2.6226e+00, 4.0211e+01, -2.3335e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.4316e+01, 1.0224e+01, -6.9230e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
1.2096e+00, 3.5784e+01, -2.3813e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-2.9777e+01, 6.6971e+00, -7.0663e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
assert torch.allclose(expected_tensor, new_lidar_points.tensor, 1e-3)
# test points translation
translation_vector = torch.tensor([0.93629336, -0.27509585, 0.21835066])
lidar_points.translate(translation_vector)
expected_tensor = torch.tensor([[
7.5602e+00, 3.9473e+01, -2.1152e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.2237e+01, 1.2325e+01, -6.7046e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
5.7123e+00, 3.5209e+01, -2.1629e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-2.8023e+01, 9.3613e+00, -6.8480e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
assert torch.allclose(expected_tensor, lidar_points.tensor, 1e-4)
# test points filter
point_range = [-10, -40, -10, 10, 40, 10]
in_range_flags = lidar_points.in_range_3d(point_range)
expected_flags = torch.tensor([True, False, True, False])
assert torch.all(in_range_flags == expected_flags)
# test points scale
lidar_points.scale(1.2)
expected_tensor = torch.tensor([[
9.0722e+00, 4.7368e+01, -2.5382e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.6685e+01, 1.4790e+01, -8.0455e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
6.8547e+00, 4.2251e+01, -2.5955e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-3.3628e+01, 1.1234e+01, -8.2176e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
assert torch.allclose(expected_tensor, lidar_points.tensor, 1e-3)
# test get_item
expected_tensor = torch.tensor(
[[-26.6848, 14.7898, -8.0455, 0.1502, 0.3707, 0.1086, 0.6297]])
assert torch.allclose(expected_tensor, lidar_points[1].tensor, 1e-4)
expected_tensor = torch.tensor(
[[-26.6848, 14.7898, -8.0455, 0.1502, 0.3707, 0.1086, 0.6297],
[6.8547, 42.2509, -2.5955, 0.6565, 0.6248, 0.6954, 0.2538]])
assert torch.allclose(expected_tensor, lidar_points[1:3].tensor, 1e-4)
mask = torch.tensor([True, False, True, False])
expected_tensor = torch.tensor(
[[9.0722, 47.3678, -2.5382, 0.6666, 0.1956, 0.4974, 0.9409],
[6.8547, 42.2509, -2.5955, 0.6565, 0.6248, 0.6954, 0.2538]])
assert torch.allclose(expected_tensor, lidar_points[mask].tensor, 1e-4)
# test length
assert len(lidar_points) == 4
# test repr
expected_repr = 'LiDARPoints(\n '\
'tensor([[ 9.0722e+00, 4.7368e+01, -2.5382e+00, '\
'6.6660e-01, 1.9560e-01,\n 4.9740e-01, '\
'9.4090e-01],\n '\
'[-2.6685e+01, 1.4790e+01, -8.0455e+00, 1.5020e-01, '\
'3.7070e-01,\n '\
'1.0860e-01, 6.2970e-01],\n '\
'[ 6.8547e+00, 4.2251e+01, -2.5955e+00, 6.5650e-01, '\
'6.2480e-01,\n '\
'6.9540e-01, 2.5380e-01],\n '\
'[-3.3628e+01, 1.1234e+01, -8.2176e+00, 2.8030e-01, '\
'2.5800e-02,\n '\
'4.8960e-01, 3.2690e-01]]))'
assert expected_repr == str(lidar_points)
# test concatenate
lidar_points_clone = lidar_points.clone()
cat_points = LiDARPoints.cat([lidar_points, lidar_points_clone])
assert torch.allclose(cat_points.tensor[:len(lidar_points)],
lidar_points.tensor)
# test iteration
for i, point in enumerate(lidar_points):
assert torch.allclose(point, lidar_points.tensor[i])
# test new_point
new_points = lidar_points.new_point([[1, 2, 3, 4, 5, 6, 7]])
assert torch.allclose(
new_points.tensor,
torch.tensor([[1, 2, 3, 4, 5, 6, 7]], dtype=lidar_points.tensor.dtype))
# test in_range_bev
point_bev_range = [-30, -40, 30, 40]
in_range_flags = lidar_points.in_range_bev(point_bev_range)
expected_flags = torch.tensor([False, True, False, False])
assert torch.all(in_range_flags == expected_flags)
# test flip
lidar_points.flip(bev_direction='horizontal')
expected_tensor = torch.tensor([[
9.0722e+00, -4.7368e+01, -2.5382e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
-2.6685e+01, -1.4790e+01, -8.0455e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
6.8547e+00, -4.2251e+01, -2.5955e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
-3.3628e+01, -1.1234e+01, -8.2176e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
assert torch.allclose(expected_tensor, lidar_points.tensor, 1e-4)
lidar_points.flip(bev_direction='vertical')
expected_tensor = torch.tensor([[
-9.0722e+00, -4.7368e+01, -2.5382e+00, 6.6660e-01, 1.9560e-01,
4.9740e-01, 9.4090e-01
],
[
2.6685e+01, -1.4790e+01, -8.0455e+00,
1.5020e-01, 3.7070e-01, 1.0860e-01,
6.2970e-01
],
[
-6.8547e+00, -4.2251e+01, -2.5955e+00,
6.5650e-01, 6.2480e-01, 6.9540e-01,
2.5380e-01
],
[
3.3628e+01, -1.1234e+01, -8.2176e+00,
2.8030e-01, 2.5800e-02, 4.8960e-01,
3.2690e-01
]])
assert torch.allclose(expected_tensor, lidar_points.tensor, 1e-4)
def test_load_points_from_multi_sweeps():
np.random.seed(0)
file_client_args = dict(backend='disk')
load_points_from_multi_sweeps_1 = LoadPointsFromMultiSweeps(
sweeps_num=9,
use_dim=[0, 1, 2, 3, 4],
file_client_args=file_client_args)
load_points_from_multi_sweeps_2 = LoadPointsFromMultiSweeps(
sweeps_num=9,
use_dim=[0, 1, 2, 3, 4],
file_client_args=file_client_args,
pad_empty_sweeps=True,
remove_close=True)
load_points_from_multi_sweeps_3 = LoadPointsFromMultiSweeps(
sweeps_num=9,
use_dim=[0, 1, 2, 3, 4],
file_client_args=file_client_args,
pad_empty_sweeps=True,
remove_close=True,
test_mode=True)
points = np.random.random([100, 5]) * 2
points = LiDARPoints(points, points_dim=5)
input_results = dict(points=points, sweeps=[], timestamp=None)
results = load_points_from_multi_sweeps_1(input_results)
assert results['points'].tensor.numpy().shape == (100, 5)
input_results = dict(points=points, sweeps=[], timestamp=None)
results = load_points_from_multi_sweeps_2(input_results)
assert results['points'].tensor.numpy().shape == (775, 5)
sensor2lidar_rotation = np.array(
[[9.99999967e-01, 1.13183067e-05, 2.56845368e-04],
[-1.12839618e-05, 9.99999991e-01, -1.33719456e-04],
[-2.56846879e-04, 1.33716553e-04, 9.99999958e-01]])
sensor2lidar_translation = np.array([-0.0009198, -0.03964854, -0.00190136])
sweep = dict(data_path='tests/data/nuscenes/sweeps/LIDAR_TOP/'
'n008-2018-09-18-12-07-26-0400__LIDAR_TOP__'
'1537287083900561.pcd.bin',
sensor2lidar_rotation=sensor2lidar_rotation,
sensor2lidar_translation=sensor2lidar_translation,
timestamp=0)
input_results = dict(points=points, sweeps=[sweep], timestamp=1.0)
results = load_points_from_multi_sweeps_1(input_results)
assert results['points'].tensor.numpy().shape == (500, 5)
input_results = dict(points=points, sweeps=[sweep], timestamp=1.0)
results = load_points_from_multi_sweeps_2(input_results)
assert results['points'].tensor.numpy().shape == (451, 5)
input_results = dict(points=points, sweeps=[sweep] * 10, timestamp=1.0)
results = load_points_from_multi_sweeps_2(input_results)
assert results['points'].tensor.numpy().shape == (3259, 5)
input_results = dict(points=points, sweeps=[sweep] * 10, timestamp=1.0)
results = load_points_from_multi_sweeps_3(input_results)
assert results['points'].tensor.numpy().shape == (3259, 5)
