252 lines
9.1 KiB
Python
252 lines
9.1 KiB
Python
"""
|
|
validate_map.py — Phase 1 acceptance harness for a SLAM-saved .ply map.
|
|
|
|
Run:
|
|
python3 -m Doc.scripts.validate_map /path/to/map.ply
|
|
[--min-points 10000] [--max-clusters 1] [--min-z-span 1.0]
|
|
|
|
On pass, writes `<map>.validated.json` next to the .ply. The GUI's
|
|
production-gate (in SLAM_GUI) checks for this sidecar before allowing
|
|
LIVE_NAV / EXTEND_MAP / LOCALIZE_MAP workflows on the map.
|
|
|
|
Checks performed:
|
|
1. Point count >= min_points
|
|
2. DBSCAN cluster count <= max_clusters (default 1 — fragmented maps
|
|
indicate SLAM tracking loss; should be remediated with MapRefiner
|
|
before validation)
|
|
3. Z-axis span (ceiling height) >= min_z_span
|
|
4. X/Y bounding-box has at least min_xy_span on the smaller axis
|
|
(rejects degenerate "point-on-a-line" scans)
|
|
|
|
Design notes:
|
|
- Like MapRefiner, the DBSCAN call runs in a subprocess so an Open3D
|
|
segfault doesn't crash the validator.
|
|
- The validation sidecar is intentionally small JSON so it can be
|
|
inspected, edited, or version-controlled if you need to override
|
|
a check.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import hashlib
|
|
import json
|
|
import os
|
|
import pickle
|
|
import subprocess
|
|
import sys
|
|
import tempfile
|
|
import time
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
|
|
# Subprocess worker — mirrors MapRefiner's crash-safe pattern.
|
|
def _worker_dbscan():
|
|
import open3d as o3d # only imported inside subprocess
|
|
in_path = sys.argv[3]
|
|
out_path = sys.argv[4]
|
|
with open(in_path, "rb") as f:
|
|
payload = pickle.load(f)
|
|
pcd = o3d.geometry.PointCloud()
|
|
pcd.points = o3d.utility.Vector3dVector(payload["points"])
|
|
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Error):
|
|
labels = np.array(pcd.cluster_dbscan(
|
|
eps=payload["eps"],
|
|
min_points=payload["min_points"],
|
|
print_progress=False,
|
|
))
|
|
with open(out_path, "wb") as f:
|
|
pickle.dump({"labels": labels}, f)
|
|
|
|
|
|
if len(sys.argv) > 2 and sys.argv[1] == "_worker" and sys.argv[2] == "dbscan":
|
|
_worker_dbscan()
|
|
sys.exit(0)
|
|
|
|
|
|
def _run_dbscan_subprocess(points: np.ndarray, eps: float, min_pts: int,
|
|
timeout_s: int = 90) -> np.ndarray | None:
|
|
"""Run DBSCAN in an isolated subprocess. Returns labels or None on
|
|
timeout/error."""
|
|
with tempfile.TemporaryDirectory() as tmp:
|
|
in_p = os.path.join(tmp, "in.pkl")
|
|
out_p = os.path.join(tmp, "out.pkl")
|
|
with open(in_p, "wb") as fh:
|
|
pickle.dump(
|
|
{"points": points, "eps": eps, "min_points": min_pts},
|
|
fh,
|
|
)
|
|
try:
|
|
r = subprocess.run(
|
|
[sys.executable, __file__, "_worker", "dbscan", in_p, out_p],
|
|
timeout=timeout_s,
|
|
capture_output=True,
|
|
)
|
|
except subprocess.TimeoutExpired:
|
|
print(f"[validate_map] DBSCAN timed out after {timeout_s}s",
|
|
file=sys.stderr)
|
|
return None
|
|
if r.returncode != 0 or not os.path.exists(out_p):
|
|
err = r.stderr.decode(errors="replace")[-300:] if r.stderr else "(no stderr)"
|
|
print(f"[validate_map] DBSCAN subprocess exit {r.returncode}: {err}",
|
|
file=sys.stderr)
|
|
return None
|
|
with open(out_p, "rb") as fh:
|
|
return pickle.load(fh)["labels"]
|
|
|
|
|
|
def _read_ply_xyz(path: str) -> np.ndarray:
|
|
"""Read XYZ from PLY (ASCII or binary little-endian). No Open3D dep
|
|
in the parent process — keeps the validator robust against Open3D
|
|
segfaults during read."""
|
|
with open(path, "rb") as f:
|
|
header_lines = []
|
|
while True:
|
|
line = f.readline()
|
|
if not line:
|
|
raise ValueError("Unexpected EOF in PLY header")
|
|
header_lines.append(line)
|
|
if line.strip() == b"end_header":
|
|
break
|
|
header = b"".join(header_lines)
|
|
text = header.decode("ascii", errors="replace")
|
|
is_binary = "format binary_little_endian" in text
|
|
n_vertex = 0
|
|
props = []
|
|
in_vertex = False
|
|
for line in text.splitlines():
|
|
if line.startswith("element vertex"):
|
|
n_vertex = int(line.split()[-1])
|
|
in_vertex = True
|
|
continue
|
|
if line.startswith("element ") and in_vertex:
|
|
break
|
|
if in_vertex and line.startswith("property"):
|
|
parts = line.split()
|
|
props.append((parts[1], parts[2]))
|
|
if n_vertex == 0:
|
|
raise ValueError("No vertex element in PLY")
|
|
|
|
if is_binary:
|
|
type_map = {
|
|
"float": ("f4", 4), "float32": ("f4", 4), "double": ("f8", 8),
|
|
"uchar": ("u1", 1), "char": ("i1", 1),
|
|
"ushort": ("u2", 2), "short": ("i2", 2),
|
|
"uint": ("u4", 4), "int": ("i4", 4),
|
|
}
|
|
dtype_list = []
|
|
for t, name in props:
|
|
if t not in type_map:
|
|
raise ValueError(f"Unknown PLY type: {t}")
|
|
dtype_list.append((name, "<" + type_map[t][0]))
|
|
arr = np.frombuffer(f.read(), dtype=np.dtype(dtype_list), count=n_vertex)
|
|
return np.column_stack([arr["x"], arr["y"], arr["z"]]).astype(np.float64)
|
|
# ASCII fallback
|
|
x_idx = next(i for i, (_, n) in enumerate(props) if n == "x")
|
|
y_idx = next(i for i, (_, n) in enumerate(props) if n == "y")
|
|
z_idx = next(i for i, (_, n) in enumerate(props) if n == "z")
|
|
xyz = np.zeros((n_vertex, 3), dtype=np.float64)
|
|
for i in range(n_vertex):
|
|
row = f.readline().decode("ascii").split()
|
|
xyz[i] = [float(row[x_idx]), float(row[y_idx]), float(row[z_idx])]
|
|
return xyz
|
|
|
|
|
|
def _md5(path: str) -> str:
|
|
h = hashlib.md5()
|
|
with open(path, "rb") as fh:
|
|
for chunk in iter(lambda: fh.read(1 << 20), b""):
|
|
h.update(chunk)
|
|
return h.hexdigest()
|
|
|
|
|
|
def validate(
|
|
ply_path: str,
|
|
*,
|
|
min_points: int = 10_000,
|
|
max_clusters: int = 1,
|
|
min_z_span: float = 1.0,
|
|
min_xy_span: float = 1.5,
|
|
dbscan_eps: float = 0.30,
|
|
dbscan_min_pts: int = 50,
|
|
) -> dict:
|
|
"""Run all checks. Returns a dict with `passed: bool` plus per-check
|
|
diagnostics. Caller decides whether to write the sidecar."""
|
|
result: dict = {
|
|
"path": str(ply_path),
|
|
"validated_at": time.strftime("%Y-%m-%dT%H:%M:%S"),
|
|
"checks": {},
|
|
"passed": False,
|
|
}
|
|
|
|
pts = _read_ply_xyz(ply_path)
|
|
n = len(pts)
|
|
result["checks"]["point_count"] = {"value": int(n), "min": min_points, "ok": n >= min_points}
|
|
|
|
if n < min_points:
|
|
result["passed"] = False
|
|
return result
|
|
|
|
x_span = float(pts[:, 0].max() - pts[:, 0].min())
|
|
y_span = float(pts[:, 1].max() - pts[:, 1].min())
|
|
z_span = float(pts[:, 2].max() - pts[:, 2].min())
|
|
smaller_xy = min(x_span, y_span)
|
|
result["checks"]["z_span_m"] = {"value": z_span, "min": min_z_span, "ok": z_span >= min_z_span}
|
|
result["checks"]["xy_span_m"] = {"value": smaller_xy, "min": min_xy_span, "ok": smaller_xy >= min_xy_span}
|
|
|
|
labels = _run_dbscan_subprocess(pts, dbscan_eps, dbscan_min_pts)
|
|
if labels is None:
|
|
result["checks"]["clusters"] = {"value": None, "max": max_clusters, "ok": False, "error": "dbscan failed"}
|
|
result["passed"] = False
|
|
return result
|
|
n_clusters = int(len({int(l) for l in labels if l >= 0}))
|
|
result["checks"]["clusters"] = {"value": n_clusters, "max": max_clusters, "ok": n_clusters <= max_clusters}
|
|
|
|
result["md5"] = _md5(ply_path)
|
|
result["passed"] = all(c.get("ok", False) for c in result["checks"].values())
|
|
return result
|
|
|
|
|
|
def write_sidecar(ply_path: str, validation: dict) -> str:
|
|
"""Write `<ply_path>.validated.json`. The path format matches what
|
|
SLAM_GUI's `_map_is_validated` checks for."""
|
|
out = Path(ply_path).with_suffix(Path(ply_path).suffix + ".validated.json")
|
|
out.write_text(json.dumps(validation, indent=2), encoding="utf-8")
|
|
return str(out)
|
|
|
|
|
|
def main() -> int:
|
|
p = argparse.ArgumentParser(description="Validate a SLAM-saved .ply map.")
|
|
p.add_argument("ply", help="Path to the .ply map file.")
|
|
p.add_argument("--min-points", type=int, default=10_000)
|
|
p.add_argument("--max-clusters", type=int, default=1)
|
|
p.add_argument("--min-z-span", type=float, default=1.0)
|
|
p.add_argument("--min-xy-span", type=float, default=1.5)
|
|
p.add_argument("--force", action="store_true", help="Write sidecar even if checks fail.")
|
|
args = p.parse_args()
|
|
|
|
if not os.path.exists(args.ply):
|
|
print(f"File not found: {args.ply}", file=sys.stderr)
|
|
return 2
|
|
|
|
res = validate(
|
|
args.ply,
|
|
min_points=args.min_points,
|
|
max_clusters=args.max_clusters,
|
|
min_z_span=args.min_z_span,
|
|
min_xy_span=args.min_xy_span,
|
|
)
|
|
|
|
print(json.dumps(res, indent=2))
|
|
if res["passed"] or args.force:
|
|
out = write_sidecar(args.ply, res)
|
|
print(f"sidecar written: {out}")
|
|
return 0
|
|
print("validation FAILED — sidecar NOT written (use --force to override)", file=sys.stderr)
|
|
return 1
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|