227 lines
8.7 KiB
Python
227 lines
8.7 KiB
Python
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
from pathlib import Path
|
|
from typing import Any, Dict, List, Optional, Sequence
|
|
|
|
import numpy as np
|
|
|
|
from SLAM_engine import load_slam_config
|
|
from SLAM_Filter import VoxelPersistenceFilter, FilterConfig as PersistFilterConfig
|
|
from SLAM_Navigation import NavigationExportConfig, NavigationExporter
|
|
|
|
|
|
_LIDAR_FRAME_DT = 0.10 # MID-360 outputs at 10 Hz → 0.10 s per frame
|
|
|
|
|
|
def _voxel_downsample_numpy(points: np.ndarray, voxel: float) -> np.ndarray:
|
|
if points is None or len(points) == 0:
|
|
return np.zeros((0, 3), dtype=np.float32)
|
|
if voxel <= 0:
|
|
return np.asarray(points, dtype=np.float32)
|
|
pts = np.asarray(points, dtype=np.float32)
|
|
keys = np.floor(pts / float(voxel)).astype(np.int32)
|
|
uniq, inv = np.unique(keys, axis=0, return_inverse=True)
|
|
out = np.zeros((len(uniq), 3), dtype=np.float64)
|
|
cnt = np.zeros((len(uniq),), dtype=np.int64)
|
|
np.add.at(out, inv, pts.astype(np.float64, copy=False))
|
|
np.add.at(cnt, inv, 1)
|
|
out /= np.maximum(cnt[:, None], 1)
|
|
return out.astype(np.float32)
|
|
|
|
|
|
def _transform_points(points: np.ndarray, pose: np.ndarray) -> np.ndarray:
|
|
p = np.asarray(points, dtype=np.float32)
|
|
tf = np.asarray(pose, dtype=np.float32)
|
|
if tf.shape != (4, 4):
|
|
return p
|
|
r = tf[:3, :3]
|
|
t = tf[:3, 3]
|
|
return (p @ r.T) + t
|
|
|
|
|
|
def load_replay_npz(path: str | Path) -> tuple[List[np.ndarray], Optional[List[np.ndarray]]]:
|
|
p = Path(path)
|
|
data = np.load(str(p), allow_pickle=True)
|
|
if "frames" not in data:
|
|
raise RuntimeError("Replay file must contain 'frames'.")
|
|
frames_raw = data["frames"]
|
|
frames: List[np.ndarray] = []
|
|
if isinstance(frames_raw, np.ndarray) and frames_raw.dtype == object:
|
|
for fr in frames_raw:
|
|
frames.append(np.asarray(fr, dtype=np.float32))
|
|
else:
|
|
arr = np.asarray(frames_raw, dtype=np.float32)
|
|
if arr.ndim == 3 and arr.shape[2] >= 3:
|
|
for i in range(arr.shape[0]):
|
|
frames.append(arr[i, :, :3].astype(np.float32))
|
|
else:
|
|
raise RuntimeError("Unsupported 'frames' format in replay npz.")
|
|
|
|
poses = None
|
|
if "poses" in data:
|
|
poses_raw = np.asarray(data["poses"])
|
|
if poses_raw.ndim == 3 and poses_raw.shape[1:] == (4, 4):
|
|
poses = [poses_raw[i].astype(np.float32) for i in range(poses_raw.shape[0])]
|
|
return frames, poses
|
|
|
|
|
|
def run_replay(
|
|
frames: Sequence[np.ndarray],
|
|
poses: Optional[Sequence[np.ndarray]] = None,
|
|
cfg: Optional[Dict[str, Any]] = None,
|
|
export_nav: bool = False,
|
|
nav_base_name: str = "replay_nav",
|
|
) -> Dict[str, Any]:
|
|
config = cfg or load_slam_config()
|
|
filt_cfg = PersistFilterConfig(
|
|
voxel_size=float(config["filter"]["voxel_size"]),
|
|
hit_threshold=int(config["filter"]["hits_threshold"]),
|
|
decay_seconds=float(config["filter"]["persistence"]["decay_seconds"]),
|
|
max_voxels=int(config["filter"]["persistence"]["max_voxels"]),
|
|
)
|
|
filt = VoxelPersistenceFilter(filt_cfg)
|
|
|
|
map_cfg = config.get("map", {})
|
|
loc_cfg = config.get("localization", {})
|
|
nav_cfg = NavigationExportConfig.from_dict(config.get("navigation_export", {}))
|
|
|
|
maps_dir = config.get("app", {}).get("maps_dir", "DataMap")
|
|
out_dir = Path(maps_dir).expanduser()
|
|
if not out_dir.is_absolute():
|
|
out_dir = (Path(__file__).resolve().parent / out_dir).resolve()
|
|
out_dir.mkdir(parents=True, exist_ok=True)
|
|
nav_exporter = NavigationExporter(nav_cfg, str(out_dir))
|
|
|
|
for i, frame in enumerate(frames):
|
|
pts = np.asarray(frame, dtype=np.float32)
|
|
if len(pts) == 0:
|
|
continue
|
|
if poses is not None and i < len(poses):
|
|
pts = _transform_points(pts, np.asarray(poses[i], dtype=np.float32))
|
|
filt.update(pts, now=float(i) * _LIDAR_FRAME_DT)
|
|
|
|
stable_world = filt.get_stable_points()
|
|
stable_save = _voxel_downsample_numpy(stable_world, float(map_cfg.get("save_voxel", 0.06)))
|
|
stable_n = int(len(stable_save))
|
|
|
|
min_map = int(map_cfg.get("min_points_to_save", 550))
|
|
min_loc = int(loc_cfg.get("min_points_for_localize", 550))
|
|
min_nav = int(nav_cfg.min_points)
|
|
nav_band_n = int(nav_exporter.count_nav_points(stable_save))
|
|
|
|
report: Dict[str, Any] = {
|
|
"frames": int(len(frames)),
|
|
"stable_points": stable_n,
|
|
"map_export_pass": bool(stable_n >= min_map),
|
|
"localization_ready": bool(stable_n >= min_loc),
|
|
"nav_band_points": nav_band_n,
|
|
"nav_export_pass": bool(nav_band_n >= min_nav),
|
|
"thresholds": {
|
|
"map": int(min_map),
|
|
"localization": int(min_loc),
|
|
"nav": int(min_nav),
|
|
},
|
|
}
|
|
|
|
if poses is not None and len(poses) >= 2:
|
|
path_len = 0.0
|
|
p0 = np.asarray(poses[0], dtype=np.float64)
|
|
p_last = np.asarray(poses[len(poses) - 1], dtype=np.float64)
|
|
prev = p0
|
|
for i in range(1, len(poses)):
|
|
cur = np.asarray(poses[i], dtype=np.float64)
|
|
path_len += float(np.linalg.norm(cur[:3, 3] - prev[:3, 3]))
|
|
prev = cur
|
|
disp = float(np.linalg.norm(p_last[:3, 3] - p0[:3, 3]))
|
|
eff = 0.0 if path_len <= 1e-9 else float(disp / path_len)
|
|
report["motion_kpi"] = {
|
|
"path_length_m": float(path_len),
|
|
"net_displacement_m": float(disp),
|
|
"path_efficiency": float(eff),
|
|
}
|
|
|
|
if export_nav and report["nav_export_pass"]:
|
|
nav_out = nav_exporter.export(nav_base_name, stable_save)
|
|
report["nav_export"] = nav_out
|
|
|
|
return report
|
|
|
|
|
|
def compare_reports(current: Dict[str, Any], baseline: Dict[str, Any], tol_ratio: float = 0.15) -> Dict[str, Any]:
|
|
checks: List[Dict[str, Any]] = []
|
|
|
|
def add_numeric(name: str):
|
|
cur = float(current.get(name, 0.0))
|
|
base = float(baseline.get(name, 0.0))
|
|
if abs(base) < 1e-9:
|
|
ok = abs(cur - base) < 1e-9
|
|
ratio = 0.0
|
|
else:
|
|
ratio = abs(cur - base) / abs(base)
|
|
ok = ratio <= float(tol_ratio)
|
|
checks.append({"metric": name, "ok": bool(ok), "current": cur, "baseline": base, "ratio": ratio})
|
|
|
|
for key in ("stable_points", "nav_band_points"):
|
|
add_numeric(key)
|
|
|
|
cur_motion = current.get("motion_kpi", {}) if isinstance(current, dict) else {}
|
|
base_motion = baseline.get("motion_kpi", {}) if isinstance(baseline, dict) else {}
|
|
if isinstance(cur_motion, dict) and isinstance(base_motion, dict):
|
|
for key in ("path_length_m", "net_displacement_m", "path_efficiency"):
|
|
if key in cur_motion and key in base_motion:
|
|
cur = float(cur_motion.get(key, 0.0))
|
|
base = float(base_motion.get(key, 0.0))
|
|
if abs(base) < 1e-9:
|
|
ok = abs(cur - base) < 1e-9
|
|
ratio = 0.0
|
|
else:
|
|
ratio = abs(cur - base) / abs(base)
|
|
ok = ratio <= float(tol_ratio)
|
|
checks.append(
|
|
{
|
|
"metric": f"motion_kpi.{key}",
|
|
"ok": bool(ok),
|
|
"current": cur,
|
|
"baseline": base,
|
|
"ratio": ratio,
|
|
}
|
|
)
|
|
|
|
for key in ("map_export_pass", "localization_ready", "nav_export_pass"):
|
|
cur = bool(current.get(key, False))
|
|
base = bool(baseline.get(key, False))
|
|
checks.append({"metric": key, "ok": bool(cur == base), "current": cur, "baseline": base})
|
|
|
|
passed = all(bool(c.get("ok", False)) for c in checks)
|
|
return {"passed": bool(passed), "checks": checks}
|
|
|
|
|
|
def main():
|
|
ap = argparse.ArgumentParser(description="Offline SLAM replay + regression report")
|
|
ap.add_argument("--input", required=True, help="Replay .npz containing frames (and optional poses)")
|
|
ap.add_argument("--out", default="", help="Optional output report JSON path")
|
|
ap.add_argument("--baseline", default="", help="Optional baseline report JSON to compare against")
|
|
ap.add_argument("--tol-ratio", type=float, default=0.15, help="Relative tolerance for numeric metrics")
|
|
ap.add_argument("--export-nav", action="store_true", help="Also export nav map if replay passes")
|
|
args = ap.parse_args()
|
|
|
|
frames, poses = load_replay_npz(args.input)
|
|
report = run_replay(frames, poses=poses, export_nav=bool(args.export_nav))
|
|
|
|
out: Dict[str, Any] = {"report": report}
|
|
if args.baseline:
|
|
base = json.loads(Path(args.baseline).read_text(encoding="utf-8"))
|
|
base_rep = base.get("report", base) if isinstance(base, dict) else {}
|
|
out["comparison"] = compare_reports(report, base_rep, tol_ratio=float(args.tol_ratio))
|
|
|
|
text = json.dumps(out, indent=2)
|
|
print(text)
|
|
if args.out:
|
|
Path(args.out).write_text(text, encoding="utf-8")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|