File size: 6,633 Bytes
4c75661 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 | """๋์ ๊ฐ์ฒด ๋ถ๋ฆฌ โ ๋ฐ์ค ๊ธฐํ, ํ์ , ๋ณด๊ฐ, ํฌ์."""
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)) # z์ถ +90ยฐ
def test_box_corners_axes_follow_devkit():
# size=(w,l,h). ํ์ ์์์ด๋ฉด x ๋ฐฉํฅ ๋ฐ๊ธธ์ด๊ฐ l/2, y ๊ฐ w/2.
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])
# +90ยฐ ์ โ ๊ธธ์ด ๋ฐฉํฅ์ด ์ ์ญ y ๋ก
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()
# pad ๋ ๋ชจ๋ ๋ฐฉํฅ์ผ๋ก ๋๋ฆฐ๋ค
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
# a: ์ฃผ์ฐจ ์ฐจ๋ โ ์ ์๋ฆฌ์ง๋ง ์ด๋
ธํ
์ด์
์ด ยฑ0.6 m ํ๋ค๋ฆฐ๋ค
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"))
# b: ์ด๋น 4 m ๋ก ๋ฌ๋ฆฌ๋ ์ฐจ
s.boxes.append(Box(ts, 1, (20.0 + 2.0 * k, 5.0, 0.0), (1.8, 4.5, 1.6), q, "vehicle.moving"))
# c: ์ ์๋ ๋ณดํ์
s.boxes.append(Box(ts, 2, (0.0, 8.0, 0.0), (0.6, 0.6, 1.7), q, "pedestrian.standing"))
# d: ํ ํคํ๋ ์๋ง ์ค์ณ ๊ฐ ์ฐจ (๋ณ์ 0 ์ด์ง๋ง moving)
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)
# ๋ ๋๋จธ EDGE_HOLD ์์ชฝ์ ๋๊ฐ, ๋ฐ๊นฅ์ None
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") # ego +x ๋ฅผ ๋ณธ๋ค
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)))
# 20 m ์, ํญ 2 m โ ๋๋ต ยฑ1000*1/19 ~ ยฑ1000*1/21 ํฝ์
. ํ๋ฉด ์ค์ ๋๋ .
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 # ํ๋ค๋ฆฐ ์ด๋
ธํ
์ด์
์ ์ค์๊ฐ
|