fix(perception): preserve quaternion alignment across binary ingress
This commit is contained in:
DCCONSTRUCTIONS
+156
@@ -0,0 +1,156 @@
|
||||
"""CPU-only numerical diagnosis on a bounded source prefix, no model execution.
|
||||
|
||||
Frozen observations select ranges to compare; they never feed a new graph.
|
||||
Probe changes only storage alignment of identical pose values, not geometry math.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import time
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import run_joint_pilot as legacy
|
||||
from pilot_graph import calibration
|
||||
|
||||
from k1link.perception.geometry_math import (
|
||||
project_map_points_kb4,
|
||||
quaternion_xyzw_to_rotation_matrix,
|
||||
)
|
||||
from k1link.perception.streaming_sensors import normalized_sensor
|
||||
|
||||
|
||||
def aligned_pose(pose, offset):
|
||||
# Force a known address modulo 64, preserving bytes and C order.
|
||||
position, quaternion = pose
|
||||
storage = np.empty(128, np.uint8)
|
||||
start = (offset - storage.ctypes.data) % 64
|
||||
q = np.ndarray((4,), dtype=np.float64, buffer=storage, offset=start)
|
||||
q[:] = quaternion
|
||||
return position, q
|
||||
|
||||
|
||||
def run(args):
|
||||
root = Path(args.output)
|
||||
root.mkdir()
|
||||
report = {
|
||||
"created_utc": datetime.now(UTC).isoformat(),
|
||||
"started_monotonic_ns": time.monotonic_ns(),
|
||||
"scope": "CPU-only offline bounded numerical diagnosis; not latency qualification",
|
||||
"numpy_version": np.__version__,
|
||||
"model_runs": 0,
|
||||
"frames": [],
|
||||
}
|
||||
with Path(args.reference).open() as stream:
|
||||
reference = [json.loads(next(stream)) for _ in range(args.frames)]
|
||||
projection = calibration(args.calibration)
|
||||
|
||||
class Sink:
|
||||
error = None
|
||||
|
||||
def put(self, bundle):
|
||||
row = {"sequence": bundle["sequence"], "available": bundle["available"]}
|
||||
report["frames"].append(row)
|
||||
if not bundle["available"]:
|
||||
return True
|
||||
position, quaternion = bundle["pose"]
|
||||
raw = position.tobytes() + quaternion.tobytes()
|
||||
wire_pose = normalized_sensor("pose", 1, 1, raw).value
|
||||
variants = {
|
||||
"archive": bundle["pose"],
|
||||
"wire": wire_pose,
|
||||
"owned_copy": tuple(np.array(a, copy=True) for a in wire_pose),
|
||||
"separate_bytes": (np.frombuffer(raw[:24], "<f8"), np.frombuffer(raw[24:], "<f8")),
|
||||
**{
|
||||
f"offset_{offset}": aligned_pose(bundle["pose"], offset)
|
||||
for offset in (0, 8, 16, 24, 32, 40, 48, 56)
|
||||
},
|
||||
}
|
||||
row["variants"] = {}
|
||||
observations = reference[bundle["sequence"]]["observations"]
|
||||
for name, pose in variants.items():
|
||||
assert all(np.array_equal(a, b) for a, b in zip(pose, bundle["pose"], strict=True))
|
||||
projected = project_map_points_kb4(
|
||||
bundle["points"],
|
||||
position_map_xyz=pose[0],
|
||||
orientation_map_from_lidar_xyzw=pose[1],
|
||||
profile=projection,
|
||||
)
|
||||
depths = dict(zip(projected.source_indices, projected.depths_m, strict=True))
|
||||
ranges = []
|
||||
for index, observation in enumerate(observations):
|
||||
if observation["basis"] != "fused" or observation["metric_geometry"] is None:
|
||||
continue
|
||||
distance = float(
|
||||
np.median([depths[k] for k in observation["source_point_ids"]])
|
||||
)
|
||||
target = observation["metric_geometry"]["range_m"]
|
||||
ranges.append(
|
||||
{
|
||||
"observation": index,
|
||||
"actual": distance,
|
||||
"reference": target,
|
||||
"exact": distance == target,
|
||||
}
|
||||
)
|
||||
row["variants"][name] = {
|
||||
"quaternion_address_mod64": pose[1].ctypes.data % 64,
|
||||
"norm_hex": float(np.linalg.norm(pose[1])).hex(),
|
||||
"rotation_sha256": hashlib.sha256(
|
||||
quaternion_xyzw_to_rotation_matrix(pose[1])
|
||||
).hexdigest(),
|
||||
"depth_sha256": hashlib.sha256(projected.depths_m).hexdigest(),
|
||||
"pixels_sha256": hashlib.sha256(projected.pixels_xy).hexdigest(),
|
||||
"source_indices_sha256": hashlib.sha256(projected.source_indices).hexdigest(),
|
||||
"ranges": ranges,
|
||||
}
|
||||
return True
|
||||
|
||||
def finish(self, error=None):
|
||||
self.error = error
|
||||
|
||||
sink = Sink()
|
||||
old_send, old_receive = legacy.send, legacy.receive
|
||||
image = bytes(1440000)
|
||||
legacy.send = lambda *args: None
|
||||
legacy.receive = lambda *args: ({"decode_ms": 0}, image)
|
||||
try:
|
||||
legacy.produce(
|
||||
args,
|
||||
SimpleNamespace(stdin=None, stdout=None),
|
||||
sink,
|
||||
SimpleNamespace(is_set=lambda: False, wait=lambda _: False),
|
||||
{},
|
||||
)
|
||||
finally:
|
||||
legacy.send, legacy.receive = old_send, old_receive
|
||||
if sink.error:
|
||||
raise ValueError(sink.error)
|
||||
report["finished_monotonic_ns"] = time.monotonic_ns()
|
||||
report["mismatches"] = {}
|
||||
for row in report["frames"]:
|
||||
for name, variant in row.get("variants", {}).items():
|
||||
report["mismatches"].setdefault(name, []).extend(
|
||||
{"sequence": row["sequence"], **value}
|
||||
for value in variant["ranges"]
|
||||
if not value["exact"]
|
||||
)
|
||||
(root / "report.json").write_text(json.dumps(report, indent=2) + "\n")
|
||||
print(json.dumps({"frames": len(report["frames"]), "mismatches": report["mismatches"]}))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--output", required=True)
|
||||
parser.add_argument("--frames", type=int, default=32)
|
||||
parser.add_argument("--reference", required=True)
|
||||
parser.add_argument("--calibration", default="/calibration.npz")
|
||||
parser.add_argument("--camera-index", default="/camera-index.jsonl")
|
||||
parser.add_argument("--sensor-archive", default="/sensor-source.npz")
|
||||
args = parser.parse_args()
|
||||
if not 1 <= args.frames <= 128:
|
||||
parser.error("bounded prefix only")
|
||||
run(args)
|
||||
Reference in New Issue
Block a user