SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(perception): add camera ego-motion evidence

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-24 13:37:39 +03:00
parent 230cba4b21
commit 7fba39a629
11 changed files with 2348 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_e26_camera_ego_motion.py
+308
View File
@@ -0,0 +1,308 @@
from __future__ import annotations
from pathlib import Path
import numpy as np
from k1link.compute.camera_ego_motion import (
CameraEgoMotionTracker,
_camera_evidence,
_fuse_camera_lidar_object,
evaluate_camera_ego_motion_benchmark,
read_camera_ego_motion_benchmark,
read_camera_ego_motion_profile,
)
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
Kb4ProjectionProfile,
project_map_points_kb4,
)
ROOT = Path(__file__).parents[1]
def _profile() -> dict[str, object]:
profile, digest = read_camera_ego_motion_profile(
ROOT
/ "experiments"
/ "perception"
/ "e26_camera_ego_motion_profile.json"
)
assert len(digest) == 64
return profile
def _projection() -> Kb4ProjectionProfile:
transform = np.eye(4, dtype=np.float64)
transform.setflags(write=False)
return Kb4ProjectionProfile(
source_id="sensor.camera.right",
calibration_slot="camera_1",
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=transform,
)
def _bbox_for_world_point(
point_map: np.ndarray,
position_map: np.ndarray,
*,
width: float = 50.0,
height: float = 60.0,
) -> list[float]:
projected = project_map_points_kb4(
[point_map],
position_map_xyz=(
float(position_map[0]),
float(position_map[1]),
float(position_map[2]),
),
orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
profile=_projection(),
)
u, v = projected.pixels_xy[0]
inset = float(
_profile()["camera_evidence"]["footpoint_inset_fraction"] # type: ignore[index]
)
bottom = float(v) + inset * height
return [
float(u) - width * 0.5,
bottom - height,
float(u) + width * 0.5,
bottom,
]
def _observe_track(
tracker: CameraEgoMotionTracker,
*,
moving: bool,
) -> dict[str, object]:
evidence: dict[str, object] = {}
for index in range(18):
session_seconds = index * 0.1
position = np.asarray([index * 0.1, 0.0, 0.0], dtype=np.float64)
point = np.asarray(
[index * 0.35 if moving else 0.0, 0.0, 6.0],
dtype=np.float64,
)
evidence = tracker.observe(
source_track_id=7,
group="person",
session_seconds=session_seconds,
bbox_xyxy=_bbox_for_world_point(point, position),
detector_score=0.9,
position_map_xyz=position,
orientation_map_from_lidar_xyzw=np.asarray(
[0.0, 0.0, 0.0, 1.0],
dtype=np.float64,
),
)
return evidence
def test_e26_profile_and_benchmark_are_pinned_and_diagnostic_only() -> None:
profile = _profile()
benchmark, digest = read_camera_ego_motion_benchmark(
ROOT / "experiments" / "perception" / "e26_motion_benchmark.json"
)
assert profile["authority"] == {
"commands_enabled": False,
"navigation_or_safety_accepted": False,
}
assert profile["fusion"]["unknown_is_occupied"] is True
assert profile["fusion"]["camera_only_metric_velocity_valid"] is False
assert benchmark["benchmark_id"] == (
"ravnoves00-camera-ego-reviewed-anchors-v1"
)
assert len(benchmark["events"]) == 11
assert len(digest) == 64
def test_e26_multiview_rays_accept_one_static_world_hypothesis() -> None:
tracker = CameraEgoMotionTracker(_profile(), _projection())
evidence = _observe_track(tracker, moving=False)
assert evidence["camera_evidence_current"] is True
assert evidence["camera_motion_state"] == "static"
assert float(evidence["camera_static_residual_p80_degrees"]) < 1e-6
assert float(evidence["camera_static_positive_depth_fraction"]) == 1.0
assert tracker.snapshot()["peak_tracks"] == 1
def test_e26_multiview_rays_reject_a_moving_world_hypothesis() -> None:
tracker = CameraEgoMotionTracker(_profile(), _projection())
evidence = _observe_track(tracker, moving=True)
assert evidence["camera_evidence_current"] is True
assert evidence["camera_motion_state"] == "dynamic"
assert float(evidence["camera_motion_confidence"]) > 0.0
def test_e26_invalid_fisheye_ray_is_unavailable_not_a_run_failure() -> None:
tracker = CameraEgoMotionTracker(_profile(), _projection())
evidence = tracker.observe(
source_track_id=99,
group="vehicle",
session_seconds=1.0,
bbox_xyxy=[100000.0, 100000.0, 100100.0, 100100.0],
detector_score=0.9,
position_map_xyz=np.zeros(3, dtype=np.float64),
orientation_map_from_lidar_xyzw=np.asarray(
[0.0, 0.0, 0.0, 1.0],
dtype=np.float64,
),
)
assert evidence["camera_evidence_current"] is False
assert tracker.snapshot()["invalid_camera_rays"] == 1
assert tracker.snapshot()["active_tracks"] == 0
def test_e26_camera_lidar_conflict_is_unknown_not_averaged() -> None:
profile = _profile()
tracker = CameraEgoMotionTracker(profile, _projection())
camera_object = {
"track_id": 9,
"association_group": "vehicle",
"bbox_xyxy": [100.0, 100.0, 160.0, 160.0],
"label": "car",
"score": 0.9,
}
lidar_object = {
**camera_object,
"track_id": 240009,
"source_track_id": 9,
"motion_state": "static",
"motion_confidence": 0.8,
"speed_mps": 0.0,
"velocity_map_mps": [0.0, 0.0, 0.0],
"cuboid_status": "accepted-world-tracked-e24-v1",
}
evidence = {
**_camera_evidence(None, "unknown", 0.0),
"camera_motion_state": "dynamic",
"camera_motion_confidence": 0.9,
"camera_evidence_current": True,
}
fused = _fuse_camera_lidar_object(
camera_object,
lidar_object,
evidence,
profile,
tracker,
)
assert fused["motion_state"] == "unknown"
assert fused["motion_status"] == "e26-explicit-camera-lidar-conflict"
assert fused["speed_mps"] is None
assert fused["velocity_map_mps"] is None
assert tracker.snapshot()["fusion_conflicts"] == 1
def test_e26_camera_only_dynamic_does_not_invent_metric_velocity() -> None:
profile = _profile()
tracker = CameraEgoMotionTracker(profile, _projection())
camera_object = {
"track_id": 12,
"association_group": "person",
"bbox_xyxy": [100.0, 100.0, 160.0, 190.0],
"label": "person",
"score": 0.9,
"speed_mps": 42.0,
"velocity_map_mps": [42.0, 0.0, 0.0],
}
evidence = {
**_camera_evidence(None, "unknown", 0.0),
"camera_motion_state": "dynamic",
"camera_motion_confidence": 0.8,
"camera_evidence_current": True,
}
fused = _fuse_camera_lidar_object(
camera_object,
None,
evidence,
profile,
tracker,
)
assert fused["motion_state"] == "dynamic"
assert fused["motion_status"] == (
"e26-camera-relative-only-no-metric-velocity"
)
assert fused["speed_mps"] is None
assert fused["velocity_map_mps"] is None
assert fused["camera_only_metric_velocity_valid"] is False
def test_e26_benchmark_separates_dynamic_hits_and_static_false_alarm() -> None:
benchmark = {
"events": [
{
"id": "moving",
"kind": "target-motion",
"window_seconds": [0.0, 1.0],
"class_group": "person",
"target_source_track_ids": [7],
"expected_motion": "dynamic",
"minimum_hits": 2,
"minimum_span_seconds": 0.1,
"minimum_coverage_fraction": 0.5,
},
{
"id": "parked",
"kind": "static-control",
"window_seconds": [0.0, 1.0],
"class_group": "vehicle",
"expected_motion": "static",
"minimum_source_observations": 2,
"minimum_coverage_fraction": 0.5,
"minimum_classified_fraction": 0.5,
"maximum_false_dynamic_fraction": 0.1,
},
]
}
source_rows = []
fusion_rows = []
for index, timestamp in enumerate((0.0, 0.2)):
source_rows.append(
{
"frame_index": index,
"session_seconds": timestamp,
"objects": [
{
"track_id": 7,
"association_group": "person",
},
{
"track_id": 8,
"association_group": "vehicle",
},
],
}
)
fusion_rows.append(
{
"frame_index": index,
"session_seconds": timestamp,
"objects": [
{
"source_track_id": 7,
"association_group": "person",
"camera_evidence_current": True,
"motion_state": "dynamic",
},
{
"source_track_id": 8,
"association_group": "vehicle",
"camera_evidence_current": True,
"motion_state": "static",
},
],
}
)
result = evaluate_camera_ego_motion_benchmark(
source_rows,
fusion_rows,
benchmark,
)
assert result["passed"] is True
assert result["passed_events"] == 2
assert result["events"][1]["false_dynamic_fraction"] == 0.0
tests/test_xgrids_calibrated_projection.py
+14
View File
@@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
map_points_to_lidar,
project_map_points_kb4,
quaternion_xyzw_to_rotation_matrix,
unproject_pixels_kb4,
)
from k1link.device_plugins.xgrids_k1.calibration_schema import (
parse_k1_factory_calibration,
@@ -68,6 +69,19 @@ def test_kb4_projection_uses_theta_polynomial_and_rejects_behind_camera() -> Non
assert projected.source_indices.tolist() == [0, 1]
def test_kb4_unprojection_round_trips_camera_directions() -> None:
profile = _identity_profile()
directions = unproject_pixels_kb4(
[[400.0, 300.0], [400.0 + 100.0 * math.pi / 4.0, 300.0]],
profile=profile,
)
np.testing.assert_allclose(
directions,
[[0.0, 0.0, 1.0], [math.sqrt(0.5), 0.0, math.sqrt(0.5)]],
atol=1e-12,
)
def test_factory_profile_binds_right_main_camera_and_scales_intrinsics() -> None:
calibration = parse_k1_factory_calibration(
(FIXTURES / "camera.yaml").read_bytes(),