import numpy as np import pytest import torch import unittest from mmdet3d.core.bbox import (BaseInstance3DBoxes, Box3DMode, CameraInstance3DBoxes, DepthInstance3DBoxes, LiDARInstance3DBoxes, bbox3d2roi, bbox3d_mapping_back) from mmdet3d.core.bbox.structures.utils import (get_box_type, limit_period, points_cam2img, rotation_3d_in_axis, xywhr2xyxyr) from mmdet3d.core.points import CameraPoints, DepthPoints, LiDARPoints def test_bbox3d_mapping_back(): bboxes = BaseInstance3DBoxes( [[ -5.24223238e+00, 4.00209696e+01, 2.97570381e-01, 2.06200000e+00, 4.40900000e+00, 1.54800000e+00, -1.48801203e+00 ], [ -2.66751588e+01, 5.59499564e+00, -9.14345860e-01, 3.43000000e-01, 4.58000000e-01, 7.82000000e-01, -4.62759755e+00 ], [ -5.80979675e+00, 3.54092357e+01, 2.00889888e-01, 2.39600000e+00, 3.96900000e+00, 1.73200000e+00, -4.65203216e+00 ], [ -3.13086877e+01, 1.09007628e+00, -1.94612112e-01, 1.94400000e+00, 3.85700000e+00, 1.72300000e+00, -2.81427027e+00 ]]) new_bboxes = bbox3d_mapping_back(bboxes, 1.1, True, True) expected_new_bboxes = torch.tensor( [[-4.7657, 36.3827, 0.2705, 1.8745, 4.0082, 1.4073, -1.4880], [-24.2501, 5.0864, -0.8312, 0.3118, 0.4164, 0.7109, -4.6276], [-5.2816, 32.1902, 0.1826, 2.1782, 3.6082, 1.5745, -4.6520], [-28.4624, 0.9910, -0.1769, 1.7673, 3.5064, 1.5664, -2.8143]]) assert torch.allclose(new_bboxes.tensor, expected_new_bboxes, atol=1e-4) def test_bbox3d2roi(): bbox_0 = torch.tensor( [[-5.2422, 4.0020, 2.9757, 2.0620, 4.4090, 1.5480, -1.4880], [-5.8097, 3.5409, 2.0088, 2.3960, 3.9690, 1.7320, -4.6520]]) bbox_1 = torch.tensor( [[-2.6675, 5.5949, -9.1434, 3.4300, 4.5800, 7.8200, -4.6275], [-3.1308, 1.0900, -1.9461, 1.9440, 3.8570, 1.7230, -2.8142]]) bbox_list = [bbox_0, bbox_1] rois = bbox3d2roi(bbox_list) expected_rois = torch.tensor( [[0.0000, -5.2422, 4.0020, 2.9757, 2.0620, 4.4090, 1.5480, -1.4880], [0.0000, -5.8097, 3.5409, 2.0088, 2.3960, 3.9690, 1.7320, -4.6520], [1.0000, -2.6675, 5.5949, -9.1434, 3.4300, 4.5800, 7.8200, -4.6275], [1.0000, -3.1308, 1.0900, -1.9461, 1.9440, 3.8570, 1.7230, -2.8142]]) assert torch.all(torch.eq(rois, expected_rois)) def test_base_boxes3d(): # test empty initialization empty_boxes = [] boxes = BaseInstance3DBoxes(empty_boxes) assert boxes.tensor.shape[0] == 0 assert boxes.tensor.shape[1] == 7 # Test init with origin gravity_center_box = np.array( [[ -5.24223238e+00, 4.00209696e+01, 2.97570381e-01, 2.06200000e+00, 4.40900000e+00, 1.54800000e+00, -1.48801203e+00 ], [ -2.66751588e+01, 5.59499564e+00, -9.14345860e-01, 3.43000000e-01, 4.58000000e-01, 7.82000000e-01, -4.62759755e+00 ], [ -5.80979675e+00, 3.54092357e+01, 2.00889888e-01, 2.39600000e+00, 3.96900000e+00, 1.73200000e+00, -4.65203216e+00 ], [ -3.13086877e+01, 1.09007628e+00, -1.94612112e-01, 1.94400000e+00, 3.85700000e+00, 1.72300000e+00, -2.81427027e+00 ]], dtype=np.float32) bottom_center_box = BaseInstance3DBoxes( gravity_center_box, origin=(0.5, 0.5, 0.5)) assert bottom_center_box.yaw.shape[0] == 4 def test_lidar_boxes3d(): # test empty initialization empty_boxes = [] boxes = LiDARInstance3DBoxes(empty_boxes) assert boxes.tensor.shape[0] == 0 assert boxes.tensor.shape[1] == 7 # Test init with origin gravity_center_box = np.array( [[ -5.24223238e+00, 4.00209696e+01, 2.97570381e-01, 2.06200000e+00, 4.40900000e+00, 1.54800000e+00, -1.48801203e+00 ], [ -2.66751588e+01, 5.59499564e+00, -9.14345860e-01, 3.43000000e-01, 4.58000000e-01, 7.82000000e-01, -4.62759755e+00 ], [ -5.80979675e+00, 3.54092357e+01, 2.00889888e-01, 2.39600000e+00, 3.96900000e+00, 1.73200000e+00, -4.65203216e+00 ], [ -3.13086877e+01, 1.09007628e+00, -1.94612112e-01, 1.94400000e+00, 3.85700000e+00, 1.72300000e+00, -2.81427027e+00 ]], dtype=np.float32) bottom_center_box = LiDARInstance3DBoxes( gravity_center_box, origin=(0.5, 0.5, 0.5)) expected_tensor = torch.tensor( [[ -5.24223238e+00, 4.00209696e+01, -4.76429619e-01, 2.06200000e+00, 4.40900000e+00, 1.54800000e+00, -1.48801203e+00 ], [ -2.66751588e+01, 5.59499564e+00, -1.30534586e+00, 3.43000000e-01, 4.58000000e-01, 7.82000000e-01, -4.62759755e+00 ], [ -5.80979675e+00, 3.54092357e+01, -6.65110112e-01, 2.39600000e+00, 3.96900000e+00, 1.73200000e+00, -4.65203216e+00 ], [ -3.13086877e+01, 1.09007628e+00, -1.05611211e+00, 1.94400000e+00, 3.85700000e+00, 1.72300000e+00, -2.81427027e+00 ]]) assert torch.allclose(expected_tensor, bottom_center_box.tensor) # Test init with numpy array np_boxes = np.array( [[1.7802081, 2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.48], [8.959413, 2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.62]], dtype=np.float32) boxes_1 = LiDARInstance3DBoxes(np_boxes) assert torch.allclose(boxes_1.tensor, torch.from_numpy(np_boxes)) # test properties assert boxes_1.volume.size(0) == 2 assert (boxes_1.center == boxes_1.bottom_center).all() assert repr(boxes) == ( 'LiDARInstance3DBoxes(\n tensor([], size=(0, 7)))') # test init with torch.Tensor th_boxes = torch.tensor( [[ 28.29669987, -0.5557558, -1.30332506, 1.47000003, 2.23000002, 1.48000002, -1.57000005 ], [ 26.66901946, 21.82302134, -1.73605708, 1.55999994, 3.48000002, 1.39999998, -1.69000006 ], [ 31.31977974, 8.16214412, -1.62177875, 1.74000001, 3.76999998, 1.48000002, 2.78999996 ]], dtype=torch.float32) boxes_2 = LiDARInstance3DBoxes(th_boxes) assert torch.allclose(boxes_2.tensor, th_boxes) # test clone/to/device boxes_2 = boxes_2.clone() boxes_1 = boxes_1.to(boxes_2.device) # test box concatenation expected_tensor = torch.tensor( [[1.7802081, 2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.48], [8.959413, 2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.62], [28.2967, -0.5557558, -1.303325, 1.47, 2.23, 1.48, -1.57], [26.66902, 21.82302, -1.736057, 1.56, 3.48, 1.4, -1.69], [31.31978, 8.162144, -1.6217787, 1.74, 3.77, 1.48, 2.79]]) boxes = LiDARInstance3DBoxes.cat([boxes_1, boxes_2]) assert torch.allclose(boxes.tensor, expected_tensor) # concatenate empty list empty_boxes = LiDARInstance3DBoxes.cat([]) assert empty_boxes.tensor.shape[0] == 0 assert empty_boxes.tensor.shape[-1] == 7 # test box flip points = torch.tensor([[1.2559, -0.6762, -1.4658], [4.7814, -0.8784, -1.3857], [6.7053, 0.2517, -0.9697], [0.6533, -0.5520, -0.5265], [4.5870, 0.5358, -1.4741]]) expected_tensor = torch.tensor( [[1.7802081, -2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.6615927], [8.959413, -2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.5215927], [28.2967, 0.5557558, -1.303325, 1.47, 2.23, 1.48, 4.7115927], [26.66902, -21.82302, -1.736057, 1.56, 3.48, 1.4, 4.8315926], [31.31978, -8.162144, -1.6217787, 1.74, 3.77, 1.48, 0.35159278]]) expected_points = torch.tensor([[1.2559, 0.6762, -1.4658], [4.7814, 0.8784, -1.3857], [6.7053, -0.2517, -0.9697], [0.6533, 0.5520, -0.5265], [4.5870, -0.5358, -1.4741]]) points = boxes.flip('horizontal', points) assert torch.allclose(boxes.tensor, expected_tensor) assert torch.allclose(points, expected_points, 1e-3) expected_tensor = torch.tensor( [[-1.7802, -2.5162, -1.7501, 1.7500, 3.3900, 1.6500, -1.6616], [-8.9594, -2.4567, -1.6357, 1.5400, 4.0100, 1.5700, -1.5216], [-28.2967, 0.5558, -1.3033, 1.4700, 2.2300, 1.4800, -4.7116], [-26.6690, -21.8230, -1.7361, 1.5600, 3.4800, 1.4000, -4.8316], [-31.3198, -8.1621, -1.6218, 1.7400, 3.7700, 1.4800, -0.3516]]) boxes_flip_vert = boxes.clone() points = boxes_flip_vert.flip('vertical', points) expected_points = torch.tensor([[-1.2559, 0.6762, -1.4658], [-4.7814, 0.8784, -1.3857], [-6.7053, -0.2517, -0.9697], [-0.6533, 0.5520, -0.5265], [-4.5870, -0.5358, -1.4741]]) assert torch.allclose(boxes_flip_vert.tensor, expected_tensor, 1e-4) assert torch.allclose(points, expected_points) # test box rotation # with input torch.Tensor points and angle expected_tensor = torch.tensor( [[1.4225, -2.7344, -1.7501, 1.7500, 3.3900, 1.6500, 1.7976], [8.5435, -3.6491, -1.6357, 1.5400, 4.0100, 1.5700, 1.6576], [28.1106, -3.2869, -1.3033, 1.4700, 2.2300, 1.4800, 4.8476], [23.4630, -25.2382, -1.7361, 1.5600, 3.4800, 1.4000, 4.9676], [29.9235, -12.3342, -1.6218, 1.7400, 3.7700, 1.4800, 0.4876]]) points, rot_mat_T = boxes.rotate(0.13603681398218053, points) expected_points = torch.tensor([[-1.1526, 0.8403, -1.4658], [-4.6181, 1.5187, -1.3857], [-6.6775, 0.6600, -0.9697], [-0.5724, 0.6355, -0.5265], [-4.6173, 0.0912, -1.4741]]) expected_rot_mat_T = torch.tensor([[0.9908, -0.1356, 0.0000], [0.1356, 0.9908, 0.0000], [0.0000, 0.0000, 1.0000]]) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points, 1e-3) assert torch.allclose(rot_mat_T, expected_rot_mat_T, 1e-3) # with input torch.Tensor points and rotation matrix points, rot_mat_T = boxes.rotate(-0.13603681398218053, points) # back rot_mat = np.array([[0.99076125, -0.13561762, 0.], [0.13561762, 0.99076125, 0.], [0., 0., 1.]]) points, rot_mat_T = boxes.rotate(rot_mat, points) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points, 1e-3) assert torch.allclose(rot_mat_T, expected_rot_mat_T, 1e-3) # with input np.ndarray points and angle points_np = np.array([[-1.0280, 0.9888, -1.4658], [-4.3695, 2.1310, -1.3857], [-6.5263, 1.5595, -0.9697], [-0.4809, 0.7073, -0.5265], [-4.5623, 0.7166, -1.4741]]) points_np, rot_mat_T_np = boxes.rotate(0.13603681398218053, points_np) expected_points_np = np.array([[-0.8844, 1.1191, -1.4658], [-4.0401, 2.7039, -1.3857], [-6.2545, 2.4302, -0.9697], [-0.3805, 0.7660, -0.5265], [-4.4230, 1.3287, -1.4741]]) expected_rot_mat_T_np = np.array([[0.9908, -0.1356, 0.0000], [0.1356, 0.9908, 0.0000], [0.0000, 0.0000, 1.0000]]) assert np.allclose(points_np, expected_points_np, 1e-3) assert np.allclose(rot_mat_T_np, expected_rot_mat_T_np, 1e-3) # with input LiDARPoints and rotation matrix points_np, rot_mat_T_np = boxes.rotate(-0.13603681398218053, points_np) lidar_points = LiDARPoints(points_np) lidar_points, rot_mat_T_np = boxes.rotate(rot_mat, lidar_points) points_np = lidar_points.tensor.numpy() assert np.allclose(points_np, expected_points_np, 1e-3) assert np.allclose(rot_mat_T_np, expected_rot_mat_T_np, 1e-3) # test box scaling expected_tensor = torch.tensor([[ 1.0443488, -2.9183323, -1.7599131, 1.7597977, 3.4089797, 1.6592377, 1.9336663 ], [ 8.014273, -4.8007393, -1.6448704, 1.5486219, 4.0324507, 1.57879, 1.7936664 ], [ 27.558605, -7.1084175, -1.310622, 1.4782301, 2.242485, 1.488286, 4.9836664 ], [ 19.934517, -28.344835, -1.7457767, 1.5687338, 3.4994833, 1.4078381, 5.1036663 ], [ 28.130915, -16.369587, -1.6308585, 1.7497417, 3.791107, 1.488286, 0.6236664 ]]) boxes.scale(1.00559866335275) assert torch.allclose(boxes.tensor, expected_tensor) # test box translation expected_tensor = torch.tensor([[ 1.1281544, -3.0507944, -1.9169292, 1.7597977, 3.4089797, 1.6592377, 1.9336663 ], [ 8.098079, -4.9332013, -1.8018866, 1.5486219, 4.0324507, 1.57879, 1.7936664 ], [ 27.64241, -7.2408795, -1.4676381, 1.4782301, 2.242485, 1.488286, 4.9836664 ], [ 20.018322, -28.477297, -1.9027928, 1.5687338, 3.4994833, 1.4078381, 5.1036663 ], [ 28.21472, -16.502048, -1.7878747, 1.7497417, 3.791107, 1.488286, 0.6236664 ]]) boxes.translate([0.0838056, -0.13246193, -0.15701613]) assert torch.allclose(boxes.tensor, expected_tensor) # test bbox in_range_bev expected_tensor = torch.tensor([1, 1, 1, 1, 1], dtype=torch.bool) mask = boxes.in_range_bev([0., -40., 70.4, 40.]) assert (mask == expected_tensor).all() mask = boxes.nonempty() assert (mask == expected_tensor).all() # test bbox in_range expected_tensor = torch.tensor([1, 1, 0, 0, 0], dtype=torch.bool) mask = boxes.in_range_3d([0, -20, -2, 22, 2, 5]) assert (mask == expected_tensor).all() # test bbox indexing index_boxes = boxes[2:5] expected_tensor = torch.tensor([[ 27.64241, -7.2408795, -1.4676381, 1.4782301, 2.242485, 1.488286, 4.9836664 ], [ 20.018322, -28.477297, -1.9027928, 1.5687338, 3.4994833, 1.4078381, 5.1036663 ], [ 28.21472, -16.502048, -1.7878747, 1.7497417, 3.791107, 1.488286, 0.6236664 ]]) assert len(index_boxes) == 3 assert torch.allclose(index_boxes.tensor, expected_tensor) index_boxes = boxes[2] expected_tensor = torch.tensor([[ 27.64241, -7.2408795, -1.4676381, 1.4782301, 2.242485, 1.488286, 4.9836664 ]]) assert len(index_boxes) == 1 assert torch.allclose(index_boxes.tensor, expected_tensor) index_boxes = boxes[[2, 4]] expected_tensor = torch.tensor([[ 27.64241, -7.2408795, -1.4676381, 1.4782301, 2.242485, 1.488286, 4.9836664 ], [ 28.21472, -16.502048, -1.7878747, 1.7497417, 3.791107, 1.488286, 0.6236664 ]]) assert len(index_boxes) == 2 assert torch.allclose(index_boxes.tensor, expected_tensor) # test iteration for i, box in enumerate(index_boxes): torch.allclose(box, expected_tensor[i]) # test properties assert torch.allclose(boxes.bottom_center, boxes.tensor[:, :3]) expected_tensor = ( boxes.tensor[:, :3] - boxes.tensor[:, 3:6] * (torch.tensor([0.5, 0.5, 0]) - torch.tensor([0.5, 0.5, 0.5]))) assert torch.allclose(boxes.gravity_center, expected_tensor) boxes.limit_yaw() assert (boxes.tensor[:, 6] <= np.pi / 2).all() assert (boxes.tensor[:, 6] >= -np.pi / 2).all() Box3DMode.convert(boxes, Box3DMode.LIDAR, Box3DMode.LIDAR) expected_tesor = boxes.tensor.clone() assert torch.allclose(expected_tesor, boxes.tensor) boxes.flip() boxes.flip() boxes.limit_yaw() assert torch.allclose(expected_tesor, boxes.tensor) # test nearest_bev expected_tensor = torch.tensor([[-0.5763, -3.9307, 2.8326, -2.1709], [6.0819, -5.7075, 10.1143, -4.1589], [26.5212, -7.9800, 28.7637, -6.5018], [18.2686, -29.2617, 21.7681, -27.6929], [27.3398, -18.3976, 29.0896, -14.6065]]) # the pytorch print loses some precision assert torch.allclose( boxes.nearest_bev, expected_tensor, rtol=1e-4, atol=1e-7) # obtained by the print of the original implementation expected_tensor = torch.tensor([[[2.4093e+00, -4.4784e+00, -1.9169e+00], [2.4093e+00, -4.4784e+00, -2.5769e-01], [-7.7767e-01, -3.2684e+00, -2.5769e-01], [-7.7767e-01, -3.2684e+00, -1.9169e+00], [3.0340e+00, -2.8332e+00, -1.9169e+00], [3.0340e+00, -2.8332e+00, -2.5769e-01], [-1.5301e-01, -1.6232e+00, -2.5769e-01], [-1.5301e-01, -1.6232e+00, -1.9169e+00]], [[9.8933e+00, -6.1340e+00, -1.8019e+00], [9.8933e+00, -6.1340e+00, -2.2310e-01], [5.9606e+00, -5.2427e+00, -2.2310e-01], [5.9606e+00, -5.2427e+00, -1.8019e+00], [1.0236e+01, -4.6237e+00, -1.8019e+00], [1.0236e+01, -4.6237e+00, -2.2310e-01], [6.3029e+00, -3.7324e+00, -2.2310e-01], [6.3029e+00, -3.7324e+00, -1.8019e+00]], [[2.8525e+01, -8.2534e+00, -1.4676e+00], [2.8525e+01, -8.2534e+00, 2.0648e-02], [2.6364e+01, -7.6525e+00, 2.0648e-02], [2.6364e+01, -7.6525e+00, -1.4676e+00], [2.8921e+01, -6.8292e+00, -1.4676e+00], [2.8921e+01, -6.8292e+00, 2.0648e-02], [2.6760e+01, -6.2283e+00, 2.0648e-02], [2.6760e+01, -6.2283e+00, -1.4676e+00]], [[2.1337e+01, -2.9870e+01, -1.9028e+00], [2.1337e+01, -2.9870e+01, -4.9495e-01], [1.8102e+01, -2.8535e+01, -4.9495e-01], [1.8102e+01, -2.8535e+01, -1.9028e+00], [2.1935e+01, -2.8420e+01, -1.9028e+00], [2.1935e+01, -2.8420e+01, -4.9495e-01], [1.8700e+01, -2.7085e+01, -4.9495e-01], [1.8700e+01, -2.7085e+01, -1.9028e+00]], [[2.6398e+01, -1.7530e+01, -1.7879e+00], [2.6398e+01, -1.7530e+01, -2.9959e-01], [2.8612e+01, -1.4452e+01, -2.9959e-01], [2.8612e+01, -1.4452e+01, -1.7879e+00], [2.7818e+01, -1.8552e+01, -1.7879e+00], [2.7818e+01, -1.8552e+01, -2.9959e-01], [3.0032e+01, -1.5474e+01, -2.9959e-01], [3.0032e+01, -1.5474e+01, -1.7879e+00]]]) # the pytorch print loses some precision assert torch.allclose(boxes.corners, expected_tensor, rtol=1e-4, atol=1e-7) # test new_box new_box1 = boxes.new_box([[1, 2, 3, 4, 5, 6, 7]]) assert torch.allclose( new_box1.tensor, torch.tensor([[1, 2, 3, 4, 5, 6, 7]], dtype=boxes.tensor.dtype)) assert new_box1.device == boxes.device assert new_box1.with_yaw == boxes.with_yaw assert new_box1.box_dim == boxes.box_dim new_box2 = boxes.new_box(np.array([[1, 2, 3, 4, 5, 6, 7]])) assert torch.allclose( new_box2.tensor, torch.tensor([[1, 2, 3, 4, 5, 6, 7]], dtype=boxes.tensor.dtype)) new_box3 = boxes.new_box(torch.tensor([[1, 2, 3, 4, 5, 6, 7]])) assert torch.allclose( new_box3.tensor, torch.tensor([[1, 2, 3, 4, 5, 6, 7]], dtype=boxes.tensor.dtype)) def test_boxes_conversion(): """Test the conversion of boxes between different modes. ComandLine: xdoctest tests/test_box3d.py::test_boxes_conversion zero """ lidar_boxes = LiDARInstance3DBoxes( [[1.7802081, 2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.48], [8.959413, 2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.62], [28.2967, -0.5557558, -1.303325, 1.47, 2.23, 1.48, -1.57], [26.66902, 21.82302, -1.736057, 1.56, 3.48, 1.4, -1.69], [31.31978, 8.162144, -1.6217787, 1.74, 3.77, 1.48, 2.79]]) cam_box_tensor = Box3DMode.convert(lidar_boxes.tensor, Box3DMode.LIDAR, Box3DMode.CAM) expected_box = lidar_boxes.convert_to(Box3DMode.CAM) assert torch.equal(expected_box.tensor, cam_box_tensor) # Some properties should be the same cam_boxes = CameraInstance3DBoxes(cam_box_tensor) assert torch.equal(cam_boxes.height, lidar_boxes.height) assert torch.equal(cam_boxes.top_height, -lidar_boxes.top_height) assert torch.equal(cam_boxes.bottom_height, -lidar_boxes.bottom_height) assert torch.allclose(cam_boxes.volume, lidar_boxes.volume) lidar_box_tensor = Box3DMode.convert(cam_box_tensor, Box3DMode.CAM, Box3DMode.LIDAR) expected_tensor = torch.tensor( [[1.7802081, 2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.48], [8.959413, 2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.62], [28.2967, -0.5557558, -1.303325, 1.47, 2.23, 1.48, -1.57], [26.66902, 21.82302, -1.736057, 1.56, 3.48, 1.4, -1.69], [31.31978, 8.162144, -1.6217787, 1.74, 3.77, 1.48, 2.79]]) assert torch.allclose(expected_tensor, lidar_box_tensor) assert torch.allclose(lidar_boxes.tensor, lidar_box_tensor) depth_box_tensor = Box3DMode.convert(cam_box_tensor, Box3DMode.CAM, Box3DMode.DEPTH) depth_to_cam_box_tensor = Box3DMode.convert(depth_box_tensor, Box3DMode.DEPTH, Box3DMode.CAM) assert torch.allclose(cam_box_tensor, depth_to_cam_box_tensor) # test similar mode conversion same_results = Box3DMode.convert(depth_box_tensor, Box3DMode.DEPTH, Box3DMode.DEPTH) assert torch.equal(same_results, depth_box_tensor) # test conversion with a given rt_mat camera_boxes = CameraInstance3DBoxes( [[0.06, 1.77, 21.4, 3.2, 1.61, 1.66, -1.54], [6.59, 1.53, 6.76, 12.78, 3.66, 2.28, 1.55], [6.71, 1.59, 22.18, 14.73, 3.64, 2.32, 1.59], [7.11, 1.58, 34.54, 10.04, 3.61, 2.32, 1.61], [7.78, 1.65, 45.95, 12.83, 3.63, 2.34, 1.64]]) rect = torch.tensor( [[0.9999239, 0.00983776, -0.00744505, 0.], [-0.0098698, 0.9999421, -0.00427846, 0.], [0.00740253, 0.00435161, 0.9999631, 0.], [0., 0., 0., 1.]], dtype=torch.float32) Trv2c = torch.tensor( [[7.533745e-03, -9.999714e-01, -6.166020e-04, -4.069766e-03], [1.480249e-02, 7.280733e-04, -9.998902e-01, -7.631618e-02], [9.998621e-01, 7.523790e-03, 1.480755e-02, -2.717806e-01], [0.000000e+00, 0.000000e+00, 0.000000e+00, 1.000000e+00]], dtype=torch.float32) expected_tensor = torch.tensor( [[ 2.16902434e+01, -4.06038554e-02, -1.61906639e+00, 1.65999997e+00, 3.20000005e+00, 1.61000001e+00, -1.53999996e+00 ], [ 7.05006905e+00, -6.57459601e+00, -1.60107949e+00, 2.27999997e+00, 1.27799997e+01, 3.66000009e+00, 1.54999995e+00 ], [ 2.24698818e+01, -6.69203759e+00, -1.50118145e+00, 2.31999993e+00, 1.47299995e+01, 3.64000010e+00, 1.59000003e+00 ], [ 3.48291965e+01, -7.09058388e+00, -1.36622983e+00, 2.31999993e+00, 1.00400000e+01, 3.60999990e+00, 1.61000001e+00 ], [ 4.62394617e+01, -7.75838800e+00, -1.32405020e+00, 2.33999991e+00, 1.28299999e+01, 3.63000011e+00, 1.63999999e+00 ]], dtype=torch.float32) rt_mat = rect @ Trv2c # test coversion with Box type cam_to_lidar_box = Box3DMode.convert(camera_boxes, Box3DMode.CAM, Box3DMode.LIDAR, rt_mat.inverse()) assert torch.allclose(cam_to_lidar_box.tensor, expected_tensor) lidar_to_cam_box = Box3DMode.convert(cam_to_lidar_box.tensor, Box3DMode.LIDAR, Box3DMode.CAM, rt_mat) assert torch.allclose(lidar_to_cam_box, camera_boxes.tensor) # test numpy convert cam_to_lidar_box = Box3DMode.convert(camera_boxes.tensor.numpy(), Box3DMode.CAM, Box3DMode.LIDAR, rt_mat.inverse().numpy()) assert np.allclose(cam_to_lidar_box, expected_tensor.numpy()) # test list convert cam_to_lidar_box = Box3DMode.convert( camera_boxes.tensor[0].numpy().tolist(), Box3DMode.CAM, Box3DMode.LIDAR, rt_mat.inverse().numpy()) assert np.allclose(np.array(cam_to_lidar_box), expected_tensor[0].numpy()) # test convert from depth to lidar depth_boxes = torch.tensor( [[2.4593, 2.5870, -0.4321, 0.8597, 0.6193, 1.0204, 3.0693], [1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 3.0601]], dtype=torch.float32) depth_boxes = DepthInstance3DBoxes(depth_boxes) depth_to_lidar_box = depth_boxes.convert_to(Box3DMode.LIDAR) expected_box = depth_to_lidar_box.convert_to(Box3DMode.DEPTH) assert torch.equal(depth_boxes.tensor, expected_box.tensor) lidar_to_depth_box = Box3DMode.convert(depth_to_lidar_box, Box3DMode.LIDAR, Box3DMode.DEPTH) assert torch.allclose(depth_boxes.tensor, lidar_to_depth_box.tensor) assert torch.allclose(depth_boxes.volume, lidar_to_depth_box.volume) # test convert from depth to camera depth_to_cam_box = Box3DMode.convert(depth_boxes, Box3DMode.DEPTH, Box3DMode.CAM) cam_to_depth_box = Box3DMode.convert(depth_to_cam_box, Box3DMode.CAM, Box3DMode.DEPTH) expected_tensor = depth_to_cam_box.convert_to(Box3DMode.DEPTH) assert torch.equal(expected_tensor.tensor, cam_to_depth_box.tensor) assert torch.allclose(depth_boxes.tensor, cam_to_depth_box.tensor) assert torch.allclose(depth_boxes.volume, cam_to_depth_box.volume) with pytest.raises(NotImplementedError): # assert invalid convert mode Box3DMode.convert(depth_boxes, Box3DMode.DEPTH, 3) def test_camera_boxes3d(): # Test init with numpy array np_boxes = np.array( [[1.7802081, 2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.48], [8.959413, 2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.62]], dtype=np.float32) boxes_1 = Box3DMode.convert( LiDARInstance3DBoxes(np_boxes), Box3DMode.LIDAR, Box3DMode.CAM) assert isinstance(boxes_1, CameraInstance3DBoxes) cam_np_boxes = Box3DMode.convert(np_boxes, Box3DMode.LIDAR, Box3DMode.CAM) assert torch.allclose(boxes_1.tensor, boxes_1.tensor.new_tensor(cam_np_boxes)) # test init with torch.Tensor th_boxes = torch.tensor( [[ 28.29669987, -0.5557558, -1.30332506, 1.47000003, 2.23000002, 1.48000002, -1.57000005 ], [ 26.66901946, 21.82302134, -1.73605708, 1.55999994, 3.48000002, 1.39999998, -1.69000006 ], [ 31.31977974, 8.16214412, -1.62177875, 1.74000001, 3.76999998, 1.48000002, 2.78999996 ]], dtype=torch.float32) cam_th_boxes = Box3DMode.convert(th_boxes, Box3DMode.LIDAR, Box3DMode.CAM) boxes_2 = CameraInstance3DBoxes(cam_th_boxes) assert torch.allclose(boxes_2.tensor, cam_th_boxes) # test clone/to/device boxes_2 = boxes_2.clone() boxes_1 = boxes_1.to(boxes_2.device) # test box concatenation expected_tensor = Box3DMode.convert( torch.tensor( [[1.7802081, 2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.48], [8.959413, 2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.62], [28.2967, -0.5557558, -1.303325, 1.47, 2.23, 1.48, -1.57], [26.66902, 21.82302, -1.736057, 1.56, 3.48, 1.4, -1.69], [31.31978, 8.162144, -1.6217787, 1.74, 3.77, 1.48, 2.79]]), Box3DMode.LIDAR, Box3DMode.CAM) boxes = CameraInstance3DBoxes.cat([boxes_1, boxes_2]) assert torch.allclose(boxes.tensor, expected_tensor) # test box flip points = torch.tensor([[0.6762, 1.4658, 1.2559], [0.8784, 1.3857, 4.7814], [-0.2517, 0.9697, 6.7053], [0.5520, 0.5265, 0.6533], [-0.5358, 1.4741, 4.5870]]) expected_tensor = Box3DMode.convert( torch.tensor( [[1.7802081, -2.516249, -1.7501148, 1.75, 3.39, 1.65, 1.6615927], [8.959413, -2.4567227, -1.6357126, 1.54, 4.01, 1.57, 1.5215927], [28.2967, 0.5557558, -1.303325, 1.47, 2.23, 1.48, 4.7115927], [26.66902, -21.82302, -1.736057, 1.56, 3.48, 1.4, 4.8315926], [31.31978, -8.162144, -1.6217787, 1.74, 3.77, 1.48, 0.35159278]]), Box3DMode.LIDAR, Box3DMode.CAM) points = boxes.flip('horizontal', points) expected_points = torch.tensor([[-0.6762, 1.4658, 1.2559], [-0.8784, 1.3857, 4.7814], [0.2517, 0.9697, 6.7053], [-0.5520, 0.5265, 0.6533], [0.5358, 1.4741, 4.5870]]) assert torch.allclose(boxes.tensor, expected_tensor) assert torch.allclose(points, expected_points, 1e-3) expected_tensor = torch.tensor( [[2.5162, 1.7501, -1.7802, 3.3900, 1.6500, 1.7500, -1.6616], [2.4567, 1.6357, -8.9594, 4.0100, 1.5700, 1.5400, -1.5216], [-0.5558, 1.3033, -28.2967, 2.2300, 1.4800, 1.4700, -4.7116], [21.8230, 1.7361, -26.6690, 3.4800, 1.4000, 1.5600, -4.8316], [8.1621, 1.6218, -31.3198, 3.7700, 1.4800, 1.7400, -0.3516]]) boxes_flip_vert = boxes.clone() points = boxes_flip_vert.flip('vertical', points) expected_points = torch.tensor([[-0.6762, 1.4658, -1.2559], [-0.8784, 1.3857, -4.7814], [0.2517, 0.9697, -6.7053], [-0.5520, 0.5265, -0.6533], [0.5358, 1.4741, -4.5870]]) assert torch.allclose(boxes_flip_vert.tensor, expected_tensor, 1e-4) assert torch.allclose(points, expected_points) # test box rotation # with input torch.Tensor points and angle expected_tensor = Box3DMode.convert( torch.tensor( [[1.4225, -2.7344, -1.7501, 1.7500, 3.3900, 1.6500, 1.7976], [8.5435, -3.6491, -1.6357, 1.5400, 4.0100, 1.5700, 1.6576], [28.1106, -3.2869, -1.3033, 1.4700, 2.2300, 1.4800, 4.8476], [23.4630, -25.2382, -1.7361, 1.5600, 3.4800, 1.4000, 4.9676], [29.9235, -12.3342, -1.6218, 1.7400, 3.7700, 1.4800, 0.4876]]), Box3DMode.LIDAR, Box3DMode.CAM) points, rot_mat_T = boxes.rotate(torch.tensor(0.13603681398218053), points) expected_points = torch.tensor([[-0.8403, 1.4658, -1.1526], [-1.5187, 1.3857, -4.6181], [-0.6600, 0.9697, -6.6775], [-0.6355, 0.5265, -0.5724], [-0.0912, 1.4741, -4.6173]]) expected_rot_mat_T = torch.tensor([[0.9908, 0.0000, -0.1356], [0.0000, 1.0000, 0.0000], [0.1356, 0.0000, 0.9908]]) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points, 1e-3) assert torch.allclose(rot_mat_T, expected_rot_mat_T, 1e-3) # with input torch.Tensor points and rotation matrix points, rot_mat_T = boxes.rotate( torch.tensor(-0.13603681398218053), points) # back rot_mat = np.array([[0.99076125, 0., -0.13561762], [0., 1., 0.], [0.13561762, 0., 0.99076125]]) points, rot_mat_T = boxes.rotate(rot_mat, points) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points, 1e-3) assert torch.allclose(rot_mat_T, expected_rot_mat_T, 1e-3) # with input np.ndarray points and angle points_np = np.array([[0.6762, 1.2559, -1.4658, 2.5359], [0.8784, 4.7814, -1.3857, 0.7167], [-0.2517, 6.7053, -0.9697, 0.5599], [0.5520, 0.6533, -0.5265, 1.0032], [-0.5358, 4.5870, -1.4741, 0.0556]]) points_np, rot_mat_T_np = boxes.rotate( torch.tensor(0.13603681398218053), points_np) expected_points_np = np.array([[0.4712, 1.2559, -1.5440, 2.5359], [0.6824, 4.7814, -1.4920, 0.7167], [-0.3809, 6.7053, -0.9266, 0.5599], [0.4755, 0.6533, -0.5965, 1.0032], [-0.7308, 4.5870, -1.3878, 0.0556]]) expected_rot_mat_T_np = np.array([[0.9908, 0.0000, -0.1356], [0.0000, 1.0000, 0.0000], [0.1356, 0.0000, 0.9908]]) assert np.allclose(points_np, expected_points_np, 1e-3) assert np.allclose(rot_mat_T_np, expected_rot_mat_T_np, 1e-3) # with input CameraPoints and rotation matrix points_np, rot_mat_T_np = boxes.rotate( torch.tensor(-0.13603681398218053), points_np) camera_points = CameraPoints(points_np, points_dim=4) camera_points, rot_mat_T_np = boxes.rotate(rot_mat, camera_points) points_np = camera_points.tensor.numpy() assert np.allclose(points_np, expected_points_np, 1e-3) assert np.allclose(rot_mat_T_np, expected_rot_mat_T_np, 1e-3) # test box scaling expected_tensor = Box3DMode.convert( torch.tensor([[ 1.0443488, -2.9183323, -1.7599131, 1.7597977, 3.4089797, 1.6592377, 1.9336663 ], [ 8.014273, -4.8007393, -1.6448704, 1.5486219, 4.0324507, 1.57879, 1.7936664 ], [ 27.558605, -7.1084175, -1.310622, 1.4782301, 2.242485, 1.488286, 4.9836664 ], [ 19.934517, -28.344835, -1.7457767, 1.5687338, 3.4994833, 1.4078381, 5.1036663 ], [ 28.130915, -16.369587, -1.6308585, 1.7497417, 3.791107, 1.488286, 0.6236664 ]]), Box3DMode.LIDAR, Box3DMode.CAM) boxes.scale(1.00559866335275) assert torch.allclose(boxes.tensor, expected_tensor) # test box translation expected_tensor = Box3DMode.convert( torch.tensor([[ 1.1281544, -3.0507944, -1.9169292, 1.7597977, 3.4089797, 1.6592377, 1.9336663 ], [ 8.098079, -4.9332013, -1.8018866, 1.5486219, 4.0324507, 1.57879, 1.7936664 ], [ 27.64241, -7.2408795, -1.4676381, 1.4782301, 2.242485, 1.488286, 4.9836664 ], [ 20.018322, -28.477297, -1.9027928, 1.5687338, 3.4994833, 1.4078381, 5.1036663 ], [ 28.21472, -16.502048, -1.7878747, 1.7497417, 3.791107, 1.488286, 0.6236664 ]]), Box3DMode.LIDAR, Box3DMode.CAM) boxes.translate(torch.tensor([0.13246193, 0.15701613, 0.0838056])) assert torch.allclose(boxes.tensor, expected_tensor) # test bbox in_range_bev expected_tensor = torch.tensor([1, 1, 1, 1, 1], dtype=torch.bool) mask = boxes.in_range_bev([0., -40., 70.4, 40.]) assert (mask == expected_tensor).all() mask = boxes.nonempty() assert (mask == expected_tensor).all() # test bbox in_range expected_tensor = torch.tensor([1, 1, 0, 0, 0], dtype=torch.bool) mask = boxes.in_range_3d([-2, -5, 0, 20, 2, 22]) assert (mask == expected_tensor).all() # test properties assert torch.allclose(boxes.bottom_center, boxes.tensor[:, :3]) expected_tensor = ( boxes.tensor[:, :3] - boxes.tensor[:, 3:6] * (torch.tensor([0.5, 1.0, 0.5]) - torch.tensor([0.5, 0.5, 0.5]))) assert torch.allclose(boxes.gravity_center, expected_tensor) boxes.limit_yaw() assert (boxes.tensor[:, 6] <= np.pi / 2).all() assert (boxes.tensor[:, 6] >= -np.pi / 2).all() Box3DMode.convert(boxes, Box3DMode.LIDAR, Box3DMode.LIDAR) expected_tesor = boxes.tensor.clone() assert torch.allclose(expected_tesor, boxes.tensor) boxes.flip() boxes.flip() boxes.limit_yaw() assert torch.allclose(expected_tesor, boxes.tensor) # test nearest_bev # BEV box in lidar coordinates (x, y) lidar_expected_tensor = torch.tensor( [[-0.5763, -3.9307, 2.8326, -2.1709], [6.0819, -5.7075, 10.1143, -4.1589], [26.5212, -7.9800, 28.7637, -6.5018], [18.2686, -29.2617, 21.7681, -27.6929], [27.3398, -18.3976, 29.0896, -14.6065]]) # BEV box in camera coordinate (-y, x) expected_tensor = lidar_expected_tensor.clone() expected_tensor[:, 0::2] = -lidar_expected_tensor[:, [3, 1]] expected_tensor[:, 1::2] = lidar_expected_tensor[:, 0::2] # the pytorch print loses some precision assert torch.allclose( boxes.nearest_bev, expected_tensor, rtol=1e-4, atol=1e-7) # obtained by the print of the original implementation expected_tensor = torch.tensor([[[3.2684e+00, 2.5769e-01, -7.7767e-01], [1.6232e+00, 2.5769e-01, -1.5301e-01], [1.6232e+00, 1.9169e+00, -1.5301e-01], [3.2684e+00, 1.9169e+00, -7.7767e-01], [4.4784e+00, 2.5769e-01, 2.4093e+00], [2.8332e+00, 2.5769e-01, 3.0340e+00], [2.8332e+00, 1.9169e+00, 3.0340e+00], [4.4784e+00, 1.9169e+00, 2.4093e+00]], [[5.2427e+00, 2.2310e-01, 5.9606e+00], [3.7324e+00, 2.2310e-01, 6.3029e+00], [3.7324e+00, 1.8019e+00, 6.3029e+00], [5.2427e+00, 1.8019e+00, 5.9606e+00], [6.1340e+00, 2.2310e-01, 9.8933e+00], [4.6237e+00, 2.2310e-01, 1.0236e+01], [4.6237e+00, 1.8019e+00, 1.0236e+01], [6.1340e+00, 1.8019e+00, 9.8933e+00]], [[7.6525e+00, -2.0648e-02, 2.6364e+01], [6.2283e+00, -2.0648e-02, 2.6760e+01], [6.2283e+00, 1.4676e+00, 2.6760e+01], [7.6525e+00, 1.4676e+00, 2.6364e+01], [8.2534e+00, -2.0648e-02, 2.8525e+01], [6.8292e+00, -2.0648e-02, 2.8921e+01], [6.8292e+00, 1.4676e+00, 2.8921e+01], [8.2534e+00, 1.4676e+00, 2.8525e+01]], [[2.8535e+01, 4.9495e-01, 1.8102e+01], [2.7085e+01, 4.9495e-01, 1.8700e+01], [2.7085e+01, 1.9028e+00, 1.8700e+01], [2.8535e+01, 1.9028e+00, 1.8102e+01], [2.9870e+01, 4.9495e-01, 2.1337e+01], [2.8420e+01, 4.9495e-01, 2.1935e+01], [2.8420e+01, 1.9028e+00, 2.1935e+01], [2.9870e+01, 1.9028e+00, 2.1337e+01]], [[1.4452e+01, 2.9959e-01, 2.8612e+01], [1.5474e+01, 2.9959e-01, 3.0032e+01], [1.5474e+01, 1.7879e+00, 3.0032e+01], [1.4452e+01, 1.7879e+00, 2.8612e+01], [1.7530e+01, 2.9959e-01, 2.6398e+01], [1.8552e+01, 2.9959e-01, 2.7818e+01], [1.8552e+01, 1.7879e+00, 2.7818e+01], [1.7530e+01, 1.7879e+00, 2.6398e+01]]]) # the pytorch print loses some precision assert torch.allclose(boxes.corners, expected_tensor, rtol=1e-4, atol=1e-7) # test init with a given origin boxes_origin_given = CameraInstance3DBoxes( th_boxes.clone(), box_dim=7, origin=(0.5, 0.5, 0.5)) expected_tensor = th_boxes.clone() expected_tensor[:, :3] = th_boxes[:, :3] + th_boxes[:, 3:6] * ( th_boxes.new_tensor((0.5, 1.0, 0.5)) - th_boxes.new_tensor( (0.5, 0.5, 0.5))) assert torch.allclose(boxes_origin_given.tensor, expected_tensor) def test_boxes3d_overlaps(): """Test the iou calculation of boxes in different modes. ComandLine: xdoctest tests/test_box3d.py::test_boxes3d_overlaps zero """ if not torch.cuda.is_available(): pytest.skip('test requires GPU and torch+cuda') # Test LiDAR boxes 3D overlaps boxes1_tensor = torch.tensor( [[1.8, -2.5, -1.8, 1.75, 3.39, 1.65, 1.6615927], [8.9, -2.5, -1.6, 1.54, 4.01, 1.57, 1.5215927], [28.3, 0.5, -1.3, 1.47, 2.23, 1.48, 4.7115927], [31.3, -8.2, -1.6, 1.74, 3.77, 1.48, 0.35]], device='cuda') boxes1 = LiDARInstance3DBoxes(boxes1_tensor) boxes2_tensor = torch.tensor([[1.2, -3.0, -1.9, 1.8, 3.4, 1.7, 1.9], [8.1, -2.9, -1.8, 1.5, 4.1, 1.6, 1.8], [31.3, -8.2, -1.6, 1.74, 3.77, 1.48, 0.35], [20.1, -28.5, -1.9, 1.6, 3.5, 1.4, 5.1]], device='cuda') boxes2 = LiDARInstance3DBoxes(boxes2_tensor) expected_iou_tensor = torch.tensor( [[0.3710, 0.0000, 0.0000, 0.0000], [0.0000, 0.3322, 0.0000, 0.0000], [0.0000, 0.0000, 0.0000, 0.0000], [0.0000, 0.0000, 1.0000, 0.0000]], device='cuda') overlaps_3d_iou = boxes1.overlaps(boxes1, boxes2) assert torch.allclose( expected_iou_tensor, overlaps_3d_iou, rtol=1e-4, atol=1e-7) expected_iof_tensor = torch.tensor( [[0.5582, 0.0000, 0.0000, 0.0000], [0.0000, 0.5025, 0.0000, 0.0000], [0.0000, 0.0000, 0.0000, 0.0000], [0.0000, 0.0000, 1.0000, 0.0000]], device='cuda') overlaps_3d_iof = boxes1.overlaps(boxes1, boxes2, mode='iof') assert torch.allclose( expected_iof_tensor, overlaps_3d_iof, rtol=1e-4, atol=1e-7) empty_boxes = [] boxes3 = LiDARInstance3DBoxes(empty_boxes) overlaps_3d_empty = boxes1.overlaps(boxes3, boxes2) assert overlaps_3d_empty.shape[0] == 0 assert overlaps_3d_empty.shape[1] == 4 # Test camera boxes 3D overlaps cam_boxes1_tensor = Box3DMode.convert(boxes1_tensor, Box3DMode.LIDAR, Box3DMode.CAM) cam_boxes1 = CameraInstance3DBoxes(cam_boxes1_tensor) cam_boxes2_tensor = Box3DMode.convert(boxes2_tensor, Box3DMode.LIDAR, Box3DMode.CAM) cam_boxes2 = CameraInstance3DBoxes(cam_boxes2_tensor) cam_overlaps_3d = cam_boxes1.overlaps(cam_boxes1, cam_boxes2) # same boxes under different coordinates should have the same iou assert torch.allclose( expected_iou_tensor, cam_overlaps_3d, rtol=1e-4, atol=1e-7) assert torch.allclose(cam_overlaps_3d, overlaps_3d_iou) with pytest.raises(AssertionError): cam_boxes1.overlaps(cam_boxes1, boxes1) with pytest.raises(AssertionError): boxes1.overlaps(cam_boxes1, boxes1) def test_depth_boxes3d(): # test empty initialization empty_boxes = [] boxes = DepthInstance3DBoxes(empty_boxes) assert boxes.tensor.shape[0] == 0 assert boxes.tensor.shape[1] == 7 # Test init with numpy array np_boxes = np.array( [[1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 3.0601], [2.3262, 3.3065, --0.44255, 0.8234, 0.5325, 1.0099, 2.9971]], dtype=np.float32) boxes_1 = DepthInstance3DBoxes(np_boxes) assert torch.allclose(boxes_1.tensor, torch.from_numpy(np_boxes)) # test properties assert boxes_1.volume.size(0) == 2 assert (boxes_1.center == boxes_1.bottom_center).all() expected_tensor = torch.tensor([[1.4856, 2.5299, -0.1093], [2.3262, 3.3065, 0.9475]]) assert torch.allclose(boxes_1.gravity_center, expected_tensor) expected_tensor = torch.tensor([[1.4856, 2.5299, 0.9385, 2.1404, 3.0601], [2.3262, 3.3065, 0.8234, 0.5325, 2.9971]]) assert torch.allclose(boxes_1.bev, expected_tensor) expected_tensor = torch.tensor([[1.0164, 1.4597, 1.9548, 3.6001], [1.9145, 3.0402, 2.7379, 3.5728]]) assert torch.allclose(boxes_1.nearest_bev, expected_tensor, 1e-4) assert repr(boxes) == ( 'DepthInstance3DBoxes(\n tensor([], size=(0, 7)))') # test init with torch.Tensor th_boxes = torch.tensor( [[2.4593, 2.5870, -0.4321, 0.8597, 0.6193, 1.0204, 3.0693], [1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 3.0601]], dtype=torch.float32) boxes_2 = DepthInstance3DBoxes(th_boxes) assert torch.allclose(boxes_2.tensor, th_boxes) # test clone/to/device boxes_2 = boxes_2.clone() boxes_1 = boxes_1.to(boxes_2.device) # test box concatenation expected_tensor = torch.tensor( [[1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 3.0601], [2.3262, 3.3065, 0.44255, 0.8234, 0.5325, 1.0099, 2.9971], [2.4593, 2.5870, -0.4321, 0.8597, 0.6193, 1.0204, 3.0693], [1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 3.0601]]) boxes = DepthInstance3DBoxes.cat([boxes_1, boxes_2]) assert torch.allclose(boxes.tensor, expected_tensor) # concatenate empty list empty_boxes = DepthInstance3DBoxes.cat([]) assert empty_boxes.tensor.shape[0] == 0 assert empty_boxes.tensor.shape[-1] == 7 # test box flip points = torch.tensor([[0.6762, 1.2559, -1.4658, 2.5359], [0.8784, 4.7814, -1.3857, 0.7167], [-0.2517, 6.7053, -0.9697, 0.5599], [0.5520, 0.6533, -0.5265, 1.0032], [-0.5358, 4.5870, -1.4741, 0.0556]]) expected_tensor = torch.tensor( [[-1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 0.0815], [-2.3262, 3.3065, 0.4426, 0.8234, 0.5325, 1.0099, 0.1445], [-2.4593, 2.5870, -0.4321, 0.8597, 0.6193, 1.0204, 0.0723], [-1.4856, 2.5299, -0.5570, 0.9385, 2.1404, 0.8954, 0.0815]]) points = boxes.flip(bev_direction='horizontal', points=points) expected_points = torch.tensor([[-0.6762, 1.2559, -1.4658, 2.5359], [-0.8784, 4.7814, -1.3857, 0.7167], [0.2517, 6.7053, -0.9697, 0.5599], [-0.5520, 0.6533, -0.5265, 1.0032], [0.5358, 4.5870, -1.4741, 0.0556]]) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points) expected_tensor = torch.tensor( [[-1.4856, -2.5299, -0.5570, 0.9385, 2.1404, 0.8954, -0.0815], [-2.3262, -3.3065, 0.4426, 0.8234, 0.5325, 1.0099, -0.1445], [-2.4593, -2.5870, -0.4321, 0.8597, 0.6193, 1.0204, -0.0723], [-1.4856, -2.5299, -0.5570, 0.9385, 2.1404, 0.8954, -0.0815]]) points = boxes.flip(bev_direction='vertical', points=points) expected_points = torch.tensor([[-0.6762, -1.2559, -1.4658, 2.5359], [-0.8784, -4.7814, -1.3857, 0.7167], [0.2517, -6.7053, -0.9697, 0.5599], [-0.5520, -0.6533, -0.5265, 1.0032], [0.5358, -4.5870, -1.4741, 0.0556]]) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points) # test box rotation # with input torch.Tensor points and angle boxes_rot = boxes.clone() expected_tensor = torch.tensor( [[-1.5434, -2.4951, -0.5570, 0.9385, 2.1404, 0.8954, -0.0585], [-2.4016, -3.2521, 0.4426, 0.8234, 0.5325, 1.0099, -0.1215], [-2.5181, -2.5298, -0.4321, 0.8597, 0.6193, 1.0204, -0.0493], [-1.5434, -2.4951, -0.5570, 0.9385, 2.1404, 0.8954, -0.0585]]) points, rot_mat_T = boxes_rot.rotate(-0.022998953275003075, points) expected_points = torch.tensor([[-0.7049, -1.2400, -1.4658, 2.5359], [-0.9881, -4.7599, -1.3857, 0.7167], [0.0974, -6.7093, -0.9697, 0.5599], [-0.5669, -0.6404, -0.5265, 1.0032], [0.4302, -4.5981, -1.4741, 0.0556]]) expected_rot_mat_T = torch.tensor([[0.9997, -0.0230, 0.0000], [0.0230, 0.9997, 0.0000], [0.0000, 0.0000, 1.0000]]) assert torch.allclose(boxes_rot.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points, 1e-3) assert torch.allclose(rot_mat_T, expected_rot_mat_T, 1e-3) # with input torch.Tensor points and rotation matrix points, rot_mat_T = boxes.rotate(0.022998953275003075, points) # back rot_mat = np.array([[0.99973554, 0.02299693, 0.], [-0.02299693, 0.99973554, 0.], [0., 0., 1.]]) points, rot_mat_T = boxes.rotate(rot_mat, points) assert torch.allclose(boxes_rot.tensor, expected_tensor, 1e-3) assert torch.allclose(points, expected_points, 1e-3) assert torch.allclose(rot_mat_T, expected_rot_mat_T, 1e-3) # with input np.ndarray points and angle points_np = np.array([[0.6762, 1.2559, -1.4658, 2.5359], [0.8784, 4.7814, -1.3857, 0.7167], [-0.2517, 6.7053, -0.9697, 0.5599], [0.5520, 0.6533, -0.5265, 1.0032], [-0.5358, 4.5870, -1.4741, 0.0556]]) points_np, rot_mat_T_np = boxes.rotate(-0.022998953275003075, points_np) expected_points_np = np.array([[0.7049, 1.2400, -1.4658, 2.5359], [0.9881, 4.7599, -1.3857, 0.7167], [-0.0974, 6.7093, -0.9697, 0.5599], [0.5669, 0.6404, -0.5265, 1.0032], [-0.4302, 4.5981, -1.4741, 0.0556]]) expected_rot_mat_T_np = np.array([[0.9997, -0.0230, 0.0000], [0.0230, 0.9997, 0.0000], [0.0000, 0.0000, 1.0000]]) expected_tensor = torch.tensor( [[-1.5434, -2.4951, -0.5570, 0.9385, 2.1404, 0.8954, -0.0585], [-2.4016, -3.2521, 0.4426, 0.8234, 0.5325, 1.0099, -0.1215], [-2.5181, -2.5298, -0.4321, 0.8597, 0.6193, 1.0204, -0.0493], [-1.5434, -2.4951, -0.5570, 0.9385, 2.1404, 0.8954, -0.0585]]) assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert np.allclose(points_np, expected_points_np, 1e-3) assert np.allclose(rot_mat_T_np, expected_rot_mat_T_np, 1e-3) # with input DepthPoints and rotation matrix points_np, rot_mat_T_np = boxes.rotate(0.022998953275003075, points_np) depth_points = DepthPoints(points_np, points_dim=4) depth_points, rot_mat_T_np = boxes.rotate(rot_mat, depth_points) points_np = depth_points.tensor.numpy() assert torch.allclose(boxes.tensor, expected_tensor, 1e-3) assert np.allclose(points_np, expected_points_np, 1e-3) assert np.allclose(rot_mat_T_np, expected_rot_mat_T_np, 1e-3) th_boxes = torch.tensor( [[0.61211395, 0.8129094, 0.10563634, 1.497534, 0.16927195, 0.27956772], [1.430009, 0.49797538, 0.9382923, 0.07694054, 0.9312509, 1.8919173]], dtype=torch.float32) boxes = DepthInstance3DBoxes(th_boxes, box_dim=6, with_yaw=False) expected_tensor = torch.tensor([[ 0.64884546, 0.78390356, 0.10563634, 1.50373348, 0.23795205, 0.27956772, 0 ], [ 1.45139421, 0.43169443, 0.93829232, 0.11967964, 0.93380373, 1.89191735, 0 ]]) boxes_3 = boxes.clone() boxes_3.rotate(-0.04599790655000615) assert torch.allclose(boxes_3.tensor, expected_tensor) boxes.rotate(torch.tensor(-0.04599790655000615)) assert torch.allclose(boxes.tensor, expected_tensor) # test bbox in_range_bev expected_tensor = torch.tensor([1, 1], dtype=torch.bool) mask = boxes.in_range_bev([0., -40., 70.4, 40.]) assert (mask == expected_tensor).all() mask = boxes.nonempty() assert (mask == expected_tensor).all() expected_tensor = torch.tensor([[[-0.1030, 0.6649, 0.1056], [-0.1030, 0.6649, 0.3852], [-0.1030, 0.9029, 0.3852], [-0.1030, 0.9029, 0.1056], [1.4007, 0.6649, 0.1056], [1.4007, 0.6649, 0.3852], [1.4007, 0.9029, 0.3852], [1.4007, 0.9029, 0.1056]], [[1.3916, -0.0352, 0.9383], [1.3916, -0.0352, 2.8302], [1.3916, 0.8986, 2.8302], [1.3916, 0.8986, 0.9383], [1.5112, -0.0352, 0.9383], [1.5112, -0.0352, 2.8302], [1.5112, 0.8986, 2.8302], [1.5112, 0.8986, 0.9383]]]) torch.allclose(boxes.corners, expected_tensor) # test points in boxes if torch.cuda.is_available(): box_idxs_of_pts = boxes.points_in_boxes(points.cuda()) expected_idxs_of_pts = torch.tensor( [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0]], device='cuda:0', dtype=torch.int32) assert torch.all(box_idxs_of_pts == expected_idxs_of_pts) # test get_surface_line_center boxes = torch.tensor( [[0.3294, 1.0359, 0.1171, 1.0822, 1.1247, 1.3721, 0.4916], [-2.4630, -2.6324, -0.1616, 0.9202, 1.7896, 0.1992, 0.3185]]) boxes = DepthInstance3DBoxes( boxes, box_dim=boxes.shape[-1], with_yaw=True, origin=(0.5, 0.5, 0.5)) surface_center, line_center = boxes.get_surface_line_center() expected_surface_center = torch.tensor([[0.3294, 1.0359, 0.8031], [0.3294, 1.0359, -0.5689], [0.5949, 1.5317, 0.1171], [0.1533, 0.5018, 0.1171], [0.8064, 0.7805, 0.1171], [-0.1845, 1.2053, 0.1171], [-2.4630, -2.6324, -0.0620], [-2.4630, -2.6324, -0.2612], [-2.0406, -1.8436, -0.1616], [-2.7432, -3.4822, -0.1616], [-2.0574, -2.8496, -0.1616], [-2.9000, -2.4883, -0.1616]]) expected_line_center = torch.tensor([[0.8064, 0.7805, 0.8031], [-0.1845, 1.2053, 0.8031], [0.5949, 1.5317, 0.8031], [0.1533, 0.5018, 0.8031], [0.8064, 0.7805, -0.5689], [-0.1845, 1.2053, -0.5689], [0.5949, 1.5317, -0.5689], [0.1533, 0.5018, -0.5689], [1.0719, 1.2762, 0.1171], [0.6672, 0.3324, 0.1171], [0.1178, 1.7871, 0.1171], [-0.3606, 0.6713, 0.1171], [-2.0574, -2.8496, -0.0620], [-2.9000, -2.4883, -0.0620], [-2.0406, -1.8436, -0.0620], [-2.7432, -3.4822, -0.0620], [-2.0574, -2.8496, -0.2612], [-2.9000, -2.4883, -0.2612], [-2.0406, -1.8436, -0.2612], [-2.7432, -3.4822, -0.2612], [-1.6350, -2.0607, -0.1616], [-2.3062, -3.6263, -0.1616], [-2.4462, -1.6264, -0.1616], [-3.1802, -3.3381, -0.1616]]) assert torch.allclose(surface_center, expected_surface_center, atol=1e-04) assert torch.allclose(line_center, expected_line_center, atol=1e-04) def test_rotation_3d_in_axis(): points = torch.tensor([[[-0.4599, -0.0471, 0.0000], [-0.4599, -0.0471, 1.8433], [-0.4599, 0.0471, 1.8433]], [[-0.2555, -0.2683, 0.0000], [-0.2555, -0.2683, 0.9072], [-0.2555, 0.2683, 0.9072]]]) rotated = rotation_3d_in_axis( points, torch.tensor([-np.pi / 10, np.pi / 10]), axis=0) expected_rotated = torch.tensor([[[0.0000, -0.4228, -0.1869], [1.8433, -0.4228, -0.1869], [1.8433, -0.4519, -0.0973]], [[0.0000, -0.3259, -0.1762], [0.9072, -0.3259, -0.1762], [0.9072, -0.1601, 0.3341]]]) assert torch.allclose(rotated, expected_rotated, 1e-3) def test_limit_period(): torch.manual_seed(0) val = torch.rand([5, 1]) result = limit_period(val) expected_result = torch.tensor([[0.4963], [0.7682], [0.0885], [0.1320], [0.3074]]) assert torch.allclose(result, expected_result, 1e-3) def test_xywhr2xyxyr(): torch.manual_seed(0) xywhr = torch.tensor([[1., 2., 3., 4., 5.], [0., 1., 2., 3., 4.]]) xyxyr = xywhr2xyxyr(xywhr) expected_xyxyr = torch.tensor([[-0.5000, 0.0000, 2.5000, 4.0000, 5.0000], [-1.0000, -0.5000, 1.0000, 2.5000, 4.0000]]) assert torch.allclose(xyxyr, expected_xyxyr) class test_get_box_type(unittest.TestCase): def test_get_box_type(self): box_type_3d, box_mode_3d = get_box_type('camera') assert box_type_3d == CameraInstance3DBoxes assert box_mode_3d == Box3DMode.CAM box_type_3d, box_mode_3d = get_box_type('depth') assert box_type_3d == DepthInstance3DBoxes assert box_mode_3d == Box3DMode.DEPTH box_type_3d, box_mode_3d = get_box_type('lidar') assert box_type_3d == LiDARInstance3DBoxes assert box_mode_3d == Box3DMode.LIDAR def test_bad_box_type(self): self.assertRaises(ValueError, get_box_type, 'test') def test_points_cam2img(): torch.manual_seed(0) points = torch.rand([5, 3]) proj_mat = torch.rand([4, 4]) point_2d_res = points_cam2img(points, proj_mat) expected_point_2d_res = torch.tensor([[0.5832, 0.6496], [0.6146, 0.7910], [0.6994, 0.7782], [0.5623, 0.6303], [0.4359, 0.6532]]) assert torch.allclose(point_2d_res, expected_point_2d_res, 1e-3)