feat: finalize corrected-route planning and Rerun recording review
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1 @@
|
||||
"""Offline, source-preserving map derivatives; no capture or vehicle authority."""
|
||||
@@ -0,0 +1,231 @@
|
||||
"""Training-only acquisition of a known start-area revisit, not live tracking.
|
||||
|
||||
The registrar is injected: this module has no device, planner, catalog or UI
|
||||
dependency. Every declared support window is evaluated before selection. No
|
||||
endpoint equality, session-name branch, or withheld-point selection is used.
|
||||
This is not arbitrary-loop discovery or a guarantee for unbounded SLAM drift.
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .smooth_correction import SurfaceLink
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ClosurePolicy:
|
||||
version: str = "known-revisit-acquisition/v2"
|
||||
reference_seconds: tuple = (10.0, 20.0, 40.0)
|
||||
query_seconds: tuple = (5.0, 10.0, 20.0, 30.0)
|
||||
primary_reference_s: float = 20.0
|
||||
primary_query_s: float = 5.0
|
||||
radius_m: float = 25.0
|
||||
cycle_m: float = 0.1
|
||||
cycle_deg: float = 0.2
|
||||
agreement_m: float = 0.5
|
||||
agreement_deg: float = 1.0
|
||||
translation_weight_m: float = 0.03
|
||||
rotation_weight_deg: float = 0.05
|
||||
|
||||
def __post_init__(self):
|
||||
values = [
|
||||
*self.reference_seconds,
|
||||
*self.query_seconds,
|
||||
self.radius_m,
|
||||
self.cycle_m,
|
||||
self.cycle_deg,
|
||||
self.agreement_m,
|
||||
self.agreement_deg,
|
||||
self.translation_weight_m,
|
||||
self.rotation_weight_deg,
|
||||
]
|
||||
if (
|
||||
not self.reference_seconds
|
||||
or not self.query_seconds
|
||||
or not np.isfinite(values).all()
|
||||
or min(values) <= 0
|
||||
or len(set(self.reference_seconds)) != len(self.reference_seconds)
|
||||
or len(set(self.query_seconds)) != len(self.query_seconds)
|
||||
or self.primary_reference_s not in self.reference_seconds
|
||||
or self.primary_query_s not in self.query_seconds
|
||||
):
|
||||
raise ValueError("Invalid closure acquisition policy.")
|
||||
|
||||
|
||||
class ClosureUnavailable(ValueError):
|
||||
def __init__(self, report):
|
||||
self.report = report
|
||||
super().__init__(report["reason"])
|
||||
|
||||
|
||||
def _apply(points, matrix):
|
||||
return points @ matrix[:3, :3].T + matrix[:3, 3]
|
||||
|
||||
|
||||
def _difference(a, b, center):
|
||||
return (
|
||||
float(np.linalg.norm(_apply(center, a) - _apply(center, b))),
|
||||
float(
|
||||
np.rad2deg(np.linalg.norm(Rotation.from_matrix(a[:3, :3].T @ b[:3, :3]).as_rotvec()))
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def acquire_closure(data, registrar, policy=None):
|
||||
"""Registrar(reference, query, seed, acquisition=bool) returns a gated fit.
|
||||
|
||||
Acquisition may allow a larger first correction. Reverse refinement MUST
|
||||
use the unchanged tracking-quality policy, starting from the inverse fit.
|
||||
Numerical exceptions are recorded as failures, never converted to identity.
|
||||
"""
|
||||
policy = policy or ClosurePolicy()
|
||||
f, p, s = data["frames"], data["poses"], data["frame_distance"]
|
||||
pts, ids, held = data["sample_points"], data["sample_frame"], data["heldout"]
|
||||
if (
|
||||
len(f) < 2
|
||||
or len(p) < 2
|
||||
or held.dtype != bool
|
||||
or held.shape != (len(f),)
|
||||
or s.shape != (len(f),)
|
||||
or ids.shape != (len(pts),)
|
||||
or ids.dtype.kind not in "iu"
|
||||
or (ids < 0).any()
|
||||
or (ids >= len(f)).any()
|
||||
or not all(np.isfinite(v).all() for v in (f, p, s, pts))
|
||||
or (np.diff(f[:, 0]) < 0).any()
|
||||
or (np.diff(s) < 0).any()
|
||||
):
|
||||
raise ValueError("Invalid closure frame ownership or chronology.")
|
||||
# One fixed spatial population in the original frame, independent of fits.
|
||||
training = ~held[ids] & (np.linalg.norm(pts - p[0, 1:4], axis=1) <= policy.radius_m)
|
||||
report = dict(
|
||||
schema_version="missioncore.closure-acquisition/v1",
|
||||
policy=asdict(policy),
|
||||
training_only=True,
|
||||
endpoint_constraint=False,
|
||||
attempts=[],
|
||||
status="rejected",
|
||||
expected_attempts=len(policy.reference_seconds) * len(policy.query_seconds),
|
||||
)
|
||||
links = {}
|
||||
for first in policy.reference_seconds:
|
||||
amask = (f[:, 0] <= f[0, 0] + first) & ~held
|
||||
a = pts[training & amask[ids]]
|
||||
for last in policy.query_seconds:
|
||||
bmask = (f[:, 0] >= f[-1, 0] - last) & ~held
|
||||
b = pts[training & bmask[ids]]
|
||||
row = dict(
|
||||
reference_s=first,
|
||||
query_s=last,
|
||||
qualified=False,
|
||||
reference_points=len(a),
|
||||
query_points=len(b),
|
||||
)
|
||||
report["attempts"].append(row)
|
||||
if np.any(amask & bmask):
|
||||
row["reason"] = "overlapping-source-windows"
|
||||
continue
|
||||
if min(len(a), len(b)) < 300:
|
||||
row["reason"] = "insufficient-training-geometry"
|
||||
continue
|
||||
try:
|
||||
fit = registrar(a, b, np.eye(4), acquisition=True)
|
||||
row["fit"] = fit
|
||||
if fit["status"] != "candidate":
|
||||
row["reason"] = "forward-quality"
|
||||
continue
|
||||
t = np.asarray(fit["T_reference_query"])
|
||||
reverse = registrar(b, a, np.linalg.inv(t), acquisition=False)
|
||||
row["reverse"] = reverse
|
||||
if reverse["status"] != "candidate":
|
||||
row["reason"] = "reverse-quality"
|
||||
continue
|
||||
center = np.median(b, axis=0)
|
||||
cycle = t @ np.asarray(reverse["T_reference_query"])
|
||||
cm, cr = _difference(cycle, np.eye(4), center)
|
||||
row.update(cycle_m=cm, cycle_deg=cr)
|
||||
if cm > policy.cycle_m or cr > policy.cycle_deg:
|
||||
row["reason"] = "bidirectional-inconsistency"
|
||||
continue
|
||||
link = SurfaceLink(
|
||||
float(np.mean(s[amask])),
|
||||
float(np.mean(s[bmask])),
|
||||
t,
|
||||
center,
|
||||
policy.translation_weight_m,
|
||||
policy.rotation_weight_deg,
|
||||
"start-area/revisit",
|
||||
)
|
||||
except (ValueError, np.linalg.LinAlgError) as exc:
|
||||
row["reason"] = "numerical-unavailable"
|
||||
row["detail"] = str(exc)
|
||||
continue
|
||||
row["qualified"] = True
|
||||
links[len(report["attempts"]) - 1] = link
|
||||
report["complete"] = len(report["attempts"]) == report["expected_attempts"]
|
||||
if not links:
|
||||
report["reason"] = "No training-only closure passed quality and reverse consistency."
|
||||
raise ClosureUnavailable(report)
|
||||
# Preserve the established short-window measurement when it qualifies.
|
||||
# Larger support is a fallback, not proof of a better measurement: mixing
|
||||
# more motion/vegetation can change an otherwise stable registration.
|
||||
# Still finish the full matrix and check competing fits before acceptance.
|
||||
selected = max(
|
||||
links,
|
||||
key=lambda i: (
|
||||
(
|
||||
report["attempts"][i]["reference_s"] == policy.primary_reference_s
|
||||
and report["attempts"][i]["query_s"] == policy.primary_query_s
|
||||
),
|
||||
report["attempts"][i]["reference_s"] * report["attempts"][i]["query_s"],
|
||||
min(report["attempts"][i]["reference_points"], report["attempts"][i]["query_points"]),
|
||||
-i,
|
||||
),
|
||||
)
|
||||
link = links[selected]
|
||||
report["selection_rule"] = "qualified-primary-else-largest-support; complete-consistency-check"
|
||||
agreement = []
|
||||
for i, other in links.items():
|
||||
# Check both patch centers; a rotation must not hide at one chosen pivot.
|
||||
distances = [
|
||||
_difference(link.T_reference_query, other.T_reference_query, c)
|
||||
for c in (link.query_center, other.query_center)
|
||||
]
|
||||
dm, deg = max(x[0] for x in distances), max(x[1] for x in distances)
|
||||
agreement.append(dict(attempt=i, distance_m=dm, angle_deg=deg))
|
||||
report.update(selected_attempt=selected, qualified_attempts=list(links), agreement=agreement)
|
||||
if any(
|
||||
r["distance_m"] > policy.agreement_m or r["angle_deg"] > policy.agreement_deg
|
||||
for r in agreement
|
||||
):
|
||||
report["reason"] = "Qualified support windows disagree; closure is ambiguous."
|
||||
raise ClosureUnavailable(report)
|
||||
report.update(status="candidate", reason=None)
|
||||
return link, report
|
||||
|
||||
|
||||
def review_acceptance(results, validation):
|
||||
"""Frozen same-source checks authorize packaging, never vehicle operation."""
|
||||
before, after = results
|
||||
seam = after["seam_holdout"]
|
||||
previous = before["seam_holdout"]
|
||||
checks = {
|
||||
"all_local_windows_qualified": validation[1]["total"] > 0
|
||||
and validation[1]["candidate_count"] == validation[1]["total"],
|
||||
"heldout_seam_present": seam["points"] >= 300 and seam["query_frames"] >= 2,
|
||||
"heldout_seam_quality": seam["overlap_05m"] >= 0.55
|
||||
and seam["inlier_rmse_m"] is not None
|
||||
and seam["inlier_rmse_m"] <= 0.25,
|
||||
"heldout_seam_not_degraded": seam["overlap_05m"] >= previous["overlap_05m"] - 0.02
|
||||
and seam["all_point_distances_m"]["median"]
|
||||
<= previous["all_point_distances_m"]["median"] + 0.02,
|
||||
}
|
||||
return dict(
|
||||
schema_version="missioncore.closure-review/v1",
|
||||
checks=checks,
|
||||
accepted=all(checks.values()),
|
||||
independent_accuracy=False,
|
||||
vehicle_control=False,
|
||||
)
|
||||
@@ -0,0 +1,271 @@
|
||||
"""Immutable, vendor-neutral map candidates. Publication is not promotion.
|
||||
|
||||
No session catalog writes or automatic latest-version selection. Readers pin a
|
||||
manifest digest, verify every artifact, and never reinterpret source traversal
|
||||
coordinates as corrected path length. No solver dependency is needed to read.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import re
|
||||
import shutil
|
||||
from copy import deepcopy
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
|
||||
import numpy as np
|
||||
|
||||
SCHEMA = "missioncore.map-reference-version/v1"
|
||||
POINTS = "points.f32"
|
||||
TRAJECTORY = "trajectory.npz"
|
||||
AUTHORITY = dict(production_promotion=False, vehicle_control=False, independent_pass_verified=False)
|
||||
|
||||
|
||||
def sha256(path):
|
||||
with Path(path).open("rb") as stream:
|
||||
return hashlib.file_digest(stream, "sha256").hexdigest()
|
||||
|
||||
|
||||
def json_bytes(value):
|
||||
return json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
|
||||
|
||||
|
||||
def _digest(value):
|
||||
if not isinstance(value, str) or not re.fullmatch(r"[a-f0-9]{64}", value):
|
||||
raise ValueError("Invalid map identity.")
|
||||
return value
|
||||
|
||||
|
||||
def _plain_file(path):
|
||||
if path.is_symlink() or not path.is_file():
|
||||
raise ValueError("Map artifact must be a regular file, not a link.")
|
||||
return path
|
||||
|
||||
|
||||
def _trajectory(path, point_count):
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
arrays = {
|
||||
key: np.array(data[key], dtype=float)
|
||||
for key in (
|
||||
"positions",
|
||||
"orientations_xyzw",
|
||||
"receipt_time_s",
|
||||
"source_distance_m",
|
||||
"frame_source_distance_m",
|
||||
"frames",
|
||||
)
|
||||
}
|
||||
p, q, t, s, fs, f = arrays.values()
|
||||
if (
|
||||
p.ndim != 2
|
||||
or p.shape[1:] != (3,)
|
||||
or len(p) < 2
|
||||
or q.shape != (len(p), 4)
|
||||
or t.shape != (len(p),)
|
||||
or s.shape != (len(p),)
|
||||
or f.ndim != 2
|
||||
or f.shape[1:] != (4,)
|
||||
or len(f) < 1
|
||||
or fs.shape != (len(f),)
|
||||
or not all(np.isfinite(a).all() for a in arrays.values())
|
||||
):
|
||||
raise ValueError("Invalid map trajectory dimensions or values.")
|
||||
if (
|
||||
(np.diff(t) <= 0).any()
|
||||
or s[0] != 0
|
||||
or (np.diff(s) < 0).any()
|
||||
or not np.allclose(np.linalg.norm(q, axis=1), 1, atol=1e-6, rtol=0)
|
||||
or (np.diff(f[:, 0]) < 0).any()
|
||||
or (np.diff(f[:, 1]) <= 0).any()
|
||||
or not np.equal(f[:, 1:], np.floor(f[:, 1:])).all()
|
||||
or (f[:, 1:] < 0).any()
|
||||
or f[0, 2] != 0
|
||||
or not np.array_equal(f[1:, 2], f[:-1, 2] + f[:-1, 3])
|
||||
or f[-1, 2] + f[-1, 3] != point_count
|
||||
or not np.allclose(fs, np.interp(f[:, 0], t, s), rtol=0, atol=1e-7)
|
||||
):
|
||||
raise ValueError("Invalid map frame ownership, clocks or source traversal binding.")
|
||||
arrays["distance_m"] = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(p, axis=0), axis=1))]
|
||||
return arrays
|
||||
|
||||
|
||||
def _source_identity(source):
|
||||
if (
|
||||
source.get("schema_version") != "missioncore.planning-source/v1"
|
||||
or source.get("reference_version")
|
||||
or source.get("units") != "m"
|
||||
or not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9._-]{0,127}", source["session_id"])
|
||||
or not source.get("source_digests")
|
||||
):
|
||||
raise ValueError("A map version requires an original, metre-based planning source.")
|
||||
return dict(
|
||||
session_id=source["session_id"],
|
||||
generation=_digest(source["generation"]),
|
||||
source_digests={k: _digest(v) for k, v in source["source_digests"].items()},
|
||||
)
|
||||
|
||||
|
||||
def publish_map_version(
|
||||
root,
|
||||
source,
|
||||
points,
|
||||
trajectory,
|
||||
*,
|
||||
expected_points_sha256,
|
||||
expected_trajectory_sha256,
|
||||
evidence,
|
||||
method,
|
||||
label,
|
||||
):
|
||||
"""Seal an already reviewed derivative in an exclusive content-addressed directory.
|
||||
|
||||
Evidence maps a simple filename to (source path, expected SHA-256). Caller
|
||||
owns scientific review and source verification; integrity is not accuracy.
|
||||
The trajectory input uses the explicit v1 keys checked by `_trajectory`.
|
||||
"""
|
||||
root = Path(root)
|
||||
root.mkdir(parents=True, exist_ok=True)
|
||||
source_id = _source_identity(source)
|
||||
if not label.strip() or not method or not evidence:
|
||||
raise ValueError("A labelled candidate requires method and review evidence.")
|
||||
inputs = {
|
||||
POINTS: (Path(points), expected_points_sha256),
|
||||
TRAJECTORY: (Path(trajectory), expected_trajectory_sha256),
|
||||
**evidence,
|
||||
}
|
||||
if len(inputs) != len(evidence) + 2:
|
||||
raise ValueError("Evidence may not replace map geometry.")
|
||||
if any(
|
||||
not re.fullmatch(r"[a-zA-Z0-9][a-zA-Z0-9._-]*", name) or name == "manifest.json"
|
||||
for name in inputs
|
||||
):
|
||||
raise ValueError("Invalid artifact name.")
|
||||
# Only this private staging directory is cleaned up on failure.
|
||||
with TemporaryDirectory(prefix=".map-version-", dir=root) as temporary:
|
||||
stage = Path(temporary)
|
||||
artifacts = {}
|
||||
for name, (path, expected) in inputs.items():
|
||||
path = _plain_file(Path(path))
|
||||
_digest(expected)
|
||||
shutil.copyfile(path, stage / name)
|
||||
if sha256(stage / name) != expected or sha256(path) != expected:
|
||||
raise ValueError("Map input changed or failed its expected digest: " + name)
|
||||
artifacts[name] = dict(sha256=expected, bytes=(stage / name).stat().st_size)
|
||||
size = artifacts[POINTS]["bytes"]
|
||||
if not size or size % 12:
|
||||
raise ValueError("Map points must be a nonempty little-endian Nx3 float32 array.")
|
||||
arrays = _trajectory(stage / TRAJECTORY, size // 12)
|
||||
if len(arrays["positions"]) != len(source["poses"]):
|
||||
raise ValueError("Map pose indices must preserve original source ownership.")
|
||||
if not np.allclose(
|
||||
arrays["source_distance_m"],
|
||||
[p["distance_m"] for p in source["poses"]],
|
||||
rtol=0,
|
||||
atol=1e-7,
|
||||
):
|
||||
raise ValueError("Map source traversal does not match the selected recording.")
|
||||
# Chunked finiteness check; never allocate the full cloud twice.
|
||||
with (stage / POINTS).open("rb") as stream:
|
||||
while block := stream.read(12 * 65536):
|
||||
if not np.isfinite(np.frombuffer(block, dtype="<f4")).all():
|
||||
raise ValueError("Map contains non-finite points.")
|
||||
doc = dict(
|
||||
schema_version=SCHEMA,
|
||||
source=source_id,
|
||||
label=label.strip(),
|
||||
units="m",
|
||||
kind="corrected-map-candidate",
|
||||
method=method,
|
||||
authority=AUTHORITY,
|
||||
point_count=size // 12,
|
||||
pose_count=len(arrays["positions"]),
|
||||
frame_count=len(arrays["frames"]),
|
||||
path_m=float(arrays["distance_m"][-1]),
|
||||
artifacts=artifacts,
|
||||
)
|
||||
payload = json_bytes(doc)
|
||||
generation = hashlib.sha256(payload).hexdigest()
|
||||
target = root / generation
|
||||
(stage / "manifest.json").write_bytes(payload)
|
||||
if target.exists():
|
||||
MapVersion(target, generation).verify()
|
||||
else:
|
||||
# Renaming a complete directory makes partial candidates undiscoverable.
|
||||
stage.rename(target)
|
||||
return MapVersion(target, generation)
|
||||
|
||||
|
||||
class MapVersion:
|
||||
def __init__(self, directory, generation):
|
||||
self.directory = Path(directory)
|
||||
self.generation = _digest(generation)
|
||||
self.document = self._manifest()
|
||||
|
||||
def _manifest(self):
|
||||
path = _plain_file(self.directory / "manifest.json")
|
||||
payload = path.read_bytes()
|
||||
if hashlib.sha256(payload).hexdigest() != self.generation:
|
||||
raise ValueError("Map manifest identity changed.")
|
||||
doc = json.loads(payload)
|
||||
if (
|
||||
doc.get("schema_version") != SCHEMA
|
||||
or doc.get("units") != "m"
|
||||
or doc.get("authority") != AUTHORITY
|
||||
or doc.get("kind") != "corrected-map-candidate"
|
||||
or not {POINTS, TRAJECTORY}.issubset(doc.get("artifacts", {}))
|
||||
):
|
||||
raise ValueError("Unsupported map version contract or authority.")
|
||||
for name, artifact in doc["artifacts"].items():
|
||||
if (
|
||||
not re.fullmatch(r"[a-zA-Z0-9][a-zA-Z0-9._-]*", name)
|
||||
or name == "manifest.json"
|
||||
or type(artifact["bytes"]) is not int
|
||||
or artifact["bytes"] < 0
|
||||
):
|
||||
raise ValueError("Invalid map artifact metadata.")
|
||||
_digest(artifact["sha256"])
|
||||
return doc
|
||||
|
||||
def verify(self):
|
||||
doc = self._manifest()
|
||||
for name, meta in doc["artifacts"].items():
|
||||
path = _plain_file(self.directory / name)
|
||||
if path.stat().st_size != meta["bytes"] or sha256(path) != meta["sha256"]:
|
||||
raise ValueError("Map artifact identity changed: " + name)
|
||||
return doc
|
||||
|
||||
def planning_source(self, original):
|
||||
original = deepcopy(original)
|
||||
doc = self.verify()
|
||||
if _source_identity(original) != doc["source"]:
|
||||
raise ValueError("Map version belongs to another source generation.")
|
||||
arrays = _trajectory(self.directory / TRAJECTORY, doc["point_count"])
|
||||
self.verify()
|
||||
if len(original["poses"]) != len(arrays["positions"]):
|
||||
raise ValueError("Map and original pose ownership differ.")
|
||||
poses = [
|
||||
{**pose, "position": xyz.tolist(), "distance_m": float(distance)}
|
||||
for pose, xyz, distance in zip(
|
||||
original["poses"], arrays["positions"], arrays["distance_m"], strict=True
|
||||
)
|
||||
]
|
||||
return {
|
||||
**original,
|
||||
"poses": poses,
|
||||
"generation": self.generation,
|
||||
"frame_id": "map/" + self.generation,
|
||||
"label": doc["label"],
|
||||
"path_m": float(arrays["distance_m"][-1]),
|
||||
"reference_version": dict(
|
||||
schema_version=SCHEMA,
|
||||
source=doc["source"],
|
||||
authority=doc["authority"],
|
||||
method=doc["method"],
|
||||
),
|
||||
}
|
||||
|
||||
def arrays(self):
|
||||
"""Only use within a verified private snapshot for a multi-tile preparation."""
|
||||
return _trajectory(self.directory / TRAJECTORY, self.document["point_count"])
|
||||
@@ -0,0 +1,58 @@
|
||||
"""Read-only projection of complete, sealed per-frame map geometry.
|
||||
|
||||
The capture clock stays untouched. Map receipt times are relative to the first
|
||||
raw message, NOT to recording start or the first pose. No fitting occurs here.
|
||||
"""
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .map_version import POINTS, TRAJECTORY, _trajectory, sha256
|
||||
|
||||
|
||||
class RecordedMapGeometry:
|
||||
def __init__(self, artifacts):
|
||||
manifest = Path(artifacts["map-version-manifest.json"])
|
||||
self.generation = sha256(manifest)
|
||||
self.document = json.loads(manifest.read_text())
|
||||
paths = {name: Path(artifacts["map-version-" + name]) for name in (POINTS, TRAJECTORY)}
|
||||
for name, path in paths.items():
|
||||
meta = self.document["artifacts"][name]
|
||||
if path.stat().st_size != meta["bytes"] or sha256(path) != meta["sha256"]:
|
||||
raise ValueError("Corrected recording geometry failed integrity validation.")
|
||||
self.arrays = _trajectory(paths[TRAJECTORY], self.document["point_count"])
|
||||
self._points = np.memmap(paths[POINTS], dtype="<f4", mode="r").reshape(-1, 3)
|
||||
|
||||
@classmethod
|
||||
def optional(cls, artifacts):
|
||||
if not artifacts or not any(key.startswith("map-version-") for key in artifacts):
|
||||
return None
|
||||
return cls(artifacts)
|
||||
|
||||
def recording_id(self, original_id):
|
||||
return hashlib.sha256((original_id + ":map:" + self.generation).encode()).hexdigest()
|
||||
|
||||
def points(self, index, receipt_time_s, count):
|
||||
t, _sequence, offset, length = self.arrays["frames"][index]
|
||||
self._check_time(t, receipt_time_s)
|
||||
if length != count:
|
||||
raise ValueError("Corrected cloud point ownership mismatch.")
|
||||
return self._points[int(offset) : int(offset + length)]
|
||||
|
||||
def pose(self, index, receipt_time_s):
|
||||
self._check_time(self.arrays["receipt_time_s"][index], receipt_time_s)
|
||||
return tuple(self.arrays["positions"][index]), tuple(
|
||||
self.arrays["orientations_xyzw"][index]
|
||||
)
|
||||
|
||||
def complete(self, point_frames, poses):
|
||||
if point_frames != len(self.arrays["frames"]) or poses != len(self.arrays["positions"]):
|
||||
raise ValueError("Corrected recording does not cover all native frames.")
|
||||
|
||||
@staticmethod
|
||||
def _check_time(expected, actual):
|
||||
if not np.isfinite(actual) or abs(expected - actual) > 1e-6:
|
||||
raise ValueError("Corrected frame receipt clock mismatch.")
|
||||
@@ -0,0 +1,118 @@
|
||||
"""One explicit default map per physical session, shared by playback and planning.
|
||||
|
||||
Candidate publication is not activation. Activation copies an immutable reviewed
|
||||
bundle into durable application storage, then replaces a small selection pointer.
|
||||
This admits laboratory reference use, not autonomous vehicle control.
|
||||
"""
|
||||
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import shutil
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
from uuid import uuid4
|
||||
|
||||
from .map_version import MapVersion, _digest, json_bytes
|
||||
|
||||
SCHEMA = "missioncore.session-map-default/v1"
|
||||
|
||||
|
||||
class SessionMapVersions:
|
||||
def __init__(self, data_dir):
|
||||
self.root = Path(data_dir) / "session-map-versions"
|
||||
|
||||
def _selection(self, session_id):
|
||||
if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9._-]{0,127}", session_id):
|
||||
raise ValueError("Invalid session identity.")
|
||||
return self.root / "selected" / (session_id + ".json")
|
||||
|
||||
def version(self, session_id, generation):
|
||||
directory = self.root / "versions" / _digest(generation)
|
||||
if not directory.exists():
|
||||
return None
|
||||
if directory.is_symlink():
|
||||
raise ValueError("Map version directory may not be a link.")
|
||||
version = MapVersion(directory, generation)
|
||||
if version.document["source"]["session_id"] != session_id:
|
||||
raise ValueError("Map belongs to another session.")
|
||||
return version
|
||||
|
||||
def selected(self, session_id):
|
||||
path = self._selection(session_id)
|
||||
if not path.exists() and not path.is_symlink():
|
||||
return None
|
||||
if path.is_symlink() or path.stat().st_size > 8192:
|
||||
raise ValueError("Invalid map selection.")
|
||||
doc = json.loads(path.read_text())
|
||||
if doc.get("schema_version") != SCHEMA or doc.get("session_id") != session_id:
|
||||
raise ValueError("Invalid map selection identity.")
|
||||
version = self.version(session_id, doc["generation"])
|
||||
if version is None:
|
||||
raise ValueError("Selected corrected map is unavailable; original was not substituted.")
|
||||
return version
|
||||
|
||||
def activate(self, version, original_sources):
|
||||
"""Explicit operator admission; keep original captures and old map versions."""
|
||||
doc = version.verify()
|
||||
session_id = doc["source"]["session_id"]
|
||||
parent = original_sources.verify(session_id, doc["source"]["generation"])
|
||||
version.planning_source(parent)
|
||||
root = self.root / "versions"
|
||||
root.mkdir(parents=True, exist_ok=True)
|
||||
target = root / version.generation
|
||||
if not target.exists():
|
||||
with TemporaryDirectory(prefix=".admit-", dir=root) as temporary:
|
||||
stage = Path(temporary) / version.generation
|
||||
stage.mkdir()
|
||||
for name in ["manifest.json", *doc["artifacts"]]:
|
||||
shutil.copyfile(version.directory / name, stage / name)
|
||||
with (stage / name).open("rb") as stream:
|
||||
os.fsync(stream.fileno())
|
||||
MapVersion(stage, version.generation).verify()
|
||||
stage.rename(target)
|
||||
_sync_directory(root)
|
||||
self.version(session_id, version.generation).verify()
|
||||
original_sources.verify(session_id, doc["source"]["generation"])
|
||||
path = self._selection(session_id)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
candidate = path.with_name("." + uuid4().hex + ".json")
|
||||
try:
|
||||
candidate.write_bytes(
|
||||
json_bytes(
|
||||
dict(
|
||||
schema_version=SCHEMA,
|
||||
session_id=session_id,
|
||||
generation=version.generation,
|
||||
uses=["recorded-playback", "laboratory-reference"],
|
||||
vehicle_control=False,
|
||||
)
|
||||
)
|
||||
)
|
||||
with candidate.open("rb") as stream:
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(candidate, path)
|
||||
_sync_directory(path.parent)
|
||||
finally:
|
||||
candidate.unlink(missing_ok=True)
|
||||
return self.selected(session_id)
|
||||
|
||||
def resolve_replay(self, command):
|
||||
from k1link.sessions.models import ReplayMapVersion
|
||||
|
||||
# An already pinned launch remains pinned even if the default changes.
|
||||
if command.map_version is not None:
|
||||
return command
|
||||
version = self.selected(command.session_id)
|
||||
if version is None:
|
||||
return command
|
||||
return replace(command, map_version=ReplayMapVersion(version.directory, version.generation))
|
||||
|
||||
|
||||
def _sync_directory(path):
|
||||
descriptor = os.open(path, os.O_RDONLY)
|
||||
try:
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
@@ -0,0 +1,239 @@
|
||||
"""Experimental smooth correction of already mapped, provenance-bearing frames.
|
||||
|
||||
This is NOT a replacement LiDAR odometer or an automatic loop detector. A caller
|
||||
must supply independently checked registrations between disjoint source windows.
|
||||
The unknown C(s) maps original-map coordinates into a corrected map. Its six
|
||||
parameters are cubic splines over traveled distance: translation and a rotation
|
||||
vector. Every individual frame receives ONE rigid C(s), preserving its geometry.
|
||||
The original map/trajectory is never mutated. The first knot fixes the gauge;
|
||||
no constraint equates the first and last scanner positions.
|
||||
|
||||
Weights below are declared engineering regularizers, NOT calibrated covariance.
|
||||
The small-rotation chart is appropriate for the measured first experiment; this
|
||||
is not a qualified solution for arbitrary large drift or an arbitrary loop graph.
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.interpolate import CubicSpline
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CorrectionPolicy:
|
||||
version: str = "smooth-map-correction-experiment/v2"
|
||||
knot_spacing_m: float = 20.0
|
||||
smoothness_length_m: float = 30.0
|
||||
rotation_lever_m: float = 20.0
|
||||
strain_weight: float = 1.0
|
||||
maximum_evaluations: int = 100
|
||||
|
||||
def __post_init__(self):
|
||||
values = [
|
||||
self.knot_spacing_m,
|
||||
self.smoothness_length_m,
|
||||
self.rotation_lever_m,
|
||||
self.strain_weight,
|
||||
self.maximum_evaluations,
|
||||
]
|
||||
if not np.isfinite(values).all() or min(values) <= 0:
|
||||
raise ValueError("Correction policy values must be finite and positive.")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SurfaceLink:
|
||||
reference_distance_m: float
|
||||
query_distance_m: float
|
||||
T_reference_query: np.ndarray
|
||||
query_center: np.ndarray
|
||||
translation_weight_m: float
|
||||
rotation_weight_deg: float
|
||||
identity: str
|
||||
|
||||
def __post_init__(self):
|
||||
t = np.asarray(self.T_reference_query, dtype=float)
|
||||
c = np.asarray(self.query_center, dtype=float)
|
||||
if (
|
||||
t.shape != (4, 4)
|
||||
or not np.isfinite(t).all()
|
||||
or not np.allclose(t[3], [0, 0, 0, 1])
|
||||
or not np.allclose(t[:3, :3].T @ t[:3, :3], np.eye(3), atol=1e-7)
|
||||
or not np.isclose(np.linalg.det(t[:3, :3]), 1)
|
||||
or c.shape != (3,)
|
||||
or not np.isfinite(c).all()
|
||||
):
|
||||
raise ValueError("A surface link requires a rigid transform and finite center.")
|
||||
values = [
|
||||
self.reference_distance_m,
|
||||
self.query_distance_m,
|
||||
self.translation_weight_m,
|
||||
self.rotation_weight_deg,
|
||||
]
|
||||
if (
|
||||
not np.isfinite(values).all()
|
||||
or min(values[:2]) < 0
|
||||
or self.reference_distance_m == self.query_distance_m
|
||||
or min(values[2:]) <= 0
|
||||
or not self.identity
|
||||
):
|
||||
raise ValueError("Invalid link distances, weights or identity.")
|
||||
t, c = t.copy(), c.copy()
|
||||
t.setflags(write=False)
|
||||
c.setflags(write=False)
|
||||
object.__setattr__(self, "T_reference_query", t)
|
||||
object.__setattr__(self, "query_center", c)
|
||||
|
||||
|
||||
class CorrectionField:
|
||||
def __init__(self, knots, parameters, origin=None):
|
||||
self.origin = np.asarray(np.zeros(3) if origin is None else origin, dtype=float).copy()
|
||||
if self.origin.shape != (3,) or not np.isfinite(self.origin).all():
|
||||
raise ValueError("Correction origin must be a finite 3D point.")
|
||||
self.knots = np.asarray(knots, dtype=float).copy()
|
||||
self.parameters = np.asarray(parameters, dtype=float).copy()
|
||||
if (
|
||||
self.knots.ndim != 1
|
||||
or len(self.knots) < 2
|
||||
or not np.isfinite(self.knots).all()
|
||||
or (np.diff(self.knots) <= 0).any()
|
||||
or self.parameters.shape != (len(self.knots), 6)
|
||||
or not np.isfinite(self.parameters).all()
|
||||
):
|
||||
raise ValueError("Invalid correction knots or parameters.")
|
||||
self.spline = CubicSpline(self.knots, self.parameters, bc_type="natural")
|
||||
|
||||
def matrices(self, distance):
|
||||
s = np.atleast_1d(np.asarray(distance, dtype=float))
|
||||
if (
|
||||
s.ndim != 1
|
||||
or not np.isfinite(s).all()
|
||||
or (s < self.knots[0] - 1e-8).any()
|
||||
or (s > self.knots[-1] + 1e-8).any()
|
||||
):
|
||||
raise ValueError("Correction cannot extrapolate beyond its source route.")
|
||||
p = self.spline(np.clip(s, self.knots[0], self.knots[-1]))
|
||||
# Avoid silently wrapping the rotation-vector chart through pi.
|
||||
if (np.linalg.norm(p[:, 3:], axis=1) >= np.pi / 2).any():
|
||||
raise ValueError("Correction exceeds the experimental small-rotation chart.")
|
||||
result = np.repeat(np.eye(4)[None], len(s), axis=0)
|
||||
result[:, :3, :3] = Rotation.from_rotvec(p[:, 3:]).as_matrix()
|
||||
result[:, :3, 3] = (
|
||||
p[:, :3] + self.origin - np.einsum("nij,j->ni", result[:, :3, :3], self.origin)
|
||||
)
|
||||
return result
|
||||
|
||||
def points(self, points, distance):
|
||||
p = np.asarray(points, dtype=float)
|
||||
if p.ndim != 2 or p.shape[1] != 3 or not np.isfinite(p).all():
|
||||
raise ValueError("Expected finite Nx3 points.")
|
||||
c = self.matrices(distance)
|
||||
if len(c) not in (1, len(p)):
|
||||
raise ValueError("One correction per frame or per point is required.")
|
||||
return np.einsum("nij,nj->ni", c[:, :3, :3], p) + c[:, :3, 3]
|
||||
|
||||
def poses(self, positions, orientations_xyzw, distance):
|
||||
c = self.matrices(distance)
|
||||
q = np.asarray(orientations_xyzw, dtype=float)
|
||||
if q.shape != (len(c), 4) or not np.isfinite(q).all():
|
||||
raise ValueError("Pose orientations must match correction coordinates.")
|
||||
return (
|
||||
self.points(positions, distance),
|
||||
(Rotation.from_matrix(c[:, :3, :3]) * Rotation.from_quat(q)).as_quat(),
|
||||
)
|
||||
|
||||
|
||||
def fit_correction(length_m, links, policy=None):
|
||||
"""Fit one separately reviewed candidate; absence of constraints is an error."""
|
||||
policy = policy or CorrectionPolicy()
|
||||
if not np.isfinite(length_m) or length_m <= 0 or not links:
|
||||
raise ValueError("A positive route length and verified links are required.")
|
||||
if any(max(e.reference_distance_m, e.query_distance_m) > length_m for e in links):
|
||||
raise ValueError("Surface link lies outside the source route.")
|
||||
knots = np.linspace(0, length_m, max(2, int(np.ceil(length_m / policy.knot_spacing_m)) + 1))
|
||||
# Three-point quadrature exactly integrates the squared cubic derivatives.
|
||||
h = np.diff(knots)
|
||||
mid = (knots[:-1] + knots[1:]) / 2
|
||||
abscissa, weight = np.polynomial.legendre.leggauss(3)
|
||||
grid = (mid[:, None] + h[:, None] * abscissa / 2).ravel()
|
||||
root_weight = np.sqrt((h[:, None] * weight / 2).ravel())[:, None]
|
||||
unit_scale = np.array([1, 1, 1, *([policy.rotation_lever_m] * 3)])
|
||||
edge_s = np.array([[e.reference_distance_m, e.query_distance_m] for e in links])
|
||||
measured_r = Rotation.from_matrix(np.stack([e.T_reference_query[:3, :3] for e in links]))
|
||||
centers = np.stack([e.query_center for e in links])
|
||||
destinations = np.stack(
|
||||
[e.T_reference_query[:3, :3] @ e.query_center + e.T_reference_query[:3, 3] for e in links]
|
||||
)
|
||||
# Geometry-bound pivot: a change of world origin must not change regularization.
|
||||
origin = destinations[0].copy()
|
||||
centers, destinations = centers - origin, destinations - origin
|
||||
tw = np.array([e.translation_weight_m for e in links])[:, None]
|
||||
rw = np.deg2rad([e.rotation_weight_deg for e in links])[:, None]
|
||||
|
||||
def field_of(x):
|
||||
return CorrectionField(knots, np.vstack([np.zeros(6), x.reshape(-1, 6)]), origin)
|
||||
|
||||
def residual(x):
|
||||
field = field_of(x)
|
||||
values = field.spline(edge_s)
|
||||
a = Rotation.from_rotvec(values[:, 0, 3:])
|
||||
b = Rotation.from_rotvec(values[:, 1, 3:])
|
||||
displacement = (
|
||||
b.apply(centers) + values[:, 1, :3] - a.apply(destinations) - values[:, 0, :3]
|
||||
) / tw
|
||||
angular = ((a * measured_r).inv() * b).as_rotvec() / rw
|
||||
first = field.spline(grid, 1) * unit_scale * root_weight * policy.strain_weight
|
||||
second = (
|
||||
field.spline(grid, 2)
|
||||
* unit_scale
|
||||
* root_weight
|
||||
* policy.strain_weight
|
||||
* policy.smoothness_length_m
|
||||
)
|
||||
return np.r_[displacement.ravel(), angular.ravel(), first.ravel(), second.ravel()]
|
||||
|
||||
result = least_squares(
|
||||
residual,
|
||||
np.zeros((len(knots) - 1) * 6),
|
||||
max_nfev=policy.maximum_evaluations,
|
||||
x_scale="jac",
|
||||
ftol=1e-8,
|
||||
xtol=1e-8,
|
||||
gtol=1e-8,
|
||||
)
|
||||
field = field_of(result.x)
|
||||
# Check interpolation, not just control points, for forbidden rotations.
|
||||
field.matrices(np.linspace(0, length_m, max(100, len(knots) * 10)))
|
||||
remaining = residual(result.x)[: len(links) * 6]
|
||||
return field, {
|
||||
"schema_version": "missioncore.smooth-map-correction/v2",
|
||||
"origin_m": origin.tolist(),
|
||||
"policy": asdict(policy),
|
||||
"converged": bool(result.success),
|
||||
"solver_message": str(result.message),
|
||||
"evaluations": int(result.nfev),
|
||||
"cost": float(result.cost),
|
||||
"optimality": float(result.optimality),
|
||||
"knots_m": knots.tolist(),
|
||||
"parameters": field.parameters.tolist(),
|
||||
"link_residuals": [
|
||||
dict(
|
||||
identity=e.identity,
|
||||
translation_m=float(np.linalg.norm(remaining[i * 3 : i * 3 + 3]) * tw[i, 0]),
|
||||
rotation_deg=float(
|
||||
np.rad2deg(
|
||||
np.linalg.norm(
|
||||
remaining[len(links) * 3 + i * 3 : len(links) * 3 + i * 3 + 3]
|
||||
)
|
||||
* rw[i, 0]
|
||||
)
|
||||
),
|
||||
)
|
||||
for i, e in enumerate(links)
|
||||
],
|
||||
"weights_are_calibrated_covariances": False,
|
||||
"status": "candidate" if result.success else "solver-failed",
|
||||
"production_promotion": False,
|
||||
"vehicle_control": False,
|
||||
}
|
||||
Reference in New Issue
Block a user