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  # ํ”๋“ค๋ฆฐ ์–ด๋…ธํ…Œ์ด์…˜์˜ ์ค‘์•™๊ฐ’