| """๋์ ๊ฐ์ฒด ๋ถ๋ฆฌ โ ๋ฐ์ค ๊ธฐํ, ํ์ , ๋ณด๊ฐ, ํฌ์.""" |
|
|
| import math |
|
|
| import pytest |
|
|
| np = pytest.importorskip("numpy") |
|
|
| from nusim.data.index import Box, CameraCalib, Frame, SceneIndex |
| from nusim.data.rig import box_corners, camera_pose, points_in_box |
| from nusim.recon.dynamic import ( |
| _hull, box_at, boxes_at, build_tracks, dynamic_instances, dynamic_mask, end_displacement, masked_instances, |
| project_box, static_instances, |
| ) |
|
|
| _YAW90 = (math.sqrt(0.5), 0.0, 0.0, math.sqrt(0.5)) |
|
|
|
|
| def test_box_corners_axes_follow_devkit(): |
| |
| c = box_corners((0, 0, 0), (2.0, 4.0, 1.0), (1, 0, 0, 0)) |
| assert np.allclose(np.abs(c).max(0), [2.0, 1.0, 0.5]) |
| |
| c = box_corners((10, 0, 0), (2.0, 4.0, 1.0), _YAW90) |
| assert np.allclose(np.abs(c - [10, 0, 0]).max(0), [1.0, 2.0, 0.5]) |
|
|
|
|
| def test_points_in_box_matches_corners(): |
| rng = np.random.default_rng(0) |
| t, size, q = (5.0, -3.0, 1.0), (1.8, 4.5, 1.6), _YAW90 |
| |
| c = box_corners(t, size, q) |
| assert points_in_box(t + (c - t) * 0.99, t, size, q).all() |
| assert not points_in_box(t + (c - t) * 1.01, t, size, q).any() |
| |
| assert points_in_box(t + (c - t) * 1.01, t, size, q, pad=0.1).all() |
| |
| p = rng.uniform(-6, 6, size=(2000, 3)) + t |
| inside = points_in_box(p, t, size, q) |
| local = p - t |
| ref = (np.abs(local[:, 1]) <= 2.25) & (np.abs(local[:, 0]) <= 0.9) & (np.abs(local[:, 2]) <= 0.8) |
| assert np.array_equal(inside, ref) |
|
|
|
|
| def _scene_with_tracks(): |
| s = SceneIndex(token="t", name="scene-test", location="singapore-onenorth", description="", |
| log_token="l", date_captured="", vehicle="", nbr_samples=10) |
| s.instances = [("a", "vehicle.car"), ("b", "vehicle.car"), ("c", "human.pedestrian.adult"), |
| ("d", "vehicle.car")] |
| rng = np.random.default_rng(1) |
| q = (1.0, 0.0, 0.0, 0.0) |
| for k in range(10): |
| ts = 1_000_000 + 500_000 * k |
| |
| s.boxes.append(Box(ts, 0, tuple(np.array([10.0, 0, 0]) + rng.uniform(-0.6, 0.6, 3)), |
| (1.8, 4.5, 1.6), q, "vehicle.parked")) |
| |
| s.boxes.append(Box(ts, 1, (20.0 + 2.0 * k, 5.0, 0.0), (1.8, 4.5, 1.6), q, "vehicle.moving")) |
| |
| s.boxes.append(Box(ts, 2, (0.0, 8.0, 0.0), (0.6, 0.6, 1.7), q, "pedestrian.standing")) |
| |
| s.boxes.append(Box(1_000_000, 3, (30.0, -5.0, 0.0), (1.8, 4.5, 1.6), q, "vehicle.moving")) |
| return s |
|
|
|
|
| def test_dynamic_rule(): |
| s = _scene_with_tracks() |
| tr = build_tracks(s) |
| assert end_displacement(tr[0]) < 1.0 |
| assert end_displacement(tr[1]) > 10.0 |
| assert dynamic_instances(s) == {1, 2, 3} |
|
|
|
|
| def test_masked_instances_agents_removes_parked_but_keeps_map_objects(): |
| s = _scene_with_tracks() |
| s.instances.append(("e", "movable_object.trafficcone")) |
| s.boxes.append(Box(1_000_000, 4, (5.0, 5.0, 0.0), (0.4, 0.4, 0.8), (1.0, 0.0, 0.0, 0.0), "")) |
| assert masked_instances(s, "dynamic") == {1, 2, 3} |
| assert masked_instances(s, "agents") == {0, 1, 2, 3} |
| with pytest.raises(ValueError): |
| masked_instances(s, "everything") |
|
|
|
|
| def test_box_interpolation(): |
| tr = build_tracks(_scene_with_tracks())[1] |
| |
| pos, q = box_at(tr, 1_000_000 + 250_000) |
| assert np.allclose(pos, [21.0, 5.0, 0.0]) |
| assert np.isclose(np.linalg.norm(q), 1.0) |
| |
| assert np.allclose(box_at(tr, 1_000_000 - 200_000)[0], [20.0, 5.0, 0.0]) |
| assert box_at(tr, 1_000_000 - 300_000) is None |
|
|
|
|
| def _front_pose(): |
| calib = CameraCalib(translation=(0.0, 0.0, 1.5), rotation=(0.5, -0.5, 0.5, -0.5), |
| intrinsic=((1000.0, 0, 800.0), (0, 1000.0, 450.0), (0, 0, 1)), |
| width=1600, height=900) |
| fr = Frame(0, "x.jpg", (0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0), True) |
| return camera_pose(fr, calib, "CAM_FRONT") |
|
|
|
|
| def test_project_box_in_front(): |
| pose = _front_pose() |
| poly = project_box(pose, box_corners((20.0, 0.0, 1.5), (2.0, 2.0, 2.0), (1, 0, 0, 0))) |
| |
| assert poly is not None |
| assert 700 < poly[:, 0].min() < 760 and 840 < poly[:, 0].max() < 900 |
| assert poly[:, 1].min() < 450 < poly[:, 1].max() |
|
|
|
|
| def test_project_box_behind_is_none_and_straddling_is_clipped(): |
| pose = _front_pose() |
| assert project_box(pose, box_corners((-20.0, 0.0, 1.5), (2, 2, 2), (1, 0, 0, 0))) is None |
| |
| |
| poly = project_box(pose, box_corners((0.0, 0.0, 1.5), (4, 4, 4), (1, 0, 0, 0))) |
| assert poly[:, 0].min() < 0 and poly[:, 0].max() > 1600 |
| assert poly[:, 1].min() < 0 and poly[:, 1].max() > 900 |
|
|
|
|
| def test_dynamic_mask_covers_box(): |
| pose = _front_pose() |
| m = dynamic_mask(pose, [((20.0, 0.0, 1.5), (2.0, 2.0, 2.0), (1, 0, 0, 0))], 800, 450, |
| scale=2, pad=0.0) |
| assert m[225, 400] and not m[10, 10] |
| assert 0.001 < m.mean() < 0.05 |
|
|
|
|
| def test_hull_square(): |
| pts = np.array([[0, 0], [1, 0], [1, 1], [0, 1], [0.5, 0.5], [0.2, 0.7]], dtype=float) |
| h = _hull(pts) |
| assert len(h) == 4 |
| assert {tuple(p) for p in h} == {(0, 0), (1, 0), (1, 1), (0, 1)} |
|
|
|
|
| def test_static_agents_masked_outside_annotation_window(): |
| """์ฃผ์ฐจ ์ฐจ๋์ ์ด๋
ธํ
์ด์
๊ตฌ๊ฐ(ํคํ๋ ์) ๋ฐ ์๊ฐ์๋ ๊ฐ์ ์๋ฆฌ์ ๊ฐ๋ฆฐ๋ค.""" |
| s = _scene_with_tracks() |
| tr = build_tracks(s) |
| late = 100_000_000 |
| assert boxes_at(tr, {0}, late) == [] |
| assert static_instances(s, "dynamic") == set() |
| st = static_instances(s, "agents") |
| assert st == {0} |
| (pos, size, rot), = boxes_at(tr, {0}, late, static=st) |
| assert np.linalg.norm(pos - [10.0, 0, 0]) < 0.7 |
|
|
