feat(perception): add camera ego-motion evidence
This commit is contained in:
DCCONSTRUCTIONS
parent
230cba4b21
commit
7fba39a629
|
|
@ -0,0 +1,261 @@
|
||||||
|
# LAB E26 — camera ego-motion evidence and conservative LiDAR fusion
|
||||||
|
|
||||||
|
Date: 2026-07-24
|
||||||
|
Status: recorded diagnostic accepted; navigation and safety acceptance are false
|
||||||
|
Immutable replay:
|
||||||
|
`LAB E26.1 · Camera + ego-motion · full RAVNOVES00`
|
||||||
|
|
||||||
|
## Objective
|
||||||
|
|
||||||
|
E25 proved that persistent map-frame LiDAR support can stop parked vehicles
|
||||||
|
from looking dynamic, but it cannot recover motion when the K1 supplies too few
|
||||||
|
current LiDAR returns. E26 adds an independent camera measurement without
|
||||||
|
inventing metric velocity:
|
||||||
|
|
||||||
|
1. preserve the original detector track in image space;
|
||||||
|
2. remove the apparent motion caused by the moving rig;
|
||||||
|
3. test whether observations from several calibrated camera poses are
|
||||||
|
consistent with one static point in `k1-map`;
|
||||||
|
4. fuse the resulting relative camera state with E25 LiDAR evidence;
|
||||||
|
5. represent disagreement and missing evidence explicitly as `unknown`;
|
||||||
|
6. keep every unknown object occupied and outside navigation authority.
|
||||||
|
|
||||||
|
The experiment changes the measurement, not the visual smoothing of 3D boxes.
|
||||||
|
Rerun remains the operator viewer. The perception result is produced before
|
||||||
|
Rerun and can be consumed independently of it.
|
||||||
|
|
||||||
|
## Immutable inputs
|
||||||
|
|
||||||
|
- Source observation: `20260720T065719Z_viewer_live` (`RAVNOVES00`).
|
||||||
|
- Published lab session: `lab-e26-1-camera-ego-motion`.
|
||||||
|
- Camera: `sensor.camera.right`, factory calibration slot `camera_1`.
|
||||||
|
- Coordinate frame: `k1-map`.
|
||||||
|
- Frames: 4,489.
|
||||||
|
- Source timeline: 35.421857292–484.044857292 s.
|
||||||
|
- Published replay duration: 448.623 s.
|
||||||
|
- Factory projection model: KB4.
|
||||||
|
- Calibration SHA-256:
|
||||||
|
`05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9`.
|
||||||
|
- Camera/detector source result:
|
||||||
|
`e10-integrated-perception-34ade557b5636717aa497fc00355b84df7063e483a175f2f5d3f04c03df1c898`.
|
||||||
|
- E25.3 LiDAR-evidence source result:
|
||||||
|
`e10-integrated-perception-9034a5cf306b379cc248c1da17639f140fc896bb55e412bef43f89974de892a3`.
|
||||||
|
- Detector: YOLOX-S, COCO-80, 640×640.
|
||||||
|
- Semantic source:
|
||||||
|
`tue-mps/cityscapes_semantic_eomt_large_1024`, FP16 autocast.
|
||||||
|
- E26 profile SHA-256:
|
||||||
|
`17668cea5a8bddd621271ed0457ca3a91e6d3957133d53213d6705d9b03d039e`.
|
||||||
|
- E26 benchmark SHA-256:
|
||||||
|
`c3523c8f1ca22631120a129d20c73a8429be8c908279cc1c3286661f33d5fe57`.
|
||||||
|
|
||||||
|
E26 does not repeat detector or segmentation inference. It reuses immutable
|
||||||
|
model outputs, E25 LiDAR evidence, rig poses and factory calibration. It writes
|
||||||
|
a new integrated result and hard-links immutable source payloads where
|
||||||
|
possible. The source results and raw sensor data are not modified.
|
||||||
|
|
||||||
|
## Camera measurement
|
||||||
|
|
||||||
|
For each original 2D detector track, E26 takes a bottom-centre image point with
|
||||||
|
a small configurable inset. The point is unprojected through the factory KB4
|
||||||
|
model into a three-dimensional camera ray. The calibrated camera-to-rig
|
||||||
|
extrinsic and the recorded rig pose transform that ray into `k1-map`.
|
||||||
|
|
||||||
|
Over a bounded two-second history, a least-squares ray intersection estimates
|
||||||
|
the one world point that would explain the observations if the target were
|
||||||
|
static. The tracker evaluates:
|
||||||
|
|
||||||
|
- median angular residual to the static-world hypothesis;
|
||||||
|
- p80 angular residual;
|
||||||
|
- positive-depth fraction;
|
||||||
|
- estimated range;
|
||||||
|
- rig baseline covered by the observations;
|
||||||
|
- observation count, time span, detector score and box size.
|
||||||
|
|
||||||
|
Low residuals support `static`. A well-observed track with residuals beyond the
|
||||||
|
class thresholds supports `dynamic`. A hypothesis behind the camera can support
|
||||||
|
`dynamic` only under the stricter score and repeated-observation gate.
|
||||||
|
Degenerate geometry, insufficient baseline, weak detections, missing pose or an
|
||||||
|
invalid KB4 edge ray yields `unknown`.
|
||||||
|
|
||||||
|
The camera branch measures relative inconsistency with a static world
|
||||||
|
hypothesis. It does not measure metric object velocity. Camera-only
|
||||||
|
`speed_mps` and `velocity_map_mps` are therefore deliberately null.
|
||||||
|
|
||||||
|
## Bounded streaming contract
|
||||||
|
|
||||||
|
The implementation is a single forward pass with no lookahead:
|
||||||
|
|
||||||
|
- history: at most 2.0 s;
|
||||||
|
- maximum 32 observations per camera track;
|
||||||
|
- maximum 256 live tracks;
|
||||||
|
- maximum idle time: 2.5 s;
|
||||||
|
- minimum observations: 5;
|
||||||
|
- minimum observation span: 0.4 s;
|
||||||
|
- minimum rig baseline: 0.25 m;
|
||||||
|
- maximum normal-matrix condition: 1,000,000;
|
||||||
|
- minimum box diagonal: 35 px;
|
||||||
|
- minimum median detector score: 0.60;
|
||||||
|
- dynamic confirmation: 2 frames;
|
||||||
|
- static confirmation: 4 frames;
|
||||||
|
- evidence decay: 1 frame.
|
||||||
|
|
||||||
|
The run reached only 34 simultaneous tracks. Five unprojectable KB4 edge rays
|
||||||
|
were rejected fail-soft and counted; detector and LiDAR objects remained in the
|
||||||
|
result as unavailable camera evidence.
|
||||||
|
|
||||||
|
## Fusion contract
|
||||||
|
|
||||||
|
Camera and LiDAR are treated as independent evidence sources:
|
||||||
|
|
||||||
|
- camera and LiDAR agree: publish the common state with conservative
|
||||||
|
confidence;
|
||||||
|
- camera and LiDAR disagree: publish explicit `unknown`, clear velocity and
|
||||||
|
increment the conflict counter;
|
||||||
|
- camera only: publish relative `static` or `dynamic` with confidence scaled by
|
||||||
|
0.65, but without metric velocity;
|
||||||
|
- LiDAR only: preserve the metric E25 state and velocity;
|
||||||
|
- neither source qualifies: publish `unknown`.
|
||||||
|
|
||||||
|
Every branch carries:
|
||||||
|
|
||||||
|
- `unknown_is_occupied=true`;
|
||||||
|
- `camera_only_metric_velocity_valid=false`;
|
||||||
|
- `navigation_or_safety_accepted=false`.
|
||||||
|
|
||||||
|
There is no command output and no planner authority in E26.
|
||||||
|
|
||||||
|
## Operator-reviewed benchmark
|
||||||
|
|
||||||
|
The benchmark contains eight dynamic target windows and three parked-vehicle
|
||||||
|
control windows. The dynamic windows are bound to reviewed source detector
|
||||||
|
track IDs, so a nearby parked object cannot satisfy a moving-object event.
|
||||||
|
|
||||||
|
Dynamic windows:
|
||||||
|
|
||||||
|
- woman with dog at 54–62 s: 24 dynamic hits, 82.76% evidence coverage;
|
||||||
|
- person loading a car at 62–69 s: best run 4 hits over 0.896 s, 81.82% coverage;
|
||||||
|
- adult and child at 111–120 s: 26 hits over 4.387 s, 91.49% coverage;
|
||||||
|
- person on the road at 124–132 s: 11 hits over 1.0 s, 76.19% coverage;
|
||||||
|
- stroller group at 151–161 s: best run 5 hits over 0.498 s, 84.0% coverage;
|
||||||
|
- oncoming vehicle at 167–173 s: 3 hits over 0.172 s, 73.33% coverage;
|
||||||
|
- vulnerable-road-user group at 170–179 s: best run 5 hits over 0.38 s,
|
||||||
|
72.09% coverage;
|
||||||
|
- same-direction vehicle at 180–188 s: 12 hits over 1.734 s, 100% coverage.
|
||||||
|
|
||||||
|
Parked-vehicle controls:
|
||||||
|
|
||||||
|
- 70–90 s: 853 source observations, 732 evidence observations,
|
||||||
|
57.51% classified, 418 static / 311 unknown / 3 dynamic,
|
||||||
|
false-dynamic fraction 0.00410;
|
||||||
|
- 132–148 s: 820 source observations, 746 evidence observations,
|
||||||
|
78.42% classified, 585 static / 161 unknown / 0 dynamic,
|
||||||
|
false-dynamic fraction 0.0;
|
||||||
|
- 161–166 s: 151 source observations, 120 evidence observations,
|
||||||
|
62.5% classified, 75 static / 45 unknown / 0 dynamic,
|
||||||
|
false-dynamic fraction 0.0.
|
||||||
|
|
||||||
|
Result: 11/11 benchmark windows pass.
|
||||||
|
|
||||||
|
This is a useful regression result, not independent ground truth. The windows
|
||||||
|
come from operator timestamps and reviewed detector-track binding on the same
|
||||||
|
recording used to develop the experiment. The result does not measure global
|
||||||
|
precision, recall, ID-switch rate, distance error or safety performance outside
|
||||||
|
those windows.
|
||||||
|
|
||||||
|
## Runtime and resource result
|
||||||
|
|
||||||
|
Camera ego-motion postprocessor:
|
||||||
|
|
||||||
|
- mean: 1.982 ms/frame;
|
||||||
|
- p50: 2.059 ms/frame;
|
||||||
|
- p95: 3.499 ms/frame;
|
||||||
|
- maximum: 106.171 ms/frame;
|
||||||
|
- camera observations: 20,513;
|
||||||
|
- created tracks: 1,466;
|
||||||
|
- peak live tracks: 34;
|
||||||
|
- pose-unavailable frames: 561;
|
||||||
|
- invalid camera rays: 5;
|
||||||
|
- published camera states: 6,105 static, 1,117 dynamic, 13,291 unknown;
|
||||||
|
- explicit camera/LiDAR conflicts: 374.
|
||||||
|
|
||||||
|
The p95 satisfies the configured 12 ms budget for this postprocessor. It is not
|
||||||
|
an end-to-end real-time measurement: detector inference, semantic inference,
|
||||||
|
sensor transport, decoding, synchronization, Rerun serialization, network
|
||||||
|
transfer and browser rendering are not included.
|
||||||
|
|
||||||
|
## Replay acceptance
|
||||||
|
|
||||||
|
The saved session is `ready`, `replayable=true`, and visible through the single
|
||||||
|
existing server on `http://127.0.0.1:8000/`.
|
||||||
|
|
||||||
|
An acceptance request through the same perception endpoint used by the browser
|
||||||
|
returned:
|
||||||
|
|
||||||
|
- HTTP 200;
|
||||||
|
- media type `application/vnd.rerun.rrd`;
|
||||||
|
- Rerun magic `RRF2`;
|
||||||
|
- payload: 69,735,518 bytes;
|
||||||
|
- SHA-256:
|
||||||
|
`0d7978468f829acb6afe2aa14f6e2f2f96d55ee35e21d8439cdd3ec3bdd107b1`;
|
||||||
|
- cold response time: 47.172 s.
|
||||||
|
|
||||||
|
The 47.172 s cold Rerun materialization is an operator-path cost, not perception
|
||||||
|
compute. It remains too slow for first connection in the field and must be
|
||||||
|
addressed separately through prebuilt/cached, chunked or live incremental
|
||||||
|
delivery. It does not invalidate the 3.499 ms motion-evidence p95, and the
|
||||||
|
3.499 ms number does not excuse the 47.172 s operator wait.
|
||||||
|
|
||||||
|
The server was not stopped, restarted or moved to another port during
|
||||||
|
acceptance.
|
||||||
|
|
||||||
|
## Validation
|
||||||
|
|
||||||
|
- Ruff passes for `src`, `tests` and `experiments/perception`.
|
||||||
|
- Strict mypy passes for the E26 compute, publication, calibration and CLI
|
||||||
|
modules.
|
||||||
|
- 44 targeted E10/E24/E25/E26 and KB4 projection tests pass.
|
||||||
|
- The complete repository `pytest` suite passes.
|
||||||
|
- The on-disk implementation, profile and benchmark SHA-256 values exactly
|
||||||
|
match those embedded in the immutable E26.1 report.
|
||||||
|
- The saved-session API reports `ready`, `replayable=true` and the expected
|
||||||
|
result ID.
|
||||||
|
- The single localhost service remains available on port 8000.
|
||||||
|
|
||||||
|
## Conclusion
|
||||||
|
|
||||||
|
E26 closes the specific E25 evidence gap on the reviewed recording: moving
|
||||||
|
people and the two moving vehicles can be detected from calibrated camera
|
||||||
|
parallax when current LiDAR support is sparse, while parked-vehicle false
|
||||||
|
dynamic remains near zero. The implementation is bounded and has no lookahead,
|
||||||
|
so its state structure is compatible with a later live shadow deployment.
|
||||||
|
|
||||||
|
The result is still not a planner-ready world model:
|
||||||
|
|
||||||
|
- camera-only motion has no metric velocity;
|
||||||
|
- 13,291 published camera observations remain unknown;
|
||||||
|
- 374 camera/LiDAR conflicts require explicit downstream handling;
|
||||||
|
- the benchmark is small and not independent;
|
||||||
|
- the detector and track association still inherit COCO-domain errors;
|
||||||
|
- no end-to-end live latency or dropped-frame envelope has been measured;
|
||||||
|
- the cold operator replay still takes approximately 47 s.
|
||||||
|
|
||||||
|
## E27 gate
|
||||||
|
|
||||||
|
E27 should not be another threshold-tuning pass. It should turn E26 into a
|
||||||
|
measurable world-model qualification:
|
||||||
|
|
||||||
|
1. create frame-level independent labels for the reviewed moving objects,
|
||||||
|
parked controls, ID switches and visibility;
|
||||||
|
2. quantify camera association integrity, dynamic/static precision and recall,
|
||||||
|
time-to-confirm and conflict duration;
|
||||||
|
3. add metric range/velocity only when LiDAR, ground contact or another
|
||||||
|
observable depth source supports it;
|
||||||
|
4. preserve occupancy and uncertainty independently of cuboid presentation;
|
||||||
|
5. replay the bounded stages at sensor cadence and record end-to-end deadline
|
||||||
|
misses, queue depth and dropped-frame policy;
|
||||||
|
6. keep the first live step shadow-only, with no navigation or safety authority;
|
||||||
|
7. prebuild or incrementally stream the operator product so connecting to the
|
||||||
|
vehicle never waits for a full 69 MB Rerun serialization.
|
||||||
|
|
||||||
|
E26.1 is accepted as a diagnostic evidence-fusion laboratory result. It is not
|
||||||
|
accepted for autonomous navigation, collision avoidance or safety decisions.
|
||||||
|
|
@ -0,0 +1,77 @@
|
||||||
|
{
|
||||||
|
"schema_version": "missioncore.e26-camera-ego-motion-profile/v1",
|
||||||
|
"profile_id": "lab-e26-kb4-multiview-static-hypothesis-v1",
|
||||||
|
"mode": "recorded-streaming-qualification",
|
||||||
|
"source": {
|
||||||
|
"source_id": "sensor.camera.right",
|
||||||
|
"coordinate_frame": "k1-map",
|
||||||
|
"calibration_slot": "camera_1",
|
||||||
|
"calibration_sha256": "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9"
|
||||||
|
},
|
||||||
|
"camera_evidence": {
|
||||||
|
"history_seconds": 2.0,
|
||||||
|
"maximum_observations_per_track": 32,
|
||||||
|
"minimum_observations": 5,
|
||||||
|
"minimum_span_seconds": 0.4,
|
||||||
|
"minimum_ego_baseline_m": 0.25,
|
||||||
|
"maximum_normal_matrix_condition": 1000000.0,
|
||||||
|
"footpoint_inset_fraction": 0.03,
|
||||||
|
"minimum_bbox_diagonal_px": 35.0,
|
||||||
|
"minimum_median_detector_score": 0.6,
|
||||||
|
"minimum_negative_depth_detector_score": 0.7,
|
||||||
|
"minimum_valid_positive_depth_fraction": 0.8,
|
||||||
|
"negative_depth_dynamic_fraction": 0.6,
|
||||||
|
"minimum_static_hypothesis_range_m": 0.8,
|
||||||
|
"maximum_static_hypothesis_range_m": 80.0,
|
||||||
|
"dynamic_median_angular_residual_degrees": {
|
||||||
|
"person": 1.8,
|
||||||
|
"bicycle": 1.8,
|
||||||
|
"motorcycle": 1.8,
|
||||||
|
"vehicle": 3.5,
|
||||||
|
"default": 3.5
|
||||||
|
},
|
||||||
|
"dynamic_p80_angular_residual_degrees": {
|
||||||
|
"person": 2.8,
|
||||||
|
"bicycle": 2.8,
|
||||||
|
"motorcycle": 2.8,
|
||||||
|
"vehicle": 5.0,
|
||||||
|
"default": 5.0
|
||||||
|
},
|
||||||
|
"static_median_angular_residual_degrees": {
|
||||||
|
"person": 1.0,
|
||||||
|
"bicycle": 1.0,
|
||||||
|
"motorcycle": 1.0,
|
||||||
|
"vehicle": 1.4,
|
||||||
|
"default": 1.4
|
||||||
|
},
|
||||||
|
"static_p80_angular_residual_degrees": {
|
||||||
|
"person": 2.0,
|
||||||
|
"bicycle": 2.0,
|
||||||
|
"motorcycle": 2.0,
|
||||||
|
"vehicle": 2.5,
|
||||||
|
"default": 2.5
|
||||||
|
},
|
||||||
|
"dynamic_confirmation_frames": 2,
|
||||||
|
"static_confirmation_frames": 4,
|
||||||
|
"evidence_decay_frames": 1
|
||||||
|
},
|
||||||
|
"fusion": {
|
||||||
|
"camera_only_confidence_scale": 0.65,
|
||||||
|
"conflict_state": "unknown",
|
||||||
|
"unknown_is_occupied": true,
|
||||||
|
"camera_only_metric_velocity_valid": false
|
||||||
|
},
|
||||||
|
"bounds": {
|
||||||
|
"maximum_tracks": 256,
|
||||||
|
"maximum_track_idle_seconds": 2.5
|
||||||
|
},
|
||||||
|
"acceptance": {
|
||||||
|
"maximum_processing_p95_ms": 12.0,
|
||||||
|
"maximum_tracks_observed": 256,
|
||||||
|
"maximum_static_control_false_dynamic_fraction": 0.1
|
||||||
|
},
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": false,
|
||||||
|
"navigation_or_safety_accepted": false
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,131 @@
|
||||||
|
{
|
||||||
|
"schema_version": "missioncore.e26-motion-benchmark/v1",
|
||||||
|
"benchmark_id": "ravnoves00-camera-ego-reviewed-anchors-v1",
|
||||||
|
"source_session_id": "20260720T065719Z_viewer_live",
|
||||||
|
"timeline": "session_seconds",
|
||||||
|
"annotation_status": "operator-approximate-with-reviewed-detector-track-binding",
|
||||||
|
"events": [
|
||||||
|
{
|
||||||
|
"id": "dynamic-person-woman-dog-left-58s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [54.0, 62.0],
|
||||||
|
"class_group": "person",
|
||||||
|
"target_source_track_ids": [83],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.2,
|
||||||
|
"minimum_coverage_fraction": 0.08
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-person-loading-car-right-65s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [62.0, 69.0],
|
||||||
|
"class_group": "person",
|
||||||
|
"target_source_track_ids": [112],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.2,
|
||||||
|
"minimum_coverage_fraction": 0.1
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "static-vehicles-before-moving-70-90s",
|
||||||
|
"kind": "static-control",
|
||||||
|
"window_seconds": [70.0, 90.0],
|
||||||
|
"class_group": "vehicle",
|
||||||
|
"expected_motion": "static",
|
||||||
|
"minimum_source_observations": 80,
|
||||||
|
"minimum_coverage_fraction": 0.2,
|
||||||
|
"minimum_classified_fraction": 0.2,
|
||||||
|
"maximum_false_dynamic_fraction": 0.1
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-person-adult-child-left-115s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [111.0, 120.0],
|
||||||
|
"class_group": "person",
|
||||||
|
"target_source_track_ids": [229],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.2,
|
||||||
|
"minimum_coverage_fraction": 0.08
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-person-road-right-127s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [124.0, 132.0],
|
||||||
|
"class_group": "person",
|
||||||
|
"target_source_track_ids": [253],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 2,
|
||||||
|
"minimum_span_seconds": 0.15,
|
||||||
|
"minimum_coverage_fraction": 0.08
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "static-vehicles-mid-run-132-148s",
|
||||||
|
"kind": "static-control",
|
||||||
|
"window_seconds": [132.0, 148.0],
|
||||||
|
"class_group": "vehicle",
|
||||||
|
"expected_motion": "static",
|
||||||
|
"minimum_source_observations": 80,
|
||||||
|
"minimum_coverage_fraction": 0.2,
|
||||||
|
"minimum_classified_fraction": 0.2,
|
||||||
|
"maximum_false_dynamic_fraction": 0.1
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-person-stroller-group-left-155s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [151.0, 161.0],
|
||||||
|
"class_group": "person",
|
||||||
|
"target_source_track_ids": [322, 328],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.2,
|
||||||
|
"minimum_coverage_fraction": 0.08
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "static-vehicles-before-oncoming-161-166s",
|
||||||
|
"kind": "static-control",
|
||||||
|
"window_seconds": [161.0, 166.0],
|
||||||
|
"class_group": "vehicle",
|
||||||
|
"expected_motion": "static",
|
||||||
|
"minimum_source_observations": 25,
|
||||||
|
"minimum_coverage_fraction": 0.15,
|
||||||
|
"minimum_classified_fraction": 0.15,
|
||||||
|
"maximum_false_dynamic_fraction": 0.1
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-vehicle-oncoming-right-170s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [167.0, 173.0],
|
||||||
|
"class_group": "vehicle",
|
||||||
|
"target_source_track_ids": [365],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.15,
|
||||||
|
"minimum_coverage_fraction": 0.1
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-person-bike-group-left-173s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [170.0, 179.0],
|
||||||
|
"class_group": "vulnerable_road_user",
|
||||||
|
"target_source_track_ids": [379, 386, 388],
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.2,
|
||||||
|
"minimum_coverage_fraction": 0.08
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "dynamic-vehicle-same-direction-right-183s",
|
||||||
|
"kind": "target-motion",
|
||||||
|
"window_seconds": [180.0, 188.0],
|
||||||
|
"class_group": "vehicle",
|
||||||
|
"target_source_track_ids": [373],
|
||||||
|
"binding_review": "frame-and-2d-track-reviewed; 391 is a parked vehicle",
|
||||||
|
"expected_motion": "dynamic",
|
||||||
|
"minimum_hits": 3,
|
||||||
|
"minimum_span_seconds": 0.2,
|
||||||
|
"minimum_coverage_fraction": 0.1
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
@ -35,6 +35,7 @@ from .jobs import (
|
||||||
validate_camera_compute_job,
|
validate_camera_compute_job,
|
||||||
)
|
)
|
||||||
from .lab_instances import (
|
from .lab_instances import (
|
||||||
|
PublishedCameraEgoMotionLabInstance,
|
||||||
PublishedIntegratedLabInstance,
|
PublishedIntegratedLabInstance,
|
||||||
PublishedPersistentSupportLabInstance,
|
PublishedPersistentSupportLabInstance,
|
||||||
PublishedTemporalLabInstance,
|
PublishedTemporalLabInstance,
|
||||||
|
|
@ -44,6 +45,7 @@ from .lab_instances import (
|
||||||
publish_e23_lab_instance,
|
publish_e23_lab_instance,
|
||||||
publish_e24_lab_instance,
|
publish_e24_lab_instance,
|
||||||
publish_e25_lab_instance,
|
publish_e25_lab_instance,
|
||||||
|
publish_e26_lab_instance,
|
||||||
publish_integrated_lab_instance,
|
publish_integrated_lab_instance,
|
||||||
)
|
)
|
||||||
from .live_perception import (
|
from .live_perception import (
|
||||||
|
|
@ -122,6 +124,7 @@ __all__ = [
|
||||||
"LivePerceptionIngress",
|
"LivePerceptionIngress",
|
||||||
"IntegratedPerceptionOverlayStore",
|
"IntegratedPerceptionOverlayStore",
|
||||||
"PublishedIntegratedLabInstance",
|
"PublishedIntegratedLabInstance",
|
||||||
|
"PublishedCameraEgoMotionLabInstance",
|
||||||
"PublishedPersistentSupportLabInstance",
|
"PublishedPersistentSupportLabInstance",
|
||||||
"PublishedTemporalLabInstance",
|
"PublishedTemporalLabInstance",
|
||||||
"PublishedWorldMotionLabInstance",
|
"PublishedWorldMotionLabInstance",
|
||||||
|
|
@ -158,6 +161,7 @@ __all__ = [
|
||||||
"publish_e23_lab_instance",
|
"publish_e23_lab_instance",
|
||||||
"publish_e24_lab_instance",
|
"publish_e24_lab_instance",
|
||||||
"publish_e25_lab_instance",
|
"publish_e25_lab_instance",
|
||||||
|
"publish_e26_lab_instance",
|
||||||
"publish_integrated_lab_instance",
|
"publish_integrated_lab_instance",
|
||||||
"validate_multirate_perception_qualification_result",
|
"validate_multirate_perception_qualification_result",
|
||||||
"prepare_recorded_qualification_slice",
|
"prepare_recorded_qualification_slice",
|
||||||
|
|
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -22,6 +22,10 @@ from k1link.sessions import (
|
||||||
publish_lab_replay_cache,
|
publish_lab_replay_cache,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
from .camera_ego_motion import (
|
||||||
|
CameraEgoMotionBuild,
|
||||||
|
build_camera_ego_motion_result,
|
||||||
|
)
|
||||||
from .inline_temporal import StreamingSemanticStabilizer, read_inline_profile
|
from .inline_temporal import StreamingSemanticStabilizer, read_inline_profile
|
||||||
from .integrated_perception import (
|
from .integrated_perception import (
|
||||||
IntegratedPerceptionResult,
|
IntegratedPerceptionResult,
|
||||||
|
|
@ -71,6 +75,14 @@ class PublishedPersistentSupportLabInstance:
|
||||||
build: PersistentSupportBuild
|
build: PersistentSupportBuild
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class PublishedCameraEgoMotionLabInstance:
|
||||||
|
binding: LabSessionBinding
|
||||||
|
job: CameraComputeJob
|
||||||
|
result: IntegratedPerceptionResult
|
||||||
|
build: CameraEgoMotionBuild
|
||||||
|
|
||||||
|
|
||||||
def publish_integrated_lab_instance(
|
def publish_integrated_lab_instance(
|
||||||
*,
|
*,
|
||||||
repository_root: Path,
|
repository_root: Path,
|
||||||
|
|
@ -709,6 +721,144 @@ def publish_e25_lab_instance(
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def publish_e26_lab_instance(
|
||||||
|
*,
|
||||||
|
repository_root: Path,
|
||||||
|
lidar_result_root: Path,
|
||||||
|
camera_result_root: Path,
|
||||||
|
profile_path: Path,
|
||||||
|
benchmark_path: Path,
|
||||||
|
lab_session_id: str,
|
||||||
|
lab_id: str,
|
||||||
|
display_name: str,
|
||||||
|
) -> PublishedCameraEgoMotionLabInstance:
|
||||||
|
"""Derive and publish one bounded camera/ego-motion evidence LAB run."""
|
||||||
|
|
||||||
|
root = repository_root.expanduser().resolve(strict=True)
|
||||||
|
jobs_root = root / ".runtime" / "compute-jobs"
|
||||||
|
results_root = root / ".runtime" / "compute-experiments" / "e10" / "worker-results"
|
||||||
|
packs_root = root / ".runtime" / "compute-experiments" / "e10" / "lidar-packs"
|
||||||
|
|
||||||
|
def source(path: Path, label: str) -> IntegratedPerceptionResult:
|
||||||
|
resolved = path.expanduser().resolve(strict=True)
|
||||||
|
document = _read_object(resolved / "result.json", resolved)
|
||||||
|
identity = document.get("identity")
|
||||||
|
if not isinstance(identity, dict) or not isinstance(identity.get("job_id"), str):
|
||||||
|
raise SessionIntegrityError(f"E26 {label} source has no job identity")
|
||||||
|
validated = validate_integrated_perception_result(
|
||||||
|
jobs_root / identity["job_id"],
|
||||||
|
resolved,
|
||||||
|
packs_root,
|
||||||
|
)
|
||||||
|
if not validated.accepted:
|
||||||
|
raise SessionIntegrityError(f"E26 {label} source result is not accepted")
|
||||||
|
return validated
|
||||||
|
|
||||||
|
lidar_source = source(lidar_result_root, "LiDAR")
|
||||||
|
camera_source = source(camera_result_root, "camera")
|
||||||
|
if lidar_source.job.source_id != camera_source.job.source_id:
|
||||||
|
raise SessionIntegrityError("E26 source camera identities differ")
|
||||||
|
|
||||||
|
lab_job = _publish_lab_job(lidar_source.job, jobs_root, lab_session_id)
|
||||||
|
lab_pack = _publish_lab_pack(
|
||||||
|
lidar_source,
|
||||||
|
lab_job,
|
||||||
|
packs_root,
|
||||||
|
lab_session_id,
|
||||||
|
)
|
||||||
|
build = build_camera_ego_motion_result(
|
||||||
|
lidar_source=lidar_source,
|
||||||
|
camera_source=camera_source,
|
||||||
|
lab_job=lab_job,
|
||||||
|
lab_pack=lab_pack,
|
||||||
|
results_root=results_root,
|
||||||
|
profile_path=profile_path,
|
||||||
|
benchmark_path=benchmark_path,
|
||||||
|
)
|
||||||
|
validated = validate_integrated_perception_result(
|
||||||
|
lab_job.job_root,
|
||||||
|
build.result_root,
|
||||||
|
packs_root,
|
||||||
|
)
|
||||||
|
if not validated.accepted:
|
||||||
|
failed = [
|
||||||
|
name
|
||||||
|
for name, accepted in build.report["acceptance"]["checks"].items()
|
||||||
|
if not accepted
|
||||||
|
]
|
||||||
|
raise SessionIntegrityError(
|
||||||
|
f"E26 camera/ego-motion artifact acceptance failed: {', '.join(failed)}"
|
||||||
|
)
|
||||||
|
|
||||||
|
store = SessionStore(root)
|
||||||
|
source_lab = store.get_lab_instance(lidar_source.job.session_id)
|
||||||
|
source_session_id = (
|
||||||
|
lidar_source.job.session_id
|
||||||
|
if source_lab is None
|
||||||
|
else source_lab.source_session_id
|
||||||
|
)
|
||||||
|
publish_lab_replay_cache(
|
||||||
|
store.data_dir,
|
||||||
|
source_session_id=source_session_id,
|
||||||
|
lab_session_id=lab_session_id,
|
||||||
|
timeline_start_ns=round(
|
||||||
|
validated.timeline_start_seconds * 1_000_000_000
|
||||||
|
),
|
||||||
|
timeline_end_ns=round(
|
||||||
|
validated.timeline_end_seconds * 1_000_000_000
|
||||||
|
),
|
||||||
|
)
|
||||||
|
metrics = build.report["metrics"]
|
||||||
|
binding = store.publish_lab_instance(
|
||||||
|
session_id=lab_session_id,
|
||||||
|
source_session_id=source_session_id,
|
||||||
|
display_name=display_name,
|
||||||
|
lab_id=lab_id,
|
||||||
|
result_kind="e26-camera-ego-motion-fusion",
|
||||||
|
result_id=validated.result_id,
|
||||||
|
source_result_id=lidar_source.result_id,
|
||||||
|
config_sha256=build.profile_sha256,
|
||||||
|
run_created_at_utc=validated.created_at_utc,
|
||||||
|
duration_seconds=(
|
||||||
|
validated.timeline_end_seconds
|
||||||
|
- validated.timeline_start_seconds
|
||||||
|
),
|
||||||
|
include_recorded_media=False,
|
||||||
|
provenance={
|
||||||
|
"schema_version": "missioncore.e26-lab-publication/v1",
|
||||||
|
"storage_mode": (
|
||||||
|
"bounded-camera-ego-motion-and-immutable-lidar-source-replay"
|
||||||
|
),
|
||||||
|
"source_result_id": lidar_source.result_id,
|
||||||
|
"camera_source_result_id": camera_source.result_id,
|
||||||
|
"source_lab_session_id": (
|
||||||
|
None if source_lab is None else source_lab.session_id
|
||||||
|
),
|
||||||
|
"source_payloads_mutated": False,
|
||||||
|
"coordinate_frame": "k1-map",
|
||||||
|
"camera_measurement": "kb4-multiview-static-world-hypothesis",
|
||||||
|
"metric_measurement": "e25-persistent-lidar-support",
|
||||||
|
"lookahead_frames": 0,
|
||||||
|
"benchmark_sha256": build.benchmark_sha256,
|
||||||
|
"benchmark_passed": metrics["benchmark"]["passed"],
|
||||||
|
"benchmark_passed_events": metrics["benchmark"]["passed_events"],
|
||||||
|
"benchmark_total_events": metrics["benchmark"]["total_events"],
|
||||||
|
"camera_ego_motion_processing_p95_ms": metrics["runtime"][
|
||||||
|
"camera_ego_motion_frame_processing_ms"
|
||||||
|
]["p95"],
|
||||||
|
"peak_tracks": metrics["runtime"]["peak_tracks"],
|
||||||
|
"camera_only_metric_velocity_valid": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
return PublishedCameraEgoMotionLabInstance(
|
||||||
|
binding=binding,
|
||||||
|
job=lab_job,
|
||||||
|
result=validated,
|
||||||
|
build=build,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
def _validate_e23_inputs(
|
def _validate_e23_inputs(
|
||||||
worker_root: Path,
|
worker_root: Path,
|
||||||
source_report_path: Path,
|
source_report_path: Path,
|
||||||
|
|
|
||||||
|
|
@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||||
map_points_to_lidar,
|
map_points_to_lidar,
|
||||||
project_map_points_kb4,
|
project_map_points_kb4,
|
||||||
quaternion_xyzw_to_rotation_matrix,
|
quaternion_xyzw_to_rotation_matrix,
|
||||||
|
unproject_pixels_kb4,
|
||||||
)
|
)
|
||||||
from k1link.device_plugins.xgrids_k1.analyze.stream_summary import (
|
from k1link.device_plugins.xgrids_k1.analyze.stream_summary import (
|
||||||
DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES,
|
DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES,
|
||||||
|
|
@ -47,5 +48,6 @@ __all__ = [
|
||||||
"quaternion_xyzw_to_rotation_matrix",
|
"quaternion_xyzw_to_rotation_matrix",
|
||||||
"run_calibrated_overlay_experiment",
|
"run_calibrated_overlay_experiment",
|
||||||
"summarize_mqtt_streams",
|
"summarize_mqtt_streams",
|
||||||
|
"unproject_pixels_kb4",
|
||||||
"validate_k1_valid_fov_mask",
|
"validate_k1_valid_fov_mask",
|
||||||
]
|
]
|
||||||
|
|
|
||||||
|
|
@ -205,6 +205,70 @@ def project_map_points_kb4(
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def unproject_pixels_kb4(
|
||||||
|
pixels_xy: npt.ArrayLike,
|
||||||
|
*,
|
||||||
|
profile: Kb4ProjectionProfile,
|
||||||
|
) -> FloatArray:
|
||||||
|
"""Return unit camera-frame rays for finite pixels under the KB4 model."""
|
||||||
|
|
||||||
|
pixels = np.asarray(pixels_xy, dtype=np.float64)
|
||||||
|
if (
|
||||||
|
pixels.ndim != 2
|
||||||
|
or pixels.shape[1:] != (2,)
|
||||||
|
or not np.isfinite(pixels).all()
|
||||||
|
):
|
||||||
|
raise CalibratedProjectionError("camera pixels must have shape (N, 2)")
|
||||||
|
if pixels.size == 0:
|
||||||
|
return np.empty((0, 3), dtype=np.float64)
|
||||||
|
fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
|
||||||
|
if not all(math.isfinite(value) and value > 0.0 for value in (fx, fy)):
|
||||||
|
raise CalibratedProjectionError("camera focal lengths must be positive")
|
||||||
|
distorted_x = (pixels[:, 0] - cx) / fx
|
||||||
|
distorted_y = (pixels[:, 1] - cy) / fy
|
||||||
|
theta_distorted = np.hypot(distorted_x, distorted_y)
|
||||||
|
theta = theta_distorted.copy()
|
||||||
|
k1, k2, k3, k4 = profile.distortion_kb4
|
||||||
|
for _ in range(12):
|
||||||
|
squared = theta * theta
|
||||||
|
polynomial = (
|
||||||
|
1.0
|
||||||
|
+ k1 * squared
|
||||||
|
+ k2 * squared**2
|
||||||
|
+ k3 * squared**3
|
||||||
|
+ k4 * squared**4
|
||||||
|
)
|
||||||
|
derivative = (
|
||||||
|
1.0
|
||||||
|
+ 3.0 * k1 * squared
|
||||||
|
+ 5.0 * k2 * squared**2
|
||||||
|
+ 7.0 * k3 * squared**3
|
||||||
|
+ 9.0 * k4 * squared**4
|
||||||
|
)
|
||||||
|
if np.any(np.abs(derivative) < 1e-9):
|
||||||
|
raise CalibratedProjectionError("KB4 inverse derivative became singular")
|
||||||
|
theta -= (theta * polynomial - theta_distorted) / derivative
|
||||||
|
if not np.isfinite(theta).all() or np.any(theta < 0.0) or np.any(theta >= math.pi):
|
||||||
|
raise CalibratedProjectionError("KB4 inverse produced invalid angles")
|
||||||
|
scale = np.divide(
|
||||||
|
np.sin(theta),
|
||||||
|
theta_distorted,
|
||||||
|
out=np.ones_like(theta),
|
||||||
|
where=theta_distorted > 1e-12,
|
||||||
|
)
|
||||||
|
directions = np.column_stack(
|
||||||
|
(
|
||||||
|
distorted_x * scale,
|
||||||
|
distorted_y * scale,
|
||||||
|
np.cos(theta),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
norms = np.linalg.norm(directions, axis=1)
|
||||||
|
if not np.isfinite(norms).all() or np.any(norms < 1e-9):
|
||||||
|
raise CalibratedProjectionError("KB4 inverse produced a zero camera ray")
|
||||||
|
return np.asarray(directions / norms[:, None], dtype=np.float64)
|
||||||
|
|
||||||
|
|
||||||
def depth_colors(depths_m: npt.ArrayLike) -> npt.NDArray[np.uint8]:
|
def depth_colors(depths_m: npt.ArrayLike) -> npt.NDArray[np.uint8]:
|
||||||
"""Return deterministic blue→cyan→green→yellow→red diagnostic colors."""
|
"""Return deterministic blue→cyan→green→yellow→red diagnostic colors."""
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -24,6 +24,7 @@ from k1link.compute import (
|
||||||
publish_e23_lab_instance,
|
publish_e23_lab_instance,
|
||||||
publish_e24_lab_instance,
|
publish_e24_lab_instance,
|
||||||
publish_e25_lab_instance,
|
publish_e25_lab_instance,
|
||||||
|
publish_e26_lab_instance,
|
||||||
publish_integrated_lab_instance,
|
publish_integrated_lab_instance,
|
||||||
)
|
)
|
||||||
from k1link.device_plugins.xgrids_k1.analyze import (
|
from k1link.device_plugins.xgrids_k1.analyze import (
|
||||||
|
|
@ -743,6 +744,93 @@ def publish_e25_lab(
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@lab_app.command("publish-e26")
|
||||||
|
def publish_e26_lab(
|
||||||
|
lidar_result: Annotated[
|
||||||
|
Path,
|
||||||
|
typer.Option(
|
||||||
|
"--lidar-result",
|
||||||
|
exists=True,
|
||||||
|
file_okay=False,
|
||||||
|
readable=True,
|
||||||
|
resolve_path=True,
|
||||||
|
help="Accepted E25 persistent-support result.",
|
||||||
|
),
|
||||||
|
],
|
||||||
|
camera_result: Annotated[
|
||||||
|
Path,
|
||||||
|
typer.Option(
|
||||||
|
"--camera-result",
|
||||||
|
exists=True,
|
||||||
|
file_okay=False,
|
||||||
|
readable=True,
|
||||||
|
resolve_path=True,
|
||||||
|
help="Accepted pre-world-motion result containing every 2D track.",
|
||||||
|
),
|
||||||
|
],
|
||||||
|
profile: Annotated[
|
||||||
|
Path,
|
||||||
|
typer.Option(
|
||||||
|
exists=True,
|
||||||
|
dir_okay=False,
|
||||||
|
readable=True,
|
||||||
|
resolve_path=True,
|
||||||
|
help="Bounded E26 camera/ego-motion profile.",
|
||||||
|
),
|
||||||
|
],
|
||||||
|
benchmark: Annotated[
|
||||||
|
Path,
|
||||||
|
typer.Option(
|
||||||
|
exists=True,
|
||||||
|
dir_okay=False,
|
||||||
|
readable=True,
|
||||||
|
resolve_path=True,
|
||||||
|
help="Spatially bound E26 motion benchmark.",
|
||||||
|
),
|
||||||
|
],
|
||||||
|
session_id: Annotated[
|
||||||
|
str,
|
||||||
|
typer.Option("--session-id", help="New immutable LAB session id."),
|
||||||
|
],
|
||||||
|
lab_id: Annotated[
|
||||||
|
str,
|
||||||
|
typer.Option("--lab-id", help="LAB marker, for example 'LAB E26.1'."),
|
||||||
|
],
|
||||||
|
display_name: Annotated[
|
||||||
|
str,
|
||||||
|
typer.Option("--display-name", help="Operator-facing saved-session title."),
|
||||||
|
],
|
||||||
|
) -> None:
|
||||||
|
"""Build and publish calibrated camera/ego-motion evidence."""
|
||||||
|
|
||||||
|
repository_root = Path(__file__).resolve().parents[4]
|
||||||
|
try:
|
||||||
|
published = publish_e26_lab_instance(
|
||||||
|
repository_root=repository_root,
|
||||||
|
lidar_result_root=lidar_result,
|
||||||
|
camera_result_root=camera_result,
|
||||||
|
profile_path=profile,
|
||||||
|
benchmark_path=benchmark,
|
||||||
|
lab_session_id=session_id,
|
||||||
|
lab_id=lab_id,
|
||||||
|
display_name=display_name,
|
||||||
|
)
|
||||||
|
except (OSError, SessionIntegrityError, RuntimeError, ValueError) as exc:
|
||||||
|
console.print(f"[red]E26 LAB publication failed:[/red] {exc}")
|
||||||
|
raise typer.Exit(code=2) from exc
|
||||||
|
metrics = published.build.report["metrics"]
|
||||||
|
console.print(
|
||||||
|
"[green]E26 LAB instance published.[/green] "
|
||||||
|
f"session={published.binding.session_id}; "
|
||||||
|
f"source={published.binding.source_session_id}; "
|
||||||
|
f"result={published.binding.result_id}; "
|
||||||
|
f"benchmark={metrics['benchmark']['passed_events']}/"
|
||||||
|
f"{metrics['benchmark']['total_events']}; "
|
||||||
|
f"p95={metrics['runtime']['camera_ego_motion_frame_processing_ms']['p95']:.3f}ms; "
|
||||||
|
"camera_metric_velocity=false; source_payloads_mutated=false"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
@app.command("serve")
|
@app.command("serve")
|
||||||
def serve_console(
|
def serve_console(
|
||||||
port: Annotated[
|
port: Annotated[
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||||
map_points_to_lidar,
|
map_points_to_lidar,
|
||||||
project_map_points_kb4,
|
project_map_points_kb4,
|
||||||
quaternion_xyzw_to_rotation_matrix,
|
quaternion_xyzw_to_rotation_matrix,
|
||||||
|
unproject_pixels_kb4,
|
||||||
)
|
)
|
||||||
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
||||||
parse_k1_factory_calibration,
|
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]
|
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:
|
def test_factory_profile_binds_right_main_camera_and_scales_intrinsics() -> None:
|
||||||
calibration = parse_k1_factory_calibration(
|
calibration = parse_k1_factory_calibration(
|
||||||
(FIXTURES / "camera.yaml").read_bytes(),
|
(FIXTURES / "camera.yaml").read_bytes(),
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue