1079 lines
45 KiB
Python
1079 lines
45 KiB
Python
"""Standalone bounded LiDAR fusion primitives for LAB E10.
|
|
|
|
The worker deliberately receives a content-addressed replay pack instead of a
|
|
K1 session or device authority. This module contains only geometry and world
|
|
state projection; it cannot discover, start, stop, or configure a scanner.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import math
|
|
import statistics
|
|
from collections import defaultdict, deque
|
|
from collections.abc import Mapping, Sequence
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
import numpy.typing as npt
|
|
|
|
PACK_SCHEMA = "missioncore.e10-lidar-replay-pack/v1"
|
|
WORLD_STATE_SCHEMA = "missioncore.live-perception-world-state/v1"
|
|
IMAGE_WIDTH = 800
|
|
IMAGE_HEIGHT = 600
|
|
|
|
FloatArray = npt.NDArray[np.float64]
|
|
|
|
|
|
def canonical_json(value: object) -> bytes:
|
|
return json.dumps(
|
|
value,
|
|
ensure_ascii=False,
|
|
sort_keys=True,
|
|
separators=(",", ":"),
|
|
allow_nan=False,
|
|
).encode()
|
|
|
|
|
|
def sha256(path: Path) -> str:
|
|
digest = hashlib.sha256()
|
|
with path.open("rb") as stream:
|
|
while chunk := stream.read(1024 * 1024):
|
|
digest.update(chunk)
|
|
return digest.hexdigest()
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class ProjectionProfile:
|
|
width: int
|
|
height: int
|
|
intrinsic_fx_fy_cx_cy: tuple[float, float, float, float]
|
|
distortion_kb4: tuple[float, float, float, float]
|
|
t_camera_from_lidar: FloatArray
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class Cuboid:
|
|
center_map: tuple[float, float, float]
|
|
half_size: tuple[float, float, float]
|
|
quaternion_xyzw: tuple[float, float, float, float]
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class CompletedCuboid:
|
|
cuboid: Cuboid
|
|
completion_fraction: float
|
|
ground_z_map: float
|
|
orientation_source: str
|
|
temporal_status: str
|
|
support_coverage_fraction: float
|
|
|
|
|
|
@dataclass(slots=True)
|
|
class _CuboidTrackState:
|
|
center: FloatArray
|
|
size: FloatArray
|
|
yaw: float
|
|
last_seconds: float
|
|
hits: int
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class TrackFusion:
|
|
track_id: int
|
|
label: str
|
|
association_group: str
|
|
score: float
|
|
bbox_xyxy: tuple[float, float, float, float]
|
|
candidate_points: int
|
|
semantic_points: int
|
|
clustered_points: int
|
|
distance_p10_m: float | None
|
|
distance_median_m: float | None
|
|
distance_smoothed_m: float | None
|
|
status: str
|
|
cuboid: Cuboid | None
|
|
source_indices: npt.NDArray[np.int64]
|
|
geometry: str = "none"
|
|
observed_cuboid: Cuboid | None = None
|
|
completion_fraction: float | None = None
|
|
ground_z_map: float | None = None
|
|
orientation_source: str | None = None
|
|
temporal_status: str | None = None
|
|
support_coverage_fraction: float | None = None
|
|
|
|
|
|
class LidarReplayPack:
|
|
"""Validated immutable LiDAR/pose replay input for one camera selection."""
|
|
|
|
def __init__(self, root: Path, *, expected_job_id: str) -> None:
|
|
self.root = root.resolve(strict=True)
|
|
manifest_path = self.root / "manifest.json"
|
|
arrays_path = self.root / "lidar-pack.npz"
|
|
manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
|
|
identity = manifest.get("identity") if isinstance(manifest, dict) else None
|
|
identity_sha256 = manifest.get("identity_sha256") if isinstance(manifest, dict) else None
|
|
if (
|
|
not isinstance(manifest, dict)
|
|
or manifest.get("schema_version") != PACK_SCHEMA
|
|
or not isinstance(identity, dict)
|
|
or identity.get("schema_version") != PACK_SCHEMA
|
|
or identity.get("job_id") != expected_job_id
|
|
or not isinstance(identity_sha256, str)
|
|
or hashlib.sha256(canonical_json(identity)).hexdigest() != identity_sha256
|
|
or self.root.name != f"e10-lidar-pack-{identity_sha256}"
|
|
or manifest.get("pack_id") != self.root.name
|
|
or manifest.get("artifact", {}).get("path") != arrays_path.name
|
|
or manifest.get("artifact", {}).get("sha256") != sha256(arrays_path)
|
|
or manifest.get("artifact", {}).get("byte_length") != arrays_path.stat().st_size
|
|
):
|
|
raise RuntimeError("LAB E10 LiDAR replay pack identity is invalid")
|
|
self.manifest = manifest
|
|
self.identity = identity
|
|
self.pack_id = self.root.name
|
|
self.arrays_path = arrays_path
|
|
self.arrays = np.load(arrays_path, allow_pickle=False)
|
|
required = {
|
|
"frame_indices",
|
|
"source_frame_indices",
|
|
"session_seconds",
|
|
"sample_available",
|
|
"cloud_offsets",
|
|
"cloud_points_map",
|
|
"pose_positions_map",
|
|
"pose_quaternions_map_from_lidar",
|
|
"lidar_camera_delta_ms",
|
|
"pose_point_delta_ms",
|
|
"intrinsic_fx_fy_cx_cy",
|
|
"distortion_kb4",
|
|
"t_camera_from_lidar",
|
|
}
|
|
if set(self.arrays.files) != required:
|
|
raise RuntimeError("LAB E10 LiDAR replay pack array set changed")
|
|
self.frame_indices = self.arrays["frame_indices"]
|
|
self.source_frame_indices = self.arrays["source_frame_indices"]
|
|
self.session_seconds = self.arrays["session_seconds"]
|
|
self.sample_available = self.arrays["sample_available"]
|
|
self.cloud_offsets = self.arrays["cloud_offsets"]
|
|
self.cloud_points = self.arrays["cloud_points_map"]
|
|
self.positions = self.arrays["pose_positions_map"]
|
|
self.quaternions = self.arrays["pose_quaternions_map_from_lidar"]
|
|
self.lidar_camera_delta_ms = self.arrays["lidar_camera_delta_ms"]
|
|
self.pose_point_delta_ms = self.arrays["pose_point_delta_ms"]
|
|
count = int(identity.get("frame_count", -1))
|
|
if (
|
|
count < 2
|
|
or self.frame_indices.dtype != np.int64
|
|
or not np.array_equal(self.frame_indices, np.arange(count, dtype=np.int64))
|
|
or self.source_frame_indices.shape != (count,)
|
|
or self.session_seconds.shape != (count,)
|
|
or self.sample_available.dtype != np.bool_
|
|
or self.sample_available.shape != (count,)
|
|
or self.cloud_offsets.shape != (count + 1,)
|
|
or self.cloud_points.ndim != 2
|
|
or self.cloud_points.shape[1:] != (3,)
|
|
or self.positions.shape != (count, 3)
|
|
or self.quaternions.shape != (count, 4)
|
|
or self.lidar_camera_delta_ms.shape != (count,)
|
|
or self.pose_point_delta_ms.shape != (count,)
|
|
or int(self.cloud_offsets[0]) != 0
|
|
or int(self.cloud_offsets[-1]) != int(self.cloud_points.shape[0])
|
|
or np.any(np.diff(self.cloud_offsets) < 0)
|
|
or not np.all(np.diff(self.session_seconds) > 0)
|
|
):
|
|
raise RuntimeError("LAB E10 LiDAR replay pack shapes are invalid")
|
|
self.profile = ProjectionProfile(
|
|
width=int(identity["projection"]["width"]),
|
|
height=int(identity["projection"]["height"]),
|
|
intrinsic_fx_fy_cx_cy=tuple(
|
|
float(value) for value in self.arrays["intrinsic_fx_fy_cx_cy"]
|
|
),
|
|
distortion_kb4=tuple(float(value) for value in self.arrays["distortion_kb4"]),
|
|
t_camera_from_lidar=np.asarray(self.arrays["t_camera_from_lidar"], dtype=np.float64),
|
|
)
|
|
if (
|
|
self.profile.width != IMAGE_WIDTH
|
|
or self.profile.height != IMAGE_HEIGHT
|
|
or self.profile.t_camera_from_lidar.shape != (4, 4)
|
|
):
|
|
raise RuntimeError("LAB E10 projection profile is incompatible")
|
|
|
|
@property
|
|
def frame_count(self) -> int:
|
|
return int(self.frame_indices.shape[0])
|
|
|
|
def close(self) -> None:
|
|
self.arrays.close()
|
|
|
|
def frame(self, index: int) -> tuple[FloatArray, tuple[float, ...], tuple[float, ...]] | None:
|
|
if not 0 <= index < self.frame_count:
|
|
raise IndexError(index)
|
|
if not bool(self.sample_available[index]):
|
|
return None
|
|
start = int(self.cloud_offsets[index])
|
|
end = int(self.cloud_offsets[index + 1])
|
|
position = tuple(float(value) for value in self.positions[index])
|
|
quaternion = tuple(float(value) for value in self.quaternions[index])
|
|
points = np.asarray(self.cloud_points[start:end], dtype=np.float64)
|
|
if not np.isfinite(points).all() or not all(
|
|
math.isfinite(value) for value in position + quaternion
|
|
):
|
|
raise RuntimeError("LAB E10 LiDAR replay frame contains non-finite geometry")
|
|
return points, position, quaternion
|
|
|
|
|
|
def quaternion_to_rotation(orientation_xyzw: Sequence[float]) -> FloatArray:
|
|
quaternion = np.asarray(orientation_xyzw, dtype=np.float64)
|
|
norm = float(np.linalg.norm(quaternion))
|
|
if quaternion.shape != (4,) or not math.isfinite(norm) or norm < 1e-9:
|
|
raise RuntimeError("LAB E10 pose quaternion is invalid")
|
|
x, y, z, w = quaternion / norm
|
|
return np.asarray(
|
|
[
|
|
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
|
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
|
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
|
|
|
|
def map_points_to_lidar(
|
|
points_map: FloatArray,
|
|
position_map: Sequence[float],
|
|
orientation_map_from_lidar: Sequence[float],
|
|
) -> FloatArray:
|
|
return (points_map - np.asarray(position_map, dtype=np.float64)) @ quaternion_to_rotation(
|
|
orientation_map_from_lidar
|
|
)
|
|
|
|
|
|
def project_points(
|
|
points_map: FloatArray,
|
|
position_map: Sequence[float],
|
|
orientation_map_from_lidar: Sequence[float],
|
|
profile: ProjectionProfile,
|
|
) -> tuple[FloatArray, FloatArray, npt.NDArray[np.int64], FloatArray]:
|
|
points_lidar = map_points_to_lidar(points_map, position_map, orientation_map_from_lidar)
|
|
transform = profile.t_camera_from_lidar
|
|
points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3]
|
|
front = points_camera[:, 2] > 1e-6
|
|
source = np.flatnonzero(front)
|
|
camera = points_camera[front]
|
|
if camera.size == 0:
|
|
return (
|
|
np.empty((0, 2), dtype=np.float64),
|
|
np.empty((0,), dtype=np.float64),
|
|
np.empty((0,), dtype=np.int64),
|
|
points_lidar,
|
|
)
|
|
x, y, z = camera.T
|
|
radial = np.hypot(x, y)
|
|
theta = np.arctan2(radial, z)
|
|
theta2 = theta * theta
|
|
k1, k2, k3, k4 = profile.distortion_kb4
|
|
distorted = theta * (1 + k1 * theta2 + k2 * theta2**2 + k3 * theta2**3 + k4 * theta2**4)
|
|
scale = np.divide(distorted, radial, out=np.zeros_like(distorted), where=radial > 1e-12)
|
|
fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
|
|
u = fx * x * scale + cx
|
|
v = fy * y * scale + cy
|
|
keep = (
|
|
np.isfinite(u)
|
|
& np.isfinite(v)
|
|
& (u >= 0)
|
|
& (u < profile.width)
|
|
& (v >= 0)
|
|
& (v < profile.height)
|
|
)
|
|
pixels = np.column_stack((u[keep], v[keep]))
|
|
depths = z[keep]
|
|
indices = source[keep].astype(np.int64, copy=False)
|
|
if depths.size:
|
|
px = np.rint(pixels[:, 0]).astype(np.int64)
|
|
py = np.rint(pixels[:, 1]).astype(np.int64)
|
|
keys = py * profile.width + px
|
|
order = np.lexsort((depths, keys))
|
|
ordered_keys = keys[order]
|
|
first = np.concatenate(([True], ordered_keys[1:] != ordered_keys[:-1]))
|
|
visible = np.sort(order[first])
|
|
pixels, depths, indices = pixels[visible], depths[visible], indices[visible]
|
|
return pixels, depths, indices, points_lidar
|
|
|
|
|
|
def _box_iou(one: Sequence[float], two: Sequence[float]) -> float:
|
|
x1, y1 = max(one[0], two[0]), max(one[1], two[1])
|
|
x2, y2 = min(one[2], two[2]), min(one[3], two[3])
|
|
intersection = max(0.0, x2 - x1) * max(0.0, y2 - y1)
|
|
union = max(0.0, one[2] - one[0]) * max(0.0, one[3] - one[1])
|
|
union += max(0.0, two[2] - two[0]) * max(0.0, two[3] - two[1]) - intersection
|
|
return 0.0 if union <= 0 else intersection / union
|
|
|
|
|
|
def _depth_cluster(
|
|
indices: npt.NDArray[np.int64], depths: FloatArray, profile: Mapping[str, Any]
|
|
) -> npt.NDArray[np.int64]:
|
|
if indices.size < 2:
|
|
return indices
|
|
ordered = indices[np.argsort(depths[indices])]
|
|
groups: list[npt.NDArray[np.int64]] = []
|
|
start = 0
|
|
for offset, gap in enumerate(np.diff(depths[ordered]), start=1):
|
|
threshold = max(
|
|
float(profile["depth_cluster_minimum_gap_m"]),
|
|
float(profile["depth_cluster_gap_fraction"]) * float(depths[ordered[offset - 1]]),
|
|
)
|
|
if float(gap) > threshold:
|
|
groups.append(ordered[start:offset])
|
|
start = offset
|
|
groups.append(ordered[start:])
|
|
return min(groups, key=lambda group: (-int(group.size), float(np.median(depths[group]))))
|
|
|
|
|
|
def _spatial_cluster(
|
|
indices: npt.NDArray[np.int64], points_lidar: FloatArray, radius: float
|
|
) -> npt.NDArray[np.int64]:
|
|
if indices.size < 2:
|
|
return indices
|
|
points = points_lidar[indices]
|
|
adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius * radius
|
|
unseen = set(range(indices.size))
|
|
groups: list[list[int]] = []
|
|
while unseen:
|
|
seed = unseen.pop()
|
|
group = [seed]
|
|
pending = [seed]
|
|
while pending:
|
|
current = pending.pop()
|
|
for neighbor in [value for value in tuple(unseen) if adjacent[current, value]]:
|
|
unseen.remove(neighbor)
|
|
pending.append(neighbor)
|
|
group.append(neighbor)
|
|
groups.append(group)
|
|
selected = min(
|
|
groups,
|
|
key=lambda group: (-len(group), float(np.median(np.linalg.norm(points[group], axis=1)))),
|
|
)
|
|
return indices[np.asarray(selected, dtype=np.int64)]
|
|
|
|
|
|
def _cuboid(points_map: FloatArray, profile: Mapping[str, Any], group: str) -> Cuboid | None:
|
|
if points_map.shape[0] < 2:
|
|
return None
|
|
xy = points_map[:, :2]
|
|
centered = xy - np.median(xy, axis=0)
|
|
covariance = centered.T @ centered / max(1, centered.shape[0] - 1)
|
|
values, vectors = np.linalg.eigh(covariance)
|
|
axis = vectors[:, int(np.argmax(values))]
|
|
yaw = math.atan2(float(axis[1]), float(axis[0]))
|
|
forward = np.asarray([math.cos(yaw), math.sin(yaw)])
|
|
side = np.asarray([-math.sin(yaw), math.cos(yaw)])
|
|
local = np.column_stack((xy @ forward, xy @ side))
|
|
lower = np.percentile(local, 5.0, axis=0)
|
|
upper = np.percentile(local, 95.0, axis=0)
|
|
lower_z, upper_z = np.percentile(points_map[:, 2], [5.0, 95.0])
|
|
spans = np.asarray([upper[0] - lower[0], upper[1] - lower[1], upper_z - lower_z])
|
|
maximum = np.asarray(profile["maximum_oriented_extent_m"][group], dtype=np.float64)
|
|
if (
|
|
not np.isfinite(spans).all()
|
|
or np.any(spans > float(profile["maximum_cuboid_span_m"]))
|
|
or np.any(spans > maximum)
|
|
):
|
|
return None
|
|
sizes = np.maximum(spans, float(profile["minimum_cuboid_extent_m"]))
|
|
local_center = (lower + upper) * 0.5
|
|
center_xy = local_center[0] * forward + local_center[1] * side
|
|
return Cuboid(
|
|
center_map=(
|
|
float(center_xy[0]),
|
|
float(center_xy[1]),
|
|
(float(lower_z) + float(upper_z)) * 0.5,
|
|
),
|
|
half_size=tuple(float(value) for value in sizes * 0.5),
|
|
quaternion_xyzw=(0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)),
|
|
)
|
|
|
|
|
|
def _box_yaw(cuboid: Cuboid) -> float:
|
|
z, w = cuboid.quaternion_xyzw[2:]
|
|
return 2.0 * math.atan2(z, w)
|
|
|
|
|
|
def _align_box_yaw(yaw: float, reference: float) -> float:
|
|
"""Align a cuboid yaw to the closest equivalent pi-periodic heading."""
|
|
|
|
return reference + ((yaw - reference + math.pi * 0.5) % math.pi) - math.pi * 0.5
|
|
|
|
|
|
def _completion_profile(profile: Mapping[str, Any]) -> dict[str, Any]:
|
|
required_labels = {"person", "bicycle", "motorcycle", "car", "truck", "bus"}
|
|
classes = profile.get("classes")
|
|
ground = profile.get("ground")
|
|
temporal = profile.get("temporal")
|
|
orientation = profile.get("orientation")
|
|
if (
|
|
profile.get("mode") != "class-prior-amodal-v1"
|
|
or profile.get("failure_policy") not in {"reject", "visible-envelope"}
|
|
or not isinstance(classes, Mapping)
|
|
or set(classes) != required_labels
|
|
or not all(isinstance(value, Mapping) for value in (ground, temporal, orientation))
|
|
):
|
|
raise RuntimeError("LAB E13 cuboid completion profile is invalid")
|
|
for label, value in classes.items():
|
|
nominal = np.asarray(value.get("nominal_size_m"), dtype=np.float64)
|
|
minimum = np.asarray(value.get("minimum_size_m"), dtype=np.float64)
|
|
maximum = np.asarray(value.get("maximum_size_m"), dtype=np.float64)
|
|
padding = np.asarray(value.get("support_padding_m"), dtype=np.float64)
|
|
if (
|
|
nominal.shape != (3,)
|
|
or minimum.shape != (3,)
|
|
or maximum.shape != (3,)
|
|
or padding.shape != (3,)
|
|
or not np.isfinite(np.concatenate((nominal, minimum, maximum, padding))).all()
|
|
or np.any(minimum <= 0)
|
|
or np.any(padding < 0)
|
|
or np.any(nominal < minimum)
|
|
or np.any(nominal > maximum)
|
|
):
|
|
raise RuntimeError(f"LAB E13 {label} size prior is invalid")
|
|
scalar_checks = (
|
|
(ground, "local_radius_m", 0.5, 10.0),
|
|
(ground, "lower_percentile", 0.0, 30.0),
|
|
(ground, "maximum_below_support_m", 0.1, 3.0),
|
|
(ground, "maximum_above_support_m", 0.0, 0.5),
|
|
(ground, "fallback_below_support_m", 0.0, 1.0),
|
|
(orientation, "minimum_anisotropy_ratio", 1.0, 20.0),
|
|
(orientation, "face_width_switch_fraction", 0.5, 2.0),
|
|
(temporal, "center_alpha", 0.01, 1.0),
|
|
(temporal, "size_alpha", 0.01, 1.0),
|
|
(temporal, "yaw_alpha", 0.01, 1.0),
|
|
(temporal, "maximum_center_innovation_m", 0.1, 20.0),
|
|
(temporal, "maximum_yaw_innovation_degrees", 1.0, 90.0),
|
|
(temporal, "maximum_idle_s", 0.05, 10.0),
|
|
(profile, "minimum_support_coverage_fraction", 0.5, 1.0),
|
|
)
|
|
for owner, name, lower, upper in scalar_checks:
|
|
try:
|
|
value = float(owner[name])
|
|
except (KeyError, TypeError, ValueError) as exc:
|
|
raise RuntimeError(f"LAB E13 cuboid completion field {name} is invalid") from exc
|
|
if not lower <= value <= upper:
|
|
raise RuntimeError(f"LAB E13 cuboid completion field {name} is invalid")
|
|
if not 1 <= int(temporal.get("confirmation_hits", 0)) <= 20:
|
|
raise RuntimeError("LAB E13 cuboid confirmation hit count is invalid")
|
|
return dict(profile)
|
|
|
|
|
|
class CuboidCompletionTracker:
|
|
"""Complete visible LiDAR surfaces into provenance-marked, smoothed cuboids.
|
|
|
|
Completion is deliberately class-prior based. The returned box is useful for
|
|
perception visualization and planning experiments, but it is never represented
|
|
as fully measured geometry or ground truth.
|
|
"""
|
|
|
|
def __init__(self, profile: Mapping[str, Any]) -> None:
|
|
self.profile = _completion_profile(profile)
|
|
self.states: dict[tuple[int, str], _CuboidTrackState] = {}
|
|
self.last_failure_reason: str | None = None
|
|
|
|
def complete(
|
|
self,
|
|
*,
|
|
track_id: int,
|
|
label: str,
|
|
support_points_map: FloatArray,
|
|
all_points_map: FloatArray,
|
|
sensor_position_map: Sequence[float],
|
|
session_seconds: float,
|
|
observed_cuboid: Cuboid,
|
|
) -> CompletedCuboid | None:
|
|
self.last_failure_reason = None
|
|
if support_points_map.shape[0] < 2 or support_points_map.shape[1:] != (3,):
|
|
self.last_failure_reason = "insufficient-support"
|
|
return None
|
|
prior = self.profile["classes"].get(label)
|
|
if not isinstance(prior, Mapping):
|
|
self.last_failure_reason = "missing-class-prior"
|
|
return None
|
|
now = float(session_seconds)
|
|
sensor = np.asarray(sensor_position_map, dtype=np.float64)
|
|
if sensor.shape != (3,) or not np.isfinite(sensor).all() or not math.isfinite(now):
|
|
self.last_failure_reason = "invalid-pose-or-time"
|
|
return None
|
|
self._prune(now)
|
|
key = (int(track_id), label)
|
|
previous = self.states.get(key)
|
|
nominal = np.asarray(prior["nominal_size_m"], dtype=np.float64)
|
|
observed_yaw = _box_yaw(observed_cuboid)
|
|
orientation_source = "support-pca"
|
|
centered = support_points_map[:, :2] - np.median(support_points_map[:, :2], axis=0)
|
|
covariance = centered.T @ centered / max(1, centered.shape[0] - 1)
|
|
values = np.linalg.eigvalsh(covariance)
|
|
anisotropy = float(values[-1] / max(values[0], 1e-9))
|
|
anisotropy_minimum = float(self.profile["orientation"]["minimum_anisotropy_ratio"])
|
|
if anisotropy >= anisotropy_minimum and nominal[0] >= nominal[1] * 1.4:
|
|
principal = np.asarray([math.cos(observed_yaw), math.sin(observed_yaw)])
|
|
projected = support_points_map[:, :2] @ principal
|
|
principal_span = float(np.percentile(projected, 95.0) - np.percentile(projected, 5.0))
|
|
if principal_span <= nominal[1] * float(
|
|
self.profile["orientation"]["face_width_switch_fraction"]
|
|
):
|
|
observed_yaw += math.pi * 0.5
|
|
orientation_source = "support-face-normal"
|
|
if previous is not None:
|
|
observed_yaw = _align_box_yaw(observed_yaw, previous.yaw)
|
|
if anisotropy < anisotropy_minimum:
|
|
observed_yaw = previous.yaw
|
|
orientation_source = "track-history"
|
|
else:
|
|
alternative = _align_box_yaw(observed_yaw + math.pi * 0.5, previous.yaw)
|
|
if abs(alternative - previous.yaw) < abs(observed_yaw - previous.yaw):
|
|
observed_yaw = alternative
|
|
orientation_source = "track-history-axis-disambiguation"
|
|
elif anisotropy < anisotropy_minimum:
|
|
bearing = math.atan2(
|
|
float(np.median(support_points_map[:, 1]) - sensor[1]),
|
|
float(np.median(support_points_map[:, 0]) - sensor[0]),
|
|
)
|
|
observed_yaw = bearing
|
|
orientation_source = "sensor-bearing-fallback"
|
|
|
|
forward = np.asarray([math.cos(observed_yaw), math.sin(observed_yaw)])
|
|
side = np.asarray([-math.sin(observed_yaw), math.cos(observed_yaw)])
|
|
local = np.column_stack(
|
|
(support_points_map[:, :2] @ forward, support_points_map[:, :2] @ side)
|
|
)
|
|
lower_xy = np.percentile(local, 5.0, axis=0)
|
|
upper_xy = np.percentile(local, 95.0, axis=0)
|
|
lower_z, upper_z = np.percentile(support_points_map[:, 2], [5.0, 95.0])
|
|
ground_z = self._ground_z(
|
|
all_points_map=all_points_map,
|
|
center_xy=np.median(support_points_map[:, :2], axis=0),
|
|
support_lower_z=float(lower_z),
|
|
)
|
|
|
|
minimum = np.asarray(prior["minimum_size_m"], dtype=np.float64)
|
|
maximum = np.asarray(prior["maximum_size_m"], dtype=np.float64)
|
|
padding = np.asarray(prior["support_padding_m"], dtype=np.float64)
|
|
observed_size = np.asarray(
|
|
[upper_xy[0] - lower_xy[0], upper_xy[1] - lower_xy[1], upper_z - lower_z],
|
|
dtype=np.float64,
|
|
)
|
|
required_size = np.asarray(
|
|
[
|
|
observed_size[0] + 2.0 * padding[0],
|
|
observed_size[1] + 2.0 * padding[1],
|
|
max(0.0, float(upper_z) - ground_z) + padding[2],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
size = np.clip(np.maximum.reduce((nominal, minimum, required_size)), minimum, maximum)
|
|
if np.any(required_size > maximum + 1e-9) or not np.isfinite(size).all():
|
|
self.last_failure_reason = "required-size-exceeds-class-bound"
|
|
return None
|
|
|
|
sensor_local = np.asarray([sensor[:2] @ forward, sensor[:2] @ side], dtype=np.float64)
|
|
center_local = np.asarray(
|
|
[
|
|
self._amodal_axis_center(
|
|
float(lower_xy[axis]),
|
|
float(upper_xy[axis]),
|
|
float(size[axis]),
|
|
float(sensor_local[axis]),
|
|
)
|
|
for axis in range(2)
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
center_xy = center_local[0] * forward + center_local[1] * side
|
|
center = np.asarray([center_xy[0], center_xy[1], ground_z + size[2] * 0.5])
|
|
yaw = observed_yaw
|
|
candidate_center = center.copy()
|
|
candidate_size = size.copy()
|
|
candidate_yaw = yaw
|
|
temporal_status = "tentative"
|
|
if previous is not None:
|
|
temporal = self.profile["temporal"]
|
|
center_innovation = float(np.linalg.norm(center - previous.center))
|
|
yaw_delta = _align_box_yaw(yaw, previous.yaw) - previous.yaw
|
|
maximum_yaw = math.radians(float(temporal["maximum_yaw_innovation_degrees"]))
|
|
if (
|
|
center_innovation > float(temporal["maximum_center_innovation_m"])
|
|
or abs(yaw_delta) > maximum_yaw
|
|
):
|
|
temporal_status = "reset-innovation"
|
|
hits = 1
|
|
else:
|
|
center = previous.center + float(temporal["center_alpha"]) * (
|
|
center - previous.center
|
|
)
|
|
size = previous.size + float(temporal["size_alpha"]) * (size - previous.size)
|
|
yaw = previous.yaw + float(temporal["yaw_alpha"]) * yaw_delta
|
|
hits = previous.hits + 1
|
|
temporal_status = (
|
|
"confirmed" if hits >= int(temporal["confirmation_hits"]) else "tentative"
|
|
)
|
|
else:
|
|
hits = 1
|
|
|
|
coverage = self._coverage(support_points_map, center, size, yaw)
|
|
minimum_coverage = float(self.profile["minimum_support_coverage_fraction"])
|
|
if coverage < minimum_coverage and previous is not None:
|
|
# A smoothed box must never drift away from its current measured support.
|
|
center = candidate_center
|
|
size = candidate_size
|
|
yaw = candidate_yaw
|
|
coverage = self._coverage(support_points_map, center, size, yaw)
|
|
temporal_status = "reset-support-coverage"
|
|
hits = 1
|
|
if coverage < minimum_coverage:
|
|
self.last_failure_reason = "support-coverage-below-threshold"
|
|
return None
|
|
self.states[key] = _CuboidTrackState(
|
|
center=center.copy(),
|
|
size=size.copy(),
|
|
yaw=float(yaw),
|
|
last_seconds=now,
|
|
hits=hits,
|
|
)
|
|
observed_volume = float(np.prod(np.maximum(observed_size, 0.0)))
|
|
completed_volume = float(np.prod(size))
|
|
completion_fraction = 1.0 - min(1.0, observed_volume / completed_volume)
|
|
return CompletedCuboid(
|
|
cuboid=Cuboid(
|
|
center_map=tuple(float(value) for value in center),
|
|
half_size=tuple(float(value) for value in size * 0.5),
|
|
quaternion_xyzw=(
|
|
0.0,
|
|
0.0,
|
|
math.sin(yaw * 0.5),
|
|
math.cos(yaw * 0.5),
|
|
),
|
|
),
|
|
completion_fraction=completion_fraction,
|
|
ground_z_map=ground_z,
|
|
orientation_source=orientation_source,
|
|
temporal_status=temporal_status,
|
|
support_coverage_fraction=coverage,
|
|
)
|
|
|
|
def _ground_z(
|
|
self,
|
|
*,
|
|
all_points_map: FloatArray,
|
|
center_xy: FloatArray,
|
|
support_lower_z: float,
|
|
) -> float:
|
|
ground = self.profile["ground"]
|
|
radius = float(ground["local_radius_m"])
|
|
if all_points_map.size:
|
|
distances = np.linalg.norm(all_points_map[:, :2] - center_xy, axis=1)
|
|
candidates = all_points_map[distances <= radius, 2]
|
|
else:
|
|
candidates = np.empty(0, dtype=np.float64)
|
|
if candidates.size:
|
|
estimate = float(np.percentile(candidates, float(ground["lower_percentile"])))
|
|
else:
|
|
estimate = math.nan
|
|
if (
|
|
not math.isfinite(estimate)
|
|
or estimate < support_lower_z - float(ground["maximum_below_support_m"])
|
|
or estimate > support_lower_z + float(ground["maximum_above_support_m"])
|
|
):
|
|
estimate = support_lower_z - float(ground["fallback_below_support_m"])
|
|
return estimate
|
|
|
|
@staticmethod
|
|
def _amodal_axis_center(lower: float, upper: float, size: float, sensor: float) -> float:
|
|
half = size * 0.5
|
|
allowed_lower = upper - half
|
|
allowed_upper = lower + half
|
|
if sensor < lower:
|
|
return allowed_upper
|
|
if sensor > upper:
|
|
return allowed_lower
|
|
return min(allowed_upper, max(allowed_lower, (lower + upper) * 0.5))
|
|
|
|
@staticmethod
|
|
def _coverage(points: FloatArray, center: FloatArray, size: FloatArray, yaw: float) -> float:
|
|
forward = np.asarray([math.cos(yaw), math.sin(yaw)])
|
|
side = np.asarray([-math.sin(yaw), math.cos(yaw)])
|
|
relative = points[:, :2] - center[:2]
|
|
local = np.column_stack((relative @ forward, relative @ side))
|
|
horizontal = np.all(np.abs(local) <= size[:2] * 0.5 + 1e-6, axis=1)
|
|
vertical = np.abs(points[:, 2] - center[2]) <= size[2] * 0.5 + 1e-6
|
|
return float(np.mean(horizontal & vertical))
|
|
|
|
def _prune(self, now: float) -> None:
|
|
maximum_idle = float(self.profile["temporal"]["maximum_idle_s"])
|
|
stale = [
|
|
key for key, state in self.states.items() if now - state.last_seconds > maximum_idle
|
|
]
|
|
for key in stale:
|
|
del self.states[key]
|
|
|
|
|
|
def fuse_tracks(
|
|
*,
|
|
tracks: list[dict[str, Any]],
|
|
semantic_map: npt.NDArray[np.uint8],
|
|
pixels: FloatArray,
|
|
depths: FloatArray,
|
|
source_indices: npt.NDArray[np.int64],
|
|
points_map: FloatArray,
|
|
points_lidar: FloatArray,
|
|
association: Mapping[str, Any],
|
|
distance_history: dict[int, deque[float]],
|
|
completion_tracker: CuboidCompletionTracker | None = None,
|
|
sensor_position_map: Sequence[float] | None = None,
|
|
session_seconds: float | None = None,
|
|
) -> tuple[TrackFusion, ...]:
|
|
vehicle_labels = set(str(value) for value in association["vehicle_labels"])
|
|
accepted_tracks: list[dict[str, Any]] = []
|
|
for track in sorted(tracks, key=lambda value: float(value["score"]), reverse=True):
|
|
label = str(track["label"])
|
|
box = tuple(float(value) for value in track["bbox_xyxy"])
|
|
if label in vehicle_labels and any(
|
|
str(other["label"]) in vehicle_labels
|
|
and _box_iou(box, other["bbox_xyxy"]) > float(association["group_nms_iou_threshold"])
|
|
for other in accepted_tracks
|
|
):
|
|
continue
|
|
accepted_tracks.append(track)
|
|
accepted_tracks.sort(key=lambda value: int(value["track_id"]))
|
|
pixel_x = np.clip(np.rint(pixels[:, 0]).astype(np.int64), 0, IMAGE_WIDTH - 1)
|
|
pixel_y = np.clip(np.rint(pixels[:, 1]).astype(np.int64), 0, IMAGE_HEIGHT - 1)
|
|
sampled_semantic = semantic_map[pixel_y, pixel_x]
|
|
result: list[TrackFusion] = []
|
|
for track in accepted_tracks:
|
|
label = str(track["label"])
|
|
group = "vehicle" if label in vehicle_labels else label
|
|
if group not in association["semantic_ids"]:
|
|
continue
|
|
track_id = int(track["track_id"])
|
|
x1, y1, x2, y2 = (float(value) for value in track["bbox_xyxy"])
|
|
width, height = x2 - x1, y2 - y1
|
|
inset = association["box_inset"]
|
|
inside = (
|
|
(pixels[:, 0] >= x1 + width * float(inset["horizontal_fraction"]))
|
|
& (pixels[:, 0] < x2 - width * float(inset["horizontal_fraction"]))
|
|
& (pixels[:, 1] >= y1 + height * float(inset["top_fraction"]))
|
|
& (pixels[:, 1] < y2 - height * float(inset["bottom_fraction"]))
|
|
)
|
|
candidates = np.flatnonzero(inside).astype(np.int64)
|
|
allowed = np.asarray(association["semantic_ids"][group], dtype=np.uint8)
|
|
compatible = candidates[np.isin(sampled_semantic[candidates], allowed)]
|
|
clustered = _depth_cluster(compatible, depths, association)
|
|
selected = _spatial_cluster(
|
|
source_indices[clustered],
|
|
points_lidar,
|
|
float(association["spatial_cluster_radius_m"][group]),
|
|
)
|
|
ranges = np.linalg.norm(points_lidar[selected], axis=1)
|
|
p10 = None if ranges.size == 0 else float(np.percentile(ranges, 10))
|
|
median = None if ranges.size == 0 else float(np.median(ranges))
|
|
minimum = int(association["minimum_support_points"][group])
|
|
cuboid = None
|
|
observed_cuboid = None
|
|
geometry = "none"
|
|
completion_fraction = None
|
|
ground_z_map = None
|
|
orientation_source = None
|
|
temporal_status = None
|
|
support_coverage_fraction = None
|
|
if compatible.size == 0:
|
|
status = "rejected-no-semantic-lidar-support"
|
|
elif selected.size < minimum:
|
|
status = f"rejected-fewer-than-{minimum}-clustered-points"
|
|
else:
|
|
cuboid = _cuboid(points_map[selected], association, group)
|
|
status = (
|
|
"accepted-point-supported-oriented-p05-p95"
|
|
if cuboid
|
|
else "rejected-implausible-cuboid"
|
|
)
|
|
if cuboid is not None:
|
|
observed_cuboid = cuboid
|
|
geometry = "point-supported-visible-surface-envelope"
|
|
smoothed = None
|
|
if cuboid is not None and median is not None:
|
|
history = distance_history[track_id]
|
|
if history:
|
|
reference = float(np.median(np.asarray(history, dtype=np.float64)))
|
|
maximum = max(
|
|
float(association["maximum_distance_innovation_m"]),
|
|
reference * float(association["maximum_distance_innovation_fraction"]),
|
|
)
|
|
if abs(median - reference) > maximum:
|
|
cuboid = None
|
|
status = "rejected-distance-innovation"
|
|
if cuboid is not None:
|
|
history.append(median)
|
|
smoothed = float(np.median(np.asarray(history, dtype=np.float64)))
|
|
if cuboid is None:
|
|
observed_cuboid = None
|
|
geometry = "none"
|
|
elif completion_tracker is not None:
|
|
if sensor_position_map is None or session_seconds is None:
|
|
raise RuntimeError("LAB E13 completion requires sensor pose and session time")
|
|
completion = completion_tracker.complete(
|
|
track_id=track_id,
|
|
label=label,
|
|
support_points_map=points_map[selected],
|
|
all_points_map=points_map,
|
|
sensor_position_map=sensor_position_map,
|
|
session_seconds=session_seconds,
|
|
observed_cuboid=observed_cuboid,
|
|
)
|
|
if completion is None:
|
|
reason = completion_tracker.last_failure_reason or "unknown"
|
|
if completion_tracker.profile["failure_policy"] == "reject":
|
|
cuboid = None
|
|
geometry = "none"
|
|
status = f"rejected-amodal-completion-{reason}"
|
|
else:
|
|
status = f"accepted-visible-envelope-completion-{reason}"
|
|
else:
|
|
cuboid = completion.cuboid
|
|
status = "accepted-class-prior-amodal-v1"
|
|
geometry = "class-prior-completed-from-visible-lidar-support"
|
|
completion_fraction = completion.completion_fraction
|
|
ground_z_map = completion.ground_z_map
|
|
orientation_source = completion.orientation_source
|
|
temporal_status = completion.temporal_status
|
|
support_coverage_fraction = completion.support_coverage_fraction
|
|
result.append(
|
|
TrackFusion(
|
|
track_id=track_id,
|
|
label=label,
|
|
association_group=group,
|
|
score=float(track["score"]),
|
|
bbox_xyxy=(x1, y1, x2, y2),
|
|
candidate_points=int(candidates.size),
|
|
semantic_points=int(compatible.size),
|
|
clustered_points=int(selected.size),
|
|
distance_p10_m=p10,
|
|
distance_median_m=median,
|
|
distance_smoothed_m=smoothed,
|
|
status=status,
|
|
cuboid=cuboid,
|
|
source_indices=selected,
|
|
geometry=geometry,
|
|
observed_cuboid=observed_cuboid,
|
|
completion_fraction=completion_fraction,
|
|
ground_z_map=ground_z_map,
|
|
orientation_source=orientation_source,
|
|
temporal_status=temporal_status,
|
|
support_coverage_fraction=support_coverage_fraction,
|
|
)
|
|
)
|
|
return tuple(result)
|
|
|
|
|
|
def fusion_document(item: TrackFusion) -> dict[str, Any]:
|
|
return {
|
|
"track_id": item.track_id,
|
|
"label": item.label,
|
|
"association_group": item.association_group,
|
|
"score": item.score,
|
|
"bbox_xyxy": list(item.bbox_xyxy),
|
|
"candidate_projected_points": item.candidate_points,
|
|
"semantic_compatible_points": item.semantic_points,
|
|
"clustered_points": item.clustered_points,
|
|
"distance_p10_m": item.distance_p10_m,
|
|
"distance_median_m": item.distance_median_m,
|
|
"distance_smoothed_m": item.distance_smoothed_m,
|
|
"cuboid_status": item.status,
|
|
"cuboid_center_map": None if item.cuboid is None else list(item.cuboid.center_map),
|
|
"cuboid_half_size": None if item.cuboid is None else list(item.cuboid.half_size),
|
|
"cuboid_quaternion_xyzw": None
|
|
if item.cuboid is None
|
|
else list(item.cuboid.quaternion_xyzw),
|
|
"geometry": item.geometry,
|
|
"observed_cuboid_center_map": None
|
|
if item.observed_cuboid is None
|
|
else list(item.observed_cuboid.center_map),
|
|
"observed_cuboid_half_size": None
|
|
if item.observed_cuboid is None
|
|
else list(item.observed_cuboid.half_size),
|
|
"observed_cuboid_quaternion_xyzw": None
|
|
if item.observed_cuboid is None
|
|
else list(item.observed_cuboid.quaternion_xyzw),
|
|
"completion_fraction": item.completion_fraction,
|
|
"ground_z_map": item.ground_z_map,
|
|
"orientation_source": item.orientation_source,
|
|
"temporal_status": item.temporal_status,
|
|
"support_coverage_fraction": item.support_coverage_fraction,
|
|
}
|
|
|
|
|
|
def clearance(points_lidar: FloatArray, profile: Mapping[str, Any]) -> dict[str, Any]:
|
|
sectors = int(profile["sector_count"])
|
|
empty = {
|
|
"schema": "diagnostic-polar-obstacle-clearance/v1",
|
|
"state": "unavailable",
|
|
"frame": "k1-lidar",
|
|
"front_m": None,
|
|
"observed_sector_fraction": 0.0,
|
|
"sector_ranges_m": [None] * sectors,
|
|
}
|
|
if points_lidar.size == 0:
|
|
return empty
|
|
ground = float(np.percentile(points_lidar[:, 2], float(profile["ground_percentile"])))
|
|
ranges = np.linalg.norm(points_lidar[:, :2], axis=1)
|
|
keep = (
|
|
(ranges >= float(profile["minimum_range_m"]))
|
|
& (ranges <= float(profile["maximum_range_m"]))
|
|
& (points_lidar[:, 2] >= ground + float(profile["minimum_height_above_ground_m"]))
|
|
& (points_lidar[:, 2] <= ground + float(profile["maximum_height_above_ground_m"]))
|
|
)
|
|
points, ranges = points_lidar[keep], ranges[keep]
|
|
if points.size == 0:
|
|
return {**empty, "ground_z_estimate_m": ground}
|
|
angles = np.arctan2(points[:, 1], points[:, 0])
|
|
indices = np.floor((angles + math.pi) / (2 * math.pi) * sectors).astype(int)
|
|
indices = np.clip(indices, 0, sectors - 1)
|
|
values = np.full(sectors, np.inf, dtype=np.float64)
|
|
np.minimum.at(values, indices, ranges)
|
|
observed = np.isfinite(values)
|
|
front = ranges[np.abs(angles) <= math.radians(float(profile["front_half_angle_degrees"]))]
|
|
return {
|
|
"schema": empty["schema"],
|
|
"state": "available",
|
|
"frame": "k1-lidar",
|
|
"front_m": None if front.size == 0 else float(np.min(front)),
|
|
"observed_sector_fraction": float(np.mean(observed)),
|
|
"sector_ranges_m": [None if not math.isfinite(value) else float(value) for value in values],
|
|
"ground_z_estimate_m": ground,
|
|
}
|
|
|
|
|
|
class WorldStateProjector:
|
|
def __init__(self, history_limit_s: float) -> None:
|
|
self.history_limit_s = history_limit_s
|
|
self.history: dict[int, deque[tuple[float, tuple[float, float, float]]]] = {}
|
|
|
|
def project(
|
|
self,
|
|
*,
|
|
frame_index: int,
|
|
source_frame_index: int,
|
|
session_seconds: float,
|
|
fusion_state: str,
|
|
fusions: Sequence[TrackFusion],
|
|
points_lidar: FloatArray,
|
|
clearance_state: Mapping[str, Any],
|
|
delivery: Mapping[str, Any],
|
|
) -> dict[str, Any]:
|
|
objects: list[dict[str, Any]] = []
|
|
for item in fusions:
|
|
if item.cuboid is None or item.distance_smoothed_m is None:
|
|
continue
|
|
center = item.cuboid.center_map
|
|
velocity, velocity_status, residual = self._velocity(
|
|
item.track_id, session_seconds, center
|
|
)
|
|
lidar_center = None
|
|
if item.source_indices.size:
|
|
lidar_center = np.median(points_lidar[item.source_indices], axis=0)
|
|
objects.append(
|
|
{
|
|
"track_id": item.track_id,
|
|
"class": item.association_group,
|
|
"detector_label": item.label,
|
|
"confidence": item.score,
|
|
"position_map_m": list(center),
|
|
"position_lidar_m": None if lidar_center is None else lidar_center.tolist(),
|
|
"orientation_map_xyzw": list(item.cuboid.quaternion_xyzw),
|
|
"size_m": [2 * value for value in item.cuboid.half_size],
|
|
"range_m": item.distance_smoothed_m,
|
|
"velocity_map_mps": None if velocity is None else list(velocity),
|
|
"speed_mps": None
|
|
if velocity is None
|
|
else math.sqrt(sum(value * value for value in velocity)),
|
|
"velocity_status": velocity_status,
|
|
"velocity_residual_m": residual,
|
|
"support_points": item.clustered_points,
|
|
"geometry": item.geometry,
|
|
"completion_fraction": item.completion_fraction,
|
|
"ground_z_map": item.ground_z_map,
|
|
"orientation_source": item.orientation_source,
|
|
"temporal_status": item.temporal_status,
|
|
"support_coverage_fraction": item.support_coverage_fraction,
|
|
}
|
|
)
|
|
self._prune(session_seconds)
|
|
return {
|
|
"schema_version": WORLD_STATE_SCHEMA,
|
|
"frame_index": frame_index,
|
|
"source_frame_index": source_frame_index,
|
|
"session_seconds": session_seconds,
|
|
"coordinate_frames": {
|
|
"world": "k1-map",
|
|
"sensor_relative": "k1-lidar",
|
|
"vehicle_body": "unavailable-no-rig-to-vehicle-transform",
|
|
},
|
|
"fusion_state": fusion_state,
|
|
"delivery": dict(delivery),
|
|
"objects": objects,
|
|
"object_count": len(objects),
|
|
"clearance": dict(clearance_state),
|
|
}
|
|
|
|
def _velocity(
|
|
self, track_id: int, now: float, center: tuple[float, float, float]
|
|
) -> tuple[tuple[float, float, float] | None, str, float | None]:
|
|
history = self.history.setdefault(track_id, deque(maxlen=32))
|
|
history.append((now, center))
|
|
while history and history[0][0] < now - self.history_limit_s:
|
|
history.popleft()
|
|
if len(history) < 4 or history[-1][0] - history[0][0] < 0.4:
|
|
return None, "unavailable-insufficient-history", None
|
|
slopes = []
|
|
values = list(history)
|
|
for left, (left_time, left_center) in enumerate(values):
|
|
for right_time, right_center in values[left + 1 :]:
|
|
delta = right_time - left_time
|
|
if delta >= 0.2:
|
|
slopes.append(
|
|
tuple(
|
|
(right_center[index] - left_center[index]) / delta for index in range(3)
|
|
)
|
|
)
|
|
if not slopes:
|
|
return None, "unavailable-insufficient-baseline", None
|
|
velocity = tuple(statistics.median(value[index] for value in slopes) for index in range(3))
|
|
latest_time, latest_center = values[-1]
|
|
residuals = []
|
|
for observed_time, observed in values:
|
|
predicted = tuple(
|
|
latest_center[index] - velocity[index] * (latest_time - observed_time)
|
|
for index in range(3)
|
|
)
|
|
residuals.append(
|
|
math.sqrt(sum((observed[index] - predicted[index]) ** 2 for index in range(3)))
|
|
)
|
|
residual = statistics.median(residuals)
|
|
speed = math.sqrt(sum(value * value for value in velocity))
|
|
if speed > 20:
|
|
return None, "rejected-speed-bound", residual
|
|
if residual > 0.75:
|
|
return None, "rejected-position-residual", residual
|
|
return velocity, "diagnostic-robust-history", residual
|
|
|
|
def _prune(self, now: float) -> None:
|
|
expired = [
|
|
key
|
|
for key, value in self.history.items()
|
|
if not value or value[-1][0] < now - self.history_limit_s
|
|
]
|
|
for key in expired:
|
|
del self.history[key]
|
|
|
|
|
|
def distance_history(length: int) -> dict[int, deque[float]]:
|
|
return defaultdict(lambda: deque(maxlen=length))
|