414 lines
14 KiB
Python
414 lines
14 KiB
Python
from __future__ import annotations
|
|
|
|
import importlib.util
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
from k1link.compute.integrated_perception import _CuboidPresentationState
|
|
|
|
|
|
def _worker_modules() -> tuple[object, object]:
|
|
root = Path(__file__).resolve().parents[1] / "experiments" / "perception"
|
|
worker = root / "worker"
|
|
sys.path.insert(0, str(root))
|
|
sys.path.insert(0, str(worker))
|
|
try:
|
|
fusion_spec = importlib.util.spec_from_file_location(
|
|
"e10_test_fusion", root / "e10_fusion_runtime.py"
|
|
)
|
|
assert fusion_spec is not None and fusion_spec.loader is not None
|
|
fusion = importlib.util.module_from_spec(fusion_spec)
|
|
sys.modules[fusion_spec.name] = fusion
|
|
fusion_spec.loader.exec_module(fusion)
|
|
runner_spec = importlib.util.spec_from_file_location(
|
|
"e10_test_runner", worker / "run_e10_integrated_perception.py"
|
|
)
|
|
assert runner_spec is not None and runner_spec.loader is not None
|
|
runner = importlib.util.module_from_spec(runner_spec)
|
|
sys.modules[runner_spec.name] = runner
|
|
runner_spec.loader.exec_module(runner)
|
|
return fusion, runner
|
|
finally:
|
|
sys.path.pop(0)
|
|
sys.path.pop(0)
|
|
|
|
|
|
def test_e10_profile_pins_integrated_realtime_budget() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
profile_path = (
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e10_integrated_perception_profile.json"
|
|
)
|
|
profile, digest = runner.read_profile(profile_path)
|
|
assert len(digest) == 64
|
|
assert profile["replay"] == {
|
|
"speed": 1.0,
|
|
"detector_queue_capacity": 2,
|
|
"semantic_queue_capacity": 1,
|
|
"semantic_sample_every_frames": 5,
|
|
"semantic_ttl_ms": 750.0,
|
|
}
|
|
assert profile["acceptance"]["detector_minimum_effective_fps"] == 9.5
|
|
assert profile["acceptance"]["maximum_p95_world_state_age_ms"] == 175.0
|
|
assert profile["acceptance"]["minimum_lidar_fused_frames"] == 450
|
|
|
|
|
|
def test_e10_semantic_loss_profile_is_explicit_and_fail_closed() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
profile_path = (
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e10_semantic_loss_profile.json"
|
|
)
|
|
profile, digest = runner.read_profile(profile_path)
|
|
assert len(digest) == 64
|
|
assert profile["mode"] == "semantic-loss-negative-control"
|
|
assert profile["semantic_loss"]["stop_after_completed_results"] == 20
|
|
assert profile["acceptance"]["minimum_stale_detector_frames"] == 450
|
|
assert profile["replay"]["semantic_ttl_ms"] == 750.0
|
|
|
|
|
|
def test_e10_full_session_profile_pins_complete_camera_epoch() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
profile_path = (
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e10_full_session_profile.json"
|
|
)
|
|
profile, digest = runner.read_profile(profile_path)
|
|
assert len(digest) == 64
|
|
assert profile["mode"] == "full-session-qualification"
|
|
assert profile["selection"] == {
|
|
"required_frame_count": 4489,
|
|
"required_source_start_frame_index": 0,
|
|
"required_source_end_frame_index": 4488,
|
|
"minimum_source_span_seconds": 448.0,
|
|
}
|
|
assert profile["acceptance"]["detector_maximum_drop_fraction"] == 0.0
|
|
assert profile["acceptance"]["minimum_lidar_fused_frames"] == 3500
|
|
|
|
|
|
def test_e13_profile_pins_provenance_marked_amodal_completion() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
profile_path = (
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e13_amodal_cuboid_profile.json"
|
|
)
|
|
|
|
profile, digest = runner.read_profile(profile_path)
|
|
|
|
assert len(digest) == 64
|
|
assert profile["mode"] == "pilot"
|
|
assert profile["cuboid_completion"]["mode"] == "class-prior-amodal-v1"
|
|
assert profile["cuboid_completion"]["classes"]["car"]["nominal_size_m"] == [
|
|
4.5,
|
|
1.85,
|
|
1.55,
|
|
]
|
|
assert profile["cuboid_completion"]["temporal"]["confirmation_hits"] == 3
|
|
|
|
|
|
def test_e19_profile_adds_ground_aware_support_without_mutating_e14() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
root = Path(__file__).resolve().parents[1] / "experiments" / "perception" / "worker"
|
|
e14, _e14_digest = runner.read_profile(root / "e14_full_session_amodal_profile.json")
|
|
e19, _e19_digest = runner.read_profile(root / "e19_ground_aware_cuboid_profile.json")
|
|
|
|
assert "object_support_ground_filter" not in e14["association"]
|
|
assert "support_duplicate_overlap_threshold" not in e14["association"]
|
|
assert e19["profile_id"] == "lab-e19-ground-aware-cuboids-v1"
|
|
assert (
|
|
e19["association"]["object_support_ground_filter"]["mode"]
|
|
== "local-ground-relative-object-support-v1"
|
|
)
|
|
assert e19["association"]["support_duplicate_overlap_threshold"] == 0.6
|
|
|
|
|
|
def test_e14_profile_combines_full_session_and_amodal_gates() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
profile_path = (
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e14_full_session_amodal_profile.json"
|
|
)
|
|
|
|
profile, digest = runner.read_profile(profile_path)
|
|
|
|
assert len(digest) == 64
|
|
assert profile["mode"] == "full-session-qualification"
|
|
assert profile["selection"] == {
|
|
"required_frame_count": 4489,
|
|
"required_source_start_frame_index": 0,
|
|
"required_source_end_frame_index": 4488,
|
|
"minimum_source_span_seconds": 448.0,
|
|
}
|
|
assert profile["cuboid_completion"]["mode"] == "class-prior-amodal-v1"
|
|
assert profile["cuboid_completion"]["failure_policy"] == "reject"
|
|
assert profile["acceptance"]["minimum_lidar_fused_frames"] == 3500
|
|
assert profile["acceptance"]["minimum_accepted_cuboids"] == 1500
|
|
|
|
|
|
def test_recorded_cuboid_presentation_holds_one_failed_association_then_expires() -> None:
|
|
state = _CuboidPresentationState(hold_ns=500_000_000)
|
|
accepted = {
|
|
"association_group": "vehicle",
|
|
"clustered_points": 14,
|
|
"cuboid_status": "accepted-class-prior-amodal-v1",
|
|
"distance_smoothed_m": 8.25,
|
|
"label": "car",
|
|
"track_id": 7,
|
|
}
|
|
initial = state.update(
|
|
1_000_000_000,
|
|
[accepted],
|
|
np.asarray([[1.0, 2.0, 3.0]], dtype=np.float32),
|
|
np.asarray([[2.25, 0.925, 0.775]], dtype=np.float32),
|
|
np.asarray([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32),
|
|
np.asarray([[118, 204, 132, 88]], dtype=np.uint8),
|
|
)
|
|
assert initial is not None
|
|
assert initial[4] == ["vehicle #7 car · 8.2 m · 14 pts"]
|
|
|
|
rejected = {"cuboid_status": "rejected-no-semantic-lidar-support", "track_id": 7}
|
|
held = state.update(
|
|
1_200_000_000,
|
|
[rejected],
|
|
np.empty((0, 3), dtype=np.float32),
|
|
np.empty((0, 3), dtype=np.float32),
|
|
np.empty((0, 4), dtype=np.float32),
|
|
np.empty((0, 4), dtype=np.uint8),
|
|
)
|
|
assert held is not None
|
|
assert held[4] == ["vehicle #7 car · 8.2 m · 14 pts · hold 200 ms"]
|
|
assert int(held[3][0, 3]) < 88
|
|
|
|
expired = state.update(
|
|
1_600_000_001,
|
|
[],
|
|
np.empty((0, 3), dtype=np.float32),
|
|
np.empty((0, 3), dtype=np.float32),
|
|
np.empty((0, 4), dtype=np.float32),
|
|
np.empty((0, 4), dtype=np.uint8),
|
|
)
|
|
assert expired is None
|
|
|
|
|
|
def test_e13_completes_a_visible_car_face_away_from_the_sensor() -> None:
|
|
fusion, runner = _worker_modules()
|
|
profile, _digest = runner.read_profile(
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e13_amodal_cuboid_profile.json"
|
|
)
|
|
y = np.linspace(-0.8, 0.8, 12)
|
|
z = np.linspace(0.35, 1.3, 5)
|
|
support = np.asarray(
|
|
[[10.0 + 0.01 * (index % 2), side, height] for index, side in enumerate(y) for height in z],
|
|
dtype=np.float64,
|
|
)
|
|
ground = np.asarray(
|
|
[[x, side, 0.0] for x in np.linspace(8.0, 12.0, 8) for side in (-1.5, 0.0, 1.5)],
|
|
dtype=np.float64,
|
|
)
|
|
observed = fusion._cuboid(support, profile["association"], "vehicle")
|
|
assert observed is not None
|
|
tracker = fusion.CuboidCompletionTracker(profile["cuboid_completion"])
|
|
|
|
completed = tracker.complete(
|
|
track_id=4,
|
|
label="car",
|
|
support_points_map=support,
|
|
all_points_map=np.concatenate((support, ground)),
|
|
sensor_position_map=(0.0, 0.0, 0.0),
|
|
session_seconds=1.0,
|
|
observed_cuboid=observed,
|
|
)
|
|
|
|
assert completed is not None
|
|
assert completed.orientation_source == "support-face-normal"
|
|
completed_size = np.asarray(completed.cuboid.half_size) * 2.0
|
|
assert completed_size[0] == 4.5
|
|
assert 1.85 <= completed_size[1] <= 2.0
|
|
assert completed_size[2] == 1.55
|
|
assert completed.cuboid.center_map[0] > 12.0
|
|
assert completed.ground_z_map == 0.0
|
|
assert completed.completion_fraction > 0.8
|
|
assert completed.support_coverage_fraction >= 0.75
|
|
|
|
|
|
def test_e13_temporal_filter_reduces_cuboid_center_jitter() -> None:
|
|
fusion, runner = _worker_modules()
|
|
profile, _digest = runner.read_profile(
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e13_amodal_cuboid_profile.json"
|
|
)
|
|
tracker = fusion.CuboidCompletionTracker(profile["cuboid_completion"])
|
|
base = np.asarray(
|
|
[
|
|
[x, 2.0 + 0.02 * (index % 2), z]
|
|
for index, x in enumerate(np.linspace(8.0, 11.5, 20))
|
|
for z in (0.4, 0.9, 1.3)
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
ground = np.asarray([[x, y, 0.0] for x in np.linspace(7.0, 13.0, 10) for y in (0.5, 2.0, 3.5)])
|
|
|
|
outputs = []
|
|
for frame, lateral_jitter in enumerate((0.0, 0.4, 0.2), start=1):
|
|
support = base + np.asarray([0.0, lateral_jitter, 0.0])
|
|
observed = fusion._cuboid(support, profile["association"], "vehicle")
|
|
assert observed is not None
|
|
value = tracker.complete(
|
|
track_id=9,
|
|
label="car",
|
|
support_points_map=support,
|
|
all_points_map=np.concatenate((support, ground)),
|
|
sensor_position_map=(0.0, 0.0, 0.0),
|
|
session_seconds=frame * 0.1,
|
|
observed_cuboid=observed,
|
|
)
|
|
assert value is not None
|
|
outputs.append(value)
|
|
|
|
lateral_shift = abs(outputs[1].cuboid.center_map[1] - outputs[0].cuboid.center_map[1])
|
|
assert lateral_shift < 0.4
|
|
assert outputs[-1].temporal_status == "confirmed"
|
|
|
|
|
|
def test_e19_ground_filter_preserves_cloud_and_excludes_ground_from_box_support() -> None:
|
|
fusion, runner = _worker_modules()
|
|
profile, _digest = runner.read_profile(
|
|
Path(__file__).resolve().parents[1]
|
|
/ "experiments"
|
|
/ "perception"
|
|
/ "worker"
|
|
/ "e19_ground_aware_cuboid_profile.json"
|
|
)
|
|
ground = np.asarray(
|
|
[[x, y, 0.0] for x in (9.5, 10.0, 10.5) for y in (-0.6, 0.0, 0.6)],
|
|
dtype=np.float64,
|
|
)
|
|
vehicle = np.asarray(
|
|
[[x, y, z] for x in (9.8, 10.2) for y in (-0.4, 0.4) for z in (0.3, 0.9, 1.4)],
|
|
dtype=np.float64,
|
|
)
|
|
cloud = np.concatenate((ground, vehicle))
|
|
unchanged = cloud.copy()
|
|
indices = np.arange(cloud.shape[0], dtype=np.int64)
|
|
|
|
filtered, ground_z, rejected = fusion._filter_object_support_by_ground(
|
|
indices,
|
|
cloud,
|
|
group="vehicle",
|
|
profile=profile["association"]["object_support_ground_filter"],
|
|
)
|
|
|
|
assert ground_z == 0.0
|
|
assert rejected == ground.shape[0]
|
|
assert filtered.tolist() == list(range(ground.shape[0], cloud.shape[0]))
|
|
np.testing.assert_array_equal(cloud, unchanged)
|
|
|
|
|
|
def test_e19_duplicate_tracks_cannot_publish_the_same_lidar_support_twice() -> None:
|
|
fusion, _runner = _worker_modules()
|
|
cuboid = fusion.Cuboid(
|
|
center_map=(10.0, 0.0, 0.8),
|
|
half_size=(2.25, 0.925, 0.775),
|
|
quaternion_xyzw=(0.0, 0.0, 0.0, 1.0),
|
|
)
|
|
|
|
def item(track_id: int, score: float, indices: list[int]) -> object:
|
|
source = np.asarray(indices, dtype=np.int64)
|
|
return fusion.TrackFusion(
|
|
track_id=track_id,
|
|
label="car",
|
|
association_group="vehicle",
|
|
score=score,
|
|
bbox_xyxy=(100.0, 100.0, 200.0, 200.0),
|
|
candidate_points=source.size,
|
|
semantic_points=source.size,
|
|
clustered_points=source.size,
|
|
distance_p10_m=9.5,
|
|
distance_median_m=10.0,
|
|
distance_smoothed_m=10.0,
|
|
status="accepted-class-prior-amodal-v1",
|
|
cuboid=cuboid,
|
|
source_indices=source,
|
|
)
|
|
|
|
result = fusion._suppress_duplicate_support_fusions(
|
|
[
|
|
item(10, 0.91, [1, 2, 3, 4, 5]),
|
|
item(11, 0.72, [1, 2, 3, 4]),
|
|
item(12, 0.80, [20, 21, 22, 23]),
|
|
],
|
|
overlap_threshold=0.6,
|
|
)
|
|
by_track = {value.track_id: value for value in result}
|
|
|
|
assert by_track[10].cuboid is not None
|
|
assert by_track[11].cuboid is None
|
|
assert by_track[11].status == "rejected-duplicate-lidar-support"
|
|
assert by_track[11].source_indices.size == 0
|
|
assert by_track[12].cuboid is not None
|
|
|
|
|
|
def test_e10_semantic_binding_is_explicit_about_freshness() -> None:
|
|
_fusion, runner = _worker_modules()
|
|
assert runner.semantic_binding(None, 2.0, 750.0) == ("unavailable", None)
|
|
value = runner.SemanticResult(
|
|
frame_index=1,
|
|
source_frame_index=1001,
|
|
session_seconds=1.0,
|
|
completion_age_ms=200.0,
|
|
completed_monotonic=3.0,
|
|
mask=np.zeros((600, 800), dtype=np.uint8),
|
|
mask_sha256="0" * 64,
|
|
class_pixels={},
|
|
)
|
|
assert runner.semantic_binding(value, 1.7, 750.0) == ("fresh", 700.0)
|
|
assert runner.semantic_binding(value, 1.8, 750.0) == (
|
|
"stale",
|
|
800.0,
|
|
)
|
|
|
|
|
|
def test_e10_projection_keeps_only_nearest_point_per_pixel() -> None:
|
|
fusion, _runner = _worker_modules()
|
|
profile = fusion.ProjectionProfile(
|
|
width=800,
|
|
height=600,
|
|
intrinsic_fx_fy_cx_cy=(100.0, 100.0, 400.0, 300.0),
|
|
distortion_kb4=(0.0, 0.0, 0.0, 0.0),
|
|
t_camera_from_lidar=np.eye(4, dtype=np.float64),
|
|
)
|
|
points = np.asarray([[0.0, 0.0, 2.0], [0.0, 0.0, 3.0]], dtype=np.float64)
|
|
pixels, depths, source, _lidar = fusion.project_points(
|
|
points,
|
|
(0.0, 0.0, 0.0),
|
|
(0.0, 0.0, 0.0, 1.0),
|
|
profile,
|
|
)
|
|
assert pixels.shape == (1, 2)
|
|
assert depths.tolist() == [2.0]
|
|
assert source.tolist() == [0]
|