From 773f37e582cb52f59bf8ce66e4b080bfa55df8d3 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 11:47:19 -0700 Subject: [PATCH 01/29] Remove unused code --- .../navigation/nav_3d/evaluator/blueprints.py | 117 ------------ .../navigation/nav_3d/evaluator/evaluator.py | 150 --------------- .../nav_3d/evaluator/mesh_loader.py | 77 -------- .../navigation/nav_3d/evaluator/scenarios.py | 171 ------------------ 4 files changed, 515 deletions(-) delete mode 100644 dimos/navigation/nav_3d/evaluator/blueprints.py delete mode 100644 dimos/navigation/nav_3d/evaluator/evaluator.py delete mode 100644 dimos/navigation/nav_3d/evaluator/mesh_loader.py delete mode 100644 dimos/navigation/nav_3d/evaluator/scenarios.py diff --git a/dimos/navigation/nav_3d/evaluator/blueprints.py b/dimos/navigation/nav_3d/evaluator/blueprints.py deleted file mode 100644 index 3e8fc27f1e..0000000000 --- a/dimos/navigation/nav_3d/evaluator/blueprints.py +++ /dev/null @@ -1,117 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Blueprint for the path-planner evaluator. - -Wires the Evaluator and MLSPlannerNative together and bridges all streams to rerun. -Run with:: - - dimos run path-planner-eval -""" - -from __future__ import annotations - -import numpy as np -import rerun as rr -from rerun._baseclasses import Archetype - -from dimos.core.coordination.blueprints import autoconnect -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.cmu_nav.modules.click_start_goal_router.click_start_goal_router import ( - ClickStartGoalRouter, -) -from dimos.navigation.nav_3d.evaluator.evaluator import Evaluator -from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative -from dimos.visualization.rerun.bridge import RerunBridgeModule -from dimos.visualization.rerun.websocket_server import RerunWebSocketServer - -_POSE_MARKER_RADIUS = 0.4 -# Small lift so graph artifacts render visibly above the surface points instead of z-fighting. -_GRAPH_Z_LIFT = 0.05 - - -def _render_start_pose(msg: PoseStamped) -> Archetype: - return rr.Points3D( - positions=[[msg.x, msg.y, msg.z]], - colors=[[0, 255, 0]], - radii=[_POSE_MARKER_RADIUS], - ) - - -def _render_goal_pose(msg: PoseStamped) -> Archetype: - return rr.Points3D( - positions=[[msg.x, msg.y, msg.z]], - colors=[[255, 0, 0]], - radii=[_POSE_MARKER_RADIUS], - ) - - -def _render_global_map(msg: PointCloud2) -> Archetype: - return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) - - -def _render_surface_map(msg: PointCloud2) -> Archetype: - return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130]) - - -def _render_nodes(msg: PointCloud2) -> Archetype: - pts, _ = msg.as_numpy() - if pts is None or len(pts) == 0: - return rr.Points3D([]) - pts = pts.copy() - pts[:, 2] += _GRAPH_Z_LIFT - return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) - - -def _render_node_edges(msg: LineSegments3D) -> Archetype: - """Color each segment by its safe-adj weight on a log-scale green->red gradient.""" - if not msg._segments: - return rr.LineStrips3D([]) - weights = np.asarray(msg._traversability, dtype=np.float64) - log_w = np.log10(np.maximum(weights, 1e-6)) - lo, hi = float(log_w.min()), float(log_w.max()) - norm = (log_w - lo) / (hi - lo) if hi > lo else np.zeros_like(log_w) - r = (255 * norm).astype(np.uint8) - g = (255 * (1.0 - norm)).astype(np.uint8) - b = np.full_like(r, 60) - a = np.full_like(r, 220) - colors = np.column_stack([r, g, b, a]) - strips = [ - [ - [p1[0], p1[1], p1[2] + _GRAPH_Z_LIFT], - [p2[0], p2[1], p2[2] + _GRAPH_Z_LIFT], - ] - for p1, p2 in msg._segments - ] - return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) - - -path_planner_eval = autoconnect( - Evaluator.blueprint(), - MLSPlannerNative.blueprint(), - ClickStartGoalRouter.blueprint(), - RerunWebSocketServer.blueprint(), - RerunBridgeModule.blueprint( - visual_override={ - "world/start_pose": _render_start_pose, - "world/goal_pose": _render_goal_pose, - "world/global_map": _render_global_map, - "world/surface_map": _render_surface_map, - "world/nodes": _render_nodes, - "world/node_edges": _render_node_edges, - } - ), -) diff --git a/dimos/navigation/nav_3d/evaluator/evaluator.py b/dimos/navigation/nav_3d/evaluator/evaluator.py deleted file mode 100644 index 502bf1a7a6..0000000000 --- a/dimos/navigation/nav_3d/evaluator/evaluator.py +++ /dev/null @@ -1,150 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Evaluate path planner modules with a set of loaded scenes. - -Sends out global map, start pose, goal pose, and listens for -paths. Then evaluate each path for various metrics. -""" - -from __future__ import annotations - -import asyncio -from collections.abc import AsyncGenerator -from dataclasses import dataclass -import time -from typing import Any - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_3d.evaluator.scenarios import ( - PlannerScenario, - default_scenarios, -) -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -@dataclass -class ScenarioResult: - name: str - expected_path: bool - got_path: bool - passed: bool - - -class EvaluatorConfig(ModuleConfig): - input_publish_delay: float = 0.2 - # Max seconds to wait for the planner's path reply per scenario. - path_timeout: float = 2.0 - # Pause between scenes - scenario_dwell: float = 2.0 - - -class Evaluator(Module): - """Drives a fixed scenario sequence through a black-box planner.""" - - config: EvaluatorConfig - - global_map: Out[PointCloud2] - start_pose: Out[PoseStamped] - goal_pose: Out[PoseStamped] - path: In[Path] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._latest_path: Path | None = None - self._path_received: asyncio.Event | None = None - self._eval_task: asyncio.Task[None] | None = None - - async def main(self) -> AsyncGenerator[None, None]: - self._path_received = asyncio.Event() - self._eval_task = asyncio.create_task(self._run_eval()) - yield - if self._eval_task is not None and not self._eval_task.done(): - self._eval_task.cancel() - try: - await self._eval_task - except asyncio.CancelledError: - pass - - async def handle_path(self, msg: Path) -> None: - self._latest_path = msg - if self._path_received is not None: - self._path_received.set() - - async def _run_eval(self) -> None: - scenarios = default_scenarios() - results: list[ScenarioResult] = [] - logger.info("Evaluator starting", scenarios=len(scenarios)) - await asyncio.sleep(1.0) - for scenario in scenarios: - result = await self._run_one(scenario) - results.append(result) - await asyncio.sleep(self.config.scenario_dwell) - self._log_summary(results) - - async def _run_one(self, scenario: PlannerScenario) -> ScenarioResult: - logger.info("Scenario start", name=scenario.name, expect_path=scenario.expect_path) - assert self._path_received is not None - - now = time.time() - scenario.global_map.ts = now - scenario.start_pose.ts = now - scenario.goal_pose.ts = now - - self.global_map.publish(scenario.global_map) - await asyncio.sleep(self.config.input_publish_delay) - self.start_pose.publish(scenario.start_pose) - await asyncio.sleep(self.config.input_publish_delay) - self.goal_pose.publish(scenario.goal_pose) - - self._latest_path = None - self._path_received.clear() - - try: - await asyncio.wait_for(self._path_received.wait(), timeout=self.config.path_timeout) - got_path = self._latest_path is not None and len(self._latest_path) > 0 - except asyncio.TimeoutError: - got_path = False - - passed = got_path == scenario.expect_path - logger.info( - "Scenario result", - name=scenario.name, - expected=scenario.expect_path, - got=got_path, - passed=passed, - ) - return ScenarioResult( - name=scenario.name, - expected_path=scenario.expect_path, - got_path=got_path, - passed=passed, - ) - - def _log_summary(self, results: list[ScenarioResult]) -> None: - n_pass = sum(1 for r in results if r.passed) - logger.info("Evaluation complete", passed=n_pass, total=len(results)) - for r in results: - logger.info( - " " + ("PASS" if r.passed else "FAIL"), - scenario=r.name, - expected=r.expected_path, - got=r.got_path, - ) diff --git a/dimos/navigation/nav_3d/evaluator/mesh_loader.py b/dimos/navigation/nav_3d/evaluator/mesh_loader.py deleted file mode 100644 index 5f2e5f76bb..0000000000 --- a/dimos/navigation/nav_3d/evaluator/mesh_loader.py +++ /dev/null @@ -1,77 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Load a 3D mesh, sample points on its surfaces, voxel-downsample.""" - -from __future__ import annotations - -from pathlib import Path - -import numpy as np -import open3d as o3d # type: ignore[import-untyped] - -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -def load_voxelized_mesh( - path: str | Path, - voxel_size: float = 0.1, - num_sample_points: int = 2_000_000, - swap_y_up_to_z_up: bool = True, - recenter: bool = True, -) -> np.ndarray: - """Load a mesh file, sample its surface, voxel-downsample. - - GLB/glTF files use Y-up; we rotate to Z-up so the result drops into the - planner's frame. ``recenter`` translates so the XY bbox is centered on - the origin and the floor (minimum z) sits at z=0. - """ - mesh = o3d.io.read_triangle_mesh(str(path)) - if len(mesh.vertices) == 0: - raise ValueError(f"Mesh {path!r} has no vertices") - logger.info( - "Mesh loaded", - path=str(path), - vertices=len(mesh.vertices), - triangles=len(mesh.triangles), - ) - - o3d.utility.random.seed(42) - pcd = mesh.sample_points_uniformly(number_of_points=num_sample_points) - points = np.asarray(pcd.points) - - if swap_y_up_to_z_up: - # 90 deg rotation around X: (x, y, z) to (x, -z, y). - points = np.column_stack([points[:, 0], -points[:, 2], points[:, 1]]) - - if recenter: - xy_center = (points[:, :2].max(axis=0) + points[:, :2].min(axis=0)) / 2 - points[:, :2] -= xy_center - points[:, 2] -= points[:, 2].min() - - # Snap each occupied cell to its voxel-grid center, with the grid - # anchored at world origin so cells line up cleanly across scenarios. - quantized = (np.floor(points / voxel_size) + 0.5) * voxel_size - centers = np.unique(quantized, axis=0).astype(np.float32) - - logger.info( - "Voxelized mesh ready", - voxels=len(centers), - voxel_size=voxel_size, - bbox_min=centers.min(axis=0).round(2).tolist(), - bbox_max=centers.max(axis=0).round(2).tolist(), - ) - return centers diff --git a/dimos/navigation/nav_3d/evaluator/scenarios.py b/dimos/navigation/nav_3d/evaluator/scenarios.py deleted file mode 100644 index 992c7c5104..0000000000 --- a/dimos/navigation/nav_3d/evaluator/scenarios.py +++ /dev/null @@ -1,171 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Sample point clouds for path planning.""" - -from __future__ import annotations - -from dataclasses import dataclass -import os -from pathlib import Path - -import numpy as np - -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_3d.evaluator.mesh_loader import load_voxelized_mesh -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -WORLD_FRAME = "map" -_WALL_HEIGHT_M = 2.0 -_WALL_THICKNESS_M = 0.5 - -MESH_PATH = os.environ.get("MESH_PATH") - - -@dataclass -class PlannerScenario: - name: str - global_map: PointCloud2 - start_pose: PoseStamped - goal_pose: PoseStamped - expect_path: bool - - -def _pose(x: float, y: float, z: float = 0.0, frame: str = WORLD_FRAME) -> PoseStamped: - return PoseStamped( - frame_id=frame, - position=[x, y, z], - orientation=[0.0, 0.0, 0.0, 1.0], - ) - - -def _wall( - x0: float, - y0: float, - x1: float, - y1: float, - *, - spacing: float = 0.1, - height: float = _WALL_HEIGHT_M, - thickness: float = _WALL_THICKNESS_M, - frame: str = WORLD_FRAME, -) -> PointCloud2: - """Sample a vertical wall as a 3D box from (x0,y0) to (x1,y1). - - Thickness extends perpendicular to the wall line in the XY plane. - """ - dx, dy = x1 - x0, y1 - y0 - length = float(np.hypot(dx, dy)) - if length == 0: - return PointCloud2.from_numpy( - np.zeros((0, 3), dtype=np.float32), frame_id=frame, timestamp=0.0 - ) - perp_x, perp_y = -dy / length, dx / length - along = np.linspace(0.0, 1.0, max(2, int(np.ceil(length / spacing)))) - perp = np.linspace(-thickness / 2, thickness / 2, max(1, int(np.ceil(thickness / spacing)) + 1)) - zs = np.linspace(0.0, height, max(2, int(np.ceil(height / spacing)))) - a, p, z = np.meshgrid(along, perp, zs, indexing="ij") - x = x0 + a.ravel() * dx + p.ravel() * perp_x - y = y0 + a.ravel() * dy + p.ravel() * perp_y - pts = np.column_stack([x, y, z.ravel()]).astype(np.float32) - return PointCloud2.from_numpy(pts, frame_id=frame, timestamp=0.0) - - -def _floor( - x_min: float = -2.0, - x_max: float = 8.0, - y_min: float = -3.0, - y_max: float = 3.0, - spacing: float = 0.25, - frame: str = WORLD_FRAME, -) -> PointCloud2: - """Flat ground plane sampled as points at z=0.""" - xs = np.arange(x_min, x_max + spacing, spacing) - ys = np.arange(y_min, y_max + spacing, spacing) - grid_xs, grid_ys = np.meshgrid(xs, ys) - pts = np.column_stack([grid_xs.ravel(), grid_ys.ravel(), np.zeros(grid_xs.size)]).astype( - np.float32 - ) - return PointCloud2.from_numpy(pts, frame_id=frame, timestamp=0.0) - - -def _map_with_walls(*walls: PointCloud2) -> PointCloud2: - return sum(walls, _floor()) - - -def empty_floor() -> PlannerScenario: - return PlannerScenario( - name="empty_floor", - global_map=_floor(), - start_pose=_pose(-1.0, 0.0, 0.2), - goal_pose=_pose(7.0, 0.0, 0.2), - expect_path=True, - ) - - -def blocked_wall() -> PlannerScenario: - return PlannerScenario( - name="blocked_wall", - global_map=_map_with_walls(_wall(3.0, -3.0, 3.0, 3.0)), - start_pose=_pose(-1.0, 0.0, 0.2), - goal_pose=_pose(6.0, 0.0, 0.2), - expect_path=False, - ) - - -def two_rooms_one_door() -> PlannerScenario: - return PlannerScenario( - name="two_rooms_one_door", - global_map=_map_with_walls( - _wall(3.0, -3.0, 3.0, -0.75), - _wall(3.0, 0.75, 3.0, 3.0), - ), - start_pose=_pose(-1.0, 0.0, 0.2), - goal_pose=_pose(6.0, 0.0, 0.2), - expect_path=True, - ) - - -def _mesh_scenarios() -> list[PlannerScenario]: - """Two scenarios on a real building mesh: ground-level traverse and a stair climb.""" - if MESH_PATH is None: - logger.info("MESH_PATH not set, skipping mesh scenarios") - return [] - if not Path(MESH_PATH).is_file(): - logger.warning("Mesh file not found, skipping mesh scenarios", path=MESH_PATH) - return [] - points = load_voxelized_mesh(MESH_PATH).astype(np.float32) - return [ - PlannerScenario( - name="mesh_outside", - global_map=PointCloud2.from_numpy(points, frame_id=WORLD_FRAME, timestamp=0.0), - start_pose=_pose(-20.45, -19.85, 1.75), - goal_pose=_pose(21.95, -4.25, 1.75), - expect_path=True, - ), - PlannerScenario( - name="mesh_up_the_stairs", - global_map=PointCloud2.from_numpy(points, frame_id=WORLD_FRAME, timestamp=0.0), - start_pose=_pose(7.15, -3.55, 2.05), - goal_pose=_pose(5.55, -2.05, 5.65), - expect_path=True, - ), - ] - - -def default_scenarios() -> list[PlannerScenario]: - return [empty_floor(), blocked_wall(), two_rooms_one_door(), *_mesh_scenarios()] From b6842b3c82cb65716ec63cc03a7c663ed09d0d73 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 14:00:06 -0700 Subject: [PATCH 02/29] Evaluator framework --- dimos/navigation/nav_3d/evaluator/__main__.py | 17 + dimos/navigation/nav_3d/evaluator/cases.py | 88 +++++ .../evaluator/cases/mid360_athens_stairs.yaml | 27 ++ dimos/navigation/nav_3d/evaluator/cli.py | 122 +++++++ dimos/navigation/nav_3d/evaluator/config.py | 96 ++++++ dimos/navigation/nav_3d/evaluator/golden.py | 224 +++++++++++++ dimos/navigation/nav_3d/evaluator/metrics.py | 148 +++++++++ .../navigation/nav_3d/evaluator/recording.py | 111 +++++++ dimos/navigation/nav_3d/evaluator/runner.py | 311 ++++++++++++++++++ .../nav_3d/evaluator/test_evaluator.py | 152 +++++++++ dimos/navigation/nav_3d/evaluator/viz.py | 206 ++++++++++++ dimos/robot/all_blueprints.py | 1 - 12 files changed, 1502 insertions(+), 1 deletion(-) create mode 100644 dimos/navigation/nav_3d/evaluator/__main__.py create mode 100644 dimos/navigation/nav_3d/evaluator/cases.py create mode 100644 dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml create mode 100644 dimos/navigation/nav_3d/evaluator/cli.py create mode 100644 dimos/navigation/nav_3d/evaluator/config.py create mode 100644 dimos/navigation/nav_3d/evaluator/golden.py create mode 100644 dimos/navigation/nav_3d/evaluator/metrics.py create mode 100644 dimos/navigation/nav_3d/evaluator/recording.py create mode 100644 dimos/navigation/nav_3d/evaluator/runner.py create mode 100644 dimos/navigation/nav_3d/evaluator/test_evaluator.py create mode 100644 dimos/navigation/nav_3d/evaluator/viz.py diff --git a/dimos/navigation/nav_3d/evaluator/__main__.py b/dimos/navigation/nav_3d/evaluator/__main__.py new file mode 100644 index 0000000000..d8f8c66af0 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/__main__.py @@ -0,0 +1,17 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.navigation.nav_3d.evaluator.cli import app + +app() diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py new file mode 100644 index 0000000000..cdcb9aea5d --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -0,0 +1,88 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Case manifests for the nav-3d evaluator. + +A suite is one YAML file per dataset under cases/. Start and goal are +foot-level world coordinates, the frame the planner consumes. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from pathlib import Path + +import yaml + +CASES_DIR = Path(__file__).parent / "cases" + + +@dataclass +class Case: + id: str + start: tuple[float, float, float] + goal: tuple[float, float, float] + weight: float = 1.0 + tags: list[str] = field(default_factory=list) + l_ref: float | None = None + + +@dataclass +class Suite: + dataset: str + cases: list[Case] + lidar_stream: str = "pointlio_lidar" + odom_stream: str = "pointlio_odometry" + path: Path | None = None + + +def load_suite(path: Path) -> Suite: + raw = yaml.safe_load(path.read_text()) + if not isinstance(raw, dict) or "dataset" not in raw or "cases" not in raw: + raise ValueError(f"{path}: suite needs 'dataset' and 'cases' keys") + cases = [] + seen: set[str] = set() + for entry in raw["cases"]: + if len(entry["start"]) != 3 or len(entry["goal"]) != 3: + raise ValueError(f"{path}: case {entry['id']}: start/goal must be xyz") + sx, sy, sz = (float(v) for v in entry["start"]) + gx, gy, gz = (float(v) for v in entry["goal"]) + case = Case( + id=str(entry["id"]), + start=(sx, sy, sz), + goal=(gx, gy, gz), + weight=float(entry.get("weight", 1.0)), + tags=[str(t) for t in entry.get("tags", [])], + l_ref=float(entry["l_ref"]) if "l_ref" in entry else None, + ) + if case.id in seen: + raise ValueError(f"{path}: duplicate case id {case.id}") + seen.add(case.id) + cases.append(case) + return Suite( + dataset=str(raw["dataset"]), + cases=cases, + lidar_stream=str(raw.get("lidar_stream", "pointlio_lidar")), + odom_stream=str(raw.get("odom_stream", "pointlio_odometry")), + path=path, + ) + + +def load_suites(paths: list[Path] | None = None) -> list[Suite]: + """Load the given manifests, or every manifest under cases/.""" + if paths is None: + paths = sorted(CASES_DIR.glob("*.yaml")) + if not paths: + raise FileNotFoundError(f"no case manifests found under {CASES_DIR}") + return [load_suite(p) for p in paths] diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml new file mode 100644 index 0000000000..97a6da82db --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -0,0 +1,27 @@ +dataset: mid360_athens_stairs +cases: + - id: flat_ground_floor + start: [-0.01, 0.01, -0.30] + goal: [1.00, -4.77, -0.27] + weight: 1 + tags: [flat] + - id: stairs_down_two_flights + start: [4.86, -4.58, -0.29] + goal: [6.40, -3.70, -6.49] + weight: 2 + tags: [stairs, down] + - id: stairs_up_from_basement + start: [6.40, -3.70, -6.49] + goal: [7.04, -3.58, 0.20] + weight: 2 + tags: [stairs, up] + - id: basement_to_top_floor + start: [7.16, -3.78, -6.07] + goal: [5.89, -1.60, 3.06] + weight: 3 + tags: [stairs, up, long] + - id: ground_to_top_floor + start: [-2.49, -1.20, -0.33] + goal: [5.86, -4.21, 3.02] + weight: 3 + tags: [stairs, up, long] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py new file mode 100644 index 0000000000..778a5002cc --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -0,0 +1,122 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Nav-3d evaluation CLI. + +Run every suite: python -m dimos.navigation.nav_3d.evaluator run +One dataset: python -m dimos.navigation.nav_3d.evaluator run --dataset stairs60_a +Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json +Override a knob: python -m dimos.navigation.nav_3d.evaluator run --set wall_clearance_m=0.05 +""" + +from __future__ import annotations + +import dataclasses +import json +from pathlib import Path + +import typer + +from dimos.navigation.nav_3d.evaluator.cases import load_suites +from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate + +app = typer.Typer(no_args_is_help=True, add_completion=False) + + +def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: + fields = {f.name: f.type for f in dataclasses.fields(EvalConfig)} + for spec in overrides: + if "=" not in spec: + raise typer.BadParameter(f"--set expects name=value, got {spec!r}") + name, value = spec.split("=", 1) + if name not in fields: + raise typer.BadParameter(f"unknown config field {name!r}") + current = getattr(cfg, name) + setattr(cfg, name, type(current)(value)) + return cfg + + +def _print_report(report: Report) -> None: + header = f"{'case':<28} {'dataset':<22} {'attr':<8} {'spl':>5} {'len':>6} {'ref':>6} {'ms':>7}" + print(header) + print("-" * len(header)) + for d in report.datasets: + for c in d.cases: + print( + f"{c.id:<28} {c.dataset:<22} {c.attribution:<8} " + f"{c.online.spl:>5.2f} {c.online.length:>6.1f} {c.l_ref:>6.1f} " + f"{c.online.plan_ms:>7.1f}" + ) + print("-" * len(header)) + for d in report.datasets: + print( + f"{d.dataset}: {d.frames} frames, online {d.online_voxels} / " + f"golden {d.golden_voxels} voxels, " + f"false-obstacle rate {d.false_obstacle_rate:.3f}, " + f"map build {d.map_build_ms / 1000:.1f}s" + ) + print( + f"\nscore {report.score:.3f} | soft {report.score_soft:.3f} | " + f"planner {report.planner_score:.3f} | " + f"success {report.n_success}/{report.n_cases} | " + f"attribution {report.attribution_counts} | " + f"plan p95 {report.plan_ms['p95']:.1f}ms" + ) + + +@app.command() +def run( + manifests: list[Path] = typer.Argument( + None, help="Suite YAMLs; defaults to every manifest under cases/" + ), + dataset: str = typer.Option(None, "--dataset", help="Only run suites for this dataset"), + json_out: Path = typer.Option(None, "--json", help="Write the full report as JSON"), + rrd_out: Path = typer.Option(None, "--rrd", help="Write a rerun recording of every case"), + workers: int = typer.Option(1, "--workers", help="Datasets evaluated in parallel processes"), + set_: list[str] = typer.Option( + None, "--set", help="Repeatable EvalConfig override, e.g. wall_clearance_m=0.05" + ), +) -> None: + suites = load_suites(manifests or None) + if dataset is not None: + suites = [s for s in suites if s.dataset == dataset] + if not suites: + raise typer.BadParameter(f"no suite for dataset {dataset!r}") + cfg = _apply_overrides(EvalConfig(), set_ or []) + report = evaluate(suites, cfg, workers=workers) + _print_report(report) + if json_out is not None: + json_out.write_text(json.dumps(report.to_dict(), indent=2)) + print(f"wrote {json_out}") + if rrd_out is not None: + from dimos.navigation.nav_3d.evaluator.viz import write_rrd + + write_rrd(report, suites, cfg, rrd_out) + + +@app.command("list") +def list_cases() -> None: + for suite in load_suites(): + print(f"{suite.dataset} ({suite.path.name if suite.path else '?'})") + for case in suite.cases: + tags = f" [{', '.join(case.tags)}]" if case.tags else "" + print( + f" {case.id}: {tuple(round(v, 2) for v in case.start)} -> " + f"{tuple(round(v, 2) for v in case.goal)} w={case.weight:g}{tags}" + ) + + +if __name__ == "__main__": + app() diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py new file mode 100644 index 0000000000..f8348c8744 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -0,0 +1,96 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from dataclasses import dataclass + +from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper +from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + + +@dataclass +class EvalConfig: + """Mapper, planner, and gate parameters. Defaults mirror production.""" + + voxel_size: float = 0.08 + max_range: float = 30.0 + ray_subsample: int = 1 + shadow_depth: float = 0.1 + grace_depth: float = 0.2 + min_health: int = -1 + max_health: int = 5 + graze_cos: float = 0.7 + support_min: int = 4 + + robot_height: float = 0.3 + max_overhead_m: float = 2.0 + surface_closing_radius: float = 0.3 + node_spacing_m: float = 1.0 + wall_clearance_m: float = 0.1 + wall_buffer_m: float = 0.75 + wall_buffer_weight: float = 100.0 + step_threshold_m: float = 0.16 + step_penalty_weight: float = 4.0 + + # Physical body envelope for the collision gate. The gate catches paths + # that penetrate obstacles, not near-grazes, so the radius is the true + # body half-width. The ground margin over the radius bounds the terrain + # slope the gate tolerates; keep margin/radius above the steepest stairs. + robot_radius: float = 0.16 + ground_margin: float = 0.25 + body_clearance: float = 0.45 + goal_tolerance: float = 0.5 + align_tol: float = 0.05 + + def make_mapper(self) -> VoxelRayMapper: + return VoxelRayMapper( + voxel_size=self.voxel_size, + max_range=self.max_range, + ray_subsample=self.ray_subsample, + shadow_depth=self.shadow_depth, + grace_depth=self.grace_depth, + min_health=self.min_health, + max_health=self.max_health, + graze_cos=self.graze_cos, + support_min=self.support_min, + ) + + def make_planner(self) -> MLSPlanner: + return MLSPlanner( + voxel_size=self.voxel_size, + robot_height=self.robot_height, + max_overhead_m=self.max_overhead_m, + surface_closing_radius=self.surface_closing_radius, + node_spacing_m=self.node_spacing_m, + wall_clearance_m=self.wall_clearance_m, + wall_buffer_m=self.wall_buffer_m, + wall_buffer_weight=self.wall_buffer_weight, + step_threshold_m=self.step_threshold_m, + step_penalty_weight=self.step_penalty_weight, + ) + + def mapper_fingerprint(self) -> dict[str, float | int]: + """The mapper parameters that determine golden map content.""" + return { + "voxel_size": self.voxel_size, + "max_range": self.max_range, + "ray_subsample": self.ray_subsample, + "shadow_depth": self.shadow_depth, + "grace_depth": self.grace_depth, + "min_health": self.min_health, + "max_health": self.max_health, + "graze_cos": self.graze_cos, + "support_min": self.support_min, + } diff --git a/dimos/navigation/nav_3d/evaluator/golden.py b/dimos/navigation/nav_3d/evaluator/golden.py new file mode 100644 index 0000000000..9c46ca9382 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/golden.py @@ -0,0 +1,224 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Golden reference map: full-recording voxel map plus walked-corridor free space. + +The golden occupancy is what returned paths are collision-checked against. +The walked corridor marks voxels the robot's body physically swept, which are +free space regardless of what any mapper claims. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import itertools +import json +from time import perf_counter +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from pathlib import Path + + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.cases import Suite + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + from dimos.navigation.nav_3d.evaluator.recording import Trajectory + +logger = setup_logger() + +_KEY_OFFSET = 1 << 20 + + +def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: + """Pack voxel indices into sortable int64 keys, one per point.""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET + return (idx[:, 0] << 42) | (idx[:, 1] << 21) | idx[:, 2] + + +def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: + """Voxel center positions for packed keys, the inverse of voxel_keys.""" + mask = (1 << 21) - 1 + idx = np.stack([keys >> 42, (keys >> 21) & mask, keys & mask], axis=1) - _KEY_OFFSET + return ((idx + 0.5) * voxel_size).astype(np.float32) + + +def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: + if len(sorted_keys) == 0: + return np.zeros(len(query), dtype=bool) + pos = np.clip(np.searchsorted(sorted_keys, query), 0, len(sorted_keys) - 1) + return np.asarray(sorted_keys[pos] == query) + + +def cylinder_offsets( + radius: float, z_lo: float, z_hi: float, voxel_size: float +) -> NDArray[np.int64]: + """Integer voxel offsets forming a vertical cylinder.""" + r_vox = int(np.ceil(radius / voxel_size)) + span = np.arange(-r_vox, r_vox + 1) + dx, dy = np.meshgrid(span, span, indexing="ij") + in_disc = (dx * voxel_size) ** 2 + (dy * voxel_size) ** 2 <= radius**2 + dz = np.arange(int(np.floor(z_lo / voxel_size)), int(np.ceil(z_hi / voxel_size)) + 1) + disc = np.stack([dx[in_disc], dy[in_disc]], axis=1) + out = np.concatenate([np.hstack([disc, np.full((len(disc), 1), z)]) for z in dz]) + return np.asarray(out, dtype=np.int64) + + +def offset_keys( + points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float +) -> NDArray[np.int64]: + """Keys of every (point voxel + offset) pair, shape (P * O,).""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET + swept = idx[:, None, :] + offsets[None, :, :] + return np.asarray((swept[..., 0] << 42) | (swept[..., 1] << 21) | swept[..., 2]) + + +def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: + """Resample a polyline so consecutive samples are at most step apart.""" + if len(points) < 2: + return points.astype(np.float32) + out = [points[:1]] + for a, b in itertools.pairwise(points): + seg = np.linalg.norm(b - a) + n = max(int(np.ceil(seg / step)), 1) + t = np.linspace(0.0, 1.0, n + 1)[1:, None] + out.append(a[None, :] * (1 - t) + b[None, :] * t) + return np.concatenate(out).astype(np.float32) + + +def walked_corridor_keys( + trajectory: Trajectory, + voxel_size: float, + radius: float, + z_lo: float, + z_hi: float, +) -> NDArray[np.int64]: + """Voxels swept by the robot body cylinder along the trajectory, sorted. + + z_lo and z_hi are relative to the odometry pose. The carved volume must + cover the collision gate's checked volume, or the walked path itself + fails the gate. + """ + dense = densify(trajectory.positions, voxel_size / 2) + offsets = cylinder_offsets(radius, z_lo, z_hi, voxel_size) + return np.unique(offset_keys(dense, offsets, voxel_size)) + + +@dataclass +class GoldenMap: + voxel_size: float + occupied: NDArray[np.float32] + occupied_keys: NDArray[np.int64] + walked_keys: NDArray[np.int64] + frames: int + add_frame_ms: dict[str, float] + build_ms: float + + def obstacle_keys(self) -> NDArray[np.int64]: + """Occupied minus walked-free, the set paths must not intersect.""" + return np.setdiff1d(self.occupied_keys, self.walked_keys, assume_unique=True) + + +CACHE_VERSION = 2 + + +def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: + digest = hashlib.sha1(json.dumps(params, sort_keys=True).encode()).hexdigest()[:10] + return db_path.parent / ".golden" / f"{db_path.stem}.{digest}.npz" + + +def load_or_build_golden( + db_path: Path, + suite: Suite, + cfg: EvalConfig, + corridor_radius: float, + corridor_z_lo: float, + corridor_z_hi: float, +) -> GoldenMap: + params: dict[str, float | int | str] = { + **cfg.mapper_fingerprint(), + "corridor_radius": corridor_radius, + "corridor_z_lo": corridor_z_lo, + "corridor_z_hi": corridor_z_hi, + "align_tol": cfg.align_tol, + "lidar_stream": suite.lidar_stream, + "odom_stream": suite.odom_stream, + "cache_version": CACHE_VERSION, + } + voxel_size = cfg.voxel_size + cache = _cache_path(db_path, params) + if cache.exists(): + data = np.load(cache) + return GoldenMap( + voxel_size=voxel_size, + occupied=data["occupied"], + occupied_keys=data["occupied_keys"], + walked_keys=data["walked_keys"], + frames=int(data["frames"]), + add_frame_ms={ + "p50": float(data["add_p50"]), + "p95": float(data["add_p95"]), + "max": float(data["add_max"]), + }, + build_ms=0.0, + ) + + logger.info("building golden map for %s (cache miss)", db_path.name) + mapper = cfg.make_mapper() + add_ms: list[float] = [] + t0 = perf_counter() + for frame in iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol): + t1 = perf_counter() + mapper.add_frame(frame.points, frame.origin) + add_ms.append((perf_counter() - t1) * 1000) + build_ms = (perf_counter() - t0) * 1000 + add_arr = np.asarray(add_ms) if add_ms else np.zeros(1) + occupied = mapper.global_map() + occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) + trajectory = load_trajectory(db_path, suite.odom_stream) + walked = walked_corridor_keys( + trajectory, voxel_size, corridor_radius, corridor_z_lo, corridor_z_hi + ) + + cache.parent.mkdir(exist_ok=True) + np.savez_compressed( + cache, + occupied=occupied, + occupied_keys=occupied_keys, + walked_keys=walked, + frames=len(add_ms), + add_p50=np.percentile(add_arr, 50), + add_p95=np.percentile(add_arr, 95), + add_max=add_arr.max(), + ) + logger.info("golden map cached: %s (%d voxels)", cache.name, len(occupied)) + return GoldenMap( + voxel_size=voxel_size, + occupied=occupied, + occupied_keys=occupied_keys, + walked_keys=walked, + frames=len(add_ms), + add_frame_ms={ + "p50": float(np.percentile(add_arr, 50)), + "p95": float(np.percentile(add_arr, 95)), + "max": float(add_arr.max()), + }, + build_ms=build_ms, + ) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py new file mode 100644 index 0000000000..e3cc8ed996 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -0,0 +1,148 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Scoring for the nav-3d evaluator: SPL, the path validity gate, references.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.golden import ( + cylinder_offsets, + densify, + key_centers, + keys_contain, + offset_keys, +) + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.recording import Trajectory + + +def path_length(waypoints: NDArray[np.float32]) -> float: + if len(waypoints) < 2: + return 0.0 + return float(np.linalg.norm(np.diff(waypoints, axis=0), axis=1).sum()) + + +def goal_reached( + waypoints: NDArray[np.float32], goal: tuple[float, float, float], tolerance: float +) -> bool: + return bool(np.linalg.norm(waypoints[-1] - np.asarray(goal, dtype=np.float32)) <= tolerance) + + +@dataclass +class GateResult: + """Collision check of a path against the golden obstacle set.""" + + valid: bool + collision_points: NDArray[np.float32] + + +def check_path( + waypoints: NDArray[np.float32], + obstacle_keys: NDArray[np.int64], + voxel_size: float, + robot_radius: float, + ground_margin: float, + body_clearance: float, +) -> GateResult: + """Sweep the robot body along foot-level waypoints against golden obstacles. + + The checked volume at each sample is a cylinder from ground_margin above + the foot (so the supporting floor never counts) up to body_clearance. + Candidate voxels come from a padded voxelized cylinder and are then + verified against the exact continuous bounds, so quantization never pulls + ground voxels into the check. + """ + samples = densify(waypoints, voxel_size / 2) + offsets = cylinder_offsets( + robot_radius + voxel_size, + ground_margin - voxel_size, + body_clearance + voxel_size, + voxel_size, + ) + keys = offset_keys(samples, offsets, voxel_size) + candidate = keys_contain(obstacle_keys, keys.ravel()).reshape(keys.shape) + s_idx, o_idx = np.nonzero(candidate) + if len(s_idx) == 0: + return GateResult(valid=True, collision_points=samples[:0]) + delta = key_centers(keys[s_idx, o_idx], voxel_size) - samples[s_idx] + exact = ( + (np.linalg.norm(delta[:, :2], axis=1) <= robot_radius) + & (delta[:, 2] >= ground_margin) + & (delta[:, 2] <= body_clearance) + ) + colliding = np.unique(s_idx[exact]) + return GateResult(valid=len(colliding) == 0, collision_points=samples[colliding]) + + +def reference_length( + trajectory: Trajectory, + start: tuple[float, float, float], + goal: tuple[float, float, float], + robot_height: float, + max_snap_m: float = 1.0, +) -> tuple[float, bool]: + """Walked-trajectory length between the poses nearest start and goal. + + Returns (length, snapped). When either endpoint is farther than max_snap_m + from the trajectory, falls back to the straight-line distance. + """ + foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) + s = np.asarray(start, dtype=np.float32) + g = np.asarray(goal, dtype=np.float32) + ds = np.linalg.norm(foot - s, axis=1) + dg = np.linalg.norm(foot - g, axis=1) + i, j = int(ds.argmin()), int(dg.argmin()) + if ds[i] > max_snap_m or dg[j] > max_snap_m: + return float(np.linalg.norm(g - s)), False + arcs = trajectory.arc_lengths() + length = abs(float(arcs[j] - arcs[i])) + float(ds[i]) + float(dg[j]) + return max(length, 1e-6), True + + +def spl(success: bool, l_ref: float, p_len: float) -> float: + if not success: + return 0.0 + return l_ref / max(p_len, l_ref) + + +def soft_progress( + end: NDArray[np.float32] | None, + start: tuple[float, float, float], + goal: tuple[float, float, float], +) -> float: + """Fraction of the start-goal distance covered by the path endpoint.""" + d0 = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) + if end is None or d0 < 1e-6: + return 0.0 + d1 = float(np.linalg.norm(np.asarray(goal, dtype=np.float32) - end)) + return float(np.clip(1.0 - d1 / d0, 0.0, 1.0)) + + +def timing_stats(samples_ms: list[float]) -> dict[str, float]: + if not samples_ms: + return {"p50": 0.0, "p95": 0.0, "max": 0.0} + arr = np.asarray(samples_ms) + return { + "p50": float(np.percentile(arr, 50)), + "p95": float(np.percentile(arr, 95)), + "max": float(arr.max()), + } diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py new file mode 100644 index 0000000000..2c20415615 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -0,0 +1,111 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Read lidar+odometry recordings into world-frame frames and a trajectory.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +if TYPE_CHECKING: + from collections.abc import Iterator + from pathlib import Path + + from numpy.typing import NDArray + + +@dataclass +class Frame: + ts: float + points: NDArray[np.float32] + origin: tuple[float, float, float] + + +@dataclass +class Trajectory: + """Odometry poses ordered by time. Positions are sensor-level, (T, 3) float32.""" + + ts: NDArray[np.float64] + positions: NDArray[np.float32] + + def arc_lengths(self) -> NDArray[np.float64]: + """Cumulative walked distance at each pose, starting at 0.""" + steps = np.linalg.norm(np.diff(self.positions, axis=0), axis=1) + return np.concatenate([[0.0], np.cumsum(steps)]) + + +def iter_world_frames( + db_path: Path, + lidar_stream: str, + odom_stream: str, + align_tol: float = 0.05, +) -> Iterator[Frame]: + """Yield lidar frames registered into the world by their aligned odometry pose. + + Clouds must be sensor-frame. Legacy recordings with pre-registered + world-frame clouds are rejected; re-record them. + """ + store = SqliteStore(path=str(db_path)) + with store: + lidar = store.stream(lidar_stream, PointCloud2).order_by("ts") + odom = store.stream(odom_stream, Odometry).order_by("ts") + for pair_obs in lidar.align(odom, tolerance=align_tol): + lidar_obs, odom_obs = pair_obs.data + if lidar_obs.data.frame_id == "world": + raise ValueError( + f"{db_path}: stream {lidar_stream!r} has pre-registered world-frame " + "clouds; this legacy format is not supported for evaluation" + ) + o = odom_obs.data + mat = Transform( + translation=Vector3(o.position.x, o.position.y, o.position.z), + rotation=Quaternion( + o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w + ), + ).to_matrix() + rot = mat[:3, :3].astype(np.float32) + trans = mat[:3, 3].astype(np.float32) + pts = lidar_obs.data.points_f32() @ rot.T + trans + yield Frame( + ts=lidar_obs.ts, + points=pts, + origin=(float(o.position.x), float(o.position.y), float(o.position.z)), + ) + + +def load_trajectory(db_path: Path, odom_stream: str) -> Trajectory: + store = SqliteStore(path=str(db_path)) + ts: list[float] = [] + positions: list[tuple[float, float, float]] = [] + with store: + for obs in store.stream(odom_stream, Odometry).order_by("ts"): + o = obs.data + ts.append(obs.ts) + positions.append((float(o.position.x), float(o.position.y), float(o.position.z))) + if not positions: + raise ValueError(f"{db_path}: no odometry in stream {odom_stream!r}") + return Trajectory( + ts=np.asarray(ts, dtype=np.float64), + positions=np.asarray(positions, dtype=np.float32), + ) diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py new file mode 100644 index 0000000000..b1956665da --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -0,0 +1,311 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Run case suites through the ray tracer and MLS planner and score them. + +Every case is planned twice: on the golden map (planner ceiling) and on the +map the online mapper built from the recording (end to end). Both paths must +pass the golden collision gate. The headline score is validity-gated SPL on +the online map. +""" + +from __future__ import annotations + +from concurrent.futures import ProcessPoolExecutor +from dataclasses import asdict, dataclass, field +import itertools +from time import perf_counter +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator import metrics +from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.golden import ( + keys_contain, + load_or_build_golden, + voxel_keys, +) +from dimos.navigation.nav_3d.evaluator.recording import load_trajectory +from dimos.utils.data import resolve_named_path +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.cases import Case, Suite + from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + +logger = setup_logger() + +MAX_COLLISIONS_KEPT = 50 + + +@dataclass +class PlanOutcome: + planned: bool + reached: bool + valid: bool + length: float + plan_ms: float + spl: float + waypoints: list[list[float]] + collisions: list[list[float]] + + @property + def success(self) -> bool: + return self.planned and self.reached and self.valid + + +@dataclass +class CaseResult: + id: str + dataset: str + start: tuple[float, float, float] + goal: tuple[float, float, float] + weight: float + tags: list[str] + l_ref: float + l_ref_snapped: bool + online: PlanOutcome + golden: PlanOutcome + attribution: str + soft_progress: float + + +@dataclass +class PlannerArtifacts: + """Graph state of one planner after its map update. Not serialized to JSON.""" + + surface_clearance: NDArray[np.float32] + nodes: NDArray[np.float32] + edges: NDArray[np.float32] + + +@dataclass +class DatasetResult: + dataset: str + cases: list[CaseResult] + walked_path_valid: bool + false_obstacle_rate: float + online_voxels: int + golden_voxels: int + map_build_ms: float + add_frame_ms: dict[str, float] + frames: int + online_artifacts: PlannerArtifacts | None = None + golden_artifacts: PlannerArtifacts | None = None + + +@dataclass +class Report: + score: float + score_soft: float + planner_score: float + false_obstacle_rate: float + n_cases: int + n_success: int + attribution_counts: dict[str, int] + plan_ms: dict[str, float] + datasets: list[DatasetResult] + config: dict[str, float | int] = field(default_factory=dict) + + def to_dict(self) -> dict[str, object]: + out = asdict(self) + for dataset in out["datasets"]: + dataset.pop("online_artifacts") + dataset.pop("golden_artifacts") + return out + + +def _run_plan( + planner: MLSPlanner, + case: Case, + l_ref: float, + obstacle_keys: NDArray[np.int64], + cfg: EvalConfig, +) -> tuple[PlanOutcome, NDArray[np.float32] | None]: + t0 = perf_counter() + waypoints = planner.plan(case.start, case.goal) + plan_ms = (perf_counter() - t0) * 1000 + if waypoints is None or len(waypoints) == 0: + return PlanOutcome(False, False, False, 0.0, plan_ms, 0.0, [], []), None + + reached = metrics.goal_reached(waypoints, case.goal, cfg.goal_tolerance) + gate = metrics.check_path( + waypoints, + obstacle_keys, + cfg.voxel_size, + cfg.robot_radius, + cfg.ground_margin, + cfg.body_clearance, + ) + length = metrics.path_length(waypoints) + success = reached and gate.valid + outcome = PlanOutcome( + planned=True, + reached=reached, + valid=gate.valid, + length=length, + plan_ms=plan_ms, + spl=metrics.spl(success, l_ref, length), + waypoints=waypoints.tolist(), + collisions=gate.collision_points[:MAX_COLLISIONS_KEPT].tolist(), + ) + return outcome, waypoints + + +def _attribution(online: PlanOutcome, golden: PlanOutcome) -> str: + if online.success: + return "ok" + if not golden.success: + return "planner" + return "mapper" + + +def run_suite(suite: Suite, cfg: EvalConfig) -> DatasetResult: + db_path = resolve_named_path(suite.dataset, ".db") + trajectory = load_trajectory(db_path, suite.odom_stream) + golden = load_or_build_golden( + db_path, + suite, + cfg, + corridor_radius=cfg.robot_radius + 0.1, + corridor_z_lo=-cfg.robot_height, + corridor_z_hi=-cfg.robot_height + cfg.body_clearance + cfg.voxel_size, + ) + obstacle_keys = golden.obstacle_keys() + + # Calibration invariant: the physically walked path must pass the gate. + # A failure here means the gate or corridor geometry is wrong, not the planner. + foot_path = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + walked_gate = metrics.check_path( + foot_path, + obstacle_keys, + cfg.voxel_size, + cfg.robot_radius, + cfg.ground_margin, + cfg.body_clearance, + ) + if not walked_gate.valid: + logger.warning( + "%s: walked trajectory fails the collision gate at %d samples; " + "case validity is unreliable", + suite.dataset, + len(walked_gate.collision_points), + ) + + # The online map equals the golden map while both use the same mapper + # config, so reuse it instead of replaying the recording a second time. + # Tier 2 replay and a separate golden mapper config will change this. + online_points = golden.occupied + online_occupied = keys_contain( + np.sort(voxel_keys(online_points, cfg.voxel_size)), golden.walked_keys + ) + false_obstacle_rate = float(online_occupied.mean()) if len(golden.walked_keys) else 0.0 + + golden_planner = cfg.make_planner() + golden_planner.update_global_map(golden.occupied) + online_planner = cfg.make_planner() + online_planner.update_global_map(online_points) + + def snapshot(planner: MLSPlanner) -> PlannerArtifacts: + return PlannerArtifacts( + surface_clearance=planner.surface_clearance_map(), + nodes=planner.nodes(), + edges=planner.node_edges(), + ) + + results: list[CaseResult] = [] + for case in suite.cases: + if case.l_ref is not None: + l_ref, snapped = case.l_ref, True + else: + l_ref, snapped = metrics.reference_length( + trajectory, case.start, case.goal, cfg.robot_height + ) + if not snapped: + logger.warning( + "%s/%s: start or goal is off the walked trajectory; " + "using straight-line reference", + suite.dataset, + case.id, + ) + golden_out, _ = _run_plan(golden_planner, case, l_ref, obstacle_keys, cfg) + online_out, online_wp = _run_plan(online_planner, case, l_ref, obstacle_keys, cfg) + end = online_wp[-1] if online_wp is not None and len(online_wp) else None + results.append( + CaseResult( + id=case.id, + dataset=suite.dataset, + start=case.start, + goal=case.goal, + weight=case.weight, + tags=case.tags, + l_ref=l_ref, + l_ref_snapped=snapped, + online=online_out, + golden=golden_out, + attribution=_attribution(online_out, golden_out), + soft_progress=metrics.soft_progress(end, case.start, case.goal), + ) + ) + + return DatasetResult( + dataset=suite.dataset, + cases=results, + walked_path_valid=walked_gate.valid, + false_obstacle_rate=false_obstacle_rate, + online_voxels=len(online_points), + golden_voxels=len(golden.occupied), + map_build_ms=golden.build_ms, + add_frame_ms=golden.add_frame_ms, + frames=golden.frames, + online_artifacts=snapshot(online_planner), + golden_artifacts=snapshot(golden_planner), + ) + + +def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = 1) -> Report: + cfg = cfg or EvalConfig() + if workers > 1 and len(suites) > 1: + with ProcessPoolExecutor(max_workers=min(workers, len(suites))) as pool: + datasets = list(pool.map(run_suite, suites, itertools.repeat(cfg))) + else: + datasets = [run_suite(suite, cfg) for suite in suites] + cases = [c for d in datasets for c in d.cases] + if not cases: + raise ValueError("no cases to evaluate") + + weights = np.array([c.weight for c in cases]) + online_spl = np.array([c.online.spl for c in cases]) + golden_spl = np.array([c.golden.spl for c in cases]) + soft = np.array([c.soft_progress if not c.online.success else c.online.spl for c in cases]) + attribution_counts: dict[str, int] = {} + for c in cases: + attribution_counts[c.attribution] = attribution_counts.get(c.attribution, 0) + 1 + + rates = [d.false_obstacle_rate for d in datasets] + return Report( + score=float(np.average(online_spl, weights=weights)), + score_soft=float(np.average(soft, weights=weights)), + planner_score=float(np.average(golden_spl, weights=weights)), + false_obstacle_rate=float(np.mean(rates)) if rates else 0.0, + n_cases=len(cases), + n_success=sum(c.online.success for c in cases), + attribution_counts=attribution_counts, + plan_ms=metrics.timing_stats([c.online.plan_ms for c in cases]), + datasets=datasets, + config=asdict(cfg), + ) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py new file mode 100644 index 0000000000..74138345b0 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -0,0 +1,152 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import numpy as np +import pytest + +from dimos.navigation.nav_3d.evaluator import metrics +from dimos.navigation.nav_3d.evaluator.cases import load_suite +from dimos.navigation.nav_3d.evaluator.golden import ( + key_centers, + keys_contain, + voxel_keys, + walked_corridor_keys, +) +from dimos.navigation.nav_3d.evaluator.recording import Trajectory + +VOXEL = 0.1 + + +def test_voxel_key_roundtrip() -> None: + pts = np.array([[0.05, 0.05, 0.05], [-3.21, 4.7, -0.09], [80.0, -80.0, 12.3]], dtype=np.float32) + centers = key_centers(voxel_keys(pts, VOXEL), VOXEL) + assert np.all(np.abs(centers - pts) <= VOXEL / 2 + 1e-5) + + +def test_keys_contain() -> None: + keys = np.sort(voxel_keys(np.array([[0, 0, 0], [1, 1, 1]], dtype=np.float32), VOXEL)) + query = voxel_keys(np.array([[0, 0, 0], [5, 5, 5]], dtype=np.float32), VOXEL) + assert keys_contain(keys, query).tolist() == [True, False] + assert keys_contain(np.array([], dtype=np.int64), query).tolist() == [False, False] + + +def _wall(x: float) -> np.ndarray: + ys, zs = np.meshgrid(np.arange(-1, 1, VOXEL), np.arange(0.05, 1.5, VOXEL)) + return np.stack([np.full(ys.size, x), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) + + +def _gate(waypoints: np.ndarray, obstacles: np.ndarray) -> metrics.GateResult: + keys = np.unique(voxel_keys(obstacles, VOXEL)) + return metrics.check_path( + waypoints, keys, VOXEL, robot_radius=0.16, ground_margin=0.25, body_clearance=0.45 + ) + + +def test_gate_blocks_wall_crossing() -> None: + path = np.array([[-1, 0, 0], [1, 0, 0]], dtype=np.float32) + result = _gate(path, _wall(0.0)) + assert not result.valid + assert len(result.collision_points) > 0 + assert np.all(np.abs(result.collision_points[:, 0]) < 0.3) + + +def test_gate_passes_clear_path() -> None: + path = np.array([[-1, 0, 0], [1, 0, 0]], dtype=np.float32) + assert _gate(path, _wall(5.0)).valid + + +def test_gate_ignores_ground() -> None: + xs, ys = np.meshgrid(np.arange(-2, 2, VOXEL), np.arange(-2, 2, VOXEL)) + floor = np.stack([xs.ravel(), ys.ravel(), np.full(xs.size, -0.05)], axis=1, dtype=np.float32) + path = np.array([[-1, 0, 0], [1, 0, 0]], dtype=np.float32) + assert _gate(path, floor).valid + + +def test_gate_tolerates_stair_slope() -> None: + """Terrain rising at stair slope inside the disc must not trigger the gate.""" + xs, ys = np.meshgrid(np.arange(-1, 2, VOXEL), np.arange(-1, 1, VOXEL)) + slope = np.stack([xs.ravel(), ys.ravel(), xs.ravel() * 0.7 - 0.05], axis=1, dtype=np.float32) + path = np.stack( + [np.arange(-0.5, 1.5, 0.1), np.zeros(20), np.arange(-0.5, 1.5, 0.1) * 0.7], axis=1 + ).astype(np.float32) + assert _gate(path, slope).valid + + +def test_walked_corridor_exempts_gate() -> None: + """A wall crossing carved by the walked corridor passes the gate.""" + wall = _wall(0.0) + path = np.array([[-1, 0, 0], [1, 0, 0]], dtype=np.float32) + traj = Trajectory( + ts=np.array([0.0, 1.0]), + positions=np.array([[-1, 0, 0.3], [1, 0, 0.3]], dtype=np.float32), + ) + walked = walked_corridor_keys(traj, VOXEL, radius=0.3, z_lo=-0.3, z_hi=0.3) + obstacles = np.setdiff1d(np.unique(voxel_keys(wall, VOXEL)), walked) + result = metrics.check_path( + path, obstacles, VOXEL, robot_radius=0.16, ground_margin=0.25, body_clearance=0.45 + ) + assert result.valid + + +def test_spl() -> None: + assert metrics.spl(False, 10.0, 10.0) == 0.0 + assert metrics.spl(True, 10.0, 10.0) == 1.0 + assert metrics.spl(True, 10.0, 20.0) == pytest.approx(0.5) + assert metrics.spl(True, 10.0, 5.0) == 1.0 + + +def test_reference_length_snaps_to_trajectory() -> None: + positions = np.stack( + [np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1 + ).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) + l_ref, snapped = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) + assert snapped + assert l_ref == pytest.approx(10.0, abs=0.01) + l_ref, snapped = metrics.reference_length(traj, (0, 5, 0), (10, 0, 0), robot_height=0.3) + assert not snapped + + +def test_path_length_and_goal() -> None: + path = np.array([[0, 0, 0], [3, 4, 0]], dtype=np.float32) + assert metrics.path_length(path) == pytest.approx(5.0) + assert metrics.goal_reached(path, (3, 4, 0.2), tolerance=0.5) + assert not metrics.goal_reached(path, (3, 4, 1.0), tolerance=0.5) + + +def test_load_suite(tmp_path) -> None: + manifest = tmp_path / "demo.yaml" + manifest.write_text( + "dataset: demo\n" + "cases:\n" + " - id: a\n" + " start: [0, 0, 0]\n" + " goal: [1, 2, 3]\n" + " weight: 2\n" + " tags: [stairs]\n" + ) + suite = load_suite(manifest) + assert suite.dataset == "demo" + assert suite.cases[0].goal == (1.0, 2.0, 3.0) + assert suite.cases[0].weight == 2.0 + + manifest.write_text( + "dataset: demo\ncases:\n" + " - {id: a, start: [0, 0, 0], goal: [1, 2, 3]}\n" + " - {id: a, start: [0, 0, 0], goal: [4, 5, 6]}\n" + ) + with pytest.raises(ValueError, match="duplicate"): + load_suite(manifest) diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py new file mode 100644 index 0000000000..19ec38aef6 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -0,0 +1,206 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Write an evaluation report into a rerun recording. + +One static scene per dataset: +- map/obstacles: golden voxels, turbo colormap by height +- map/walked_free: voxels the robot's body swept while recording, so they are + proven free space and exempt from the collision gate (magenta) +- walked_path: the recorded foot path (white) +- planner_online, planner_golden: the planner graph each map produced. + Surface cells colored by wall clearance (red inside the hard clearance), + nodes yellow, edges colored white to red by log traversal cost. +- cases/: start (cyan), goal (orange), online and golden planned paths + colored by verdict (green valid, red gate-invalid, yellow unreached), and + the gate's collision samples (red dots) +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +import rerun as rr + +from dimos.navigation.nav_3d.evaluator.golden import keys_contain, load_or_build_golden, voxel_keys +from dimos.navigation.nav_3d.evaluator.recording import load_trajectory +from dimos.utils.data import resolve_named_path + +if TYPE_CHECKING: + from pathlib import Path + + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.cases import Suite + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + from dimos.navigation.nav_3d.evaluator.runner import PlannerArtifacts, PlanOutcome, Report + +WALKED_FREE_COLOR = [230, 60, 230] +WALKED_PATH_COLOR = [255, 255, 255] +START_COLOR = [0, 255, 255] +GOAL_COLOR = [255, 140, 0] +COLLISION_COLOR = [255, 0, 0] + +VALID_PATH_COLOR = [0, 220, 0] +INVALID_PATH_COLOR = [255, 0, 0] +UNREACHED_PATH_COLOR = [255, 200, 0] + +NODE_COLOR = [255, 200, 0] +CLEARANCE_CLAMP_M = 1.0 + + +def _turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: + import matplotlib.pyplot as plt + + z = points[:, 2].astype(np.float64) + span = float(z.max() - z.min()) if len(z) else 0.0 + t = (z - z.min()) / max(span, 1e-6) + return np.asarray(plt.get_cmap("turbo")(t)[:, :3] * 255, dtype=np.uint8) + + +def _clearance_colors(clearance: NDArray[np.float32], hard_clearance: float) -> NDArray[np.uint8]: + norm = np.clip(np.nan_to_num(clearance / CLEARANCE_CLAMP_M, nan=1.0, posinf=1.0), 0.0, 1.0) + blocked = np.array([4.0, 8.0, 48.0]) + clear = np.array([150.0, 200.0, 255.0]) + out = np.asarray(blocked + norm[:, None] * (clear - blocked), dtype=np.uint8) + out[clearance < hard_clearance] = (255, 0, 0) + return out + + +def _edge_cost_colors(costs: NDArray[np.float32]) -> NDArray[np.uint8]: + t = np.log1p(np.maximum(costs, 0.0)) + t = t / max(float(t.max()), 1e-6) + low = np.array([220.0, 220.0, 220.0]) + high = np.array([255.0, 40.0, 40.0]) + return np.asarray(low + t[:, None] * (high - low), dtype=np.uint8) + + +def _log_planner(entity: str, artifacts: PlannerArtifacts | None, cfg: EvalConfig) -> None: + if artifacts is None: + return + surface = artifacts.surface_clearance + if surface.size: + rr.log( + f"{entity}/surface", + rr.Points3D( + surface[:, :3], + colors=_clearance_colors(surface[:, 3], cfg.wall_clearance_m), + radii=cfg.voxel_size / 4, + ), + static=True, + ) + if artifacts.nodes.size: + rr.log( + f"{entity}/nodes", + rr.Points3D(artifacts.nodes, colors=[NODE_COLOR], radii=0.05), + static=True, + ) + edges = artifacts.edges + if edges.size: + rr.log( + f"{entity}/edges", + rr.LineStrips3D( + edges[:, :6].reshape(-1, 2, 3), + colors=_edge_cost_colors(edges[:, 6]), + radii=0.008, + ), + static=True, + ) + + +def _outcome_color(outcome: PlanOutcome) -> list[int]: + if outcome.success: + return VALID_PATH_COLOR + if outcome.planned and not outcome.valid: + return INVALID_PATH_COLOR + return UNREACHED_PATH_COLOR + + +def _log_path(entity: str, outcome: PlanOutcome, radius: float) -> None: + if not outcome.waypoints: + return + rr.log( + entity, + rr.LineStrips3D([outcome.waypoints], colors=[_outcome_color(outcome)], radii=radius), + static=True, + ) + if outcome.collisions: + rr.log( + f"{entity}/collisions", + rr.Points3D(outcome.collisions, colors=[COLLISION_COLOR], radii=radius * 3), + static=True, + ) + + +def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) -> None: + rr.init("nav3d_eval", recording_id="nav3d_eval") + rr.save(str(out)) + + suites_by_dataset = {suite.dataset: suite for suite in suites} + for dataset in report.datasets: + suite = suites_by_dataset[dataset.dataset] + db_path = resolve_named_path(suite.dataset, ".db") + golden = load_or_build_golden( + db_path, + suite, + cfg, + corridor_radius=cfg.robot_radius + 0.1, + corridor_z_lo=-cfg.robot_height, + corridor_z_hi=-cfg.robot_height + cfg.body_clearance + cfg.voxel_size, + ) + trajectory = load_trajectory(db_path, suite.odom_stream) + root = dataset.dataset + + walked_free = keys_contain(golden.walked_keys, voxel_keys(golden.occupied, cfg.voxel_size)) + obstacles = golden.occupied[~walked_free] + rr.log( + f"{root}/map/obstacles", + rr.Points3D(obstacles, colors=_turbo_by_height(obstacles), radii=cfg.voxel_size / 4), + static=True, + ) + rr.log( + f"{root}/map/walked_free", + rr.Points3D( + golden.occupied[walked_free], colors=[WALKED_FREE_COLOR], radii=cfg.voxel_size / 3 + ), + static=True, + ) + foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + rr.log( + f"{root}/walked_path", + rr.LineStrips3D([foot], colors=[WALKED_PATH_COLOR], radii=0.015), + static=True, + ) + + _log_planner(f"{root}/planner_online", dataset.online_artifacts, cfg) + _log_planner(f"{root}/planner_golden", dataset.golden_artifacts, cfg) + + for case in dataset.cases: + base = f"{root}/cases/{case.id}" + rr.log( + f"{base}/start", + rr.Points3D([case.start], colors=[START_COLOR], radii=0.12), + static=True, + ) + rr.log( + f"{base}/goal", + rr.Points3D([case.goal], colors=[GOAL_COLOR], radii=0.12), + static=True, + ) + _log_path(f"{base}/online", case.online, radius=0.04) + _log_path(f"{base}/golden", case.golden, radius=0.02) + + print(f"wrote {out}") + print(f"open with: rerun {out}") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 47f56ff32d..160780a8bc 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -82,7 +82,6 @@ "mid360-realsense-record-with-pcap": "dimos.robot.assembly.mid360_realsense_30:mid360_realsense_record_with_pcap", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", - "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-hosted-go2": "dimos.teleop.quest_hosted.blueprints:teleop_hosted_go2", "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam", "teleop-hosted-go2-transport": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_transport", From 97978aaacc1c44e6b23f7d8ece5ff300a2c43a12 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 16:20:04 -0700 Subject: [PATCH 03/29] Auto dataset generation --- dimos/navigation/nav_3d/evaluator/cases.py | 25 ++ .../nav_3d/evaluator/cases/china_office.yaml | 167 ++++++++ .../evaluator/cases/mid360_athens_stairs.yaml | 105 +++-- dimos/navigation/nav_3d/evaluator/cli.py | 156 +++++++- dimos/navigation/nav_3d/evaluator/generate.py | 370 ++++++++++++++++++ dimos/navigation/nav_3d/evaluator/golden.py | 51 +-- dimos/navigation/nav_3d/evaluator/metrics.py | 19 +- dimos/navigation/nav_3d/evaluator/runner.py | 43 +- .../nav_3d/evaluator/test_evaluator.py | 138 ++++++- dimos/navigation/nav_3d/evaluator/viz.py | 36 +- 10 files changed, 948 insertions(+), 162 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/cases/china_office.yaml create mode 100644 dimos/navigation/nav_3d/evaluator/generate.py diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index cdcb9aea5d..befa30e98c 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -86,3 +86,28 @@ def load_suites(paths: list[Path] | None = None) -> list[Suite]: if not paths: raise FileNotFoundError(f"no case manifests found under {CASES_DIR}") return [load_suite(p) for p in paths] + + +def save_suite(suite: Suite, path: Path | None = None) -> Path: + """Write the suite manifest as YAML. Defaults to cases/.yaml.""" + path = path or suite.path or CASES_DIR / f"{suite.dataset}.yaml" + doc: dict[str, object] = {"dataset": suite.dataset} + if suite.lidar_stream != "pointlio_lidar": + doc["lidar_stream"] = suite.lidar_stream + if suite.odom_stream != "pointlio_odometry": + doc["odom_stream"] = suite.odom_stream + entries = [] + for case in suite.cases: + entry: dict[str, object] = { + "id": case.id, + "start": [round(float(v), 3) for v in case.start], + "goal": [round(float(v), 3) for v in case.goal], + "weight": case.weight, + "tags": case.tags, + } + if case.l_ref is not None: + entry["l_ref"] = round(case.l_ref, 3) + entries.append(entry) + doc["cases"] = entries + path.write_text(yaml.safe_dump(doc, sort_keys=False, default_flow_style=None)) + return path diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml new file mode 100644 index 0000000000..821187f069 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -0,0 +1,167 @@ +dataset: china_office +cases: +- id: auto_00_up + start: [-10.2, 15.8, -0.48] + goal: [-8.12, 14.6, 2.56] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_01_down + start: [-8.12, 14.6, 2.56] + goal: [-10.2, 15.8, -0.48] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_02_down + start: [-3.64, -0.76, 3.04] + goal: [-1.4, -1.8, -0.4] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_03_up + start: [-1.4, -1.8, -0.4] + goal: [-3.64, -0.76, 3.04] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_04_up + start: [1.24, -17.64, 1.28] + goal: [11.48, -13.56, 4.32] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_05_down + start: [11.48, -13.56, 4.32] + goal: [1.24, -17.64, 1.28] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_06_down + start: [3.72, 9.64, 3.12] + goal: [5.64, 23.64, -0.24] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_07_up + start: [5.64, 23.64, -0.24] + goal: [3.72, 9.64, 3.12] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_08_up + start: [8.2, -32.52, -0.48] + goal: [8.28, -2.04, 4.16] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_09_down + start: [8.28, -2.04, 4.16] + goal: [8.2, -32.52, -0.48] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_10_up + start: [18.84, -23.96, -1.36] + goal: [-4.6, 8.2, 6.56] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_11_down + start: [-4.6, 8.2, 6.56] + goal: [18.84, -23.96, -1.36] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_12_down + start: [-1.72, -10.28, 2.96] + goal: [11.96, 5.4, -0.96] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_13_up + start: [11.96, 5.4, -0.96] + goal: [-1.72, -10.28, 2.96] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_14_down + start: [5.4, 3.32, 3.2] + goal: [14.52, -6.36, -1.28] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_15_up + start: [14.52, -6.36, -1.28] + goal: [5.4, 3.32, 3.2] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_16_down + start: [-1.72, 12.52, 2.96] + goal: [17.4, -32.76, -0.96] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_17_up + start: [17.4, -32.76, -0.96] + goal: [-1.72, 12.52, 2.96] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_18_down + start: [11.24, -6.76, 4.16] + goal: [9.24, 15.56, -0.48] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_19_up + start: [9.24, 15.56, -0.48] + goal: [11.24, -6.76, 4.16] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_20_up + start: [6.04, -24.2, -0.48] + goal: [-7.72, 5.96, 2.88] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_21_down + start: [-7.72, 5.96, 2.88] + goal: [6.04, -24.2, -0.48] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_22_up + start: [17.0, -15.48, -1.28] + goal: [1.96, -0.28, 3.12] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_23_down + start: [1.96, -0.28, 3.12] + goal: [17.0, -15.48, -1.28] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_24_flat + start: [5.72, -9.72, -0.4] + goal: [4.36, -11.96, -0.48] + weight: 1.0 + tags: [auto, flat] +- id: auto_25_flat + start: [8.2, -17.32, -0.4] + goal: [3.32, 16.12, -0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_26_flat + start: [10.76, 10.12, -0.72] + goal: [14.6, -27.88, -0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_27_flat + start: [1.08, 4.84, -0.4] + goal: [13.56, -32.28, -0.88] + weight: 1.0 + tags: [auto, flat] +- id: auto_28_flat + start: [13.32, -0.28, -1.12] + goal: [17.88, -19.64, -1.36] + weight: 1.0 + tags: [auto, flat] +- id: auto_29_flat + start: [-5.64, 9.8, -0.4] + goal: [-8.52, 10.52, -0.48] + weight: 1.0 + tags: [auto, flat] +- id: auto_30_flat + start: [10.6, -34.92, -0.8] + goal: [0.52, -6.28, -0.4] + weight: 1.0 + tags: [auto, flat] +- id: auto_31_flat + start: [-1.08, -16.6, 2.64] + goal: [-2.76, -15.48, 3.04] + weight: 1.0 + tags: [auto, flat] +- id: auto_32_flat + start: [4.04, 5.16, 0.16] + goal: [2.36, 11.96, -0.32] + weight: 1.0 + tags: [auto, flat] diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index 97a6da82db..a9722fb1a0 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -1,27 +1,82 @@ dataset: mid360_athens_stairs cases: - - id: flat_ground_floor - start: [-0.01, 0.01, -0.30] - goal: [1.00, -4.77, -0.27] - weight: 1 - tags: [flat] - - id: stairs_down_two_flights - start: [4.86, -4.58, -0.29] - goal: [6.40, -3.70, -6.49] - weight: 2 - tags: [stairs, down] - - id: stairs_up_from_basement - start: [6.40, -3.70, -6.49] - goal: [7.04, -3.58, 0.20] - weight: 2 - tags: [stairs, up] - - id: basement_to_top_floor - start: [7.16, -3.78, -6.07] - goal: [5.89, -1.60, 3.06] - weight: 3 - tags: [stairs, up, long] - - id: ground_to_top_floor - start: [-2.49, -1.20, -0.33] - goal: [5.86, -4.21, 3.02] - weight: 3 - tags: [stairs, up, long] +- id: auto_00_up + start: [-2.52, -0.52, -0.32] + goal: [1.32, -0.84, 2.72] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_01_down + start: [1.32, -0.84, 2.72] + goal: [-2.52, -0.52, -0.32] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_02_down + start: [6.44, -5.56, -1.44] + goal: [7.24, -3.96, -6.08] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_03_up + start: [7.24, -3.96, -6.08] + goal: [6.44, -5.56, -1.44] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_04_up + start: [0.68, -4.12, -0.32] + goal: [8.04, -0.76, 3.04] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_05_down + start: [8.04, -0.76, 3.04] + goal: [0.68, -4.12, -0.32] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_06_up + start: [-2.36, -4.36, -0.32] + goal: [5.88, -4.52, 2.96] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_07_down + start: [5.88, -4.52, 2.96] + goal: [-2.36, -4.36, -0.32] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_08_up + start: [-2.04, 3.16, -0.48] + goal: [-0.2, -3.0, 2.56] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_09_down + start: [-0.2, -3.0, 2.56] + goal: [-2.04, 3.16, -0.48] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_10_up + start: [5.96, -3.64, -3.52] + goal: [0.12, -0.76, -0.32] + weight: 3.0 + tags: [auto, stairs, up, long] +- id: auto_11_down + start: [0.12, -0.76, -0.32] + goal: [5.96, -3.64, -3.52] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_12_flat + start: [5.16, -3.96, -3.44] + goal: [7.48, -3.8, -2.96] + weight: 1.0 + tags: [auto, flat] +- id: auto_13_flat + start: [-2.04, 3.16, -0.48] + goal: [5.48, -5.56, -0.96] + weight: 1.0 + tags: [auto, flat] +- id: auto_14_flat + start: [6.28, -5.64, 2.24] + goal: [-0.04, -1.96, 2.56] + weight: 1.0 + tags: [auto, flat] +- id: auto_15_flat + start: [7.24, -3.96, -6.08] + goal: [5.08, -3.8, -6.48] + weight: 1.0 + tags: [auto, flat] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 778a5002cc..24e11d5302 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -15,9 +15,11 @@ """Nav-3d evaluation CLI. Run every suite: python -m dimos.navigation.nav_3d.evaluator run -One dataset: python -m dimos.navigation.nav_3d.evaluator run --dataset stairs60_a +One dataset: python -m dimos.navigation.nav_3d.evaluator run --dataset mid360_athens_stairs Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json Override a knob: python -m dimos.navigation.nav_3d.evaluator run --set wall_clearance_m=0.05 +New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a +Curate a case: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z """ from __future__ import annotations @@ -25,12 +27,34 @@ import dataclasses import json from pathlib import Path +import sqlite3 +from typing import TYPE_CHECKING +import numpy as np import typer -from dimos.navigation.nav_3d.evaluator.cases import load_suites +from dimos.navigation.nav_3d.evaluator.cases import ( + CASES_DIR, + Case, + Suite, + load_suite, + load_suites, + save_suite, +) from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.generate import ( + GenerationParams, + drift_stats, + generate_cases, + snap_to_surface, +) +from dimos.navigation.nav_3d.evaluator.golden import load_or_build_golden +from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate +from dimos.utils.data import get_data_dir, resolve_named_path + +if TYPE_CHECKING: + from numpy.typing import NDArray app = typer.Typer(no_args_is_help=True, add_completion=False) @@ -64,7 +88,6 @@ def _print_report(report: Report) -> None: print( f"{d.dataset}: {d.frames} frames, online {d.online_voxels} / " f"golden {d.golden_voxels} voxels, " - f"false-obstacle rate {d.false_obstacle_rate:.3f}, " f"map build {d.map_build_ms / 1000:.1f}s" ) print( @@ -91,7 +114,12 @@ def run( ) -> None: suites = load_suites(manifests or None) if dataset is not None: - suites = [s for s in suites if s.dataset == dataset] + wanted = Path(dataset).stem + suites = [ + s + for s in suites + if s.dataset == dataset or (s.path is not None and s.path.stem == wanted) + ] if not suites: raise typer.BadParameter(f"no suite for dataset {dataset!r}") cfg = _apply_overrides(EvalConfig(), set_ or []) @@ -106,6 +134,126 @@ def run( write_rrd(report, suites, cfg, rrd_out) +def _copy_recording(src: Path, dest: Path) -> None: + """Copy via the sqlite backup API so WAL sidecar content is never lost.""" + with sqlite3.connect(src) as source, sqlite3.connect(dest) as target: + source.backup(target) + + +def _snap_or_fail( + label: str, + point: tuple[float, float, float], + surface: NDArray[np.float32], + snap_max_m: float, +) -> tuple[float, float, float]: + snapped = snap_to_surface(np.asarray(point, dtype=np.float32), surface, snap_max_m) + if snapped is None: + raise typer.BadParameter( + f"{label} {point} is more than {snap_max_m}m from any standable surface" + ) + return (float(snapped[0]), float(snapped[1]), float(snapped[2])) + + +@app.command() +def ingest( + source: Path = typer.Argument( + ..., help="Recording to ingest: a mem2.db file or the directory holding one" + ), + name: str = typer.Option(..., "--name", help="Dataset name; becomes data/.db"), + lidar_stream: str = typer.Option("pointlio_lidar", "--lidar-stream"), + odom_stream: str = typer.Option("pointlio_odometry", "--odom-stream"), + max_cases: int = typer.Option( + 0, "--max-cases", help="Auto-generated case cap; 0 scales with recording length" + ), + force: bool = typer.Option(False, "--force", help="Overwrite dataset and manifest"), +) -> None: + """Register a recording as a dataset: copy, drift-check, map, generate cases.""" + src = source / "mem2.db" if source.is_dir() else source + if not src.exists(): + raise typer.BadParameter(f"{src} does not exist") + manifest = CASES_DIR / f"{name}.yaml" + if manifest.exists() and not force: + raise typer.BadParameter(f"{manifest} already exists; pass --force to regenerate") + dest = get_data_dir() / f"{name}.db" + if src.resolve() != dest.resolve(): + if dest.exists() and not force: + raise typer.BadParameter(f"{dest} already exists; pass --force to overwrite") + print(f"copying {src} -> {dest}") + _copy_recording(src, dest) + + suite = Suite(dataset=name, cases=[], lidar_stream=lidar_stream, odom_stream=odom_stream) + trajectory = load_trajectory(dest, odom_stream) + arcs = trajectory.arc_lengths() + print( + f"trajectory: {len(trajectory.positions)} poses, " + f"{trajectory.ts[-1] - trajectory.ts[0]:.0f}s, {arcs[-1]:.1f}m walked, " + f"z [{trajectory.positions[:, 2].min():.2f}, {trajectory.positions[:, 2].max():.2f}]" + ) + drift = drift_stats(trajectory) + closure = f"{drift.closure_m:.2f}m" if drift.closure_m is not None else "n/a" + print( + f"drift: {drift.revisit_count} same-floor revisits, " + f"z mismatch p95 {drift.revisit_dz_p95:.2f}m, loop closure {closure}" + ) + for warning in drift.warnings: + print(f"WARNING: {warning}") + + cfg = EvalConfig() + golden = load_or_build_golden(dest, suite, cfg) + planner = cfg.make_planner() + planner.update_global_map(golden.occupied) + gen = GenerationParams(max_cases=max_cases or None) + suite.cases = generate_cases(trajectory, golden, planner.surface_map(), cfg, gen) + if not suite.cases: + raise typer.Exit(code=1) + floor = min(gen.min_cases, gen.resolve_max_cases(float(arcs[-1]))) + if len(suite.cases) < floor: + print( + f"WARNING: only {len(suite.cases)} cases generated; the recording " + "may be too short or too uniform for more" + ) + path = save_suite(suite, manifest) + print(f"\n{len(suite.cases)} cases -> {path}") + for case in suite.cases: + print(f" {case.id}: w={case.weight:g} [{', '.join(case.tags)}]") + print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {name}") + + +@app.command("add-case") +def add_case( + dataset: str = typer.Argument(..., help="Dataset whose manifest gets the case"), + start: tuple[float, float, float] = typer.Option(..., "--start", help="Foot-level xyz"), + goal: tuple[float, float, float] = typer.Option(..., "--goal", help="Foot-level xyz"), + case_id: str = typer.Option(None, "--id", help="Case id; default manual_"), + tags: str = typer.Option("manual", "--tags", help="Comma-separated tags"), + weight: float = typer.Option(2.0, "--weight"), + snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), +) -> None: + """Append a curated case, with both endpoints snapped to the golden surface.""" + manifest = CASES_DIR / f"{dataset}.yaml" + if not manifest.exists(): + raise typer.BadParameter(f"no manifest {manifest}; run ingest first") + suite = load_suite(manifest) + cfg = EvalConfig() + golden = load_or_build_golden(resolve_named_path(dataset, ".db"), suite, cfg) + planner = cfg.make_planner() + planner.update_global_map(golden.occupied) + surface = planner.surface_map() + + case = Case( + id=case_id or f"manual_{sum(c.id.startswith('manual_') for c in suite.cases):02d}", + start=_snap_or_fail("start", start, surface, snap_max), + goal=_snap_or_fail("goal", goal, surface, snap_max), + weight=weight, + tags=[t.strip() for t in tags.split(",") if t.strip()], + ) + if any(c.id == case.id for c in suite.cases): + raise typer.BadParameter(f"case id {case.id!r} already exists in {manifest}") + suite.cases.append(case) + save_suite(suite, manifest) + print(f"added {case.id}: {case.start} -> {case.goal} to {manifest}") + + @app.command("list") def list_cases() -> None: for suite in load_suites(): diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py new file mode 100644 index 0000000000..6ace4da516 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -0,0 +1,370 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generate evaluation cases from a recorded trajectory. + +Candidate pairs are sampled along the walked path, so both endpoints are +physically proven reachable. A pair is kept only when it is non-trivial: +the straight start-goal line collides with golden obstacles, the walked +route detours well past the straight-line distance, or the pair climbs. +Endpoints snap to the golden surface so drift between passes cannot leave +a case floating off the map. Generation is deterministic. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator import metrics +from dimos.navigation.nav_3d.evaluator.cases import Case + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + from dimos.navigation.nav_3d.evaluator.golden import GoldenMap + from dimos.navigation.nav_3d.evaluator.recording import Trajectory + +STAIRS_DZ_M = 0.5 +LONG_STAIRS_DZ_M = 1.5 +LONG_STAIRS_WALKED_M = 20.0 + + +@dataclass +class GenerationParams: + min_separation_m: float = 3.0 + min_euclid_m: float = 2.0 + detour_ratio_min: float = 1.3 + snap_max_m: float = 1.0 + bin_size_m: float = 2.0 + waypoint_spacing_m: float = 1.0 + # None scales the case count with the walked distance. + max_cases: int | None = None + # Two cases are duplicates when both endpoints land within this radius. + dedupe_radius_m: float = 1.5 + # Share of slots reserved for flat cases when the recording has them. + flat_fraction: float = 0.25 + # Coverage sectors: a case earns a slot first by connecting a sector pair + # no accepted case connects yet. + sector_size_m: float = 8.0 + sector_z_m: float = 1.5 + # A sector may anchor at most this many selected cases, which prevents a + # single high-priority spot from becoming the hub of every case. + endpoint_reuse_max: int = 2 + # Floor on the case count. When strict selection falls short, a relaxed + # pass ignores the sector caps and the flat quota to reach it. + min_cases: int = 10 + + def resolve_max_cases(self, walked_total_m: float) -> int: + if self.max_cases is not None: + return self.max_cases + return int(np.clip(walked_total_m / 25.0, 16, 48)) + + +@dataclass +class Candidate: + start: tuple[float, float, float] + goal: tuple[float, float, float] + walked_m: float + detour_ratio: float + dz: float + + @property + def priority(self) -> float: + return ( + min(self.detour_ratio, 3.0) + + 2.0 * min(abs(self.dz), 3.0) + + 0.5 * min(self.walked_m / 50.0, 2.0) + ) + + +def snap_to_surface( + point: NDArray[np.float32], + surface: NDArray[np.float32], + snap_max_m: float, +) -> NDArray[np.float32] | None: + """Nearest standable surface cell, or None when the point is off the map. + + Horizontal distance dominates so drift in z between passes does not pull + the snap onto another floor. + """ + hd = np.linalg.norm(surface[:, :2] - point[:2], axis=1) + zd = np.abs(surface[:, 2] - point[2]) + score = hd + np.where(zd < 1.0, zd * 0.5, np.inf) + best = int(score.argmin()) + if not np.isfinite(score[best]) or hd[best] > snap_max_m: + return None + return np.asarray(surface[best], dtype=np.float32) + + +def _subsample_indices(trajectory: Trajectory, spacing_m: float) -> NDArray[np.int64]: + arcs = trajectory.arc_lengths() + targets = np.arange(0.0, arcs[-1], spacing_m) + return np.unique(np.searchsorted(arcs, targets)) + + +def generate_cases( + trajectory: Trajectory, + golden: GoldenMap, + surface: NDArray[np.float32], + cfg: EvalConfig, + params: GenerationParams | None = None, +) -> list[Case]: + params = params or GenerationParams() + obstacle_keys = golden.occupied_keys + arcs = trajectory.arc_lengths() + foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + + idx = _subsample_indices(trajectory, params.waypoint_spacing_m) + snaps = np.full((len(idx), 3), np.nan, dtype=np.float32) + for n, i in enumerate(idx): + hit = snap_to_surface(foot[i], surface, params.snap_max_m) + if hit is not None: + snaps[n] = hit + ok = np.isfinite(snaps[:, 0]) + way_arcs = arcs[idx] + + candidates: dict[tuple[int, ...], Candidate] = {} + for ai in range(len(idx)): + if not ok[ai]: + continue + sa = snaps[ai] + later = np.arange(ai + 1, len(idx)) + later = later[ok[later]] + if not len(later): + continue + walked = way_arcs[later] - way_arcs[ai] + deltas = snaps[later] - sa + euclid = np.linalg.norm(deltas, axis=1) + keep = (walked >= params.min_separation_m) & (euclid >= params.min_euclid_m) + for bi, w, e in zip(later[keep], walked[keep], euclid[keep], strict=True): + sb = snaps[bi] + dz = float(sb[2] - sa[2]) + detour = float(w / e) + if detour < params.detour_ratio_min and abs(dz) < STAIRS_DZ_M: + # A long near-straight flat pair is trivial; not worth a sweep. + if e > 30.0: + continue + # Only pairs not already qualified pay for the line sweep. + line = np.stack([sa, sb]) + blocked = not metrics.check_path( + line, + obstacle_keys, + cfg.voxel_size, + cfg.robot_radius, + cfg.ground_margin, + cfg.body_clearance, + ).valid + if not blocked: + continue + cand = Candidate( + start=(float(sa[0]), float(sa[1]), float(sa[2])), + goal=(float(sb[0]), float(sb[1]), float(sb[2])), + walked_m=float(w), + detour_ratio=detour, + dz=dz, + ) + bins = np.floor(np.array([*sa[:2], *sb[:2]]) / params.bin_size_m).astype(int) + dz_sign = int(np.sign(dz)) if abs(dz) >= STAIRS_DZ_M else 0 + key = (*bins, dz_sign) + best = candidates.get(key) + if best is None or cand.priority > best.priority: + candidates[key] = cand + + ranked = sorted(candidates.values(), key=lambda c: (-c.priority, c.start, c.goal)) + selected = _select_diverse(ranked, params, params.resolve_max_cases(float(arcs[-1]))) + cases = [_to_case(cand, n) for n, cand in enumerate(selected)] + return cases + + +def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> bool: + a = np.array([*cand.start, *cand.goal]) + for other in accepted: + b = np.array([*other.start, *other.goal]) + if np.linalg.norm(a[:3] - b[:3]) < radius and np.linalg.norm(a[3:] - b[3:]) < radius: + return True + return False + + +def _reversed(cand: Candidate) -> Candidate: + return Candidate( + start=cand.goal, + goal=cand.start, + walked_m=cand.walked_m, + detour_ratio=cand.detour_ratio, + dz=-cand.dz, + ) + + +def _select_diverse( + ranked: list[Candidate], params: GenerationParams, max_cases: int +) -> list[Candidate]: + """Spread-greedy selection: each slot goes to the candidate whose score is + its priority plus how far its endpoints are from every endpoint already in + use. Coverage is the objective, not a filter, so cases spread across the + map instead of fanning out of the highest-priority spot. A sector-usage + cap bounds hub reuse outright. Stair candidates also claim their reversed + direction, and the flat quota keeps stairs from crowding out flats. + When the strict pass yields fewer than min_cases, sector-capped candidates + are revived and a relaxed pass without sector caps or the flat quota + backfills up to the floor. + """ + if not ranked: + return [] + flat_target = int(max_cases * params.flat_fraction) + stairs_cap = max_cases - flat_target + + starts = np.array([c.start for c in ranked], dtype=np.float32) + goals = np.array([c.goal for c in ranked], dtype=np.float32) + priorities = np.array([c.priority for c in ranked], dtype=np.float32) + is_stairs = np.array([abs(c.dz) >= STAIRS_DZ_M for c in ranked]) + spread_cap = 2.0 * params.sector_size_m + + def sector(p: NDArray[np.float32]) -> tuple[int, ...]: + return ( + int(np.floor(p[0] / params.sector_size_m)), + int(np.floor(p[1] / params.sector_size_m)), + round(float(p[2]) / params.sector_z_m), + ) + + usage: dict[tuple[int, ...], int] = {} + used_points: list[NDArray[np.float32]] = [] + alive = np.ones(len(ranked), dtype=bool) + sector_capped: list[int] = [] + stairs: list[Candidate] = [] + flats: list[Candidate] = [] + + def fill(target: int, relax: bool) -> None: + while alive.any() and len(stairs) + len(flats) < target: + if used_points: + used = np.stack(used_points) + d_start = np.linalg.norm(starts[:, None] - used[None], axis=2).min(axis=1) + d_goal = np.linalg.norm(goals[:, None] - used[None], axis=2).min(axis=1) + spread = np.minimum(d_start, spread_cap) + np.minimum(d_goal, spread_cap) + else: + spread = np.full(len(ranked), 2.0 * spread_cap, dtype=np.float32) + score = priorities + 0.4 * spread + score[~alive] = -np.inf + if not relax and len(stairs) + 1 >= stairs_cap: + score[is_stairs] = -np.inf + if not np.isfinite(score).any(): + break + n = int(score.argmax()) + alive[n] = False + cand = ranked[n] + sa, sb = sector(starts[n]), sector(goals[n]) + if not relax and ( + usage.get(sa, 0) >= params.endpoint_reuse_max + or usage.get(sb, 0) >= params.endpoint_reuse_max + ): + sector_capped.append(n) + continue + bucket = stairs if is_stairs[n] else flats + if _is_duplicate(cand, bucket, params.dedupe_radius_m): + continue + usage[sa] = usage.get(sa, 0) + 1 + usage[sb] = usage.get(sb, 0) + 1 + used_points.append(starts[n]) + used_points.append(goals[n]) + bucket.append(cand) + if bucket is stairs and (relax or len(stairs) < stairs_cap): + bucket.append(_reversed(cand)) + + fill(max_cases, relax=False) + min_cases = min(params.min_cases, max_cases) + if len(stairs) + len(flats) < min_cases: + alive[sector_capped] = True + fill(min_cases, relax=True) + + return (stairs + flats)[:max_cases] + + +def _to_case(cand: Candidate, n: int) -> Case: + if cand.dz >= STAIRS_DZ_M: + kind, tags = "up", ["auto", "stairs", "up"] + elif cand.dz <= -STAIRS_DZ_M: + kind, tags = "down", ["auto", "stairs", "down"] + else: + kind, tags = "flat", ["auto", "flat"] + weight = 1.0 + if kind != "flat": + weight = 2.0 + if abs(cand.dz) >= LONG_STAIRS_DZ_M or cand.walked_m >= LONG_STAIRS_WALKED_M: + weight = 3.0 + tags.append("long") + return Case( + id=f"auto_{n:02d}_{kind}", + start=cand.start, + goal=cand.goal, + weight=weight, + tags=tags, + ) + + +@dataclass +class DriftStats: + """Consistency of same-floor revisits, plus loop closure when one exists.""" + + revisit_count: int + revisit_dz_p95: float + closure_m: float | None + warnings: list[str] = field(default_factory=list) + + +def drift_stats( + trajectory: Trajectory, + revisit_gap_s: float = 30.0, + revisit_radius_m: float = 0.5, + dz_warn_m: float = 0.3, + closure_warn_m: float = 1.0, +) -> DriftStats: + p = trajectory.positions + ts = trajectory.ts + idx = _subsample_indices(trajectory, 0.5) + dzs: list[float] = [] + for i in idx: + earlier = idx[ts[idx] < ts[i] - revisit_gap_s] + if not len(earlier): + continue + hd = np.linalg.norm(p[earlier, :2] - p[i, :2], axis=1) + near = earlier[hd < revisit_radius_m] + if not len(near): + continue + dz = np.abs(p[near, 2] - p[i, 2]) + same_floor = dz[dz < 1.0] + if len(same_floor): + dzs.append(float(same_floor.min())) + + closure: float | None = None + if np.linalg.norm(p[-1, :2] - p[0, :2]) < 2.0: + closure = float(np.linalg.norm(p[-1] - p[0])) + + warnings = [] + dz_p95 = float(np.percentile(dzs, 95)) if dzs else 0.0 + if dz_p95 > dz_warn_m: + warnings.append( + f"same-floor revisit z mismatch p95 {dz_p95:.2f}m exceeds {dz_warn_m}m; " + "the recording may be too drifty for reliable evaluation" + ) + if closure is not None and closure > closure_warn_m: + warnings.append(f"loop closure error {closure:.2f}m exceeds {closure_warn_m}m") + return DriftStats( + revisit_count=len(dzs), + revisit_dz_p95=dz_p95, + closure_m=closure, + warnings=warnings, + ) diff --git a/dimos/navigation/nav_3d/evaluator/golden.py b/dimos/navigation/nav_3d/evaluator/golden.py index 9c46ca9382..5e894436b5 100644 --- a/dimos/navigation/nav_3d/evaluator/golden.py +++ b/dimos/navigation/nav_3d/evaluator/golden.py @@ -12,11 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Golden reference map: full-recording voxel map plus walked-corridor free space. +"""Golden reference map built from every frame of a recording. The golden occupancy is what returned paths are collision-checked against. -The walked corridor marks voxels the robot's body physically swept, which are -free space regardless of what any mapper claims. """ from __future__ import annotations @@ -30,7 +28,7 @@ import numpy as np -from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory +from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -40,7 +38,6 @@ from dimos.navigation.nav_3d.evaluator.cases import Suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig - from dimos.navigation.nav_3d.evaluator.recording import Trajectory logger = setup_logger() @@ -103,40 +100,17 @@ def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: return np.concatenate(out).astype(np.float32) -def walked_corridor_keys( - trajectory: Trajectory, - voxel_size: float, - radius: float, - z_lo: float, - z_hi: float, -) -> NDArray[np.int64]: - """Voxels swept by the robot body cylinder along the trajectory, sorted. - - z_lo and z_hi are relative to the odometry pose. The carved volume must - cover the collision gate's checked volume, or the walked path itself - fails the gate. - """ - dense = densify(trajectory.positions, voxel_size / 2) - offsets = cylinder_offsets(radius, z_lo, z_hi, voxel_size) - return np.unique(offset_keys(dense, offsets, voxel_size)) - - @dataclass class GoldenMap: voxel_size: float occupied: NDArray[np.float32] occupied_keys: NDArray[np.int64] - walked_keys: NDArray[np.int64] frames: int add_frame_ms: dict[str, float] build_ms: float - def obstacle_keys(self) -> NDArray[np.int64]: - """Occupied minus walked-free, the set paths must not intersect.""" - return np.setdiff1d(self.occupied_keys, self.walked_keys, assume_unique=True) - -CACHE_VERSION = 2 +CACHE_VERSION = 3 def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: @@ -144,19 +118,9 @@ def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: return db_path.parent / ".golden" / f"{db_path.stem}.{digest}.npz" -def load_or_build_golden( - db_path: Path, - suite: Suite, - cfg: EvalConfig, - corridor_radius: float, - corridor_z_lo: float, - corridor_z_hi: float, -) -> GoldenMap: +def load_or_build_golden(db_path: Path, suite: Suite, cfg: EvalConfig) -> GoldenMap: params: dict[str, float | int | str] = { **cfg.mapper_fingerprint(), - "corridor_radius": corridor_radius, - "corridor_z_lo": corridor_z_lo, - "corridor_z_hi": corridor_z_hi, "align_tol": cfg.align_tol, "lidar_stream": suite.lidar_stream, "odom_stream": suite.odom_stream, @@ -170,7 +134,6 @@ def load_or_build_golden( voxel_size=voxel_size, occupied=data["occupied"], occupied_keys=data["occupied_keys"], - walked_keys=data["walked_keys"], frames=int(data["frames"]), add_frame_ms={ "p50": float(data["add_p50"]), @@ -192,17 +155,12 @@ def load_or_build_golden( add_arr = np.asarray(add_ms) if add_ms else np.zeros(1) occupied = mapper.global_map() occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) - trajectory = load_trajectory(db_path, suite.odom_stream) - walked = walked_corridor_keys( - trajectory, voxel_size, corridor_radius, corridor_z_lo, corridor_z_hi - ) cache.parent.mkdir(exist_ok=True) np.savez_compressed( cache, occupied=occupied, occupied_keys=occupied_keys, - walked_keys=walked, frames=len(add_ms), add_p50=np.percentile(add_arr, 50), add_p95=np.percentile(add_arr, 95), @@ -213,7 +171,6 @@ def load_or_build_golden( voxel_size=voxel_size, occupied=occupied, occupied_keys=occupied_keys, - walked_keys=walked, frames=len(add_ms), add_frame_ms={ "p50": float(np.percentile(add_arr, 50)), diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index e3cc8ed996..04a5c21542 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -100,21 +100,28 @@ def reference_length( robot_height: float, max_snap_m: float = 1.0, ) -> tuple[float, bool]: - """Walked-trajectory length between the poses nearest start and goal. + """Shortest walked length the trajectory demonstrates between start and goal. - Returns (length, snapped). When either endpoint is farther than max_snap_m - from the trajectory, falls back to the straight-line distance. + The robot usually passes each spot several times, so the reference is the + minimum arc distance over every combination of start and goal visits, not + the arc between single nearest poses, which would include any wandering in + between. Returns (length, snapped). When either endpoint is farther than + max_snap_m from the trajectory, falls back to the straight-line distance. """ foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) s = np.asarray(start, dtype=np.float32) g = np.asarray(goal, dtype=np.float32) ds = np.linalg.norm(foot - s, axis=1) dg = np.linalg.norm(foot - g, axis=1) - i, j = int(ds.argmin()), int(dg.argmin()) - if ds[i] > max_snap_m or dg[j] > max_snap_m: + if ds.min() > max_snap_m or dg.min() > max_snap_m: return float(np.linalg.norm(g - s)), False arcs = trajectory.arc_lengths() - length = abs(float(arcs[j] - arcs[i])) + float(ds[i]) + float(dg[j]) + near_s = np.flatnonzero(ds <= max_snap_m) + near_g = np.flatnonzero(dg <= max_snap_m) + pair_arcs = np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) + best = np.unravel_index(pair_arcs.argmin(), pair_arcs.shape) + i, j = int(near_s[best[0]]), int(near_g[best[1]]) + length = float(pair_arcs[best]) + float(ds[i]) + float(dg[j]) return max(length, 1e-6), True diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index b1956665da..ec80ca1324 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -33,9 +33,7 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.golden import ( - keys_contain, load_or_build_golden, - voxel_keys, ) from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.utils.data import resolve_named_path @@ -97,8 +95,6 @@ class PlannerArtifacts: class DatasetResult: dataset: str cases: list[CaseResult] - walked_path_valid: bool - false_obstacle_rate: float online_voxels: int golden_voxels: int map_build_ms: float @@ -113,7 +109,6 @@ class Report: score: float score_soft: float planner_score: float - false_obstacle_rate: float n_cases: int n_success: int attribution_counts: dict[str, int] @@ -177,43 +172,13 @@ def _attribution(online: PlanOutcome, golden: PlanOutcome) -> str: def run_suite(suite: Suite, cfg: EvalConfig) -> DatasetResult: db_path = resolve_named_path(suite.dataset, ".db") trajectory = load_trajectory(db_path, suite.odom_stream) - golden = load_or_build_golden( - db_path, - suite, - cfg, - corridor_radius=cfg.robot_radius + 0.1, - corridor_z_lo=-cfg.robot_height, - corridor_z_hi=-cfg.robot_height + cfg.body_clearance + cfg.voxel_size, - ) - obstacle_keys = golden.obstacle_keys() - - # Calibration invariant: the physically walked path must pass the gate. - # A failure here means the gate or corridor geometry is wrong, not the planner. - foot_path = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) - walked_gate = metrics.check_path( - foot_path, - obstacle_keys, - cfg.voxel_size, - cfg.robot_radius, - cfg.ground_margin, - cfg.body_clearance, - ) - if not walked_gate.valid: - logger.warning( - "%s: walked trajectory fails the collision gate at %d samples; " - "case validity is unreliable", - suite.dataset, - len(walked_gate.collision_points), - ) + golden = load_or_build_golden(db_path, suite, cfg) + obstacle_keys = golden.occupied_keys # The online map equals the golden map while both use the same mapper # config, so reuse it instead of replaying the recording a second time. # Tier 2 replay and a separate golden mapper config will change this. online_points = golden.occupied - online_occupied = keys_contain( - np.sort(voxel_keys(online_points, cfg.voxel_size)), golden.walked_keys - ) - false_obstacle_rate = float(online_occupied.mean()) if len(golden.walked_keys) else 0.0 golden_planner = cfg.make_planner() golden_planner.update_global_map(golden.occupied) @@ -265,8 +230,6 @@ def snapshot(planner: MLSPlanner) -> PlannerArtifacts: return DatasetResult( dataset=suite.dataset, cases=results, - walked_path_valid=walked_gate.valid, - false_obstacle_rate=false_obstacle_rate, online_voxels=len(online_points), golden_voxels=len(golden.occupied), map_build_ms=golden.build_ms, @@ -296,12 +259,10 @@ def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = for c in cases: attribution_counts[c.attribution] = attribution_counts.get(c.attribution, 0) + 1 - rates = [d.false_obstacle_rate for d in datasets] return Report( score=float(np.average(online_spl, weights=weights)), score_soft=float(np.average(soft, weights=weights)), planner_score=float(np.average(golden_spl, weights=weights)), - false_obstacle_rate=float(np.mean(rates)) if rates else 0.0, n_cases=len(cases), n_success=sum(c.online.success for c in cases), attribution_counts=attribution_counts, diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 74138345b0..3d695333c8 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -14,16 +14,26 @@ from __future__ import annotations +from types import SimpleNamespace + import numpy as np import pytest from dimos.navigation.nav_3d.evaluator import metrics -from dimos.navigation.nav_3d.evaluator.cases import load_suite +from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator.generate import ( + Candidate, + GenerationParams, + _select_diverse, + drift_stats, + generate_cases, + snap_to_surface, +) from dimos.navigation.nav_3d.evaluator.golden import ( + GoldenMap, key_centers, keys_contain, voxel_keys, - walked_corridor_keys, ) from dimos.navigation.nav_3d.evaluator.recording import Trajectory @@ -85,22 +95,6 @@ def test_gate_tolerates_stair_slope() -> None: assert _gate(path, slope).valid -def test_walked_corridor_exempts_gate() -> None: - """A wall crossing carved by the walked corridor passes the gate.""" - wall = _wall(0.0) - path = np.array([[-1, 0, 0], [1, 0, 0]], dtype=np.float32) - traj = Trajectory( - ts=np.array([0.0, 1.0]), - positions=np.array([[-1, 0, 0.3], [1, 0, 0.3]], dtype=np.float32), - ) - walked = walked_corridor_keys(traj, VOXEL, radius=0.3, z_lo=-0.3, z_hi=0.3) - obstacles = np.setdiff1d(np.unique(voxel_keys(wall, VOXEL)), walked) - result = metrics.check_path( - path, obstacles, VOXEL, robot_radius=0.16, ground_margin=0.25, body_clearance=0.45 - ) - assert result.valid - - def test_spl() -> None: assert metrics.spl(False, 10.0, 10.0) == 0.0 assert metrics.spl(True, 10.0, 10.0) == 1.0 @@ -120,6 +114,18 @@ def test_reference_length_snaps_to_trajectory() -> None: assert not snapped +def test_reference_length_uses_shortest_revisit() -> None: + """An out-and-back trajectory must not inflate the reference with the loop.""" + out = np.stack([np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1) + detour = np.stack([np.full(101, 10.0), np.linspace(0, 30, 101), np.full(101, 0.3)], axis=1) + back = detour[::-1] + positions = np.concatenate([out, detour, back]).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 30, len(positions)), positions=positions) + l_ref, snapped = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) + assert snapped + assert l_ref == pytest.approx(10.0, abs=0.2) + + def test_path_length_and_goal() -> None: path = np.array([[0, 0, 0], [3, 4, 0]], dtype=np.float32) assert metrics.path_length(path) == pytest.approx(5.0) @@ -127,6 +133,102 @@ def test_path_length_and_goal() -> None: assert not metrics.goal_reached(path, (3, 4, 1.0), tolerance=0.5) +def test_generate_cases_around_wall() -> None: + """A U-shaped walk around a wall must yield non-trivial cases spanning it.""" + wall_pts = _wall(10.0) + wall_keys = np.unique(voxel_keys(wall_pts, VOXEL)) + golden = GoldenMap( + voxel_size=VOXEL, + occupied=wall_pts, + occupied_keys=wall_keys, + frames=1, + add_frame_ms={"p50": 0.0, "p95": 0.0, "max": 0.0}, + build_ms=0.0, + ) + xs, ys = np.meshgrid(np.arange(0, 20, VOXEL), np.arange(-3, 6, VOXEL)) + surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) + + legs = [ + np.stack([np.linspace(2, 8, 40), np.zeros(40)], axis=1), + np.stack([np.full(40, 8.0), np.linspace(0, 4, 40)], axis=1), + np.stack([np.linspace(8, 12, 40), np.full(40, 4.0)], axis=1), + np.stack([np.full(40, 12.0), np.linspace(4, 0, 40)], axis=1), + np.stack([np.linspace(12, 18, 40), np.zeros(40)], axis=1), + ] + xy = np.concatenate(legs) + positions = np.column_stack([xy, np.full(len(xy), 0.3)]).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 60, len(positions)), positions=positions) + + cfg = SimpleNamespace( + robot_height=0.3, + voxel_size=VOXEL, + robot_radius=0.16, + ground_margin=0.25, + body_clearance=0.45, + ) + cases = generate_cases(traj, golden, surface, cfg, GenerationParams(max_cases=10)) + assert cases + assert len({c.id for c in cases}) == len(cases) + spans_wall = [c for c in cases if (c.start[0] - 10) * (c.goal[0] - 10) < 0] + assert spans_wall + for c in cases: + assert abs(c.start[2]) < 1e-5 and abs(c.goal[2]) < 1e-5 + assert "flat" in c.tags + + +def test_select_diverse_backfills_to_min_cases() -> None: + """Sector caps must not starve a dataset below the case floor.""" + candidates = [ + Candidate(start=(x, 0.0, 0.0), goal=(x, 20.0, 0.0), walked_m=30.0, detour_ratio=1.5, dz=0.0) + for x in np.arange(0.0, 16.0, 2.0) + ] + strict = _select_diverse(candidates, GenerationParams(min_cases=0), max_cases=12) + assert len(strict) == 4 + backfilled = _select_diverse(candidates, GenerationParams(min_cases=10), max_cases=12) + assert len(backfilled) == 8 + assert len({(c.start, c.goal) for c in backfilled}) == 8 + + +def test_snap_to_surface() -> None: + xs, ys = np.meshgrid(np.arange(0, 2, VOXEL), np.arange(0, 2, VOXEL)) + surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) + snapped = snap_to_surface(np.array([1.0, 1.0, 0.4], dtype=np.float32), surface, 1.0) + assert snapped is not None + assert abs(snapped[2]) < 1e-6 + assert np.linalg.norm(snapped[:2] - [1.0, 1.0]) < VOXEL + assert snap_to_surface(np.array([9.0, 9.0, 0.0], dtype=np.float32), surface, 1.0) is None + assert snap_to_surface(np.array([1.0, 1.0, 5.0], dtype=np.float32), surface, 1.0) is None + + +def test_drift_stats_flags_z_mismatch() -> None: + n = 400 + ts = np.linspace(0, 120, n) + out = np.stack([np.linspace(0, 20, n // 2), np.zeros(n // 2), np.zeros(n // 2)], axis=1) + back = np.stack([np.linspace(20, 0, n // 2), np.zeros(n // 2), np.full(n // 2, 0.6)], axis=1) + drifty = Trajectory(ts=ts, positions=np.concatenate([out, back]).astype(np.float32)) + stats = drift_stats(drifty) + assert stats.revisit_dz_p95 > 0.3 + assert stats.warnings + + clean = Trajectory(ts=ts, positions=np.concatenate([out, out[::-1]]).astype(np.float32)) + assert not drift_stats(clean).warnings + + +def test_save_suite_roundtrip(tmp_path) -> None: + suite = Suite( + dataset="demo", + cases=[Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), weight=2.0, tags=["x"])], + lidar_stream="other_lidar", + ) + path = save_suite(suite, tmp_path / "demo.yaml") + loaded = load_suite(path) + assert loaded.dataset == "demo" + assert loaded.lidar_stream == "other_lidar" + assert loaded.odom_stream == "pointlio_odometry" + assert loaded.cases[0].goal == (1.0, 2.0, 3.0) + assert loaded.cases[0].tags == ["x"] + + def test_load_suite(tmp_path) -> None: manifest = tmp_path / "demo.yaml" manifest.write_text( diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 19ec38aef6..4c2e479314 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -16,15 +16,14 @@ One static scene per dataset: - map/obstacles: golden voxels, turbo colormap by height -- map/walked_free: voxels the robot's body swept while recording, so they are - proven free space and exempt from the collision gate (magenta) - walked_path: the recorded foot path (white) - planner_online, planner_golden: the planner graph each map produced. Surface cells colored by wall clearance (red inside the hard clearance), nodes yellow, edges colored white to red by log traversal cost. - cases/: start (cyan), goal (orange), online and golden planned paths colored by verdict (green valid, red gate-invalid, yellow unreached), and - the gate's collision samples (red dots) + the gate's collision samples (red dots). Failed cases also get a thin red + start-to-goal line so the intended connection is visible even with no path. """ from __future__ import annotations @@ -34,7 +33,7 @@ import numpy as np import rerun as rr -from dimos.navigation.nav_3d.evaluator.golden import keys_contain, load_or_build_golden, voxel_keys +from dimos.navigation.nav_3d.evaluator.golden import load_or_build_golden from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.utils.data import resolve_named_path @@ -47,7 +46,6 @@ from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.runner import PlannerArtifacts, PlanOutcome, Report -WALKED_FREE_COLOR = [230, 60, 230] WALKED_PATH_COLOR = [255, 255, 255] START_COLOR = [0, 255, 255] GOAL_COLOR = [255, 140, 0] @@ -152,28 +150,16 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - for dataset in report.datasets: suite = suites_by_dataset[dataset.dataset] db_path = resolve_named_path(suite.dataset, ".db") - golden = load_or_build_golden( - db_path, - suite, - cfg, - corridor_radius=cfg.robot_radius + 0.1, - corridor_z_lo=-cfg.robot_height, - corridor_z_hi=-cfg.robot_height + cfg.body_clearance + cfg.voxel_size, - ) + golden = load_or_build_golden(db_path, suite, cfg) trajectory = load_trajectory(db_path, suite.odom_stream) root = dataset.dataset - walked_free = keys_contain(golden.walked_keys, voxel_keys(golden.occupied, cfg.voxel_size)) - obstacles = golden.occupied[~walked_free] rr.log( f"{root}/map/obstacles", - rr.Points3D(obstacles, colors=_turbo_by_height(obstacles), radii=cfg.voxel_size / 4), - static=True, - ) - rr.log( - f"{root}/map/walked_free", rr.Points3D( - golden.occupied[walked_free], colors=[WALKED_FREE_COLOR], radii=cfg.voxel_size / 3 + golden.occupied, + colors=_turbo_by_height(golden.occupied), + radii=cfg.voxel_size / 4, ), static=True, ) @@ -199,6 +185,14 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - rr.Points3D([case.goal], colors=[GOAL_COLOR], radii=0.12), static=True, ) + if not case.online.success: + rr.log( + f"{base}/intent", + rr.LineStrips3D( + [[case.start, case.goal]], colors=[INVALID_PATH_COLOR], radii=0.003 + ), + static=True, + ) _log_path(f"{base}/online", case.online, radius=0.04) _log_path(f"{base}/golden", case.golden, radius=0.02) From 44f19d2fd0814c762ec1655727cdb021e2e9b105 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 18:19:06 -0700 Subject: [PATCH 04/29] Incremental tests as well --- .../nav_3d/evaluator/cases/china_office.yaml | 170 ++++----- .../evaluator/cases/mid360_athens_stairs.yaml | 82 ++--- dimos/navigation/nav_3d/evaluator/cli.py | 47 ++- dimos/navigation/nav_3d/evaluator/config.py | 2 +- .../navigation/nav_3d/evaluator/final_map.py | 325 ++++++++++++++++++ dimos/navigation/nav_3d/evaluator/generate.py | 71 ++-- dimos/navigation/nav_3d/evaluator/golden.py | 181 ---------- dimos/navigation/nav_3d/evaluator/metrics.py | 50 ++- dimos/navigation/nav_3d/evaluator/runner.py | 285 ++++++++++----- .../nav_3d/evaluator/test_evaluator.py | 109 +++++- dimos/navigation/nav_3d/evaluator/viz.py | 32 +- 11 files changed, 839 insertions(+), 515 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/final_map.py delete mode 100644 dimos/navigation/nav_3d/evaluator/golden.py diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml index 821187f069..396ac7fd2b 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -6,162 +6,162 @@ cases: weight: 3.0 tags: [auto, stairs, up, long] - id: auto_01_down - start: [-8.12, 14.6, 2.56] - goal: [-10.2, 15.8, -0.48] + start: [-3.72, -4.6, 2.96] + goal: [0.04, 0.04, -0.4] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_02_down - start: [-3.64, -0.76, 3.04] - goal: [-1.4, -1.8, -0.4] - weight: 3.0 - tags: [auto, stairs, down, long] -- id: auto_03_up - start: [-1.4, -1.8, -0.4] - goal: [-3.64, -0.76, 3.04] +- id: auto_02_up + start: [4.12, -21.72, -0.48] + goal: [9.56, -14.28, 4.24] weight: 3.0 tags: [auto, stairs, up, long] -- id: auto_04_up - start: [1.24, -17.64, 1.28] - goal: [11.48, -13.56, 4.32] +- id: auto_03_down + start: [2.52, 10.84, 3.12] + goal: [5.64, 23.64, -0.24] weight: 3.0 - tags: [auto, stairs, up, long] -- id: auto_05_down - start: [11.48, -13.56, 4.32] - goal: [1.24, -17.64, 1.28] + tags: [auto, stairs, down, long] +- id: auto_04_down + start: [10.44, -4.12, 4.16] + goal: [16.52, -32.28, -0.96] weight: 3.0 tags: [auto, stairs, down, long] +- id: auto_05_up + start: [18.2, -20.6, -1.36] + goal: [-1.4, -14.2, 2.96] + weight: 3.0 + tags: [auto, stairs, up, long] - id: auto_06_down - start: [3.72, 9.64, 3.12] - goal: [5.64, 23.64, -0.24] + start: [-4.6, 4.52, 6.32] + goal: [11.72, 6.36, -0.96] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_07_up - start: [5.64, 23.64, -0.24] - goal: [3.72, 9.64, 3.12] +- id: auto_07_down + start: [6.36, 2.6, 3.2] + goal: [15.4, -9.88, -1.28] weight: 3.0 - tags: [auto, stairs, up, long] + tags: [auto, stairs, down, long] - id: auto_08_up - start: [8.2, -32.52, -0.48] - goal: [8.28, -2.04, 4.16] + start: [7.72, -33.8, -0.72] + goal: [-3.32, 11.32, 2.88] weight: 3.0 tags: [auto, stairs, up, long] - id: auto_09_down - start: [8.28, -2.04, 4.16] - goal: [8.2, -32.52, -0.48] + start: [12.2, -9.24, 4.16] + goal: [3.56, -7.72, -0.4] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_10_up - start: [18.84, -23.96, -1.36] - goal: [-4.6, 8.2, 6.56] +- id: auto_10_down + start: [-7.4, 7.32, 2.8] + goal: [9.8, 14.04, -0.56] weight: 3.0 - tags: [auto, stairs, up, long] + tags: [auto, stairs, down, long] - id: auto_11_down - start: [-4.6, 8.2, 6.56] - goal: [18.84, -23.96, -1.36] + start: [-2.52, 0.68, 4.0] + goal: [13.4, -1.16, -1.2] weight: 3.0 tags: [auto, stairs, down, long] - id: auto_12_down - start: [-1.72, -10.28, 2.96] - goal: [11.96, 5.4, -0.96] + start: [6.28, -2.04, 4.16] + goal: [3.32, 16.12, -0.24] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_13_up - start: [11.96, 5.4, -0.96] - goal: [-1.72, -10.28, 2.96] - weight: 3.0 - tags: [auto, stairs, up, long] -- id: auto_14_down - start: [5.4, 3.32, 3.2] - goal: [14.52, -6.36, -1.28] +- id: auto_13_down + start: [-1.72, -9.72, 2.96] + goal: [5.64, -13.56, -0.4] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_15_up - start: [14.52, -6.36, -1.28] - goal: [5.4, 3.32, 3.2] +- id: auto_14_up + start: [7.48, -27.48, -0.48] + goal: [-6.2, 3.48, 2.88] weight: 3.0 tags: [auto, stairs, up, long] -- id: auto_16_down - start: [-1.72, 12.52, 2.96] - goal: [17.4, -32.76, -0.96] +- id: auto_15_up + start: [19.8, -27.8, -1.12] + goal: [2.6, -16.2, 2.08] weight: 3.0 - tags: [auto, stairs, down, long] -- id: auto_17_up - start: [17.4, -32.76, -0.96] - goal: [-1.72, 12.52, 2.96] + tags: [auto, stairs, up, long] +- id: auto_16_up + start: [0.6, 6.6, -0.4] + goal: [2.04, -0.28, 3.12] weight: 3.0 tags: [auto, stairs, up, long] +- id: auto_17_down + start: [4.28, 5.96, 3.12] + goal: [17.0, -15.48, -1.28] + weight: 3.0 + tags: [auto, stairs, down, long] - id: auto_18_down - start: [11.24, -6.76, 4.16] - goal: [9.24, 15.56, -0.48] + start: [-4.6, 8.2, 6.56] + goal: [8.84, 18.84, -0.4] weight: 3.0 tags: [auto, stairs, down, long] - id: auto_19_up - start: [9.24, 15.56, -0.48] - goal: [11.24, -6.76, 4.16] + start: [-8.84, 10.76, -0.48] + goal: [-1.4, -2.04, 3.12] weight: 3.0 tags: [auto, stairs, up, long] - id: auto_20_up - start: [6.04, -24.2, -0.48] - goal: [-7.72, 5.96, 2.88] + start: [14.6, -27.88, -0.24] + goal: [1.0, 13.56, 2.96] weight: 3.0 tags: [auto, stairs, up, long] - id: auto_21_down - start: [-7.72, 5.96, 2.88] - goal: [6.04, -24.2, -0.48] + start: [-7.4, 11.96, 3.04] + goal: [8.2, -17.32, -0.4] weight: 3.0 tags: [auto, stairs, down, long] - id: auto_22_up - start: [17.0, -15.48, -1.28] - goal: [1.96, -0.28, 3.12] + start: [-0.36, -4.44, -0.4] + goal: [11.24, -6.76, 4.16] weight: 3.0 tags: [auto, stairs, up, long] -- id: auto_23_down - start: [1.96, -0.28, 3.12] - goal: [17.0, -15.48, -1.28] +- id: auto_23_up + start: [-0.28, 10.92, -0.4] + goal: [0.6, -12.52, 3.12] weight: 3.0 - tags: [auto, stairs, down, long] + tags: [auto, stairs, up, long] - id: auto_24_flat - start: [5.72, -9.72, -0.4] - goal: [4.36, -11.96, -0.48] + start: [12.52, -33.4, -0.8] + goal: [14.52, -5.96, -1.28] weight: 1.0 tags: [auto, flat] - id: auto_25_flat - start: [8.2, -17.32, -0.4] - goal: [3.32, 16.12, -0.24] + start: [10.68, 9.88, -0.72] + goal: [5.24, 12.52, -0.24] weight: 1.0 tags: [auto, flat] - id: auto_26_flat - start: [10.76, 10.12, -0.72] - goal: [14.6, -27.88, -0.24] + start: [19.16, -24.44, -1.36] + goal: [12.6, 2.68, -1.04] weight: 1.0 tags: [auto, flat] - id: auto_27_flat - start: [1.08, 4.84, -0.4] - goal: [13.56, -32.28, -0.88] + start: [3.56, -18.36, 0.16] + goal: [8.92, -13.32, -0.32] weight: 1.0 tags: [auto, flat] - id: auto_28_flat - start: [13.32, -0.28, -1.12] - goal: [17.88, -19.64, -1.36] + start: [-4.92, 11.0, -0.4] + goal: [10.6, -34.92, -0.8] weight: 1.0 tags: [auto, flat] - id: auto_29_flat - start: [-5.64, 9.8, -0.4] - goal: [-8.52, 10.52, -0.48] + start: [-0.12, -17.08, 2.08] + goal: [2.76, 7.4, 1.6] weight: 1.0 tags: [auto, flat] - id: auto_30_flat - start: [10.6, -34.92, -0.8] - goal: [0.52, -6.28, -0.4] + start: [6.04, -24.2, -0.48] + goal: [7.0, 14.12, -0.08] weight: 1.0 tags: [auto, flat] - id: auto_31_flat - start: [-1.08, -16.6, 2.64] - goal: [-2.76, -15.48, 3.04] + start: [8.36, -30.44, -0.48] + goal: [18.28, -32.28, -0.96] weight: 1.0 tags: [auto, flat] - id: auto_32_flat - start: [4.04, 5.16, 0.16] - goal: [2.36, 11.96, -0.32] + start: [11.32, 8.12, -0.88] + goal: [5.72, -9.72, -0.4] weight: 1.0 tags: [auto, flat] diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index a9722fb1a0..0b721a21d3 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -1,82 +1,52 @@ dataset: mid360_athens_stairs cases: - id: auto_00_up - start: [-2.52, -0.52, -0.32] + start: [-0.12, -0.6, -0.32] goal: [1.32, -0.84, 2.72] weight: 3.0 tags: [auto, stairs, up, long] -- id: auto_01_down - start: [1.32, -0.84, 2.72] - goal: [-2.52, -0.52, -0.32] - weight: 3.0 - tags: [auto, stairs, down, long] -- id: auto_02_down - start: [6.44, -5.56, -1.44] - goal: [7.24, -3.96, -6.08] - weight: 3.0 - tags: [auto, stairs, down, long] -- id: auto_03_up +- id: auto_01_up start: [7.24, -3.96, -6.08] goal: [6.44, -5.56, -1.44] weight: 3.0 tags: [auto, stairs, up, long] -- id: auto_04_up - start: [0.68, -4.12, -0.32] - goal: [8.04, -0.76, 3.04] - weight: 3.0 - tags: [auto, stairs, up, long] -- id: auto_05_down +- id: auto_02_down start: [8.04, -0.76, 3.04] - goal: [0.68, -4.12, -0.32] + goal: [-2.36, -4.36, -0.32] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_06_up - start: [-2.36, -4.36, -0.32] - goal: [5.88, -4.52, 2.96] - weight: 3.0 - tags: [auto, stairs, up, long] -- id: auto_07_down +- id: auto_03_down start: [5.88, -4.52, 2.96] - goal: [-2.36, -4.36, -0.32] + goal: [2.28, -6.52, -0.32] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_08_up - start: [-2.04, 3.16, -0.48] - goal: [-0.2, -3.0, 2.56] - weight: 3.0 - tags: [auto, stairs, up, long] -- id: auto_09_down +- id: auto_04_down start: [-0.2, -3.0, 2.56] goal: [-2.04, 3.16, -0.48] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_10_up - start: [5.96, -3.64, -3.52] - goal: [0.12, -0.76, -0.32] +- id: auto_05_down + start: [0.52, -4.68, -0.32] + goal: [5.96, -3.64, -3.52] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_06_up + start: [6.36, -5.4, -4.48] + goal: [-2.36, 0.44, -0.4] weight: 3.0 tags: [auto, stairs, up, long] -- id: auto_11_down - start: [0.12, -0.76, -0.32] - goal: [5.96, -3.64, -3.52] +- id: auto_07_down + start: [6.68, -5.56, 2.08] + goal: [5.72, -5.48, -1.04] weight: 3.0 tags: [auto, stairs, down, long] -- id: auto_12_flat - start: [5.16, -3.96, -3.44] - goal: [7.48, -3.8, -2.96] - weight: 1.0 - tags: [auto, flat] -- id: auto_13_flat - start: [-2.04, 3.16, -0.48] - goal: [5.48, -5.56, -0.96] - weight: 1.0 - tags: [auto, flat] -- id: auto_14_flat - start: [6.28, -5.64, 2.24] - goal: [-0.04, -1.96, 2.56] - weight: 1.0 - tags: [auto, flat] -- id: auto_15_flat - start: [7.24, -3.96, -6.08] - goal: [5.08, -3.8, -6.48] +- id: auto_08_down + start: [-0.2, -3.0, 2.56] + goal: [7.16, -3.8, -3.12] + weight: 3.0 + tags: [auto, stairs, down, long] +- id: auto_09_flat + start: [5.08, -3.8, -6.48] + goal: [7.24, -3.96, -6.08] weight: 1.0 tags: [auto, flat] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 24e11d5302..cb423706f0 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -26,6 +26,7 @@ import dataclasses import json +import os from pathlib import Path import sqlite3 from typing import TYPE_CHECKING @@ -42,13 +43,13 @@ save_suite, ) from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.generate import ( GenerationParams, drift_stats, generate_cases, snap_to_surface, ) -from dimos.navigation.nav_3d.evaluator.golden import load_or_build_golden from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate from dimos.utils.data import get_data_dir, resolve_named_path @@ -73,29 +74,35 @@ def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: def _print_report(report: Report) -> None: - header = f"{'case':<28} {'dataset':<22} {'attr':<8} {'spl':>5} {'len':>6} {'ref':>6} {'ms':>7}" + header = ( + f"{'case':<28} {'dataset':<22} {'inc':>5} {'fin':>5} " + f"{'len':>6} {'ref':>6} {'vox':>8} {'ms':>7}" + ) print(header) print("-" * len(header)) for d in report.datasets: for c in d.cases: print( - f"{c.id:<28} {c.dataset:<22} {c.attribution:<8} " - f"{c.online.spl:>5.2f} {c.online.length:>6.1f} {c.l_ref:>6.1f} " - f"{c.online.plan_ms:>7.1f}" + f"{c.id:<28} {c.dataset:<22} " + f"{c.online.spl:>5.2f} {c.final.spl:>5.2f} " + f"{c.online.length:>6.1f} {c.l_ref:>6.1f} " + f"{c.online_voxels:>8d} {c.online.plan_ms:>7.1f}" ) print("-" * len(header)) for d in report.datasets: print( - f"{d.dataset}: {d.frames} frames, online {d.online_voxels} / " - f"golden {d.golden_voxels} voxels, " + f"{d.dataset}: {d.frames} frames, " + f"final {d.final_voxels} voxels, " f"map build {d.map_build_ms / 1000:.1f}s" ) print( f"\nscore {report.score:.3f} | soft {report.score_soft:.3f} | " - f"planner {report.planner_score:.3f} | " - f"success {report.n_success}/{report.n_cases} | " - f"attribution {report.attribution_counts} | " - f"plan p95 {report.plan_ms['p95']:.1f}ms" + f"final {report.final_score:.3f} | " + f"success inc {report.n_success}/{report.n_cases} " + f"fin {report.n_success_final}/{report.n_cases} | " + f"outcomes {report.outcome_counts} | " + f"plan p95 {report.plan_ms['p95']:.1f}ms | " + f"map update p95 {report.map_update_ms['p95']:.0f}ms" ) @@ -107,7 +114,11 @@ def run( dataset: str = typer.Option(None, "--dataset", help="Only run suites for this dataset"), json_out: Path = typer.Option(None, "--json", help="Write the full report as JSON"), rrd_out: Path = typer.Option(None, "--rrd", help="Write a rerun recording of every case"), - workers: int = typer.Option(1, "--workers", help="Datasets evaluated in parallel processes"), + workers: int = typer.Option( + os.cpu_count() or 1, + "--workers", + help="Total parallelism: dataset processes x checkpoint threads", + ), set_: list[str] = typer.Option( None, "--set", help="Repeatable EvalConfig override, e.g. wall_clearance_m=0.05" ), @@ -199,11 +210,11 @@ def ingest( print(f"WARNING: {warning}") cfg = EvalConfig() - golden = load_or_build_golden(dest, suite, cfg) + final = load_or_build_final_map(dest, suite, cfg) planner = cfg.make_planner() - planner.update_global_map(golden.occupied) + planner.update_global_map(final.occupied) gen = GenerationParams(max_cases=max_cases or None) - suite.cases = generate_cases(trajectory, golden, planner.surface_map(), cfg, gen) + suite.cases = generate_cases(trajectory, final, planner.surface_map(), cfg, gen) if not suite.cases: raise typer.Exit(code=1) floor = min(gen.min_cases, gen.resolve_max_cases(float(arcs[-1]))) @@ -229,15 +240,15 @@ def add_case( weight: float = typer.Option(2.0, "--weight"), snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), ) -> None: - """Append a curated case, with both endpoints snapped to the golden surface.""" + """Append a curated case, with both endpoints snapped to the final surface.""" manifest = CASES_DIR / f"{dataset}.yaml" if not manifest.exists(): raise typer.BadParameter(f"no manifest {manifest}; run ingest first") suite = load_suite(manifest) cfg = EvalConfig() - golden = load_or_build_golden(resolve_named_path(dataset, ".db"), suite, cfg) + final = load_or_build_final_map(resolve_named_path(dataset, ".db"), suite, cfg) planner = cfg.make_planner() - planner.update_global_map(golden.occupied) + planner.update_global_map(final.occupied) surface = planner.surface_map() case = Case( diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index f8348c8744..db2694e31a 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -82,7 +82,7 @@ def make_planner(self) -> MLSPlanner: ) def mapper_fingerprint(self) -> dict[str, float | int]: - """The mapper parameters that determine golden map content.""" + """The mapper parameters that determine final map content.""" return { "voxel_size": self.voxel_size, "max_range": self.max_range, diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py new file mode 100644 index 0000000000..a526565df6 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -0,0 +1,325 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Final map: the mapper's output over every frame of a recording. + +Not ground truth, just the most complete map the pipeline produces, so it +serves as the collision reference for returned paths. The same replay also +produces incremental checkpoints: the occupied set at chosen mid-recording +times, which is what the robot had seen by then. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import itertools +import json +from time import perf_counter +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from collections.abc import Iterable, Iterator + from pathlib import Path + + from numpy.typing import NDArray + + from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper + from dimos.navigation.nav_3d.evaluator.cases import Suite + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + from dimos.navigation.nav_3d.evaluator.recording import Frame + +logger = setup_logger() + +_KEY_OFFSET = 1 << 20 + + +def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: + """Pack voxel indices into sortable int64 keys, one per point.""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET + return (idx[:, 0] << 42) | (idx[:, 1] << 21) | idx[:, 2] + + +def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: + """Voxel center positions for packed keys, the inverse of voxel_keys.""" + mask = (1 << 21) - 1 + idx = np.stack([keys >> 42, (keys >> 21) & mask, keys & mask], axis=1) - _KEY_OFFSET + return ((idx + 0.5) * voxel_size).astype(np.float32) + + +def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: + if len(sorted_keys) == 0: + return np.zeros(len(query), dtype=bool) + pos = np.clip(np.searchsorted(sorted_keys, query), 0, len(sorted_keys) - 1) + return np.asarray(sorted_keys[pos] == query) + + +def cylinder_offsets( + radius: float, z_lo: float, z_hi: float, voxel_size: float +) -> NDArray[np.int64]: + """Integer voxel offsets forming a vertical cylinder.""" + r_vox = int(np.ceil(radius / voxel_size)) + span = np.arange(-r_vox, r_vox + 1) + dx, dy = np.meshgrid(span, span, indexing="ij") + in_disc = (dx * voxel_size) ** 2 + (dy * voxel_size) ** 2 <= radius**2 + dz = np.arange(int(np.floor(z_lo / voxel_size)), int(np.ceil(z_hi / voxel_size)) + 1) + disc = np.stack([dx[in_disc], dy[in_disc]], axis=1) + out = np.concatenate([np.hstack([disc, np.full((len(disc), 1), z)]) for z in dz]) + return np.asarray(out, dtype=np.int64) + + +def offset_keys( + points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float +) -> NDArray[np.int64]: + """Keys of every (point voxel + offset) pair, shape (P * O,).""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET + swept = idx[:, None, :] + offsets[None, :, :] + return np.asarray((swept[..., 0] << 42) | (swept[..., 1] << 21) | swept[..., 2]) + + +def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: + """Resample a polyline so consecutive samples are at most step apart.""" + if len(points) < 2: + return points.astype(np.float32) + out = [points[:1]] + for a, b in itertools.pairwise(points): + seg = np.linalg.norm(b - a) + n = max(int(np.ceil(seg / step)), 1) + t = np.linspace(0.0, 1.0, n + 1)[1:, None] + out.append(a[None, :] * (1 - t) + b[None, :] * t) + return np.concatenate(out).astype(np.float32) + + +@dataclass +class FinalMap: + voxel_size: float + occupied: NDArray[np.float32] + occupied_keys: NDArray[np.int64] + frames: int + add_frame_ms: dict[str, float] + build_ms: float + + +@dataclass +class MapCheckpoints: + """Map state at increasing times, delta-encoded between snapshots. + + occupied tracks the mapper's healthy voxels. observed tracks every voxel a + raw lidar return ever landed in, mapper-independent, so it only grows. + """ + + times: NDArray[np.float64] + added: list[NDArray[np.int64]] + removed: list[NDArray[np.int64]] + observed_added: list[NDArray[np.int64]] + + def iter_snapshots(self) -> Iterator[tuple[NDArray[np.int64], NDArray[np.int64]]]: + """Yield (occupied_keys, newly_observed_keys) in time order. + + occupied comes as the full sorted set. observed only ever grows and + gets intersected by its consumer, so it arrives as the delta since + the previous checkpoint instead of the accumulated multi-million-key + set. + """ + keys = np.array([], dtype=np.int64) + for add, rem, obs in zip(self.added, self.removed, self.observed_added, strict=True): + keys = np.union1d(np.setdiff1d(keys, rem, assume_unique=True), add) + yield keys, obs + + +CACHE_VERSION = 3 +CHECKPOINT_CACHE_VERSION = 2 + + +def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: + digest = hashlib.sha1(json.dumps(params, sort_keys=True).encode()).hexdigest()[:10] + return db_path.parent / ".final" / f"{db_path.stem}.{digest}.npz" + + +def _final_params(suite: Suite, cfg: EvalConfig) -> dict[str, float | int | str]: + return { + **cfg.mapper_fingerprint(), + "align_tol": cfg.align_tol, + "lidar_stream": suite.lidar_stream, + "odom_stream": suite.odom_stream, + "cache_version": CACHE_VERSION, + } + + +def replay_frames( + frames: Iterable[Frame], + mapper: VoxelRayMapper, + voxel_size: float, + times: NDArray[np.float64], +) -> tuple[FinalMap, list[NDArray[np.int64]], list[NDArray[np.int64]]]: + """Feed frames through the mapper in order, snapshotting state as each + requested time is passed. A snapshot holds exactly the frames with + ts <= its time. Times past the last frame get the final state. Returns + the final map, the occupied-key snapshots, and the observed-key snapshots + (every voxel a raw lidar return had landed in by that time). + """ + snapshots: list[NDArray[np.int64]] = [] + observed_snapshots: list[NDArray[np.int64]] = [] + observed = np.array([], dtype=np.int64) + pending: list[NDArray[np.int64]] = [] + + def merged() -> NDArray[np.int64]: + nonlocal observed + if pending: + observed = np.union1d(observed, np.concatenate(pending)) + pending.clear() + return observed + + add_ms: list[float] = [] + t0 = perf_counter() + for frame in frames: + while len(snapshots) < len(times) and frame.ts > times[len(snapshots)]: + snapshots.append(np.unique(voxel_keys(mapper.global_map(), voxel_size))) + observed_snapshots.append(merged()) + t1 = perf_counter() + mapper.add_frame(frame.points, frame.origin) + add_ms.append((perf_counter() - t1) * 1000) + pts = frame.points[np.isfinite(frame.points).all(axis=1)] + pending.append(np.unique(voxel_keys(pts, voxel_size))) + if sum(len(p) for p in pending) > 4_000_000: + merged() + build_ms = (perf_counter() - t0) * 1000 + occupied = mapper.global_map() + occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) + final_observed = merged() + while len(snapshots) < len(times): + snapshots.append(occupied_keys) + observed_snapshots.append(final_observed) + add_arr = np.asarray(add_ms) if add_ms else np.zeros(1) + final = FinalMap( + voxel_size=voxel_size, + occupied=occupied, + occupied_keys=occupied_keys, + frames=len(add_ms), + add_frame_ms={ + "p50": float(np.percentile(add_arr, 50)), + "p95": float(np.percentile(add_arr, 95)), + "max": float(add_arr.max()), + }, + build_ms=build_ms, + ) + return final, snapshots, observed_snapshots + + +def _save_final(cache: Path, final: FinalMap) -> None: + cache.parent.mkdir(exist_ok=True) + np.savez_compressed( + cache, + occupied=final.occupied, + occupied_keys=final.occupied_keys, + frames=final.frames, + add_p50=final.add_frame_ms["p50"], + add_p95=final.add_frame_ms["p95"], + add_max=final.add_frame_ms["max"], + ) + logger.info("final map cached: %s (%d voxels)", cache.name, len(final.occupied)) + + +def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> FinalMap: + cache = _cache_path(db_path, _final_params(suite, cfg)) + if cache.exists(): + data = np.load(cache) + return FinalMap( + voxel_size=cfg.voxel_size, + occupied=data["occupied"], + occupied_keys=data["occupied_keys"], + frames=int(data["frames"]), + add_frame_ms={ + "p50": float(data["add_p50"]), + "p95": float(data["add_p95"]), + "max": float(data["add_max"]), + }, + build_ms=0.0, + ) + + logger.info("building final map for %s (cache miss)", db_path.name) + final, _, _ = replay_frames( + iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol), + cfg.make_mapper(), + cfg.voxel_size, + np.array([], dtype=np.float64), + ) + _save_final(cache, final) + return final + + +def encode_deltas( + snapshots: list[NDArray[np.int64]], +) -> tuple[list[NDArray[np.int64]], list[NDArray[np.int64]]]: + added: list[NDArray[np.int64]] = [] + removed: list[NDArray[np.int64]] = [] + prev = np.array([], dtype=np.int64) + for keys in snapshots: + added.append(np.setdiff1d(keys, prev, assume_unique=True)) + removed.append(np.setdiff1d(prev, keys, assume_unique=True)) + prev = keys + return added, removed + + +def load_or_build_checkpoints( + db_path: Path, suite: Suite, cfg: EvalConfig, times: NDArray[np.float64] +) -> MapCheckpoints: + """Occupied key sets at the requested times, deduped and sorted. + + A cache miss replays the whole recording once. The replay's final state + also fills the final cache when that is missing. + """ + times = np.unique(np.asarray(times, dtype=np.float64)) + params: dict[str, float | int | str] = { + **_final_params(suite, cfg), + "kind": "checkpoints", + "times_sha": hashlib.sha1(times.tobytes()).hexdigest()[:10], + "checkpoint_version": CHECKPOINT_CACHE_VERSION, + } + cache = _cache_path(db_path, params) + if cache.exists(): + data = np.load(cache) + n = len(data["times"]) + return MapCheckpoints( + times=data["times"], + added=[data[f"add_{i}"] for i in range(n)], + removed=[data[f"rem_{i}"] for i in range(n)], + observed_added=[data[f"obs_{i}"] for i in range(n)], + ) + + logger.info("building %d map checkpoints for %s (cache miss)", len(times), db_path.name) + final, snapshots, observed = replay_frames( + iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol), + cfg.make_mapper(), + cfg.voxel_size, + times, + ) + final_cache = _cache_path(db_path, _final_params(suite, cfg)) + if not final_cache.exists(): + _save_final(final_cache, final) + added, removed = encode_deltas(snapshots) + observed_added, _ = encode_deltas(observed) + arrays: dict[str, NDArray[np.int64] | NDArray[np.float64]] = {"times": times} + arrays |= {f"add_{i}": a for i, a in enumerate(added)} + arrays |= {f"rem_{i}": r for i, r in enumerate(removed)} + arrays |= {f"obs_{i}": o for i, o in enumerate(observed_added)} + cache.parent.mkdir(exist_ok=True) + np.savez_compressed(cache, **arrays) # type: ignore[arg-type] + logger.info("checkpoints cached: %s", cache.name) + return MapCheckpoints(times=times, added=added, removed=removed, observed_added=observed_added) diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 6ace4da516..719c7428b5 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -16,9 +16,13 @@ Candidate pairs are sampled along the walked path, so both endpoints are physically proven reachable. A pair is kept only when it is non-trivial: -the straight start-goal line collides with golden obstacles, the walked +the straight start-goal line collides with final obstacles, the walked route detours well past the straight-line distance, or the pair climbs. -Endpoints snap to the golden surface so drift between passes cannot leave +Every case points backward in time: the goal is a spot the robot had +already visited when it stood at the start, so an incremental map built up +to the start time has seen the goal and a demonstrated route. The forward +direction is emitted too when the start is revisited after the goal. +Endpoints snap to the final surface so drift between passes cannot leave a case floating off the map. Generation is deterministic. """ @@ -36,7 +40,7 @@ from numpy.typing import NDArray from dimos.navigation.nav_3d.evaluator.config import EvalConfig - from dimos.navigation.nav_3d.evaluator.golden import GoldenMap + from dimos.navigation.nav_3d.evaluator.final_map import FinalMap from dimos.navigation.nav_3d.evaluator.recording import Trajectory STAIRS_DZ_M = 0.5 @@ -119,13 +123,13 @@ def _subsample_indices(trajectory: Trajectory, spacing_m: float) -> NDArray[np.i def generate_cases( trajectory: Trajectory, - golden: GoldenMap, + final: FinalMap, surface: NDArray[np.float32], cfg: EvalConfig, params: GenerationParams | None = None, ) -> list[Case]: params = params or GenerationParams() - obstacle_keys = golden.occupied_keys + obstacle_keys = final.occupied_keys arcs = trajectory.arc_lengths() foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) @@ -143,6 +147,8 @@ def generate_cases( if not ok[ai]: continue sa = snaps[ai] + near_a = np.linalg.norm(foot - sa, axis=1) <= params.snap_max_m + last_visit_a = float(trajectory.ts[near_a].max()) if near_a.any() else -np.inf later = np.arange(ai + 1, len(idx)) later = later[ok[later]] if not len(later): @@ -171,19 +177,27 @@ def generate_cases( ).valid if not blocked: continue - cand = Candidate( - start=(float(sa[0]), float(sa[1]), float(sa[2])), - goal=(float(sb[0]), float(sb[1]), float(sb[2])), - walked_m=float(w), - detour_ratio=detour, - dz=dz, - ) - bins = np.floor(np.array([*sa[:2], *sb[:2]]) / params.bin_size_m).astype(int) - dz_sign = int(np.sign(dz)) if abs(dz) >= STAIRS_DZ_M else 0 - key = (*bins, dz_sign) - best = candidates.get(key) - if best is None or cand.priority > best.priority: - candidates[key] = cand + # Backward in time is always causal; forward only when the start + # spot is revisited after the goal visit. + directed = [(sb, sa, -dz)] + if last_visit_a >= float(trajectory.ts[idx[bi]]): + directed.append((sa, sb, dz)) + for p_start, p_goal, d_dz in directed: + cand = Candidate( + start=(float(p_start[0]), float(p_start[1]), float(p_start[2])), + goal=(float(p_goal[0]), float(p_goal[1]), float(p_goal[2])), + walked_m=float(w), + detour_ratio=detour, + dz=d_dz, + ) + bins = np.floor(np.array([*p_start[:2], *p_goal[:2]]) / params.bin_size_m).astype( + int + ) + dz_sign = int(np.sign(d_dz)) if abs(d_dz) >= STAIRS_DZ_M else 0 + key = (*bins, dz_sign) + best = candidates.get(key) + if best is None or cand.priority > best.priority: + candidates[key] = cand ranked = sorted(candidates.values(), key=lambda c: (-c.priority, c.start, c.goal)) selected = _select_diverse(ranked, params, params.resolve_max_cases(float(arcs[-1]))) @@ -200,16 +214,6 @@ def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> return False -def _reversed(cand: Candidate) -> Candidate: - return Candidate( - start=cand.goal, - goal=cand.start, - walked_m=cand.walked_m, - detour_ratio=cand.detour_ratio, - dz=-cand.dz, - ) - - def _select_diverse( ranked: list[Candidate], params: GenerationParams, max_cases: int ) -> list[Candidate]: @@ -217,11 +221,10 @@ def _select_diverse( its priority plus how far its endpoints are from every endpoint already in use. Coverage is the objective, not a filter, so cases spread across the map instead of fanning out of the highest-priority spot. A sector-usage - cap bounds hub reuse outright. Stair candidates also claim their reversed - direction, and the flat quota keeps stairs from crowding out flats. - When the strict pass yields fewer than min_cases, sector-capped candidates - are revived and a relaxed pass without sector caps or the flat quota - backfills up to the floor. + cap bounds hub reuse outright, and the flat quota keeps stairs from + crowding out flats. When the strict pass yields fewer than min_cases, + sector-capped candidates are revived and a relaxed pass without sector + caps or the flat quota backfills up to the floor. """ if not ranked: return [] @@ -281,8 +284,6 @@ def fill(target: int, relax: bool) -> None: used_points.append(starts[n]) used_points.append(goals[n]) bucket.append(cand) - if bucket is stairs and (relax or len(stairs) < stairs_cap): - bucket.append(_reversed(cand)) fill(max_cases, relax=False) min_cases = min(params.min_cases, max_cases) diff --git a/dimos/navigation/nav_3d/evaluator/golden.py b/dimos/navigation/nav_3d/evaluator/golden.py deleted file mode 100644 index 5e894436b5..0000000000 --- a/dimos/navigation/nav_3d/evaluator/golden.py +++ /dev/null @@ -1,181 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Golden reference map built from every frame of a recording. - -The golden occupancy is what returned paths are collision-checked against. -""" - -from __future__ import annotations - -from dataclasses import dataclass -import hashlib -import itertools -import json -from time import perf_counter -from typing import TYPE_CHECKING - -import numpy as np - -from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from pathlib import Path - - from numpy.typing import NDArray - - from dimos.navigation.nav_3d.evaluator.cases import Suite - from dimos.navigation.nav_3d.evaluator.config import EvalConfig - -logger = setup_logger() - -_KEY_OFFSET = 1 << 20 - - -def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: - """Pack voxel indices into sortable int64 keys, one per point.""" - idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - return (idx[:, 0] << 42) | (idx[:, 1] << 21) | idx[:, 2] - - -def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: - """Voxel center positions for packed keys, the inverse of voxel_keys.""" - mask = (1 << 21) - 1 - idx = np.stack([keys >> 42, (keys >> 21) & mask, keys & mask], axis=1) - _KEY_OFFSET - return ((idx + 0.5) * voxel_size).astype(np.float32) - - -def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: - if len(sorted_keys) == 0: - return np.zeros(len(query), dtype=bool) - pos = np.clip(np.searchsorted(sorted_keys, query), 0, len(sorted_keys) - 1) - return np.asarray(sorted_keys[pos] == query) - - -def cylinder_offsets( - radius: float, z_lo: float, z_hi: float, voxel_size: float -) -> NDArray[np.int64]: - """Integer voxel offsets forming a vertical cylinder.""" - r_vox = int(np.ceil(radius / voxel_size)) - span = np.arange(-r_vox, r_vox + 1) - dx, dy = np.meshgrid(span, span, indexing="ij") - in_disc = (dx * voxel_size) ** 2 + (dy * voxel_size) ** 2 <= radius**2 - dz = np.arange(int(np.floor(z_lo / voxel_size)), int(np.ceil(z_hi / voxel_size)) + 1) - disc = np.stack([dx[in_disc], dy[in_disc]], axis=1) - out = np.concatenate([np.hstack([disc, np.full((len(disc), 1), z)]) for z in dz]) - return np.asarray(out, dtype=np.int64) - - -def offset_keys( - points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float -) -> NDArray[np.int64]: - """Keys of every (point voxel + offset) pair, shape (P * O,).""" - idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - swept = idx[:, None, :] + offsets[None, :, :] - return np.asarray((swept[..., 0] << 42) | (swept[..., 1] << 21) | swept[..., 2]) - - -def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: - """Resample a polyline so consecutive samples are at most step apart.""" - if len(points) < 2: - return points.astype(np.float32) - out = [points[:1]] - for a, b in itertools.pairwise(points): - seg = np.linalg.norm(b - a) - n = max(int(np.ceil(seg / step)), 1) - t = np.linspace(0.0, 1.0, n + 1)[1:, None] - out.append(a[None, :] * (1 - t) + b[None, :] * t) - return np.concatenate(out).astype(np.float32) - - -@dataclass -class GoldenMap: - voxel_size: float - occupied: NDArray[np.float32] - occupied_keys: NDArray[np.int64] - frames: int - add_frame_ms: dict[str, float] - build_ms: float - - -CACHE_VERSION = 3 - - -def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: - digest = hashlib.sha1(json.dumps(params, sort_keys=True).encode()).hexdigest()[:10] - return db_path.parent / ".golden" / f"{db_path.stem}.{digest}.npz" - - -def load_or_build_golden(db_path: Path, suite: Suite, cfg: EvalConfig) -> GoldenMap: - params: dict[str, float | int | str] = { - **cfg.mapper_fingerprint(), - "align_tol": cfg.align_tol, - "lidar_stream": suite.lidar_stream, - "odom_stream": suite.odom_stream, - "cache_version": CACHE_VERSION, - } - voxel_size = cfg.voxel_size - cache = _cache_path(db_path, params) - if cache.exists(): - data = np.load(cache) - return GoldenMap( - voxel_size=voxel_size, - occupied=data["occupied"], - occupied_keys=data["occupied_keys"], - frames=int(data["frames"]), - add_frame_ms={ - "p50": float(data["add_p50"]), - "p95": float(data["add_p95"]), - "max": float(data["add_max"]), - }, - build_ms=0.0, - ) - - logger.info("building golden map for %s (cache miss)", db_path.name) - mapper = cfg.make_mapper() - add_ms: list[float] = [] - t0 = perf_counter() - for frame in iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol): - t1 = perf_counter() - mapper.add_frame(frame.points, frame.origin) - add_ms.append((perf_counter() - t1) * 1000) - build_ms = (perf_counter() - t0) * 1000 - add_arr = np.asarray(add_ms) if add_ms else np.zeros(1) - occupied = mapper.global_map() - occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) - - cache.parent.mkdir(exist_ok=True) - np.savez_compressed( - cache, - occupied=occupied, - occupied_keys=occupied_keys, - frames=len(add_ms), - add_p50=np.percentile(add_arr, 50), - add_p95=np.percentile(add_arr, 95), - add_max=add_arr.max(), - ) - logger.info("golden map cached: %s (%d voxels)", cache.name, len(occupied)) - return GoldenMap( - voxel_size=voxel_size, - occupied=occupied, - occupied_keys=occupied_keys, - frames=len(add_ms), - add_frame_ms={ - "p50": float(np.percentile(add_arr, 50)), - "p95": float(np.percentile(add_arr, 95)), - "max": float(add_arr.max()), - }, - build_ms=build_ms, - ) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 04a5c21542..6d01ffc850 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -21,7 +21,7 @@ import numpy as np -from dimos.navigation.nav_3d.evaluator.golden import ( +from dimos.navigation.nav_3d.evaluator.final_map import ( cylinder_offsets, densify, key_centers, @@ -49,7 +49,7 @@ def goal_reached( @dataclass class GateResult: - """Collision check of a path against the golden obstacle set.""" + """Collision check of a path against the final obstacle set.""" valid: bool collision_points: NDArray[np.float32] @@ -63,7 +63,7 @@ def check_path( ground_margin: float, body_clearance: float, ) -> GateResult: - """Sweep the robot body along foot-level waypoints against golden obstacles. + """Sweep the robot body along foot-level waypoints against final obstacles. The checked volume at each sample is a cylinder from ground_margin above the foot (so the supporting floor never counts) up to body_clearance. @@ -93,19 +93,35 @@ def check_path( return GateResult(valid=len(colliding) == 0, collision_points=samples[colliding]) +@dataclass +class Reference: + """Demonstrated route between a case's endpoints.""" + + length: float + snapped: bool + # When the robot stood at the start about to walk the route; inf when + # the endpoints are off the trajectory or no causal pair exists. + start_ts: float + # True when the goal was visited before the chosen start visit, so a + # planner at start_ts targets a place the robot has already been. + causal: bool + + def reference_length( trajectory: Trajectory, start: tuple[float, float, float], goal: tuple[float, float, float], robot_height: float, max_snap_m: float = 1.0, -) -> tuple[float, bool]: +) -> Reference: """Shortest walked length the trajectory demonstrates between start and goal. The robot usually passes each spot several times, so the reference is the - minimum arc distance over every combination of start and goal visits, not - the arc between single nearest poses, which would include any wandering in - between. Returns (length, snapped). When either endpoint is farther than + minimum route length over every combination of start and goal visits, not + the route between single nearest poses, which would include any wandering + in between. Only causal pairs count when one exists: the goal visited + before the start, so an incremental map at the start time has seen the + goal and the demonstrated route. When either endpoint is farther than max_snap_m from the trajectory, falls back to the straight-line distance. """ foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) @@ -114,15 +130,23 @@ def reference_length( ds = np.linalg.norm(foot - s, axis=1) dg = np.linalg.norm(foot - g, axis=1) if ds.min() > max_snap_m or dg.min() > max_snap_m: - return float(np.linalg.norm(g - s)), False + return Reference(float(np.linalg.norm(g - s)), False, float("inf"), False) arcs = trajectory.arc_lengths() near_s = np.flatnonzero(ds <= max_snap_m) near_g = np.flatnonzero(dg <= max_snap_m) - pair_arcs = np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) - best = np.unravel_index(pair_arcs.argmin(), pair_arcs.shape) - i, j = int(near_s[best[0]]), int(near_g[best[1]]) - length = float(pair_arcs[best]) + float(ds[i]) + float(dg[j]) - return max(length, 1e-6), True + totals = ( + np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) + + ds[near_s][:, None] + + dg[near_g][None, :] + ) + backward = trajectory.ts[near_g][None, :] <= trajectory.ts[near_s][:, None] + causal = bool(backward.any()) + if causal: + totals = np.where(backward, totals, np.inf) + best = np.unravel_index(totals.argmin(), totals.shape) + i = int(near_s[best[0]]) + start_ts = float(trajectory.ts[i]) if causal else float("inf") + return Reference(max(float(totals[best]), 1e-6), True, start_ts, causal) def spl(success: bool, l_ref: float, p_len: float) -> float: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index ec80ca1324..a0f1d8cff9 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -14,17 +14,25 @@ """Run case suites through the ray tracer and MLS planner and score them. -Every case is planned twice: on the golden map (planner ceiling) and on the -map the online mapper built from the recording (end to end). Both paths must -pass the golden collision gate. The headline score is validity-gated SPL on -the online map. +Every case is planned twice. The online plan runs on the incremental map: +what the mapper had built by the moment the robot stood at the case's start, +about to walk the demonstrated route back to a goal it already visited. The +final plan runs on the final map: the same pipeline fed the whole recording. +The final map is not ground truth, just the most complete map this pipeline +produces, so a failure on it means the whole pipeline cannot solve the case +even with all the data. The final path is gated against the full final +occupancy; the online path only against the final obstacles the sensor had +returns from by plan time: hitting a wall no lidar return ever came from is +not an error, but hitting one the sensor saw and the mapper dropped is. The +headline score is validity-gated SPL on the incremental map. """ from __future__ import annotations -from concurrent.futures import ProcessPoolExecutor +from concurrent.futures import ProcessPoolExecutor, ThreadPoolExecutor from dataclasses import asdict, dataclass, field import itertools +import threading from time import perf_counter from typing import TYPE_CHECKING @@ -32,14 +40,18 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.config import EvalConfig -from dimos.navigation.nav_3d.evaluator.golden import ( - load_or_build_golden, +from dimos.navigation.nav_3d.evaluator.final_map import ( + key_centers, + load_or_build_checkpoints, + load_or_build_final_map, ) from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.utils.data import resolve_named_path from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from collections.abc import Iterator + from numpy.typing import NDArray from dimos.navigation.nav_3d.evaluator.cases import Case, Suite @@ -48,6 +60,9 @@ logger = setup_logger() MAX_COLLISIONS_KEPT = 50 +# The goal counts as seen when the incremental map has an occupied voxel +# within this distance of it at plan time. +GOAL_SEEN_RADIUS_M = 1.0 @dataclass @@ -66,6 +81,14 @@ def success(self) -> bool: return self.planned and self.reached and self.valid +@dataclass +class PlannerArtifacts: + """Graph state of one planner after its map update. Not serialized to JSON.""" + + surface_clearance: NDArray[np.float32] + edges: NDArray[np.float32] + + @dataclass class CaseResult: id: str @@ -76,51 +99,50 @@ class CaseResult: tags: list[str] l_ref: float l_ref_snapped: bool + plan_ts: float + online_voxels: int + map_update_ms: float + goal_seen: bool online: PlanOutcome - golden: PlanOutcome - attribution: str + final: PlanOutcome soft_progress: float - - -@dataclass -class PlannerArtifacts: - """Graph state of one planner after its map update. Not serialized to JSON.""" - - surface_clearance: NDArray[np.float32] - nodes: NDArray[np.float32] - edges: NDArray[np.float32] + # Planner graph on the incremental map, kept only for failed cases. + online_artifacts: PlannerArtifacts | None = None @dataclass class DatasetResult: dataset: str cases: list[CaseResult] - online_voxels: int - golden_voxels: int + final_voxels: int map_build_ms: float add_frame_ms: dict[str, float] frames: int - online_artifacts: PlannerArtifacts | None = None - golden_artifacts: PlannerArtifacts | None = None + final_artifacts: PlannerArtifacts | None = None @dataclass class Report: score: float score_soft: float - planner_score: float + final_score: float n_cases: int n_success: int - attribution_counts: dict[str, int] + n_success_final: int + # The incremental and final runs are independent tests per case; these + # count the four pass/fail combinations. + outcome_counts: dict[str, int] plan_ms: dict[str, float] + map_update_ms: dict[str, float] datasets: list[DatasetResult] config: dict[str, float | int] = field(default_factory=dict) def to_dict(self) -> dict[str, object]: out = asdict(self) for dataset in out["datasets"]: - dataset.pop("online_artifacts") - dataset.pop("golden_artifacts") + dataset.pop("final_artifacts") + for case in dataset["cases"]: + case.pop("online_artifacts") return out @@ -161,112 +183,195 @@ def _run_plan( return outcome, waypoints -def _attribution(online: PlanOutcome, golden: PlanOutcome) -> str: - if online.success: - return "ok" - if not golden.success: - return "planner" - return "mapper" +def _goal_seen(online_points: NDArray[np.float32], goal: tuple[float, float, float]) -> bool: + if len(online_points) == 0: + return False + d = np.linalg.norm(online_points - np.asarray(goal, dtype=np.float32), axis=1) + return bool(d.min() <= GOAL_SEEN_RADIUS_M) + + +def _snapshot(planner: MLSPlanner) -> PlannerArtifacts: + return PlannerArtifacts( + surface_clearance=planner.surface_clearance_map(), + edges=planner.node_edges(), + ) -def run_suite(suite: Suite, cfg: EvalConfig) -> DatasetResult: +def run_suite(suite: Suite, cfg: EvalConfig, threads: int = 1) -> DatasetResult: db_path = resolve_named_path(suite.dataset, ".db") trajectory = load_trajectory(db_path, suite.odom_stream) - golden = load_or_build_golden(db_path, suite, cfg) - obstacle_keys = golden.occupied_keys - - # The online map equals the golden map while both use the same mapper - # config, so reuse it instead of replaying the recording a second time. - # Tier 2 replay and a separate golden mapper config will change this. - online_points = golden.occupied - - golden_planner = cfg.make_planner() - golden_planner.update_global_map(golden.occupied) - online_planner = cfg.make_planner() - online_planner.update_global_map(online_points) - - def snapshot(planner: MLSPlanner) -> PlannerArtifacts: - return PlannerArtifacts( - surface_clearance=planner.surface_clearance_map(), - nodes=planner.nodes(), - edges=planner.node_edges(), - ) - - results: list[CaseResult] = [] + final = load_or_build_final_map(db_path, suite, cfg) + obstacle_keys = final.occupied_keys + + refs: list[metrics.Reference] = [] for case in suite.cases: - if case.l_ref is not None: - l_ref, snapped = case.l_ref, True - else: - l_ref, snapped = metrics.reference_length( - trajectory, case.start, case.goal, cfg.robot_height + ref = metrics.reference_length(trajectory, case.start, case.goal, cfg.robot_height) + if not ref.snapped: + logger.warning( + "%s/%s: start or goal is off the walked trajectory; " + "using straight-line reference and the full map", + suite.dataset, + case.id, ) - if not snapped: - logger.warning( - "%s/%s: start or goal is off the walked trajectory; " - "using straight-line reference", - suite.dataset, - case.id, + elif not ref.causal: + logger.warning( + "%s/%s: goal is never visited before the start; planning on the full map", + suite.dataset, + case.id, + ) + if case.l_ref is not None: + ref = metrics.Reference(case.l_ref, ref.snapped, ref.start_ts, ref.causal) + refs.append(ref) + + start_ts = np.array([r.start_ts for r in refs], dtype=np.float64) + checkpoints = load_or_build_checkpoints(db_path, suite, cfg, start_ts) + case_ckpt = np.searchsorted(checkpoints.times, start_ts) + + final_planner = cfg.make_planner() + final_planner.update_global_map(final.occupied) + + results: list[CaseResult | None] = [None] * len(suite.cases) + + def process_checkpoint( + k: int, + keys: NDArray[np.int64], + online_gate_keys: NDArray[np.int64], + online_planner: MLSPlanner, + ) -> None: + online_points = key_centers(keys, cfg.voxel_size) + t0 = perf_counter() + if len(online_points): + online_planner.update_global_map(online_points) + map_update_ms = (perf_counter() - t0) * 1000 + for ci in np.flatnonzero(case_ckpt == k): + case, ref = suite.cases[ci], refs[ci] + final_out, _ = _run_plan(final_planner, case, ref.length, obstacle_keys, cfg) + if len(online_points): + online_out, online_wp = _run_plan( + online_planner, case, ref.length, online_gate_keys, cfg ) - golden_out, _ = _run_plan(golden_planner, case, l_ref, obstacle_keys, cfg) - online_out, online_wp = _run_plan(online_planner, case, l_ref, obstacle_keys, cfg) - end = online_wp[-1] if online_wp is not None and len(online_wp) else None - results.append( - CaseResult( + else: + online_out = PlanOutcome(False, False, False, 0.0, 0.0, 0.0, [], []) + online_wp = None + end = online_wp[-1] if online_wp is not None and len(online_wp) else None + goal_seen = _goal_seen(online_points, case.goal) + results[ci] = CaseResult( id=case.id, dataset=suite.dataset, start=case.start, goal=case.goal, weight=case.weight, tags=case.tags, - l_ref=l_ref, - l_ref_snapped=snapped, + l_ref=ref.length, + l_ref_snapped=ref.snapped, + plan_ts=float(checkpoints.times[k]), + online_voxels=len(keys), + map_update_ms=map_update_ms, + goal_seen=goal_seen, online=online_out, - golden=golden_out, - attribution=_attribution(online_out, golden_out), + final=final_out, soft_progress=metrics.soft_progress(end, case.start, case.goal), + online_artifacts=None + if online_out.success or not len(online_points) + else _snapshot(online_planner), ) - ) + active = {int(k) for k in case_ckpt} + tls = threading.local() + + def task(k: int, keys: NDArray[np.int64], gate: NDArray[np.int64]) -> None: + planner = getattr(tls, "planner", None) + if planner is None: + planner = tls.planner = cfg.make_planner() + try: + process_checkpoint(k, keys, gate, planner) + finally: + in_flight.release() + + def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64], NDArray[np.int64]]]: + """Walk the delta chain once. The online gate only holds obstacles the + sensor had returns from by plan time; obstacles never observed are not + the planner's fault.""" + gate = np.array([], dtype=np.int64) + for k, (keys, observed_new) in enumerate(checkpoints.iter_snapshots()): + fresh = np.intersect1d(obstacle_keys, observed_new, assume_unique=True) + if len(fresh): + gate = np.union1d(gate, fresh) + if k in active: + yield k, keys, gate + + # The planner releases the GIL and parallelizes updates internally via a + # shared rayon pool, so a few worker threads interleave the serial parts + # of checkpoint updates without oversubscribing. The semaphore bounds how + # many reconstructed snapshots are held in memory at once. + if threads > 1: + in_flight = threading.BoundedSemaphore(threads * 2) + with ThreadPoolExecutor(max_workers=threads) as pool: + futures = [] + for item in snapshot_stream(): + in_flight.acquire() + futures.append(pool.submit(task, *item)) + for future in futures: + future.result() + else: + online_planner = cfg.make_planner() + for k, keys, gate in snapshot_stream(): + process_checkpoint(k, keys, gate, online_planner) + + done = [r for r in results if r is not None] + if len(done) != len(suite.cases): + raise RuntimeError(f"{suite.dataset}: {len(suite.cases) - len(done)} cases not planned") return DatasetResult( dataset=suite.dataset, - cases=results, - online_voxels=len(online_points), - golden_voxels=len(golden.occupied), - map_build_ms=golden.build_ms, - add_frame_ms=golden.add_frame_ms, - frames=golden.frames, - online_artifacts=snapshot(online_planner), - golden_artifacts=snapshot(golden_planner), + cases=done, + final_voxels=len(final.occupied), + map_build_ms=final.build_ms, + add_frame_ms=final.add_frame_ms, + frames=final.frames, + final_artifacts=_snapshot(final_planner), ) def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = 1) -> Report: + """Score every suite. workers is total parallelism: datasets spread over + processes and each dataset's checkpoints over threads.""" cfg = cfg or EvalConfig() if workers > 1 and len(suites) > 1: + threads = max(1, workers // len(suites)) with ProcessPoolExecutor(max_workers=min(workers, len(suites))) as pool: - datasets = list(pool.map(run_suite, suites, itertools.repeat(cfg))) + datasets = list( + pool.map(run_suite, suites, itertools.repeat(cfg), itertools.repeat(threads)) + ) else: - datasets = [run_suite(suite, cfg) for suite in suites] + datasets = [run_suite(suite, cfg, threads=workers) for suite in suites] cases = [c for d in datasets for c in d.cases] if not cases: raise ValueError("no cases to evaluate") weights = np.array([c.weight for c in cases]) online_spl = np.array([c.online.spl for c in cases]) - golden_spl = np.array([c.golden.spl for c in cases]) + final_spl = np.array([c.final.spl for c in cases]) soft = np.array([c.soft_progress if not c.online.success else c.online.spl for c in cases]) - attribution_counts: dict[str, int] = {} + outcome_counts = {"both": 0, "final_only": 0, "incremental_only": 0, "neither": 0} for c in cases: - attribution_counts[c.attribution] = attribution_counts.get(c.attribution, 0) + 1 + key = { + (True, True): "both", + (False, True): "final_only", + (True, False): "incremental_only", + (False, False): "neither", + }[(c.online.success, c.final.success)] + outcome_counts[key] += 1 return Report( score=float(np.average(online_spl, weights=weights)), score_soft=float(np.average(soft, weights=weights)), - planner_score=float(np.average(golden_spl, weights=weights)), + final_score=float(np.average(final_spl, weights=weights)), n_cases=len(cases), n_success=sum(c.online.success for c in cases), - attribution_counts=attribution_counts, + n_success_final=sum(c.final.success for c in cases), + outcome_counts=outcome_counts, plan_ms=metrics.timing_stats([c.online.plan_ms for c in cases]), + map_update_ms=metrics.timing_stats([c.map_update_ms for c in cases]), datasets=datasets, config=asdict(cfg), ) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 3d695333c8..9bcc33b310 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -14,6 +14,7 @@ from __future__ import annotations +import itertools from types import SimpleNamespace import numpy as np @@ -21,6 +22,15 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator.final_map import ( + FinalMap, + MapCheckpoints, + encode_deltas, + key_centers, + keys_contain, + replay_frames, + voxel_keys, +) from dimos.navigation.nav_3d.evaluator.generate import ( Candidate, GenerationParams, @@ -29,13 +39,7 @@ generate_cases, snap_to_surface, ) -from dimos.navigation.nav_3d.evaluator.golden import ( - GoldenMap, - key_centers, - keys_contain, - voxel_keys, -) -from dimos.navigation.nav_3d.evaluator.recording import Trajectory +from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory VOXEL = 0.1 @@ -107,11 +111,19 @@ def test_reference_length_snaps_to_trajectory() -> None: [np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1 ).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) - l_ref, snapped = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) - assert snapped - assert l_ref == pytest.approx(10.0, abs=0.01) - l_ref, snapped = metrics.reference_length(traj, (0, 5, 0), (10, 0, 0), robot_height=0.3) - assert not snapped + # Walking toward a never-yet-visited goal is not causal. + ref = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) + assert ref.snapped + assert ref.length == pytest.approx(10.0, abs=0.01) + assert not ref.causal + assert ref.start_ts == float("inf") + # Returning to the walk's origin is causal. + ref = metrics.reference_length(traj, (10, 0, 0), (0, 0, 0), robot_height=0.3) + assert ref.causal + assert 9.0 <= ref.start_ts <= 10.0 + ref = metrics.reference_length(traj, (0, 5, 0), (10, 0, 0), robot_height=0.3) + assert not ref.snapped + assert ref.start_ts == float("inf") def test_reference_length_uses_shortest_revisit() -> None: @@ -121,9 +133,9 @@ def test_reference_length_uses_shortest_revisit() -> None: back = detour[::-1] positions = np.concatenate([out, detour, back]).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 30, len(positions)), positions=positions) - l_ref, snapped = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) - assert snapped - assert l_ref == pytest.approx(10.0, abs=0.2) + ref = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) + assert ref.snapped + assert ref.length == pytest.approx(10.0, abs=0.2) def test_path_length_and_goal() -> None: @@ -137,7 +149,7 @@ def test_generate_cases_around_wall() -> None: """A U-shaped walk around a wall must yield non-trivial cases spanning it.""" wall_pts = _wall(10.0) wall_keys = np.unique(voxel_keys(wall_pts, VOXEL)) - golden = GoldenMap( + final = FinalMap( voxel_size=VOXEL, occupied=wall_pts, occupied_keys=wall_keys, @@ -166,7 +178,7 @@ def test_generate_cases_around_wall() -> None: ground_margin=0.25, body_clearance=0.45, ) - cases = generate_cases(traj, golden, surface, cfg, GenerationParams(max_cases=10)) + cases = generate_cases(traj, final, surface, cfg, GenerationParams(max_cases=10)) assert cases assert len({c.id for c in cases}) == len(cases) spans_wall = [c for c in cases if (c.start[0] - 10) * (c.goal[0] - 10) < 0] @@ -214,6 +226,69 @@ def test_drift_stats_flags_z_mismatch() -> None: assert not drift_stats(clean).warnings +def test_checkpoint_deltas_roundtrip() -> None: + snapshots = [ + np.array([1, 2, 3], dtype=np.int64), + np.array([2, 3, 4, 5], dtype=np.int64), + np.array([4, 5], dtype=np.int64), + ] + observed = [ + np.array([1, 2, 3], dtype=np.int64), + np.array([1, 2, 3, 4, 5], dtype=np.int64), + np.array([1, 2, 3, 4, 5, 9], dtype=np.int64), + ] + added, removed = encode_deltas(snapshots) + observed_added, _ = encode_deltas(observed) + ckpt = MapCheckpoints( + times=np.arange(3, dtype=np.float64), + added=added, + removed=removed, + observed_added=observed_added, + ) + seen = np.array([], dtype=np.int64) + for (orig_keys, orig_obs), (keys, obs_new) in zip( + zip(snapshots, observed, strict=True), ckpt.iter_snapshots(), strict=True + ): + assert np.array_equal(orig_keys, keys) + seen = np.union1d(seen, obs_new) + assert np.array_equal(orig_obs, seen) + + +def test_replay_frames_snapshots_grow_with_time() -> None: + """Each checkpoint must contain exactly the frames seen up to its time.""" + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + + cfg = EvalConfig(voxel_size=VOXEL, support_min=1) + + def frame_at(ts: float, x: float) -> Frame: + return Frame(ts=ts, points=_wall(x), origin=(x - 2.0, 0.0, 0.5)) + + # A voxel needs a second observation to persist, so hit each wall twice. + frames = [ + frame_at(0.0, 5.0), + frame_at(0.1, 5.0), + frame_at(1.0, 8.0), + frame_at(1.1, 8.0), + frame_at(2.0, 11.0), + frame_at(2.1, 11.0), + ] + times = np.array([0.5, 1.5, np.inf]) + final, snapshots, observed = replay_frames(frames, cfg.make_mapper(), VOXEL, times) + assert final.frames == 6 + sizes = [len(s) for s in snapshots] + assert 0 < sizes[0] < sizes[1] < sizes[2] + assert np.array_equal(snapshots[2], final.occupied_keys) + for earlier, later in itertools.pairwise(snapshots): + assert keys_contain(later, earlier).all() + # The observed set holds raw returns causally: wall 1 by the first + # checkpoint, wall 3 only at the end. + wall1 = np.unique(voxel_keys(_wall(5.0), VOXEL)) + wall3 = np.unique(voxel_keys(_wall(11.0), VOXEL)) + assert keys_contain(observed[0], wall1).all() + assert not keys_contain(observed[0], wall3).any() + assert keys_contain(observed[2], wall3).all() + + def test_save_suite_roundtrip(tmp_path) -> None: suite = Suite( dataset="demo", diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 4c2e479314..00e93bc978 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -15,15 +15,16 @@ """Write an evaluation report into a rerun recording. One static scene per dataset: -- map/obstacles: golden voxels, turbo colormap by height +- map/obstacles: final voxels, turbo colormap by height - walked_path: the recorded foot path (white) -- planner_online, planner_golden: the planner graph each map produced. +- planner_final: the planner graph the full aggregated map produced. Surface cells colored by wall clearance (red inside the hard clearance), - nodes yellow, edges colored white to red by log traversal cost. -- cases/: start (cyan), goal (orange), online and golden planned paths + edges colored white to red by log traversal cost. +- cases/: start (cyan), goal (orange), online and final planned paths colored by verdict (green valid, red gate-invalid, yellow unreached), and the gate's collision samples (red dots). Failed cases also get a thin red - start-to-goal line so the intended connection is visible even with no path. + start-to-goal intent line and a known/ layer: the planner graph on the + incremental map at plan time, i.e. what the robot knew when it failed. """ from __future__ import annotations @@ -33,7 +34,7 @@ import numpy as np import rerun as rr -from dimos.navigation.nav_3d.evaluator.golden import load_or_build_golden +from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.utils.data import resolve_named_path @@ -55,7 +56,6 @@ INVALID_PATH_COLOR = [255, 0, 0] UNREACHED_PATH_COLOR = [255, 200, 0] -NODE_COLOR = [255, 200, 0] CLEARANCE_CLAMP_M = 1.0 @@ -99,12 +99,6 @@ def _log_planner(entity: str, artifacts: PlannerArtifacts | None, cfg: EvalConfi ), static=True, ) - if artifacts.nodes.size: - rr.log( - f"{entity}/nodes", - rr.Points3D(artifacts.nodes, colors=[NODE_COLOR], radii=0.05), - static=True, - ) edges = artifacts.edges if edges.size: rr.log( @@ -150,15 +144,15 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - for dataset in report.datasets: suite = suites_by_dataset[dataset.dataset] db_path = resolve_named_path(suite.dataset, ".db") - golden = load_or_build_golden(db_path, suite, cfg) + final = load_or_build_final_map(db_path, suite, cfg) trajectory = load_trajectory(db_path, suite.odom_stream) root = dataset.dataset rr.log( f"{root}/map/obstacles", rr.Points3D( - golden.occupied, - colors=_turbo_by_height(golden.occupied), + final.occupied, + colors=_turbo_by_height(final.occupied), radii=cfg.voxel_size / 4, ), static=True, @@ -170,8 +164,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - static=True, ) - _log_planner(f"{root}/planner_online", dataset.online_artifacts, cfg) - _log_planner(f"{root}/planner_golden", dataset.golden_artifacts, cfg) + _log_planner(f"{root}/planner_final", dataset.final_artifacts, cfg) for case in dataset.cases: base = f"{root}/cases/{case.id}" @@ -193,8 +186,9 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - ), static=True, ) + _log_planner(f"{base}/known", case.online_artifacts, cfg) _log_path(f"{base}/online", case.online, radius=0.04) - _log_path(f"{base}/golden", case.golden, radius=0.02) + _log_path(f"{base}/final", case.final, radius=0.02) print(f"wrote {out}") print(f"open with: rerun {out}") From 2ec00e28ad9cf09269b9e821ba61efd334dd1e87 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 19:08:04 -0700 Subject: [PATCH 05/29] Better guards and validation --- dimos/navigation/nav_3d/evaluator/cli.py | 6 +- dimos/navigation/nav_3d/evaluator/config.py | 18 ++- dimos/navigation/nav_3d/evaluator/metrics.py | 105 ++++++++++++-- dimos/navigation/nav_3d/evaluator/runner.py | 52 +++++-- .../nav_3d/evaluator/test_evaluator.py | 128 +++++++++++++++++- dimos/navigation/nav_3d/evaluator/viz.py | 19 ++- 6 files changed, 304 insertions(+), 24 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index cb423706f0..4105534b63 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -76,16 +76,20 @@ def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: def _print_report(report: Report) -> None: header = ( f"{'case':<28} {'dataset':<22} {'inc':>5} {'fin':>5} " - f"{'len':>6} {'ref':>6} {'vox':>8} {'ms':>7}" + f"{'len':>6} {'ref':>6} {'miss':>6} {'clr':>6} {'vox':>8} {'ms':>7}" ) print(header) print("-" * len(header)) for d in report.datasets: for c in d.cases: + clr = ( + f"{c.online.min_clearance:>6.2f}" if c.online.min_clearance is not None else " " * 6 + ) print( f"{c.id:<28} {c.dataset:<22} " f"{c.online.spl:>5.2f} {c.final.spl:>5.2f} " f"{c.online.length:>6.1f} {c.l_ref:>6.1f} " + f"{c.online.goal_miss:>6.1f} {clr} " f"{c.online_voxels:>8d} {c.online.plan_ms:>7.1f}" ) print("-" * len(header)) diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index db2694e31a..ebb07146c5 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -22,7 +22,11 @@ @dataclass class EvalConfig: - """Mapper, planner, and gate parameters. Defaults mirror production.""" + """Mapper, planner, and gate parameters. + + Defaults mirror production. Body and capability bounds are sized for the + Unitree Go2 (0.31m wide, 0.40m tall, ~0.16m stair risers). + """ voxel_size: float = 0.08 max_range: float = 30.0 @@ -53,6 +57,18 @@ class EvalConfig: body_clearance: float = 0.45 goal_tolerance: float = 0.5 align_tol: float = 0.05 + # Paths must stand on final-map occupancy within support_radius_m of + # each sample and support_depth_m below it. The radius models the Go2 + # straddling small scan holes (0.7m footprint), not its body width. + support_radius_m: float = 0.35 + support_depth_m: float = 0.35 + # Climb limits, checked over a stride-scale window so planner cell + # quantization does not read as a cliff. The slope bound comes from the + # steepest climbs the Go2 demonstrated on the Athens stairs, where + # switchback corners locally exceed the spec-sheet 40 degrees. + max_slope: float = 1.2 + max_step_m: float = 0.2 + kinematic_window_m: float = 0.5 def make_mapper(self) -> VoxelRayMapper: return VoxelRayMapper( diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 6d01ffc850..6c7292f62b 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -47,12 +47,22 @@ def goal_reached( return bool(np.linalg.norm(waypoints[-1] - np.asarray(goal, dtype=np.float32)) <= tolerance) +# Clearance margins are only measured out to this horizontal distance from +# the body surface; anything farther reports the cap. +MARGIN_CAP_M = 0.3 + + @dataclass class GateResult: - """Collision check of a path against the final obstacle set.""" + """Collision check of a path against an obstacle key set.""" valid: bool collision_points: NDArray[np.float32] + # Horizontal distance from the body surface to the nearest obstacle in + # the gate's z band, minimized along the path. Negative is penetration + # depth; capped at MARGIN_CAP_M when nothing is near. Gives a smooth + # how-close-to-flipping signal next to the binary verdict. + min_clearance_m: float def check_path( @@ -63,7 +73,7 @@ def check_path( ground_margin: float, body_clearance: float, ) -> GateResult: - """Sweep the robot body along foot-level waypoints against final obstacles. + """Sweep the robot body along foot-level waypoints against obstacles. The checked volume at each sample is a cylinder from ground_margin above the foot (so the supporting floor never counts) up to body_clearance. @@ -73,7 +83,7 @@ def check_path( """ samples = densify(waypoints, voxel_size / 2) offsets = cylinder_offsets( - robot_radius + voxel_size, + robot_radius + MARGIN_CAP_M + voxel_size, ground_margin - voxel_size, body_clearance + voxel_size, voxel_size, @@ -82,15 +92,90 @@ def check_path( candidate = keys_contain(obstacle_keys, keys.ravel()).reshape(keys.shape) s_idx, o_idx = np.nonzero(candidate) if len(s_idx) == 0: - return GateResult(valid=True, collision_points=samples[:0]) + return GateResult(valid=True, collision_points=samples[:0], min_clearance_m=MARGIN_CAP_M) delta = key_centers(keys[s_idx, o_idx], voxel_size) - samples[s_idx] - exact = ( - (np.linalg.norm(delta[:, :2], axis=1) <= robot_radius) - & (delta[:, 2] >= ground_margin) - & (delta[:, 2] <= body_clearance) - ) + hd = np.linalg.norm(delta[:, :2], axis=1) + in_band = (delta[:, 2] >= ground_margin) & (delta[:, 2] <= body_clearance) + exact = in_band & (hd <= robot_radius) + clearance = float(hd[in_band].min() - robot_radius) if in_band.any() else MARGIN_CAP_M colliding = np.unique(s_idx[exact]) - return GateResult(valid=len(colliding) == 0, collision_points=samples[colliding]) + return GateResult( + valid=len(colliding) == 0, + collision_points=samples[colliding], + min_clearance_m=min(clearance, MARGIN_CAP_M), + ) + + +@dataclass +class SupportResult: + """Ground check: every path sample must stand on mapped occupancy.""" + + valid: bool + unsupported_points: NDArray[np.float32] + + +def check_support( + waypoints: NDArray[np.float32], + support_keys: NDArray[np.int64], + voxel_size: float, + radius: float, + depth: float, +) -> SupportResult: + """Require occupied voxels beneath every path sample. + + A path across a void collides with nothing, so the collision gate alone + cannot catch fabricated bridges. Each densified sample must have at least + one occupied voxel within radius horizontally and from depth below the + foot up to one voxel above it. + """ + samples = densify(waypoints, voxel_size) + offsets = cylinder_offsets(radius, -depth, voxel_size, voxel_size) + keys = offset_keys(samples, offsets, voxel_size) + supported = keys_contain(support_keys, keys.ravel()).reshape(keys.shape).any(axis=1) + return SupportResult(bool(supported.all()), samples[~supported]) + + +@dataclass +class KinematicsResult: + """Steppability check of the path profile.""" + + valid: bool + violation_points: NDArray[np.float32] + + +def _resample(waypoints: NDArray[np.float32], spacing: float) -> NDArray[np.float32]: + """Points every spacing meters of 3D arc length along the polyline.""" + steps = np.linalg.norm(np.diff(waypoints, axis=0), axis=1) + arc = np.concatenate([[0.0], np.cumsum(steps)]) + if arc[-1] <= spacing: + return waypoints[[0, -1]] + s = np.append(np.arange(0.0, arc[-1], spacing), arc[-1]) + return np.stack([np.interp(s, arc, waypoints[:, i]) for i in range(3)], axis=1).astype( + np.float32 + ) + + +def check_kinematics( + waypoints: NDArray[np.float32], + max_slope: float, + max_step_m: float, + window_m: float, +) -> KinematicsResult: + """Reject paths that climb steeper than the robot can. + + The profile is resampled at window_m of arc length so single-cell + quantization in planner waypoints does not read as a cliff. Each + resampled segment may rise at most max_slope times its horizontal run, + with a max_step_m floor so stair risers between close samples pass. + """ + if len(waypoints) < 2: + return KinematicsResult(True, waypoints[:0]) + profile = _resample(waypoints, window_m) + d = np.diff(profile, axis=0) + rise = np.abs(d[:, 2]) + run = np.linalg.norm(d[:, :2], axis=1) + bad = rise > np.maximum(run * max_slope, max_step_m) + return KinematicsResult(not bad.any(), profile[1:][bad]) @dataclass diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index a0f1d8cff9..6a20e532d1 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -23,8 +23,10 @@ even with all the data. The final path is gated against the full final occupancy; the online path only against the final obstacles the sensor had returns from by plan time: hitting a wall no lidar return ever came from is -not an error, but hitting one the sensor saw and the mapper dropped is. The -headline score is validity-gated SPL on the incremental map. +not an error, but hitting one the sensor saw and the mapper dropped is. +Every path must also stand on final-map occupancy (no fabricated bridges) +and stay within the robot's climb envelope. The headline score is +validity-gated SPL on the incremental map. """ from __future__ import annotations @@ -70,15 +72,27 @@ class PlanOutcome: planned: bool reached: bool valid: bool + # Every sample stands on final-map occupancy; fabricated bridges fail. + supported: bool + # No segment rises steeper than the robot can climb. + kinematic: bool length: float plan_ms: float spl: float + # How far the path end is from the goal; start-to-goal distance when no + # path was planned. Smooth counterpart to the binary reached flag. + goal_miss: float + # Gate margin along the path (see GateResult.min_clearance_m); None when + # no path was planned. + min_clearance: float | None waypoints: list[list[float]] collisions: list[list[float]] + unsupported: list[list[float]] + steep: list[list[float]] @property def success(self) -> bool: - return self.planned and self.reached and self.valid + return self.planned and self.reached and self.valid and self.supported and self.kinematic @dataclass @@ -151,13 +165,14 @@ def _run_plan( case: Case, l_ref: float, obstacle_keys: NDArray[np.int64], + support_keys: NDArray[np.int64], cfg: EvalConfig, ) -> tuple[PlanOutcome, NDArray[np.float32] | None]: t0 = perf_counter() waypoints = planner.plan(case.start, case.goal) plan_ms = (perf_counter() - t0) * 1000 if waypoints is None or len(waypoints) == 0: - return PlanOutcome(False, False, False, 0.0, plan_ms, 0.0, [], []), None + return _no_plan(case, plan_ms), None reached = metrics.goal_reached(waypoints, case.goal, cfg.goal_tolerance) gate = metrics.check_path( @@ -168,21 +183,40 @@ def _run_plan( cfg.ground_margin, cfg.body_clearance, ) + support = metrics.check_support( + waypoints, support_keys, cfg.voxel_size, cfg.support_radius_m, cfg.support_depth_m + ) + kinematics = metrics.check_kinematics( + waypoints, cfg.max_slope, cfg.max_step_m, cfg.kinematic_window_m + ) length = metrics.path_length(waypoints) - success = reached and gate.valid + success = reached and gate.valid and support.valid and kinematics.valid outcome = PlanOutcome( planned=True, reached=reached, valid=gate.valid, + supported=support.valid, + kinematic=kinematics.valid, length=length, plan_ms=plan_ms, spl=metrics.spl(success, l_ref, length), + goal_miss=float(np.linalg.norm(waypoints[-1] - np.asarray(case.goal, dtype=np.float32))), + min_clearance=gate.min_clearance_m, waypoints=waypoints.tolist(), collisions=gate.collision_points[:MAX_COLLISIONS_KEPT].tolist(), + unsupported=support.unsupported_points[:MAX_COLLISIONS_KEPT].tolist(), + steep=kinematics.violation_points[:MAX_COLLISIONS_KEPT].tolist(), ) return outcome, waypoints +def _no_plan(case: Case, plan_ms: float) -> PlanOutcome: + miss = float(np.linalg.norm(np.asarray(case.goal) - np.asarray(case.start))) + return PlanOutcome( + False, False, False, True, True, 0.0, plan_ms, 0.0, miss, None, [], [], [], [] + ) + + def _goal_seen(online_points: NDArray[np.float32], goal: tuple[float, float, float]) -> bool: if len(online_points) == 0: return False @@ -245,13 +279,15 @@ def process_checkpoint( map_update_ms = (perf_counter() - t0) * 1000 for ci in np.flatnonzero(case_ckpt == k): case, ref = suite.cases[ci], refs[ci] - final_out, _ = _run_plan(final_planner, case, ref.length, obstacle_keys, cfg) + final_out, _ = _run_plan( + final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg + ) if len(online_points): online_out, online_wp = _run_plan( - online_planner, case, ref.length, online_gate_keys, cfg + online_planner, case, ref.length, online_gate_keys, obstacle_keys, cfg ) else: - online_out = PlanOutcome(False, False, False, 0.0, 0.0, 0.0, [], []) + online_out = _no_plan(case, 0.0) online_wp = None end = online_wp[-1] if online_wp is not None and len(online_wp) else None goal_seen = _goal_seen(online_points, case.goal) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 9bcc33b310..cb0f390a8f 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -22,6 +22,7 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.final_map import ( FinalMap, MapCheckpoints, @@ -40,6 +41,7 @@ snap_to_surface, ) from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory +from dimos.navigation.nav_3d.evaluator.runner import _run_plan VOXEL = 0.1 @@ -99,6 +101,19 @@ def test_gate_tolerates_stair_slope() -> None: assert _gate(path, slope).valid +def test_gate_reports_clearance_margin() -> None: + wall = _wall(10.0) + graze = np.array([[9.7, -0.5, 0], [9.7, 0.5, 0]], dtype=np.float32) + result = _gate(graze, wall) + assert result.valid + assert result.min_clearance_m == pytest.approx(0.35 - 0.16, abs=0.02) + crossing = _gate(np.array([[9, 0, 0], [11, 0, 0]], dtype=np.float32), wall) + assert not crossing.valid + assert crossing.min_clearance_m < 0 + far = _gate(np.array([[2, 0, 0], [3, 0, 0]], dtype=np.float32), wall) + assert far.min_clearance_m == metrics.MARGIN_CAP_M + + def test_spl() -> None: assert metrics.spl(False, 10.0, 10.0) == 0.0 assert metrics.spl(True, 10.0, 10.0) == 1.0 @@ -256,8 +271,6 @@ def test_checkpoint_deltas_roundtrip() -> None: def test_replay_frames_snapshots_grow_with_time() -> None: """Each checkpoint must contain exactly the frames seen up to its time.""" - from dimos.navigation.nav_3d.evaluator.config import EvalConfig - cfg = EvalConfig(voxel_size=VOXEL, support_min=1) def frame_at(ts: float, x: float) -> Frame: @@ -289,6 +302,117 @@ def frame_at(ts: float, x: float) -> Frame: assert keys_contain(observed[2], wall3).all() +class _StubPlanner: + """Returns a fixed path regardless of the map, for gaming the scorer.""" + + def __init__(self, waypoints: np.ndarray | None) -> None: + self._waypoints = waypoints + + def plan( + self, start: tuple[float, float, float], goal: tuple[float, float, float] + ) -> np.ndarray | None: + return self._waypoints + + +def _floor(x_lo: float = 0.0, x_hi: float = 20.0) -> np.ndarray: + xs, ys = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(-2, 6, VOXEL)) + return np.stack([xs.ravel(), ys.ravel(), np.full(xs.size, -0.05)], axis=1, dtype=np.float32) + + +def _meta_scene() -> tuple[np.ndarray, EvalConfig, Case]: + """A floored corridor with a wall at x=10 between x=2 and x=18.""" + scene = np.concatenate([_floor(), _wall(10.0)]) + keys = np.unique(voxel_keys(scene, VOXEL)) + cfg = EvalConfig(voxel_size=VOXEL) + case = Case(id="meta", start=(2.0, 0.0, 0.0), goal=(18.0, 0.0, 0.0)) + return keys, cfg, case + + +def _u_route() -> np.ndarray: + return np.array( + [[2, 0, 0], [2, 4, 0], [18, 4, 0], [18, 0, 0]], + dtype=np.float32, + ) + + +def test_meta_straight_line_cheat_scores_zero() -> None: + """A planner that ignores the map and beelines must not score.""" + keys, cfg, case = _meta_scene() + line = np.array([case.start, case.goal], dtype=np.float32) + out, _ = _run_plan(_StubPlanner(line), case, 24.0, keys, keys, cfg) + assert out.planned and out.reached and out.supported + assert not out.valid + assert out.spl == 0.0 + assert out.collisions + assert out.min_clearance is not None and out.min_clearance < 0 + + +def test_meta_no_path_scores_zero_with_miss() -> None: + keys, cfg, case = _meta_scene() + out, _ = _run_plan(_StubPlanner(None), case, 24.0, keys, keys, cfg) + assert not out.planned + assert out.spl == 0.0 + assert out.goal_miss == pytest.approx(16.0) + assert out.min_clearance is None + + +def test_meta_demonstrated_route_scores_full() -> None: + """The route the robot actually walked must earn full SPL.""" + keys, cfg, case = _meta_scene() + route = _u_route() + l_ref = metrics.path_length(route) + out, _ = _run_plan(_StubPlanner(route), case, l_ref, keys, keys, cfg) + assert out.success + assert out.spl == pytest.approx(1.0) + assert out.goal_miss == 0.0 + assert out.min_clearance == metrics.MARGIN_CAP_M + + +def test_meta_everything_occupied_fails_even_good_routes() -> None: + """An all-occupied map must collapse the score, not inflate it.""" + xs, ys, zs = np.meshgrid( + np.arange(0, 20, VOXEL), np.arange(-2, 6, VOXEL), np.arange(0.3, 0.5, VOXEL) + ) + everything = np.stack([xs.ravel(), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) + keys = np.unique(voxel_keys(np.concatenate([_floor(), everything]), VOXEL)) + _, cfg, case = _meta_scene() + out, _ = _run_plan(_StubPlanner(_u_route()), case, 24.0, keys, keys, cfg) + assert out.planned and out.reached + assert not out.valid + assert out.spl == 0.0 + + +def test_meta_floating_bridge_fails_support() -> None: + """A path across a floor gap collides with nothing but must still fail.""" + gapped = np.concatenate([_floor(0.0, 6.0), _floor(14.0, 20.0)]) + keys = np.unique(voxel_keys(gapped, VOXEL)) + _, cfg, case = _meta_scene() + line = np.array([case.start, case.goal], dtype=np.float32) + out, _ = _run_plan(_StubPlanner(line), case, 24.0, keys, keys, cfg) + assert out.planned and out.reached and out.valid + assert not out.supported + assert out.spl == 0.0 + assert out.unsupported + gap_x = np.asarray(out.unsupported, dtype=np.float32)[:, 0] + assert gap_x.min() > 5.5 and gap_x.max() < 14.5 + + +def test_check_kinematics_rejects_cliff_jumps() -> None: + stairs = np.array([[0, 0, 0], [0.4, 0, 0.16], [0.8, 0, 0.32]], dtype=np.float32) + assert metrics.check_kinematics(stairs, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid + riser = np.array([[0, 0, 0], [0.08, 0, 0.16]], dtype=np.float32) + assert metrics.check_kinematics(riser, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid + # A double riser between adjacent cells is quantization, not a cliff. + quantized = np.array( + [[0, 0, 0], [0.4, 0, 0.08], [0.56, 0, 0.4], [0.96, 0, 0.48]], dtype=np.float32 + ) + assert metrics.check_kinematics(quantized, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid + cliff = np.array([[0, 0, 0], [0.2, 0, 0.9], [1, 0, 0.9]], dtype=np.float32) + result = metrics.check_kinematics(cliff, max_slope=1.0, max_step_m=0.2, window_m=0.5) + assert not result.valid + assert len(result.violation_points) >= 1 + + def test_save_suite_roundtrip(tmp_path) -> None: suite = Suite( dataset="demo", diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 00e93bc978..132f5797bf 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -21,8 +21,9 @@ Surface cells colored by wall clearance (red inside the hard clearance), edges colored white to red by log traversal cost. - cases/: start (cyan), goal (orange), online and final planned paths - colored by verdict (green valid, red gate-invalid, yellow unreached), and - the gate's collision samples (red dots). Failed cases also get a thin red + colored by verdict (green valid, red gate-invalid, yellow unreached), the + gate's collision samples (red dots), unsupported samples (magenta), and + too-steep waypoints (purple). Failed cases also get a thin red start-to-goal intent line and a known/ layer: the planner graph on the incremental map at plan time, i.e. what the robot knew when it failed. """ @@ -51,6 +52,8 @@ START_COLOR = [0, 255, 255] GOAL_COLOR = [255, 140, 0] COLLISION_COLOR = [255, 0, 0] +UNSUPPORTED_COLOR = [255, 0, 255] +STEEP_COLOR = [160, 32, 240] VALID_PATH_COLOR = [0, 220, 0] INVALID_PATH_COLOR = [255, 0, 0] @@ -134,6 +137,18 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float) -> None: rr.Points3D(outcome.collisions, colors=[COLLISION_COLOR], radii=radius * 3), static=True, ) + if outcome.unsupported: + rr.log( + f"{entity}/unsupported", + rr.Points3D(outcome.unsupported, colors=[UNSUPPORTED_COLOR], radii=radius * 3), + static=True, + ) + if outcome.steep: + rr.log( + f"{entity}/steep", + rr.Points3D(outcome.steep, colors=[STEEP_COLOR], radii=radius * 3), + static=True, + ) def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) -> None: From 2d2ef90d49dba1a09164ceeafa2867d46a31edab Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 20:41:11 -0700 Subject: [PATCH 06/29] Manual selector --- dimos/navigation/nav_3d/evaluator/cases.py | 6 + .../nav_3d/evaluator/cases/china_office.yaml | 6 + dimos/navigation/nav_3d/evaluator/cli.py | 154 +++++++++++++++--- dimos/navigation/nav_3d/evaluator/runner.py | 70 +++++++- .../nav_3d/evaluator/test_evaluator.py | 26 ++- dimos/navigation/nav_3d/evaluator/viz.py | 16 +- 6 files changed, 245 insertions(+), 33 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index befa30e98c..1c2fa06906 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -36,6 +36,9 @@ class Case: weight: float = 1.0 tags: list[str] = field(default_factory=list) l_ref: float | None = None + # Human-certified infeasible pair: the correct answer is to refuse. + # Evaluated on the final map only, scored 1.0 for refusal. + expect_fail: bool = False @dataclass @@ -65,6 +68,7 @@ def load_suite(path: Path) -> Suite: weight=float(entry.get("weight", 1.0)), tags=[str(t) for t in entry.get("tags", [])], l_ref=float(entry["l_ref"]) if "l_ref" in entry else None, + expect_fail=bool(entry.get("expect_fail", False)), ) if case.id in seen: raise ValueError(f"{path}: duplicate case id {case.id}") @@ -107,6 +111,8 @@ def save_suite(suite: Suite, path: Path | None = None) -> Path: } if case.l_ref is not None: entry["l_ref"] = round(case.l_ref, 3) + if case.expect_fail: + entry["expect_fail"] = True entries.append(entry) doc["cases"] = entries path.write_text(yaml.safe_dump(doc, sort_keys=False, default_flow_style=None)) diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml index 396ac7fd2b..0263d3b30a 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -165,3 +165,9 @@ cases: goal: [5.72, -9.72, -0.4] weight: 1.0 tags: [auto, flat] +- id: neg_00 + start: [5.0, 2.44, 3.2] + goal: [9.32, 5.24, 6.4] + weight: 2.0 + tags: [manual, negative] + expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 4105534b63..abb998e8f3 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -25,6 +25,7 @@ from __future__ import annotations import dataclasses +import itertools import json import os from pathlib import Path @@ -234,39 +235,148 @@ def ingest( print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {name}") +def _append_case( + suite: Suite, + manifest: Path, + surface: NDArray[np.float32], + start: tuple[float, float, float], + goal: tuple[float, float, float], + case_id: str | None, + tags: list[str], + weight: float, + snap_max: float, + expect_fail: bool, +) -> Case: + prefix = "neg" if expect_fail else "manual" + snapped_goal = snap_to_surface(np.asarray(goal, dtype=np.float32), surface, snap_max) + if snapped_goal is not None: + goal = (float(snapped_goal[0]), float(snapped_goal[1]), float(snapped_goal[2])) + elif expect_fail: + # An infeasible goal may sit on geometry with no standable surface. + print(f"note: goal {goal} is off any standable surface; keeping it as picked") + else: + raise typer.BadParameter(f"goal {goal} is more than {snap_max}m from standable surface") + case = Case( + id=case_id or f"{prefix}_{sum(c.id.startswith(f'{prefix}_') for c in suite.cases):02d}", + start=_snap_or_fail("start", start, surface, snap_max), + goal=goal, + weight=weight, + tags=tags, + expect_fail=expect_fail, + ) + if any(c.id == case.id for c in suite.cases): + raise typer.BadParameter(f"case id {case.id!r} already exists in {manifest}") + suite.cases.append(case) + save_suite(suite, manifest) + kind = "negative (must refuse)" if expect_fail else "positive" + print(f"added {kind} {case.id}: {case.start} -> {case.goal} to {manifest}") + return case + + +def _load_for_curation(dataset: str) -> tuple[Suite, Path, NDArray[np.float32], EvalConfig]: + manifest = CASES_DIR / f"{dataset}.yaml" + if not manifest.exists(): + raise typer.BadParameter(f"no manifest {manifest}; run ingest first") + suite = load_suite(manifest) + cfg = EvalConfig() + final = load_or_build_final_map(resolve_named_path(dataset, ".db"), suite, cfg) + planner = cfg.make_planner() + planner.update_global_map(final.occupied) + return suite, manifest, planner.surface_map(), cfg + + @app.command("add-case") def add_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the case"), start: tuple[float, float, float] = typer.Option(..., "--start", help="Foot-level xyz"), goal: tuple[float, float, float] = typer.Option(..., "--goal", help="Foot-level xyz"), - case_id: str = typer.Option(None, "--id", help="Case id; default manual_"), - tags: str = typer.Option("manual", "--tags", help="Comma-separated tags"), + case_id: str = typer.Option(None, "--id", help="Case id; default manual_ or neg_"), + tags: str = typer.Option(None, "--tags", help="Comma-separated tags"), weight: float = typer.Option(2.0, "--weight"), snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), + expect_fail: bool = typer.Option( + False, "--expect-fail", help="Certified-infeasible pair; the planner must refuse" + ), ) -> None: - """Append a curated case, with both endpoints snapped to the final surface.""" - manifest = CASES_DIR / f"{dataset}.yaml" - if not manifest.exists(): - raise typer.BadParameter(f"no manifest {manifest}; run ingest first") - suite = load_suite(manifest) - cfg = EvalConfig() + """Append a curated case, with endpoints snapped to the final surface.""" + suite, manifest, surface, _ = _load_for_curation(dataset) + default_tags = "manual,negative" if expect_fail else "manual" + _append_case( + suite, + manifest, + surface, + start, + goal, + case_id, + [t.strip() for t in (tags or default_tags).split(",") if t.strip()], + weight, + snap_max, + expect_fail, + ) + + +@app.command("pick-case") +def pick_case( + dataset: str = typer.Argument(..., help="Dataset whose manifest gets the cases"), + expect_fail: bool = typer.Option( + False, "--expect-fail", help="Picked pairs are certified infeasible; planner must refuse" + ), + weight: float = typer.Option(2.0, "--weight"), + snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), +) -> None: + """Pick cases by clicking the map: shift+click start then goal, repeat, then close. + + Opens an Open3D window with the final map colored by height and the + walked path in white. Every consecutive pair of picked points becomes + one case, snapped and appended to the manifest. + """ + import open3d as o3d # type: ignore[import-untyped] + + from dimos.navigation.nav_3d.evaluator.viz import _turbo_by_height + + suite, manifest, surface, cfg = _load_for_curation(dataset) final = load_or_build_final_map(resolve_named_path(dataset, ".db"), suite, cfg) - planner = cfg.make_planner() - planner.update_global_map(final.occupied) - surface = planner.surface_map() + trajectory = load_trajectory(resolve_named_path(dataset, ".db"), suite.odom_stream) + foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) - case = Case( - id=case_id or f"manual_{sum(c.id.startswith('manual_') for c in suite.cases):02d}", - start=_snap_or_fail("start", start, surface, snap_max), - goal=_snap_or_fail("goal", goal, surface, snap_max), - weight=weight, - tags=[t.strip() for t in tags.split(",") if t.strip()], + points = np.concatenate([final.occupied, foot]) + colors = np.concatenate( + [_turbo_by_height(final.occupied), np.full((len(foot), 3), 255, dtype=np.uint8)] ) - if any(c.id == case.id for c in suite.cases): - raise typer.BadParameter(f"case id {case.id!r} already exists in {manifest}") - suite.cases.append(case) - save_suite(suite, manifest) - print(f"added {case.id}: {case.start} -> {case.goal} to {manifest}") + cloud = o3d.geometry.PointCloud() + cloud.points = o3d.utility.Vector3dVector(points.astype(np.float64)) + cloud.colors = o3d.utility.Vector3dVector(colors.astype(np.float64) / 255.0) + + print("shift+click START then GOAL for each case (shift+right-click undoes); close to save") + vis = o3d.visualization.VisualizerWithEditing() + vis.create_window(window_name=f"pick cases: {dataset}") + vis.add_geometry(cloud) + vis.run() + vis.destroy_window() + picked = vis.get_picked_points() + + if len(picked) % 2: + print(f"odd number of picks ({len(picked)}); dropping the last one") + picked = picked[:-1] + if not picked: + print("no points picked; nothing added") + return + for start_idx, goal_idx in itertools.batched(picked, 2): + start = tuple(float(v) for v in points[start_idx]) + goal = tuple(float(v) for v in points[goal_idx]) + _append_case( + suite, + manifest, + surface, + (start[0], start[1], start[2]), + (goal[0], goal[1], goal[2]), + None, + ["manual", "negative"] if expect_fail else ["manual"], + weight, + snap_max, + expect_fail, + ) + print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {dataset}") @app.command("list") diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 6a20e532d1..4a577def90 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -32,7 +32,7 @@ from __future__ import annotations from concurrent.futures import ProcessPoolExecutor, ThreadPoolExecutor -from dataclasses import asdict, dataclass, field +from dataclasses import asdict, dataclass, field, replace import itertools import threading from time import perf_counter @@ -76,6 +76,9 @@ class PlanOutcome: supported: bool # No segment rises steeper than the robot can climb. kinematic: bool + # For an ordinary case: all of the above. For an expect_fail case: the + # planner correctly refused the infeasible goal. + success: bool length: float plan_ms: float spl: float @@ -90,10 +93,6 @@ class PlanOutcome: unsupported: list[list[float]] steep: list[list[float]] - @property - def success(self) -> bool: - return self.planned and self.reached and self.valid and self.supported and self.kinematic - @dataclass class PlannerArtifacts: @@ -117,6 +116,7 @@ class CaseResult: online_voxels: int map_update_ms: float goal_seen: bool + expect_fail: bool online: PlanOutcome final: PlanOutcome soft_progress: float @@ -197,6 +197,7 @@ def _run_plan( valid=gate.valid, supported=support.valid, kinematic=kinematics.valid, + success=success, length=length, plan_ms=plan_ms, spl=metrics.spl(success, l_ref, length), @@ -213,10 +214,35 @@ def _run_plan( def _no_plan(case: Case, plan_ms: float) -> PlanOutcome: miss = float(np.linalg.norm(np.asarray(case.goal) - np.asarray(case.start))) return PlanOutcome( - False, False, False, True, True, 0.0, plan_ms, 0.0, miss, None, [], [], [], [] + planned=False, + reached=False, + valid=False, + supported=True, + kinematic=True, + success=False, + length=0.0, + plan_ms=plan_ms, + spl=0.0, + goal_miss=miss, + min_clearance=None, + waypoints=[], + collisions=[], + unsupported=[], + steep=[], ) +def score_negative(raw: PlanOutcome) -> PlanOutcome: + """Invert an outcome for a human-certified infeasible case. + + The planner succeeds by refusing. Any goal-reaching path it returns is a + false positive scored zero, whether or not the gates would have caught + it, because the planner claimed a route that does not exist. + """ + refused = not (raw.planned and raw.reached) + return replace(raw, success=refused, spl=1.0 if refused else 0.0) + + def _goal_seen(online_points: NDArray[np.float32], goal: tuple[float, float, float]) -> bool: if len(online_points) == 0: return False @@ -239,6 +265,11 @@ def run_suite(suite: Suite, cfg: EvalConfig, threads: int = 1) -> DatasetResult: refs: list[metrics.Reference] = [] for case in suite.cases: + if case.expect_fail: + # Infeasible by certification: no demonstrated route, no plan time. + miss = float(np.linalg.norm(np.asarray(case.goal) - np.asarray(case.start))) + refs.append(metrics.Reference(miss, False, float("inf"), False)) + continue ref = metrics.reference_length(trajectory, case.start, case.goal, cfg.robot_height) if not ref.snapped: logger.warning( @@ -260,12 +291,38 @@ def run_suite(suite: Suite, cfg: EvalConfig, threads: int = 1) -> DatasetResult: start_ts = np.array([r.start_ts for r in refs], dtype=np.float64) checkpoints = load_or_build_checkpoints(db_path, suite, cfg, start_ts) case_ckpt = np.searchsorted(checkpoints.times, start_ts) + negative = np.array([c.expect_fail for c in suite.cases]) + case_ckpt[negative] = -1 final_planner = cfg.make_planner() final_planner.update_global_map(final.occupied) results: list[CaseResult | None] = [None] * len(suite.cases) + for ci in np.flatnonzero(negative): + case, ref = suite.cases[ci], refs[ci] + outcome = score_negative( + _run_plan(final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg)[0] + ) + results[ci] = CaseResult( + id=case.id, + dataset=suite.dataset, + start=case.start, + goal=case.goal, + weight=case.weight, + tags=case.tags, + l_ref=ref.length, + l_ref_snapped=False, + plan_ts=float("inf"), + online_voxels=len(final.occupied), + map_update_ms=0.0, + goal_seen=True, + expect_fail=True, + online=outcome, + final=outcome, + soft_progress=outcome.spl, + ) + def process_checkpoint( k: int, keys: NDArray[np.int64], @@ -304,6 +361,7 @@ def process_checkpoint( online_voxels=len(keys), map_update_ms=map_update_ms, goal_seen=goal_seen, + expect_fail=False, online=online_out, final=final_out, soft_progress=metrics.soft_progress(end, case.start, case.goal), diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index cb0f390a8f..73a2784b0b 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -41,7 +41,7 @@ snap_to_surface, ) from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory -from dimos.navigation.nav_3d.evaluator.runner import _run_plan +from dimos.navigation.nav_3d.evaluator.runner import _run_plan, score_negative VOXEL = 0.1 @@ -397,6 +397,23 @@ def test_meta_floating_bridge_fails_support() -> None: assert gap_x.min() > 5.5 and gap_x.max() < 14.5 +def test_meta_negative_case_scoring() -> None: + """A certified-infeasible case scores 1.0 for refusal, 0.0 for any claim.""" + keys, cfg, case = _meta_scene() + refused, _ = _run_plan(_StubPlanner(None), case, 16.0, keys, keys, cfg) + out = score_negative(refused) + assert out.success + assert out.spl == 1.0 + claimed, _ = _run_plan(_StubPlanner(_u_route()), case, 16.0, keys, keys, cfg) + out = score_negative(claimed) + assert not out.success + assert out.spl == 0.0 + # A path that wanders but never reaches the goal is still a refusal. + wander = np.array([case.start, [4.0, 2.0, 0.0]], dtype=np.float32) + partial, _ = _run_plan(_StubPlanner(wander), case, 16.0, keys, keys, cfg) + assert score_negative(partial).success + + def test_check_kinematics_rejects_cliff_jumps() -> None: stairs = np.array([[0, 0, 0], [0.4, 0, 0.16], [0.8, 0, 0.32]], dtype=np.float32) assert metrics.check_kinematics(stairs, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid @@ -416,7 +433,10 @@ def test_check_kinematics_rejects_cliff_jumps() -> None: def test_save_suite_roundtrip(tmp_path) -> None: suite = Suite( dataset="demo", - cases=[Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), weight=2.0, tags=["x"])], + cases=[ + Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), weight=2.0, tags=["x"]), + Case(id="neg", start=(0.0, 0.0, 0.0), goal=(5.0, 5.0, 5.0), expect_fail=True), + ], lidar_stream="other_lidar", ) path = save_suite(suite, tmp_path / "demo.yaml") @@ -426,6 +446,8 @@ def test_save_suite_roundtrip(tmp_path) -> None: assert loaded.odom_stream == "pointlio_odometry" assert loaded.cases[0].goal == (1.0, 2.0, 3.0) assert loaded.cases[0].tags == ["x"] + assert not loaded.cases[0].expect_fail + assert loaded.cases[1].expect_fail def test_load_suite(tmp_path) -> None: diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 132f5797bf..f9d8170fa6 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -54,6 +54,7 @@ COLLISION_COLOR = [255, 0, 0] UNSUPPORTED_COLOR = [255, 0, 255] STEEP_COLOR = [160, 32, 240] +NEGATIVE_INTENT_COLOR = [255, 255, 0] VALID_PATH_COLOR = [0, 220, 0] INVALID_PATH_COLOR = [255, 0, 0] @@ -185,15 +186,24 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - base = f"{root}/cases/{case.id}" rr.log( f"{base}/start", - rr.Points3D([case.start], colors=[START_COLOR], radii=0.12), + rr.Points3D([case.start], colors=[START_COLOR], radii=0.05), static=True, ) rr.log( f"{base}/goal", - rr.Points3D([case.goal], colors=[GOAL_COLOR], radii=0.12), + rr.Points3D([case.goal], colors=[GOAL_COLOR], radii=0.05), static=True, ) - if not case.online.success: + if case.expect_fail: + # Always visible, so a correct refusal is reviewable too. + rr.log( + f"{base}/intent", + rr.LineStrips3D( + [[case.start, case.goal]], colors=[NEGATIVE_INTENT_COLOR], radii=0.006 + ), + static=True, + ) + elif not case.online.success: rr.log( f"{base}/intent", rr.LineStrips3D( From f8591cb1eadeca8ea2e13a08a5bf560553c46414 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 15 Jul 2026 20:53:12 -0700 Subject: [PATCH 07/29] Turn off stuff by default --- dimos/navigation/nav_3d/evaluator/viz.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index f9d8170fa6..e8ff4675dd 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -34,6 +34,7 @@ import numpy as np import rerun as rr +import rerun.blueprint as rrb from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.recording import load_trajectory @@ -152,6 +153,17 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float) -> None: ) +def _dataset_view(root: str, case_ids: list[str]) -> rrb.Spatial3DView: + """One view per dataset, planner graph edges hidden until toggled on.""" + hidden = [f"{root}/planner_final/edges"] + hidden += [f"{root}/cases/{cid}/known/edges" for cid in case_ids] + return rrb.Spatial3DView( + origin=f"/{root}", + name=root, + overrides={path: rrb.EntityBehavior(visible=False) for path in hidden}, + ) + + def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) -> None: rr.init("nav3d_eval", recording_id="nav3d_eval") rr.save(str(out)) @@ -215,5 +227,8 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - _log_path(f"{base}/online", case.online, radius=0.04) _log_path(f"{base}/final", case.final, radius=0.02) + views = [_dataset_view(d.dataset, [c.id for c in d.cases]) for d in report.datasets] + rr.send_blueprint(rrb.Blueprint(rrb.Tabs(*views) if len(views) > 1 else views[0])) + print(f"wrote {out}") print(f"open with: rerun {out}") From 574110d2c56c6a8cbfec92e6e7ebe6edf33e793e Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 16 Jul 2026 12:03:48 -0700 Subject: [PATCH 08/29] Default weight to 1 and run by tag --- .../nav_3d/evaluator/cases/china_office.yaml | 50 +++++++++---------- .../evaluator/cases/mid360_athens_stairs.yaml | 18 +++---- dimos/navigation/nav_3d/evaluator/cli.py | 17 ++++++- dimos/navigation/nav_3d/evaluator/config.py | 2 +- dimos/navigation/nav_3d/evaluator/generate.py | 12 ++--- dimos/navigation/nav_3d/evaluator/runner.py | 27 ++++++++++ 6 files changed, 82 insertions(+), 44 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml index 0263d3b30a..bf1dcec6fb 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -3,122 +3,122 @@ cases: - id: auto_00_up start: [-10.2, 15.8, -0.48] goal: [-8.12, 14.6, 2.56] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_01_down start: [-3.72, -4.6, 2.96] goal: [0.04, 0.04, -0.4] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_02_up start: [4.12, -21.72, -0.48] goal: [9.56, -14.28, 4.24] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_03_down start: [2.52, 10.84, 3.12] goal: [5.64, 23.64, -0.24] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_04_down start: [10.44, -4.12, 4.16] goal: [16.52, -32.28, -0.96] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_05_up start: [18.2, -20.6, -1.36] goal: [-1.4, -14.2, 2.96] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_06_down start: [-4.6, 4.52, 6.32] goal: [11.72, 6.36, -0.96] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_07_down start: [6.36, 2.6, 3.2] goal: [15.4, -9.88, -1.28] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_08_up start: [7.72, -33.8, -0.72] goal: [-3.32, 11.32, 2.88] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_09_down start: [12.2, -9.24, 4.16] goal: [3.56, -7.72, -0.4] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_10_down start: [-7.4, 7.32, 2.8] goal: [9.8, 14.04, -0.56] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_11_down start: [-2.52, 0.68, 4.0] goal: [13.4, -1.16, -1.2] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_12_down start: [6.28, -2.04, 4.16] goal: [3.32, 16.12, -0.24] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_13_down start: [-1.72, -9.72, 2.96] goal: [5.64, -13.56, -0.4] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_14_up start: [7.48, -27.48, -0.48] goal: [-6.2, 3.48, 2.88] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_15_up start: [19.8, -27.8, -1.12] goal: [2.6, -16.2, 2.08] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_16_up start: [0.6, 6.6, -0.4] goal: [2.04, -0.28, 3.12] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_17_down start: [4.28, 5.96, 3.12] goal: [17.0, -15.48, -1.28] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_18_down start: [-4.6, 8.2, 6.56] goal: [8.84, 18.84, -0.4] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_19_up start: [-8.84, 10.76, -0.48] goal: [-1.4, -2.04, 3.12] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_20_up start: [14.6, -27.88, -0.24] goal: [1.0, 13.56, 2.96] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_21_down start: [-7.4, 11.96, 3.04] goal: [8.2, -17.32, -0.4] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_22_up start: [-0.36, -4.44, -0.4] goal: [11.24, -6.76, 4.16] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_23_up start: [-0.28, 10.92, -0.4] goal: [0.6, -12.52, 3.12] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_24_flat start: [12.52, -33.4, -0.8] @@ -168,6 +168,6 @@ cases: - id: neg_00 start: [5.0, 2.44, 3.2] goal: [9.32, 5.24, 6.4] - weight: 2.0 + weight: 1.0 tags: [manual, negative] expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index 0b721a21d3..dff5df679f 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -3,47 +3,47 @@ cases: - id: auto_00_up start: [-0.12, -0.6, -0.32] goal: [1.32, -0.84, 2.72] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_01_up start: [7.24, -3.96, -6.08] goal: [6.44, -5.56, -1.44] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_02_down start: [8.04, -0.76, 3.04] goal: [-2.36, -4.36, -0.32] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_03_down start: [5.88, -4.52, 2.96] goal: [2.28, -6.52, -0.32] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_04_down start: [-0.2, -3.0, 2.56] goal: [-2.04, 3.16, -0.48] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_05_down start: [0.52, -4.68, -0.32] goal: [5.96, -3.64, -3.52] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_06_up start: [6.36, -5.4, -4.48] goal: [-2.36, 0.44, -0.4] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, up, long] - id: auto_07_down start: [6.68, -5.56, 2.08] goal: [5.72, -5.48, -1.04] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_08_down start: [-0.2, -3.0, 2.56] goal: [7.16, -3.8, -3.12] - weight: 3.0 + weight: 1.0 tags: [auto, stairs, down, long] - id: auto_09_flat start: [5.08, -3.8, -6.48] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index abb998e8f3..7e9471cc49 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -100,6 +100,9 @@ def _print_report(report: Report) -> None: f"final {d.final_voxels} voxels, " f"map build {d.map_build_ms / 1000:.1f}s" ) + print(f"\n{'by tag':<12} {'inc':>5} {'fin':>5} {'n':>4}") + for tag, s in report.by_tag.items(): + print(f"{tag:<12} {s.inc_score:>5.2f} {s.fin_score:>5.2f} {s.n:>4}") print( f"\nscore {report.score:.3f} | soft {report.score_soft:.3f} | " f"final {report.final_score:.3f} | " @@ -117,6 +120,9 @@ def run( None, help="Suite YAMLs; defaults to every manifest under cases/" ), dataset: str = typer.Option(None, "--dataset", help="Only run suites for this dataset"), + tag: list[str] = typer.Option( + None, "--tag", help="Only run cases carrying every given tag, e.g. --tag stairs --tag up" + ), json_out: Path = typer.Option(None, "--json", help="Write the full report as JSON"), rrd_out: Path = typer.Option(None, "--rrd", help="Write a rerun recording of every case"), workers: int = typer.Option( @@ -138,6 +144,13 @@ def run( ] if not suites: raise typer.BadParameter(f"no suite for dataset {dataset!r}") + if tag: + wanted_tags = set(tag) + for s in suites: + s.cases = [c for c in s.cases if wanted_tags <= set(c.tags)] + suites = [s for s in suites if s.cases] + if not suites: + raise typer.BadParameter(f"no cases carry all tags {tag}") cfg = _apply_overrides(EvalConfig(), set_ or []) report = evaluate(suites, cfg, workers=workers) _print_report(report) @@ -292,7 +305,7 @@ def add_case( goal: tuple[float, float, float] = typer.Option(..., "--goal", help="Foot-level xyz"), case_id: str = typer.Option(None, "--id", help="Case id; default manual_ or neg_"), tags: str = typer.Option(None, "--tags", help="Comma-separated tags"), - weight: float = typer.Option(2.0, "--weight"), + weight: float = typer.Option(1.0, "--weight"), snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), expect_fail: bool = typer.Option( False, "--expect-fail", help="Certified-infeasible pair; the planner must refuse" @@ -321,7 +334,7 @@ def pick_case( expect_fail: bool = typer.Option( False, "--expect-fail", help="Picked pairs are certified infeasible; planner must refuse" ), - weight: float = typer.Option(2.0, "--weight"), + weight: float = typer.Option(1.0, "--weight"), snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), ) -> None: """Pick cases by clicking the map: shift+click start then goal, repeat, then close. diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index ebb07146c5..26b33f1d9a 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -40,7 +40,7 @@ class EvalConfig: robot_height: float = 0.3 max_overhead_m: float = 2.0 - surface_closing_radius: float = 0.3 + surface_closing_radius: float = 0.4 node_spacing_m: float = 1.0 wall_clearance_m: float = 0.1 wall_buffer_m: float = 0.75 diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 719c7428b5..11edb3a992 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -301,17 +301,15 @@ def _to_case(cand: Candidate, n: int) -> Case: kind, tags = "down", ["auto", "stairs", "down"] else: kind, tags = "flat", ["auto", "flat"] - weight = 1.0 - if kind != "flat": - weight = 2.0 - if abs(cand.dz) >= LONG_STAIRS_DZ_M or cand.walked_m >= LONG_STAIRS_WALKED_M: - weight = 3.0 - tags.append("long") + if kind != "flat" and ( + abs(cand.dz) >= LONG_STAIRS_DZ_M or cand.walked_m >= LONG_STAIRS_WALKED_M + ): + tags.append("long") return Case( id=f"auto_{n:02d}_{kind}", start=cand.start, goal=cand.goal, - weight=weight, + weight=1.0, tags=tags, ) diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 4a577def90..d213e16394 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -135,6 +135,17 @@ class DatasetResult: final_artifacts: PlannerArtifacts | None = None +@dataclass +class TagStats: + """Aggregate scores over every case carrying a given tag.""" + + n: int + inc_score: float + fin_score: float + inc_success: int + fin_success: int + + @dataclass class Report: score: float @@ -146,6 +157,9 @@ class Report: # The incremental and final runs are independent tests per case; these # count the four pass/fail combinations. outcome_counts: dict[str, int] + # Score sliced by case tag (stairs, flat, up, down, ...), so a config's + # effect on each terrain class is visible next to the aggregate. + by_tag: dict[str, TagStats] plan_ms: dict[str, float] map_update_ms: dict[str, float] datasets: list[DatasetResult] @@ -456,6 +470,18 @@ def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = }[(c.online.success, c.final.success)] outcome_counts[key] += 1 + by_tag: dict[str, TagStats] = {} + for tag in sorted({t for c in cases for t in c.tags}): + tc = [c for c in cases if tag in c.tags] + w = np.array([c.weight for c in tc]) + by_tag[tag] = TagStats( + n=len(tc), + inc_score=float(np.average([c.online.spl for c in tc], weights=w)), + fin_score=float(np.average([c.final.spl for c in tc], weights=w)), + inc_success=sum(c.online.success for c in tc), + fin_success=sum(c.final.success for c in tc), + ) + return Report( score=float(np.average(online_spl, weights=weights)), score_soft=float(np.average(soft, weights=weights)), @@ -464,6 +490,7 @@ def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = n_success=sum(c.online.success for c in cases), n_success_final=sum(c.final.success for c in cases), outcome_counts=outcome_counts, + by_tag=by_tag, plan_ms=metrics.timing_stats([c.online.plan_ms for c in cases]), map_update_ms=metrics.timing_stats([c.map_update_ms for c in cases]), datasets=datasets, From 6f044d54283db37df3dd6c5d7f7b60f88fb9dddf Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 16 Jul 2026 15:19:18 -0700 Subject: [PATCH 09/29] Redo the selector --- .../nav_3d/evaluator/cases/china_office.yaml | 5 + dimos/navigation/nav_3d/evaluator/cli.py | 111 +++--- dimos/navigation/nav_3d/evaluator/config.py | 2 +- .../navigation/nav_3d/evaluator/final_map.py | 8 +- dimos/navigation/nav_3d/evaluator/generate.py | 30 +- dimos/navigation/nav_3d/evaluator/metrics.py | 6 +- dimos/navigation/nav_3d/evaluator/picker.py | 329 ++++++++++++++++++ .../navigation/nav_3d/evaluator/recording.py | 2 +- dimos/navigation/nav_3d/evaluator/runner.py | 30 +- .../nav_3d/evaluator/test_evaluator.py | 67 +++- dimos/navigation/nav_3d/evaluator/viz.py | 5 +- dimos/robot/all_blueprints.py | 1 - pyproject.toml | 2 + uv.lock | 5 + 14 files changed, 485 insertions(+), 118 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/picker.py diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml index bf1dcec6fb..051522b168 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -171,3 +171,8 @@ cases: weight: 1.0 tags: [manual, negative] expect_fail: true +- id: manual_00 + start: [13.88, 1.48, -1.04] + goal: [12.28, 8.6, -0.8] + weight: 1.0 + tags: [manual, flat] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 7e9471cc49..acb8eb07a9 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -16,16 +16,18 @@ Run every suite: python -m dimos.navigation.nav_3d.evaluator run One dataset: python -m dimos.navigation.nav_3d.evaluator run --dataset mid360_athens_stairs +Only some cases: python -m dimos.navigation.nav_3d.evaluator run --tag stairs --tag up Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json Override a knob: python -m dimos.navigation.nav_3d.evaluator run --set wall_clearance_m=0.05 New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a -Curate a case: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z +Pick cases by click: python -m dimos.navigation.nav_3d.evaluator pick-case office_a +Curate by coords: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z """ from __future__ import annotations +import contextlib import dataclasses -import itertools import json import os from pathlib import Path @@ -134,6 +136,7 @@ def run( None, "--set", help="Repeatable EvalConfig override, e.g. wall_clearance_m=0.05" ), ) -> None: + """Evaluate every case suite and print scores. The headline is incremental-map SPL.""" suites = load_suites(manifests or None) if dataset is not None: wanted = Path(dataset).stem @@ -158,6 +161,7 @@ def run( json_out.write_text(json.dumps(report.to_dict(), indent=2)) print(f"wrote {json_out}") if rrd_out is not None: + # Lazy: viz pulls in rerun, only needed with --rrd. from dimos.navigation.nav_3d.evaluator.viz import write_rrd write_rrd(report, suites, cfg, rrd_out) @@ -165,7 +169,10 @@ def run( def _copy_recording(src: Path, dest: Path) -> None: """Copy via the sqlite backup API so WAL sidecar content is never lost.""" - with sqlite3.connect(src) as source, sqlite3.connect(dest) as target: + with ( + contextlib.closing(sqlite3.connect(src)) as source, + contextlib.closing(sqlite3.connect(dest)) as target, + ): source.backup(target) @@ -331,69 +338,75 @@ def add_case( @app.command("pick-case") def pick_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the cases"), - expect_fail: bool = typer.Option( - False, "--expect-fail", help="Picked pairs are certified infeasible; planner must refuse" - ), weight: float = typer.Option(1.0, "--weight"), snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), ) -> None: - """Pick cases by clicking the map: shift+click start then goal, repeat, then close. + """Pick cases by shift+clicking the map in a browser viewer. - Opens an Open3D window with the final map colored by height and the - walked path in white. Every consecutive pair of picked points becomes - one case, snapped and appended to the manifest. + Serves the final map and walked path with viser. Shift+click picks + start/goal pairs. The side panel tags negatives, undoes picks, and saves + pairs to the manifest, snapped like add-case. """ - import open3d as o3d # type: ignore[import-untyped] - - from dimos.navigation.nav_3d.evaluator.viz import _turbo_by_height + # Lazy: picker/viz pull in viser and matplotlib, only needed for pick-case. + from dimos.navigation.nav_3d.evaluator.picker import pick_cases + from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height suite, manifest, surface, cfg = _load_for_curation(dataset) final = load_or_build_final_map(resolve_named_path(dataset, ".db"), suite, cfg) trajectory = load_trajectory(resolve_named_path(dataset, ".db"), suite.odom_stream) foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) - points = np.concatenate([final.occupied, foot]) - colors = np.concatenate( - [_turbo_by_height(final.occupied), np.full((len(foot), 3), 255, dtype=np.uint8)] + def full_tags(negative: bool, extra: list[str]) -> list[str]: + tags = ["manual"] + (["negative"] if negative else []) + return tags + [t for t in extra if t not in tags] + + def save_pair( + start: tuple[float, float, float], + goal: tuple[float, float, float], + negative: bool, + extra_tags: list[str], + case_id: str | None, + ) -> tuple[bool, str, str | None]: + try: + case = _append_case( + suite, + manifest, + surface, + start, + goal, + case_id, + full_tags(negative, extra_tags), + weight, + snap_max, + negative, + ) + except typer.BadParameter as err: + return False, str(err), None + return True, f"saved {case.id} [{', '.join(case.tags)}]", case.id + + def update_case( + saved_id: str, new_id: str, negative: bool, extra_tags: list[str] + ) -> tuple[bool, str, str | None]: + case = next((c for c in suite.cases if c.id == saved_id), None) + if case is None: + return False, f"case {saved_id!r} not found in manifest", None + if new_id != saved_id and any(c.id == new_id for c in suite.cases): + return False, f"case id {new_id!r} already exists", None + case.id = new_id + case.tags = full_tags(negative, extra_tags) + case.expect_fail = negative + save_suite(suite, manifest) + return True, f"updated {case.id} [{', '.join(case.tags)}]", case.id + + pick_cases( + dataset, final.occupied, turbo_by_height(final.occupied), foot, save_pair, update_case ) - cloud = o3d.geometry.PointCloud() - cloud.points = o3d.utility.Vector3dVector(points.astype(np.float64)) - cloud.colors = o3d.utility.Vector3dVector(colors.astype(np.float64) / 255.0) - - print("shift+click START then GOAL for each case (shift+right-click undoes); close to save") - vis = o3d.visualization.VisualizerWithEditing() - vis.create_window(window_name=f"pick cases: {dataset}") - vis.add_geometry(cloud) - vis.run() - vis.destroy_window() - picked = vis.get_picked_points() - - if len(picked) % 2: - print(f"odd number of picks ({len(picked)}); dropping the last one") - picked = picked[:-1] - if not picked: - print("no points picked; nothing added") - return - for start_idx, goal_idx in itertools.batched(picked, 2): - start = tuple(float(v) for v in points[start_idx]) - goal = tuple(float(v) for v in points[goal_idx]) - _append_case( - suite, - manifest, - surface, - (start[0], start[1], start[2]), - (goal[0], goal[1], goal[2]), - None, - ["manual", "negative"] if expect_fail else ["manual"], - weight, - snap_max, - expect_fail, - ) print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {dataset}") @app.command("list") def list_cases() -> None: + """Print every dataset's cases with endpoints, weights, and tags.""" for suite in load_suites(): print(f"{suite.dataset} ({suite.path.name if suite.path else '?'})") for case in suite.cases: diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 26b33f1d9a..acece97b88 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -51,7 +51,7 @@ class EvalConfig: # Physical body envelope for the collision gate. The gate catches paths # that penetrate obstacles, not near-grazes, so the radius is the true # body half-width. The ground margin over the radius bounds the terrain - # slope the gate tolerates; keep margin/radius above the steepest stairs. + # slope the gate tolerates. Keep margin/radius above the steepest stairs. robot_radius: float = 0.16 ground_margin: float = 0.25 body_clearance: float = 0.45 diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index a526565df6..f35337235e 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -168,11 +168,9 @@ def replay_frames( voxel_size: float, times: NDArray[np.float64], ) -> tuple[FinalMap, list[NDArray[np.int64]], list[NDArray[np.int64]]]: - """Feed frames through the mapper in order, snapshotting state as each - requested time is passed. A snapshot holds exactly the frames with - ts <= its time. Times past the last frame get the final state. Returns - the final map, the occupied-key snapshots, and the observed-key snapshots - (every voxel a raw lidar return had landed in by that time). + """Feed frames through the mapper in order, snapshotting at each requested + time. A snapshot holds exactly the frames with ts <= its time. Returns the + final map, the occupied-key snapshots, and the observed-key snapshots. """ snapshots: list[NDArray[np.int64]] = [] observed_snapshots: list[NDArray[np.int64]] = [] diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 11edb3a992..4c9defa5ca 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -15,15 +15,12 @@ """Generate evaluation cases from a recorded trajectory. Candidate pairs are sampled along the walked path, so both endpoints are -physically proven reachable. A pair is kept only when it is non-trivial: -the straight start-goal line collides with final obstacles, the walked -route detours well past the straight-line distance, or the pair climbs. -Every case points backward in time: the goal is a spot the robot had -already visited when it stood at the start, so an incremental map built up -to the start time has seen the goal and a demonstrated route. The forward -direction is emitted too when the start is revisited after the goal. -Endpoints snap to the final surface so drift between passes cannot leave -a case floating off the map. Generation is deterministic. +physically proven reachable, and kept only when non-trivial (the straight +line collides, the route detours, or the pair climbs). Cases point backward +in time so an incremental map built to the start has already seen the goal +and a demonstrated route, with the forward direction emitted when the start +is revisited after the goal. Endpoints snap to the final surface so drift +cannot leave a case floating off the map. Generation is deterministic. """ from __future__ import annotations @@ -162,7 +159,7 @@ def generate_cases( dz = float(sb[2] - sa[2]) detour = float(w / e) if detour < params.detour_ratio_min and abs(dz) < STAIRS_DZ_M: - # A long near-straight flat pair is trivial; not worth a sweep. + # A long near-straight flat pair is trivial. Not worth a sweep. if e > 30.0: continue # Only pairs not already qualified pay for the line sweep. @@ -177,7 +174,7 @@ def generate_cases( ).valid if not blocked: continue - # Backward in time is always causal; forward only when the start + # Backward in time is always causal. Forward only when the start # spot is revisited after the goal visit. directed = [(sb, sa, -dz)] if last_visit_a >= float(trajectory.ts[idx[bi]]): @@ -217,14 +214,9 @@ def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> def _select_diverse( ranked: list[Candidate], params: GenerationParams, max_cases: int ) -> list[Candidate]: - """Spread-greedy selection: each slot goes to the candidate whose score is - its priority plus how far its endpoints are from every endpoint already in - use. Coverage is the objective, not a filter, so cases spread across the - map instead of fanning out of the highest-priority spot. A sector-usage - cap bounds hub reuse outright, and the flat quota keeps stairs from - crowding out flats. When the strict pass yields fewer than min_cases, - sector-capped candidates are revived and a relaxed pass without sector - caps or the flat quota backfills up to the floor. + """Spread-greedy selection scored by priority plus endpoint distance from + already-used points, with a sector cap and flat quota. A relaxed pass + backfills to min_cases when the strict pass falls short. """ if not ranked: return [] diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 6c7292f62b..87dcfd3fd0 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -48,7 +48,7 @@ def goal_reached( # Clearance margins are only measured out to this horizontal distance from -# the body surface; anything farther reports the cap. +# the body surface. Anything farther reports the cap. MARGIN_CAP_M = 0.3 @@ -60,7 +60,7 @@ class GateResult: collision_points: NDArray[np.float32] # Horizontal distance from the body surface to the nearest obstacle in # the gate's z band, minimized along the path. Negative is penetration - # depth; capped at MARGIN_CAP_M when nothing is near. Gives a smooth + # depth, capped at MARGIN_CAP_M when nothing is near. Gives a smooth # how-close-to-flipping signal next to the binary verdict. min_clearance_m: float @@ -184,7 +184,7 @@ class Reference: length: float snapped: bool - # When the robot stood at the start about to walk the route; inf when + # When the robot stood at the start about to walk the route. Inf when # the endpoints are off the trajectory or no causal pair exists. start_ts: float # True when the goal was visited before the chosen start visit, so a diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py new file mode 100644 index 0000000000..8dd27918c0 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -0,0 +1,329 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Browser point-picking for case curation, served by viser. + +Opens a dark-themed local web viewer with the final map and walked path. +Shift+click picks points in start/goal pairs. Every pair gets its own panel +entry with the coordinates, a name field, geometry-suggested tag checkboxes, +custom tags, and a negative toggle. Pairs save individually or all at once, +and stay editable after saving: rename or retag and press the pair's button +again to update the manifest. Plain clicks and drags only move the camera. +""" + +from __future__ import annotations + +import threading +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.generate import ( + LONG_STAIRS_DZ_M, + LONG_STAIRS_WALKED_M, + STAIRS_DZ_M, +) + +if TYPE_CHECKING: + from collections.abc import Callable + + from numpy.typing import NDArray + import viser + + # (start, goal, negative, extra_tags, case_id) -> (ok, message, saved_id) + SavePair = Callable[ + [tuple[float, float, float], tuple[float, float, float], bool, list[str], str | None], + tuple[bool, str, str | None], + ] + # (saved_id, new_id, negative, extra_tags) -> (ok, message, saved_id) + UpdateCase = Callable[[str, str, bool, list[str]], tuple[bool, str, str | None]] + +# Selection cone half-angle around the click ray. Wide enough to hit a voxel +# point from across a room, narrow enough to stay on the intended surface. +PICK_CONE_RAD = 0.008 +START_COLOR = (0, 255, 255) +GOAL_COLOR = (255, 140, 0) +PAIR_COLOR = (255, 255, 0) +SUGGESTED_TAGS = ("stairs", "flat", "up", "down", "long", "doorway") + +INSTRUCTIONS = """**shift+click** picks START then GOAL, repeated per case. +Plain drag orbits, scroll zooms, right-drag pans. +""" + + +def pick_along_ray( + points: NDArray[np.float32], + origin: NDArray[np.float64], + direction: NDArray[np.float64], + cone_rad: float = PICK_CONE_RAD, +) -> NDArray[np.float32] | None: + """Nearest cloud point inside a small cone around the click ray.""" + rel = points.astype(np.float64) - origin + t = rel @ direction + ahead = t > 0.05 + if not ahead.any(): + return None + t = t[ahead] + perp = np.linalg.norm(rel[ahead] - t[:, None] * direction, axis=1) + angle = perp / t + for widen in (1.0, 4.0): + hit = angle < cone_rad * widen + if hit.any(): + idx = np.flatnonzero(ahead)[hit] + return np.asarray(points[idx[np.argmin(t[hit])]]) + return None + + +def suggested_tags(start: NDArray[np.float32], goal: NDArray[np.float32]) -> set[str]: + """Geometry-derived tag suggestions, mirroring auto-generation's rules.""" + dz = float(goal[2] - start[2]) + euclid = float(np.linalg.norm(goal - start)) + tags: set[str] = set() + if abs(dz) >= STAIRS_DZ_M: + tags |= {"stairs", "up" if dz > 0 else "down"} + else: + tags.add("flat") + if abs(dz) >= LONG_STAIRS_DZ_M or euclid >= LONG_STAIRS_WALKED_M: + tags.add("long") + return tags + + +class _PairEntry: + """One picked start/goal pair and its editable panel widgets.""" + + def __init__( + self, + server: viser.ViserServer, + n: int, + start: NDArray[np.float32], + goal: NDArray[np.float32], + save_pair: SavePair, + update_case: UpdateCase, + lock: threading.Lock, + ) -> None: + self._server = server + self._n = n + self.start = start + self.goal = goal + self._save_pair = save_pair + self._update_case = update_case + self._lock = lock + self.saved_id: str | None = None + self._name = "" + self._checked = suggested_tags(start, goal) + self._custom = "" + self._negative = False + self._status = "unsaved" + self._build(expanded=True, order=None) + + def _build(self, *, expanded: bool, order: float | None) -> None: + server = self._server + start, goal = self.start, self.goal + self.folder = server.gui.add_folder( + f"pair {self._n}", order=order, expand_by_default=expanded + ) + with self.folder: + server.gui.add_markdown( + f"({start[0]:.1f}, {start[1]:.1f}, {start[2]:.1f}) → " + f"({goal[0]:.1f}, {goal[1]:.1f}, {goal[2]:.1f})" + ) + self.id_text = server.gui.add_text( + "name", initial_value=self._name, hint="empty = auto id" + ) + with server.gui.add_folder("tags", expand_by_default=True): + self.tag_boxes = { + tag: server.gui.add_checkbox(tag, tag in self._checked) + for tag in SUGGESTED_TAGS + } + self.custom_text = server.gui.add_text( + "custom", initial_value=self._custom, hint="comma-separated" + ) + self.negative_box = server.gui.add_checkbox("negative (must refuse)", self._negative) + self.message = server.gui.add_markdown(self._status) + self.button = server.gui.add_button("save / update") + + @self.button.on_click + def _(_event: object) -> None: + # save_unsaved calls save_or_update already holding the lock; + # the button path runs on a bare viser callback thread and must + # take it to serialize suite/manifest mutation. + with self._lock: + self.save_or_update() + + def remove(self) -> None: + self.folder.remove() + + def _snapshot(self) -> None: + self._name = self.id_text.value + self._checked = {tag for tag, box in self.tag_boxes.items() if box.value} + self._custom = self.custom_text.value + self._negative = self.negative_box.value + + def extra_tags(self) -> list[str]: + tags = [tag for tag, box in self.tag_boxes.items() if box.value] + tags += [t.strip() for t in self.custom_text.value.split(",") if t.strip()] + return tags + + def save_or_update(self) -> None: + name = self.id_text.value.strip() + if self.saved_id is None: + ok, msg, saved = self._save_pair( + (float(self.start[0]), float(self.start[1]), float(self.start[2])), + (float(self.goal[0]), float(self.goal[1]), float(self.goal[2])), + self.negative_box.value, + self.extra_tags(), + name or None, + ) + else: + ok, msg, saved = self._update_case( + self.saved_id, name or self.saved_id, self.negative_box.value, self.extra_tags() + ) + print(msg) + if not (ok and saved is not None): + self.message.content = f"**FAILED**: {msg}" + return + # Folders cannot be collapsed live in viser, expand_by_default is + # only read when the folder is first created. Rebuild it collapsed + # in place instead. + self.saved_id = saved + self._snapshot() + self._name = saved + self._status = msg + order = self.folder.order + self.folder.remove() + self._build(expanded=False, order=order) + + +def pick_cases( + dataset: str, + map_points: NDArray[np.float32], + map_colors: NDArray[np.uint8], + walked: NDArray[np.float32], + save_pair: SavePair, + update_case: UpdateCase, +) -> None: + """Serve the picker until the user exits from the panel or hits ctrl-c.""" + import viser + + server = viser.ViserServer(label=f"Pair Picker - {dataset}", verbose=False) + server.gui.configure_theme(dark_mode=True) + server.scene.set_background_image(np.full((1, 1, 3), 14, dtype=np.uint8)) + server.scene.set_up_direction("+z") + server.scene.add_point_cloud( + "/map", + map_points, + map_colors, + point_size=0.035, + point_shape="circle", + precision="float32", + ) + if len(walked) >= 2: + segments = np.stack([walked[:-1], walked[1:]], axis=1) + server.scene.add_line_segments( + "/walked_path", segments, colors=(255, 255, 255), line_width=2.0 + ) + + center = map_points.mean(axis=0) + span = float(np.ptp(map_points[:, :2])) + + @server.on_client_connect + def _(client: viser.ClientHandle) -> None: + client.camera.position = tuple(center + np.array([0.6 * span, 0.6 * span, 0.45 * span])) + client.camera.look_at = tuple(center) + + server.gui.add_markdown(INSTRUCTIONS) + undo_button = server.gui.add_button("undo last pick") + save_all_button = server.gui.add_button("save all unsaved") + exit_button = server.gui.add_button("save all & exit") + + lock = threading.Lock() + stop = threading.Event() + picks: list[NDArray[np.float32]] = [] + pairs: list[_PairEntry] = [] + markers: list[viser.SceneNodeHandle] = [] + pair_count = 0 + + @server.scene.on_click(modifier="shift") + def _(event: viser.SceneClickEvent) -> None: + nonlocal pair_count + point = pick_along_ray( + map_points, np.asarray(event.ray_origin), np.asarray(event.ray_direction) + ) + if point is None: + return + with lock: + is_goal = len(picks) % 2 == 1 + picks.append(point) + n = len(picks) + markers.append( + server.scene.add_icosphere( + f"/picks/p{n}", + radius=0.09, + color=GOAL_COLOR if is_goal else START_COLOR, + position=(float(point[0]), float(point[1]), float(point[2]) + 0.05), + ) + ) + if is_goal: + start, goal = picks[-2], picks[-1] + markers.append( + server.scene.add_line_segments( + f"/picks/l{n}", + np.stack([start, goal])[None], + colors=PAIR_COLOR, + line_width=2.5, + ) + ) + pair_count += 1 + pairs.append( + _PairEntry(server, pair_count, start, goal, save_pair, update_case, lock) + ) + + @undo_button.on_click + def _(_event: object) -> None: + with lock: + if not picks: + return + if len(picks) % 2 == 0: + # Completing pick of the last pair. Saved cases stay in the + # manifest, only the panel entry and markers go away. + pair = pairs.pop() + pair.remove() + markers.pop().remove() # pair line + picks.pop() + markers.pop().remove() + + def save_unsaved() -> None: + with lock: + for pair in pairs: + if pair.saved_id is None: + pair.save_or_update() + + @save_all_button.on_click + def _(_event: object) -> None: + save_unsaved() + + @exit_button.on_click + def _(_event: object) -> None: + save_unsaved() + stop.set() + + print("picker running; ctrl-c to exit (unsaved pairs are discarded)") + try: + stop.wait() + except KeyboardInterrupt: + unsaved = sum(1 for p in pairs if p.saved_id is None) + if unsaved: + print(f"discarded {unsaved} unsaved pair(s)") + finally: + server.stop() diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py index 2c20415615..384ebcd5c1 100644 --- a/dimos/navigation/nav_3d/evaluator/recording.py +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -64,7 +64,7 @@ def iter_world_frames( """Yield lidar frames registered into the world by their aligned odometry pose. Clouds must be sensor-frame. Legacy recordings with pre-registered - world-frame clouds are rejected; re-record them. + world-frame clouds are rejected. Re-record them. """ store = SqliteStore(path=str(db_path)) with store: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index d213e16394..6437453b3f 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -14,19 +14,13 @@ """Run case suites through the ray tracer and MLS planner and score them. -Every case is planned twice. The online plan runs on the incremental map: -what the mapper had built by the moment the robot stood at the case's start, -about to walk the demonstrated route back to a goal it already visited. The -final plan runs on the final map: the same pipeline fed the whole recording. -The final map is not ground truth, just the most complete map this pipeline -produces, so a failure on it means the whole pipeline cannot solve the case -even with all the data. The final path is gated against the full final -occupancy; the online path only against the final obstacles the sensor had -returns from by plan time: hitting a wall no lidar return ever came from is -not an error, but hitting one the sensor saw and the mapper dropped is. -Every path must also stand on final-map occupancy (no fabricated bridges) -and stay within the robot's climb envelope. The headline score is -validity-gated SPL on the incremental map. +Every case is planned twice: online on the incremental map built up to the +case's start time, and final on the map fed the whole recording. The final +map is not ground truth, only the most complete map the pipeline produces. +The final path is gated against full final occupancy, the online path only +against obstacles the sensor had returns from by plan time. Every path must +also stand on final-map occupancy and stay within the climb envelope. The +headline score is validity-gated SPL on the incremental map. """ from __future__ import annotations @@ -72,7 +66,7 @@ class PlanOutcome: planned: bool reached: bool valid: bool - # Every sample stands on final-map occupancy; fabricated bridges fail. + # Every sample stands on final-map occupancy. Fabricated bridges fail. supported: bool # No segment rises steeper than the robot can climb. kinematic: bool @@ -82,10 +76,10 @@ class PlanOutcome: length: float plan_ms: float spl: float - # How far the path end is from the goal; start-to-goal distance when no + # How far the path end is from the goal. Start-to-goal distance when no # path was planned. Smooth counterpart to the binary reached flag. goal_miss: float - # Gate margin along the path (see GateResult.min_clearance_m); None when + # Gate margin along the path (see GateResult.min_clearance_m). None when # no path was planned. min_clearance: float | None waypoints: list[list[float]] @@ -154,7 +148,7 @@ class Report: n_cases: int n_success: int n_success_final: int - # The incremental and final runs are independent tests per case; these + # The incremental and final runs are independent tests per case. These # count the four pass/fail combinations. outcome_counts: dict[str, int] # Score sliced by case tag (stairs, flat, up, down, ...), so a config's @@ -398,7 +392,7 @@ def task(k: int, keys: NDArray[np.int64], gate: NDArray[np.int64]) -> None: def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64], NDArray[np.int64]]]: """Walk the delta chain once. The online gate only holds obstacles the - sensor had returns from by plan time; obstacles never observed are not + sensor had returns from by plan time. Obstacles never observed are not the planner's fault.""" gate = np.array([], dtype=np.int64) for k, (keys, observed_new) in enumerate(checkpoints.iter_snapshots()): diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 73a2784b0b..0b32e53a10 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -15,7 +15,8 @@ from __future__ import annotations import itertools -from types import SimpleNamespace +from pathlib import Path +from typing import TYPE_CHECKING, cast import numpy as np import pytest @@ -43,6 +44,9 @@ from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory from dimos.navigation.nav_3d.evaluator.runner import _run_plan, score_negative +if TYPE_CHECKING: + from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + VOXEL = 0.1 @@ -186,13 +190,7 @@ def test_generate_cases_around_wall() -> None: positions = np.column_stack([xy, np.full(len(xy), 0.3)]).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 60, len(positions)), positions=positions) - cfg = SimpleNamespace( - robot_height=0.3, - voxel_size=VOXEL, - robot_radius=0.16, - ground_margin=0.25, - body_clearance=0.45, - ) + cfg = EvalConfig(voxel_size=VOXEL) cases = generate_cases(traj, final, surface, cfg, GenerationParams(max_cases=10)) assert cases assert len({c.id for c in cases}) == len(cases) @@ -206,7 +204,13 @@ def test_generate_cases_around_wall() -> None: def test_select_diverse_backfills_to_min_cases() -> None: """Sector caps must not starve a dataset below the case floor.""" candidates = [ - Candidate(start=(x, 0.0, 0.0), goal=(x, 20.0, 0.0), walked_m=30.0, detour_ratio=1.5, dz=0.0) + Candidate( + start=(float(x), 0.0, 0.0), + goal=(float(x), 20.0, 0.0), + walked_m=30.0, + detour_ratio=1.5, + dz=0.0, + ) for x in np.arange(0.0, 16.0, 2.0) ] strict = _select_diverse(candidates, GenerationParams(min_cases=0), max_cases=12) @@ -314,6 +318,10 @@ def plan( return self._waypoints +def _stub(waypoints: np.ndarray | None) -> MLSPlanner: + return cast("MLSPlanner", _StubPlanner(waypoints)) + + def _floor(x_lo: float = 0.0, x_hi: float = 20.0) -> np.ndarray: xs, ys = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(-2, 6, VOXEL)) return np.stack([xs.ravel(), ys.ravel(), np.full(xs.size, -0.05)], axis=1, dtype=np.float32) @@ -339,7 +347,7 @@ def test_meta_straight_line_cheat_scores_zero() -> None: """A planner that ignores the map and beelines must not score.""" keys, cfg, case = _meta_scene() line = np.array([case.start, case.goal], dtype=np.float32) - out, _ = _run_plan(_StubPlanner(line), case, 24.0, keys, keys, cfg) + out, _ = _run_plan(_stub(line), case, 24.0, keys, keys, cfg) assert out.planned and out.reached and out.supported assert not out.valid assert out.spl == 0.0 @@ -349,7 +357,7 @@ def test_meta_straight_line_cheat_scores_zero() -> None: def test_meta_no_path_scores_zero_with_miss() -> None: keys, cfg, case = _meta_scene() - out, _ = _run_plan(_StubPlanner(None), case, 24.0, keys, keys, cfg) + out, _ = _run_plan(_stub(None), case, 24.0, keys, keys, cfg) assert not out.planned assert out.spl == 0.0 assert out.goal_miss == pytest.approx(16.0) @@ -361,7 +369,7 @@ def test_meta_demonstrated_route_scores_full() -> None: keys, cfg, case = _meta_scene() route = _u_route() l_ref = metrics.path_length(route) - out, _ = _run_plan(_StubPlanner(route), case, l_ref, keys, keys, cfg) + out, _ = _run_plan(_stub(route), case, l_ref, keys, keys, cfg) assert out.success assert out.spl == pytest.approx(1.0) assert out.goal_miss == 0.0 @@ -376,7 +384,7 @@ def test_meta_everything_occupied_fails_even_good_routes() -> None: everything = np.stack([xs.ravel(), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) keys = np.unique(voxel_keys(np.concatenate([_floor(), everything]), VOXEL)) _, cfg, case = _meta_scene() - out, _ = _run_plan(_StubPlanner(_u_route()), case, 24.0, keys, keys, cfg) + out, _ = _run_plan(_stub(_u_route()), case, 24.0, keys, keys, cfg) assert out.planned and out.reached assert not out.valid assert out.spl == 0.0 @@ -388,7 +396,7 @@ def test_meta_floating_bridge_fails_support() -> None: keys = np.unique(voxel_keys(gapped, VOXEL)) _, cfg, case = _meta_scene() line = np.array([case.start, case.goal], dtype=np.float32) - out, _ = _run_plan(_StubPlanner(line), case, 24.0, keys, keys, cfg) + out, _ = _run_plan(_stub(line), case, 24.0, keys, keys, cfg) assert out.planned and out.reached and out.valid assert not out.supported assert out.spl == 0.0 @@ -400,17 +408,17 @@ def test_meta_floating_bridge_fails_support() -> None: def test_meta_negative_case_scoring() -> None: """A certified-infeasible case scores 1.0 for refusal, 0.0 for any claim.""" keys, cfg, case = _meta_scene() - refused, _ = _run_plan(_StubPlanner(None), case, 16.0, keys, keys, cfg) + refused, _ = _run_plan(_stub(None), case, 16.0, keys, keys, cfg) out = score_negative(refused) assert out.success assert out.spl == 1.0 - claimed, _ = _run_plan(_StubPlanner(_u_route()), case, 16.0, keys, keys, cfg) + claimed, _ = _run_plan(_stub(_u_route()), case, 16.0, keys, keys, cfg) out = score_negative(claimed) assert not out.success assert out.spl == 0.0 # A path that wanders but never reaches the goal is still a refusal. wander = np.array([case.start, [4.0, 2.0, 0.0]], dtype=np.float32) - partial, _ = _run_plan(_StubPlanner(wander), case, 16.0, keys, keys, cfg) + partial, _ = _run_plan(_stub(wander), case, 16.0, keys, keys, cfg) assert score_negative(partial).success @@ -430,7 +438,28 @@ def test_check_kinematics_rejects_cliff_jumps() -> None: assert len(result.violation_points) >= 1 -def test_save_suite_roundtrip(tmp_path) -> None: +def test_pick_along_ray() -> None: + from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray + + wall = _wall(10.0) + origin = np.array([0.0, 0.0, 0.5]) + target = np.array([10.0, 0.35, 0.75]) + direction = target - origin + direction /= np.linalg.norm(direction) + picked = pick_along_ray(wall, origin, direction) + assert picked is not None + assert np.linalg.norm(picked - target) < 0.15 + # The nearest surface along the ray wins over one behind it. + two_walls = np.concatenate([_wall(10.0), _wall(15.0)]) + picked = pick_along_ray(two_walls, origin, direction) + assert picked is not None + assert abs(picked[0] - 10.0) < 0.2 + # A ray into empty space picks nothing. + up = np.array([0.0, 0.0, 1.0]) + assert pick_along_ray(wall, origin, up) is None + + +def test_save_suite_roundtrip(tmp_path: Path) -> None: suite = Suite( dataset="demo", cases=[ @@ -450,7 +479,7 @@ def test_save_suite_roundtrip(tmp_path) -> None: assert loaded.cases[1].expect_fail -def test_load_suite(tmp_path) -> None: +def test_load_suite(tmp_path: Path) -> None: manifest = tmp_path / "demo.yaml" manifest.write_text( "dataset: demo\n" diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index e8ff4675dd..47f47433d5 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -64,7 +64,8 @@ CLEARANCE_CLAMP_M = 1.0 -def _turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: +def turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: + # Lazy: matplotlib is a heavy viz-only dependency. import matplotlib.pyplot as plt z = points[:, 2].astype(np.float64) @@ -180,7 +181,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - f"{root}/map/obstacles", rr.Points3D( final.occupied, - colors=_turbo_by_height(final.occupied), + colors=turbo_by_height(final.occupied), radii=cfg.voxel_size / 4, ), static=True, diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 160780a8bc..29d3478c5a 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -174,7 +174,6 @@ "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", "episode-monitor-module": "dimos.learning.collection.episode_monitor.EpisodeMonitorModule", - "evaluator": "dimos.navigation.nav_3d.evaluator.evaluator.Evaluator", "far-planner": "dimos.navigation.cmu_nav.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder", diff --git a/pyproject.toml b/pyproject.toml index e15b8e2408..649e4fd15c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -203,6 +203,8 @@ visualization = [ # Rerun URDF robot visualization. yourdfpy depends on trimesh[easy], # which pulls embreex; embreex has no Linux aarch64 wheel. "yourdfpy>=0.0.60; sys_platform != 'linux' or platform_machine != 'aarch64'", + # Browser point-picking for nav-3d evaluator case curation. + "viser[urdf]>=1.0.29", ] learning = [ diff --git a/uv.lock b/uv.lock index b3dfd7a5a8..71d66e3646 100644 --- a/uv.lock +++ b/uv.lock @@ -1684,6 +1684,7 @@ base = [ { name = "transformers", extra = ["torch"] }, { name = "ultralytics" }, { name = "uvicorn" }, + { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] cpu = [ @@ -1788,6 +1789,7 @@ unitree = [ { name = "ultralytics" }, { name = "unitree-webrtc-connect" }, { name = "uvicorn" }, + { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] unitree-dds = [ @@ -1822,11 +1824,13 @@ unitree-dds = [ { name = "unitree-sdk2py-dimos" }, { name = "unitree-webrtc-connect" }, { name = "uvicorn" }, + { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] visualization = [ { name = "dimos-viewer" }, { name = "rerun-sdk" }, + { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] web = [ @@ -2115,6 +2119,7 @@ requires-dist = [ { name = "usd-core", marker = "extra == 'scene'", specifier = ">=23.11" }, { name = "uvicorn", marker = "extra == 'web'", specifier = ">=0.34.0" }, { name = "viser", extras = ["urdf"], marker = "extra == 'manipulation'", specifier = ">=1.0.29" }, + { name = "viser", extras = ["urdf"], marker = "extra == 'visualization'", specifier = ">=1.0.29" }, { name = "websocket-client", specifier = ">=1.8" }, { name = "xacro", marker = "extra == 'manipulation'" }, { name = "xarm-python-sdk", marker = "extra == 'manipulation'", specifier = ">=1.17.0" }, From 3fba8ecc1487bde917ee2f70a39ca9702dfb6db1 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 16 Jul 2026 16:01:39 -0700 Subject: [PATCH 10/29] Diff against previous runs --- dimos/navigation/nav_3d/evaluator/cli.py | 31 +++++++ .../nav_3d/evaluator/test_evaluator.py | 89 ++++++++++++++++++- dimos/navigation/nav_3d/evaluator/tripwire.py | 89 +++++++++++++++++++ 3 files changed, 207 insertions(+), 2 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/tripwire.py diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index acb8eb07a9..52709ec240 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -19,6 +19,7 @@ Only some cases: python -m dimos.navigation.nav_3d.evaluator run --tag stairs --tag up Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json Override a knob: python -m dimos.navigation.nav_3d.evaluator run --set wall_clearance_m=0.05 +Compare two runs: python -m dimos.navigation.nav_3d.evaluator diff old.json new.json New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a Pick cases by click: python -m dimos.navigation.nav_3d.evaluator pick-case office_a Curate by coords: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z @@ -37,6 +38,7 @@ import numpy as np import typer +from dimos.navigation.nav_3d.evaluator import tripwire from dimos.navigation.nav_3d.evaluator.cases import ( CASES_DIR, Case, @@ -116,6 +118,35 @@ def _print_report(report: Report) -> None: ) +@app.command("diff") +def diff_reports( + old: Path = typer.Argument(..., help="Baseline report JSON from `run --json`"), + new: Path = typer.Argument(..., help="Candidate report JSON from `run --json`"), +) -> None: + """Name every case whose pass/fail flipped between two runs. + + Exits 1 when any case regressed, so a keep/discard loop can gate on it. + """ + old_report = json.loads(old.read_text()) + new_report = json.loads(new.read_text()) + print( + f"score {old_report['score']:.3f} -> {new_report['score']:.3f} | " + f"final {old_report['final_score']:.3f} -> {new_report['final_score']:.3f}" + ) + d = tripwire.diff(old_report, new_report) + print(f"{len(d.fixed)} fixed, {len(d.broke)} broke") + for flip in d.fixed: + print(f" fixed: {flip.key} ({flip.test})") + for flip in d.broke: + print(f" BROKE: {flip.key} ({flip.test}: pass -> fail)") + for key in d.added: + print(f" new case: {key}") + for key in d.removed: + print(f" case gone: {key}") + if d.broke: + raise typer.Exit(code=1) + + @app.command() def run( manifests: list[Path] = typer.Argument( diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 0b32e53a10..1b00a24706 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -14,14 +14,16 @@ from __future__ import annotations +from dataclasses import replace import itertools +import json from pathlib import Path from typing import TYPE_CHECKING, cast import numpy as np import pytest -from dimos.navigation.nav_3d.evaluator import metrics +from dimos.navigation.nav_3d.evaluator import metrics, tripwire from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.final_map import ( @@ -42,7 +44,14 @@ snap_to_surface, ) from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory -from dimos.navigation.nav_3d.evaluator.runner import _run_plan, score_negative +from dimos.navigation.nav_3d.evaluator.runner import ( + CaseResult, + DatasetResult, + Report, + _no_plan, + _run_plan, + score_negative, +) if TYPE_CHECKING: from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner @@ -502,3 +511,79 @@ def test_load_suite(tmp_path: Path) -> None: ) with pytest.raises(ValueError, match="duplicate"): load_suite(manifest) + + +def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, object]: + """Report JSON from a {dataset: {case_id: (inc, fin)}} pass/fail spec.""" + datasets = [] + for dataset, cases in spec.items(): + results = [] + for case_id, (inc, fin) in cases.items(): + case = Case(id=case_id, start=(0.0, 0.0, 0.0), goal=(1.0, 0.0, 0.0)) + results.append( + CaseResult( + id=case_id, + dataset=dataset, + start=case.start, + goal=case.goal, + weight=1.0, + tags=[], + l_ref=1.0, + l_ref_snapped=False, + plan_ts=0.0, + online_voxels=0, + map_update_ms=0.0, + goal_seen=True, + expect_fail=False, + online=replace(_no_plan(case, 0.0), success=inc), + final=replace(_no_plan(case, 0.0), success=fin), + soft_progress=0.0, + ) + ) + datasets.append( + DatasetResult( + dataset=dataset, + cases=results, + final_voxels=0, + map_build_ms=0.0, + add_frame_ms={}, + frames=0, + ) + ) + report = Report( + score=0.0, + score_soft=0.0, + final_score=0.0, + n_cases=0, + n_success=0, + n_success_final=0, + outcome_counts={}, + by_tag={}, + plan_ms={}, + map_update_ms={}, + datasets=datasets, + ) + return json.loads(json.dumps(report.to_dict())) + + +def test_tripwire_outcomes() -> None: + report = _tripwire_report({"office": {"a": (True, False), "b": (False, True)}}) + assert tripwire.outcomes(report) == { + "office": {"a": {"inc": True, "fin": False}, "b": {"inc": False, "fin": True}} + } + d = tripwire.diff(report, report) + assert d.fixed == [] and d.broke == [] and d.added == [] and d.removed == [] + + +def test_tripwire_diff_names_every_flip() -> None: + old = _tripwire_report( + {"office": {"a": (False, True), "b": (True, True), "gone": (True, True)}} + ) + new = _tripwire_report( + {"office": {"a": (True, True), "b": (False, False), "fresh": (True, True)}} + ) + d = tripwire.diff(old, new) + assert [(f.key, f.test) for f in d.fixed] == [("office/a", "inc")] + assert [(f.key, f.test) for f in d.broke] == [("office/b", "inc"), ("office/b", "fin")] + assert d.added == ["office/fresh"] + assert d.removed == ["office/gone"] diff --git a/dimos/navigation/nav_3d/evaluator/tripwire.py b/dimos/navigation/nav_3d/evaluator/tripwire.py new file mode 100644 index 0000000000..5ba2d43276 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/tripwire.py @@ -0,0 +1,89 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Per-case pass/fail diff between two report JSONs. + +The aggregate score can rise while individual cases flip from pass to fail. +Diffing two reports names every flip, so a change is judged case by case +rather than by the average alone. Stateless: which report counts as the +baseline is the caller's decision, typically the last kept run. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import cast + +TESTS = ("inc", "fin") + +Outcomes = dict[str, dict[str, dict[str, bool]]] + + +@dataclass +class Flip: + """One case whose pass/fail state changed on one of the two tests.""" + + key: str + test: str + passed: bool + + +@dataclass +class ReportDiff: + fixed: list[Flip] + broke: list[Flip] + # Case ids present in only one of the two reports. + added: list[str] + removed: list[str] + + +def outcomes(report: dict[str, object]) -> Outcomes: + """Pass/fail of both tests for every case in a `run --json` report.""" + out: Outcomes = {} + for dataset in cast("list[dict[str, object]]", report["datasets"]): + cases: dict[str, dict[str, bool]] = {} + for case in cast("list[dict[str, object]]", dataset["cases"]): + online = cast("dict[str, object]", case["online"]) + final = cast("dict[str, object]", case["final"]) + cases[cast("str", case["id"])] = { + "inc": bool(online["success"]), + "fin": bool(final["success"]), + } + out[cast("str", dataset["dataset"])] = dict(sorted(cases.items())) + return out + + +def diff(old_report: dict[str, object], new_report: dict[str, object]) -> ReportDiff: + old, new = outcomes(old_report), outcomes(new_report) + fixed: list[Flip] = [] + broke: list[Flip] = [] + added: list[str] = [] + for dataset, cases in new.items(): + old_cases = old.get(dataset, {}) + for case_id, tests in cases.items(): + key = f"{dataset}/{case_id}" + if case_id not in old_cases: + added.append(key) + continue + for test in TESTS: + was, now = old_cases[case_id][test], tests[test] + if was != now: + (fixed if now else broke).append(Flip(key, test, now)) + removed = [ + f"{dataset}/{case_id}" + for dataset, cases in old.items() + for case_id in cases + if case_id not in new.get(dataset, {}) + ] + return ReportDiff(fixed, broke, sorted(added), sorted(removed)) From 78f1c32eab49e1d801690efb2f35954d0ef35725 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 16 Jul 2026 17:51:16 -0700 Subject: [PATCH 11/29] Improvements --- dimos/mapping/ray_tracing/rust/flake.nix | 2 +- dimos/navigation/nav_3d/evaluator/cli.py | 20 +++++++++ dimos/navigation/nav_3d/evaluator/picker.py | 29 +++++++++++-- .../nav_3d/evaluator/test_evaluator.py | 11 +++++ dimos/navigation/nav_3d/evaluator/tripwire.py | 41 +++++++++++++++++++ 5 files changed, 99 insertions(+), 4 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/flake.nix b/dimos/mapping/ray_tracing/rust/flake.nix index 897d8818a2..15c40909f3 100644 --- a/dimos/mapping/ray_tracing/rust/flake.nix +++ b/dimos/mapping/ray_tracing/rust/flake.nix @@ -34,7 +34,7 @@ cargoRoot = "dimos/mapping/ray_tracing/rust"; buildAndTestSubdir = "dimos/mapping/ray_tracing/rust"; - cargoHash = "sha256-0d0dlNDvDplA7oWTyUWOCOlS74Zie8uMQ+ps6lXntOI="; + cargoHash = "sha256-6a8GHRSKI6mjg9HNbrestCud8xZtF8HaD0bWVMbl7N8="; meta.mainProgram = "voxel_ray_tracing"; }; diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 52709ec240..c36cd1329b 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -20,6 +20,7 @@ Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json Override a knob: python -m dimos.navigation.nav_3d.evaluator run --set wall_clearance_m=0.05 Compare two runs: python -m dimos.navigation.nav_3d.evaluator diff old.json new.json +Determinism check: run twice with --json, then diff a.json b.json --exact New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a Pick cases by click: python -m dimos.navigation.nav_3d.evaluator pick-case office_a Curate by coords: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z @@ -122,10 +123,18 @@ def _print_report(report: Report) -> None: def diff_reports( old: Path = typer.Argument(..., help="Baseline report JSON from `run --json`"), new: Path = typer.Argument(..., help="Candidate report JSON from `run --json`"), + exact: bool = typer.Option( + False, + "--exact", + help="Require bit-identical results (ignoring timings); " + "two runs of the same code must pass", + ), ) -> None: """Name every case whose pass/fail flipped between two runs. Exits 1 when any case regressed, so a keep/discard loop can gate on it. + With --exact, exits 1 on any non-timing difference at all; running the + suite twice and exact-diffing the reports is the determinism check. """ old_report = json.loads(old.read_text()) new_report = json.loads(new.read_text()) @@ -143,6 +152,17 @@ def diff_reports( print(f" new case: {key}") for key in d.removed: print(f" case gone: {key}") + if exact: + differences = tripwire.exact_differences(old_report, new_report) + if differences: + shown = 20 + print(f"{len(differences)} exact difference(s):") + for line in differences[:shown]: + print(f" {line}") + if len(differences) > shown: + print(f" ... and {len(differences) - shown} more") + raise typer.Exit(code=1) + print("exact: reports identical") if d.broke: raise typer.Exit(code=1) diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index 8dd27918c0..1202448656 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -25,7 +25,7 @@ from __future__ import annotations import threading -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Literal, cast import numpy as np @@ -220,11 +220,11 @@ def pick_cases( server.gui.configure_theme(dark_mode=True) server.scene.set_background_image(np.full((1, 1, 3), 14, dtype=np.uint8)) server.scene.set_up_direction("+z") - server.scene.add_point_cloud( + cloud = server.scene.add_point_cloud( "/map", map_points, map_colors, - point_size=0.035, + point_size=0.025, point_shape="circle", precision="float32", ) @@ -243,6 +243,29 @@ def _(client: viser.ClientHandle) -> None: client.camera.look_at = tuple(center) server.gui.add_markdown(INSTRUCTIONS) + with server.gui.add_folder("display", expand_by_default=False): + size_slider = server.gui.add_slider( + "point size", min=0.005, max=0.08, step=0.0025, initial_value=cloud.point_size + ) + shape_dropdown = server.gui.add_dropdown( + "shape", ("circle", "rounded", "square", "diamond"), initial_value="circle" + ) + shaded_box = server.gui.add_checkbox("shaded", True) + + @size_slider.on_update + def _(_event: object) -> None: + cloud.point_size = size_slider.value + + @shape_dropdown.on_update + def _(_event: object) -> None: + cloud.point_shape = cast( + "Literal['circle', 'rounded', 'square', 'diamond']", shape_dropdown.value + ) + + @shaded_box.on_update + def _(_event: object) -> None: + cloud.point_shading = "gradient" if shaded_box.value else "flat" + undo_button = server.gui.add_button("undo last pick") save_all_button = server.gui.add_button("save all unsaved") exit_button = server.gui.add_button("save all & exit") diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 1b00a24706..4fb4914342 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -575,6 +575,17 @@ def test_tripwire_outcomes() -> None: assert d.fixed == [] and d.broke == [] and d.added == [] and d.removed == [] +def test_tripwire_exact_differences() -> None: + report = _tripwire_report({"office": {"a": (True, False)}}) + assert tripwire.exact_differences(report, report) == [] + changed = json.loads(json.dumps(report)) + changed["datasets"][0]["cases"][0]["online"]["length"] = 12.34 + changed["datasets"][0]["cases"][0]["online"]["plan_ms"] = 99.0 + diffs = tripwire.exact_differences(report, changed) + assert len(diffs) == 1 + assert "length" in diffs[0] and "12.34" in diffs[0] + + def test_tripwire_diff_names_every_flip() -> None: old = _tripwire_report( {"office": {"a": (False, True), "b": (True, True), "gone": (True, True)}} diff --git a/dimos/navigation/nav_3d/evaluator/tripwire.py b/dimos/navigation/nav_3d/evaluator/tripwire.py index 5ba2d43276..cf5d25952d 100644 --- a/dimos/navigation/nav_3d/evaluator/tripwire.py +++ b/dimos/navigation/nav_3d/evaluator/tripwire.py @@ -87,3 +87,44 @@ def diff(old_report: dict[str, object], new_report: dict[str, object]) -> Report if case_id not in new.get(dataset, {}) ] return ReportDiff(fixed, broke, sorted(added), sorted(removed)) + + +# Wall-clock fields legitimately differ between runs of identical code. +TIMING_KEYS = frozenset({"plan_ms", "map_update_ms", "map_build_ms", "add_frame_ms"}) + + +def _strip_timing(value: object) -> object: + if isinstance(value, dict): + return {k: _strip_timing(v) for k, v in value.items() if k not in TIMING_KEYS} + if isinstance(value, list): + return [_strip_timing(v) for v in value] + return value + + +def _walk(path: str, old: object, new: object, out: list[str]) -> None: + if isinstance(old, dict) and isinstance(new, dict): + for key in sorted(old.keys() | new.keys()): + if key not in old or key not in new: + out.append(f"{path}.{key}: only in {'old' if key in old else 'new'}") + else: + _walk(f"{path}.{key}", old[key], new[key], out) + elif isinstance(old, list) and isinstance(new, list): + if len(old) != len(new): + out.append(f"{path}: length {len(old)} != {len(new)}") + return + for i, (o, n) in enumerate(zip(old, new, strict=True)): + _walk(f"{path}[{i}]", o, n, out) + elif old != new: + out.append(f"{path}: {old!r} != {new!r}") + + +def exact_differences(old_report: dict[str, object], new_report: dict[str, object]) -> list[str]: + """Every non-timing field that differs between two reports, at full precision. + + Two runs of identical code must produce an empty list. This is the + determinism gate: it holds for any algorithm under test, present or + future, because it checks the results rather than the implementation. + """ + out: list[str] = [] + _walk("report", _strip_timing(old_report), _strip_timing(new_report), out) + return out From ce4cb9a535e792745f60fc3d6e42b8ad1bf56014 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 17 Jul 2026 12:25:49 -0700 Subject: [PATCH 12/29] Updates --- dimos/navigation/nav_3d/evaluator/cli.py | 83 +++-- dimos/navigation/nav_3d/evaluator/config.py | 74 ++--- dimos/navigation/nav_3d/evaluator/generate.py | 57 +--- dimos/navigation/nav_3d/evaluator/picker.py | 300 +++++++++++++----- .../nav_3d/evaluator/test_evaluator.py | 33 +- dimos/navigation/nav_3d/evaluator/tripwire.py | 16 + dimos/navigation/nav_3d/evaluator/viz.py | 4 +- 7 files changed, 329 insertions(+), 238 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index c36cd1329b..287802eb38 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -18,7 +18,7 @@ One dataset: python -m dimos.navigation.nav_3d.evaluator run --dataset mid360_athens_stairs Only some cases: python -m dimos.navigation.nav_3d.evaluator run --tag stairs --tag up Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json -Override a knob: python -m dimos.navigation.nav_3d.evaluator run --set wall_clearance_m=0.05 +Override a gate: python -m dimos.navigation.nav_3d.evaluator run --set goal_tolerance=0.4 Compare two runs: python -m dimos.navigation.nav_3d.evaluator diff old.json new.json Determinism check: run twice with --json, then diff a.json b.json --exact New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a @@ -52,7 +52,6 @@ from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.generate import ( GenerationParams, - drift_stats, generate_cases, snap_to_surface, ) @@ -152,6 +151,9 @@ def diff_reports( print(f" new case: {key}") for key in d.removed: print(f" case gone: {key}") + violations = tripwire.perf_violations(new_report) + for violation in violations: + print(f"PERF BUDGET EXCEEDED: {violation}") if exact: differences = tripwire.exact_differences(old_report, new_report) if differences: @@ -163,7 +165,7 @@ def diff_reports( print(f" ... and {len(differences) - shown} more") raise typer.Exit(code=1) print("exact: reports identical") - if d.broke: + if d.broke or violations: raise typer.Exit(code=1) @@ -184,7 +186,7 @@ def run( help="Total parallelism: dataset processes x checkpoint threads", ), set_: list[str] = typer.Option( - None, "--set", help="Repeatable EvalConfig override, e.g. wall_clearance_m=0.05" + None, "--set", help="Repeatable EvalConfig override, e.g. goal_tolerance=0.4" ), ) -> None: """Evaluate every case suite and print scores. The headline is incremental-map SPL.""" @@ -208,7 +210,10 @@ def run( cfg = _apply_overrides(EvalConfig(), set_ or []) report = evaluate(suites, cfg, workers=workers) _print_report(report) + for violation in tripwire.perf_violations(report.to_dict()): + print(f"PERF BUDGET EXCEEDED: {violation}") if json_out is not None: + json_out.parent.mkdir(parents=True, exist_ok=True) json_out.write_text(json.dumps(report.to_dict(), indent=2)) print(f"wrote {json_out}") if rrd_out is not None: @@ -249,12 +254,12 @@ def ingest( name: str = typer.Option(..., "--name", help="Dataset name; becomes data/.db"), lidar_stream: str = typer.Option("pointlio_lidar", "--lidar-stream"), odom_stream: str = typer.Option("pointlio_odometry", "--odom-stream"), - max_cases: int = typer.Option( - 0, "--max-cases", help="Auto-generated case cap; 0 scales with recording length" + cases: int = typer.Option( + 0, "--cases", help="Exact auto-generated case count; 0 scales with recording length" ), force: bool = typer.Option(False, "--force", help="Overwrite dataset and manifest"), ) -> None: - """Register a recording as a dataset: copy, drift-check, map, generate cases.""" + """Register a recording as a dataset: copy, map, generate cases.""" src = source / "mem2.db" if source.is_dir() else source if not src.exists(): raise typer.BadParameter(f"{src} does not exist") @@ -276,20 +281,13 @@ def ingest( f"{trajectory.ts[-1] - trajectory.ts[0]:.0f}s, {arcs[-1]:.1f}m walked, " f"z [{trajectory.positions[:, 2].min():.2f}, {trajectory.positions[:, 2].max():.2f}]" ) - drift = drift_stats(trajectory) - closure = f"{drift.closure_m:.2f}m" if drift.closure_m is not None else "n/a" - print( - f"drift: {drift.revisit_count} same-floor revisits, " - f"z mismatch p95 {drift.revisit_dz_p95:.2f}m, loop closure {closure}" - ) - for warning in drift.warnings: - print(f"WARNING: {warning}") - cfg = EvalConfig() final = load_or_build_final_map(dest, suite, cfg) planner = cfg.make_planner() planner.update_global_map(final.occupied) - gen = GenerationParams(max_cases=max_cases or None) + gen = GenerationParams(max_cases=cases or None) + if cases: + gen.min_cases = cases suite.cases = generate_cases(trajectory, final, planner.surface_map(), cfg, gen) if not suite.cases: raise typer.Exit(code=1) @@ -392,11 +390,12 @@ def pick_case( weight: float = typer.Option(1.0, "--weight"), snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), ) -> None: - """Pick cases by shift+clicking the map in a browser viewer. + """Pick and edit cases by shift+clicking the map in a browser viewer. - Serves the final map and walked path with viser. Shift+click picks - start/goal pairs. The side panel tags negatives, undoes picks, and saves - pairs to the manifest, snapped like add-case. + Serves the final map, the walked path, and every case already in the + manifest as an editable panel entry. Shift+click picks new start/goal + pairs. Any case can be renamed, retagged, flipped negative, or deleted; + new pairs save to the manifest snapped like add-case. """ # Lazy: picker/viz pull in viser and matplotlib, only needed for pick-case. from dimos.navigation.nav_3d.evaluator.picker import pick_cases @@ -415,9 +414,9 @@ def save_pair( start: tuple[float, float, float], goal: tuple[float, float, float], negative: bool, - extra_tags: list[str], + tags: list[str], case_id: str | None, - ) -> tuple[bool, str, str | None]: + ) -> tuple[bool, str, str | None, list[str] | None]: try: case = _append_case( suite, @@ -426,31 +425,49 @@ def save_pair( start, goal, case_id, - full_tags(negative, extra_tags), + full_tags(negative, tags), weight, snap_max, negative, ) except typer.BadParameter as err: - return False, str(err), None - return True, f"saved {case.id} [{', '.join(case.tags)}]", case.id + return False, str(err), None, None + return True, f"saved {case.id} [{', '.join(case.tags)}]", case.id, list(case.tags) def update_case( - saved_id: str, new_id: str, negative: bool, extra_tags: list[str] - ) -> tuple[bool, str, str | None]: + saved_id: str, new_id: str, negative: bool, tags: list[str] + ) -> tuple[bool, str, str | None, list[str] | None]: case = next((c for c in suite.cases if c.id == saved_id), None) if case is None: - return False, f"case {saved_id!r} not found in manifest", None + return False, f"case {saved_id!r} not found in manifest", None, None if new_id != saved_id and any(c.id == new_id for c in suite.cases): - return False, f"case id {new_id!r} already exists", None + return False, f"case id {new_id!r} already exists", None, None case.id = new_id - case.tags = full_tags(negative, extra_tags) + # Tags round-trip verbatim; the negative checkbox owns only the + # negative tag, so auto/manual provenance survives edits. + plain = [t for t in tags if t != "negative"] + case.tags = plain + (["negative"] if negative else []) case.expect_fail = negative save_suite(suite, manifest) - return True, f"updated {case.id} [{', '.join(case.tags)}]", case.id + return True, f"updated {case.id} [{', '.join(case.tags)}]", case.id, list(case.tags) + + def delete_case(saved_id: str) -> tuple[bool, str]: + case = next((c for c in suite.cases if c.id == saved_id), None) + if case is None: + return False, f"case {saved_id!r} not found in manifest" + suite.cases.remove(case) + save_suite(suite, manifest) + return True, f"deleted {saved_id} from {manifest.name}" pick_cases( - dataset, final.occupied, turbo_by_height(final.occupied), foot, save_pair, update_case + dataset, + final.occupied, + turbo_by_height(final.occupied), + foot, + suite.cases, + save_pair, + update_case, + delete_case, ) print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {dataset}") diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index acece97b88..12387beaa6 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -22,31 +22,20 @@ @dataclass class EvalConfig: - """Mapper, planner, and gate parameters. + """Harness and gate parameters, sized for the Unitree Go2. - Defaults mirror production. Body and capability bounds are sized for the - Unitree Go2 (0.31m wide, 0.40m tall, ~0.16m stair risers). + (0.31m wide, 0.40m tall, ~0.16m stair risers.) + + Algorithm tuning lives in the algorithm packages as their constructor + defaults. The evaluator fixes only the shared voxel resolution, the + sensor range, the robot's sensor height, and the physical body and + capability bounds it gates against. Improving the algorithm means + changing the algorithm, never this file. """ voxel_size: float = 0.08 max_range: float = 30.0 - ray_subsample: int = 1 - shadow_depth: float = 0.1 - grace_depth: float = 0.2 - min_health: int = -1 - max_health: int = 5 - graze_cos: float = 0.7 - support_min: int = 4 - robot_height: float = 0.3 - max_overhead_m: float = 2.0 - surface_closing_radius: float = 0.4 - node_spacing_m: float = 1.0 - wall_clearance_m: float = 0.1 - wall_buffer_m: float = 0.75 - wall_buffer_weight: float = 100.0 - step_threshold_m: float = 0.16 - step_penalty_weight: float = 4.0 # Physical body envelope for the collision gate. The gate catches paths # that penetrate obstacles, not near-grazes, so the radius is the true @@ -70,43 +59,20 @@ class EvalConfig: max_step_m: float = 0.2 kinematic_window_m: float = 0.5 + # An improvement must not buy score with compute. p95 over the suite. + plan_p95_budget_ms: float = 50.0 + map_update_p95_budget_ms: float = 1000.0 + def make_mapper(self) -> VoxelRayMapper: - return VoxelRayMapper( - voxel_size=self.voxel_size, - max_range=self.max_range, - ray_subsample=self.ray_subsample, - shadow_depth=self.shadow_depth, - grace_depth=self.grace_depth, - min_health=self.min_health, - max_health=self.max_health, - graze_cos=self.graze_cos, - support_min=self.support_min, - ) + return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) def make_planner(self) -> MLSPlanner: - return MLSPlanner( - voxel_size=self.voxel_size, - robot_height=self.robot_height, - max_overhead_m=self.max_overhead_m, - surface_closing_radius=self.surface_closing_radius, - node_spacing_m=self.node_spacing_m, - wall_clearance_m=self.wall_clearance_m, - wall_buffer_m=self.wall_buffer_m, - wall_buffer_weight=self.wall_buffer_weight, - step_threshold_m=self.step_threshold_m, - step_penalty_weight=self.step_penalty_weight, - ) + return MLSPlanner(voxel_size=self.voxel_size, robot_height=self.robot_height) def mapper_fingerprint(self) -> dict[str, float | int]: - """The mapper parameters that determine final map content.""" - return { - "voxel_size": self.voxel_size, - "max_range": self.max_range, - "ray_subsample": self.ray_subsample, - "shadow_depth": self.shadow_depth, - "grace_depth": self.grace_depth, - "min_health": self.min_health, - "max_health": self.max_health, - "graze_cos": self.graze_cos, - "support_min": self.support_min, - } + """Cache key parameters for the final map. + + Mapper internals are deliberately not fingerprinted. Changes to the + mapper, code or defaults, require wiping data/.final instead. + """ + return {"voxel_size": self.voxel_size, "max_range": self.max_range} diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 4c9defa5ca..35a305c441 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -25,7 +25,7 @@ from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass from typing import TYPE_CHECKING import numpy as np @@ -304,58 +304,3 @@ def _to_case(cand: Candidate, n: int) -> Case: weight=1.0, tags=tags, ) - - -@dataclass -class DriftStats: - """Consistency of same-floor revisits, plus loop closure when one exists.""" - - revisit_count: int - revisit_dz_p95: float - closure_m: float | None - warnings: list[str] = field(default_factory=list) - - -def drift_stats( - trajectory: Trajectory, - revisit_gap_s: float = 30.0, - revisit_radius_m: float = 0.5, - dz_warn_m: float = 0.3, - closure_warn_m: float = 1.0, -) -> DriftStats: - p = trajectory.positions - ts = trajectory.ts - idx = _subsample_indices(trajectory, 0.5) - dzs: list[float] = [] - for i in idx: - earlier = idx[ts[idx] < ts[i] - revisit_gap_s] - if not len(earlier): - continue - hd = np.linalg.norm(p[earlier, :2] - p[i, :2], axis=1) - near = earlier[hd < revisit_radius_m] - if not len(near): - continue - dz = np.abs(p[near, 2] - p[i, 2]) - same_floor = dz[dz < 1.0] - if len(same_floor): - dzs.append(float(same_floor.min())) - - closure: float | None = None - if np.linalg.norm(p[-1, :2] - p[0, :2]) < 2.0: - closure = float(np.linalg.norm(p[-1] - p[0])) - - warnings = [] - dz_p95 = float(np.percentile(dzs, 95)) if dzs else 0.0 - if dz_p95 > dz_warn_m: - warnings.append( - f"same-floor revisit z mismatch p95 {dz_p95:.2f}m exceeds {dz_warn_m}m; " - "the recording may be too drifty for reliable evaluation" - ) - if closure is not None and closure > closure_warn_m: - warnings.append(f"loop closure error {closure:.2f}m exceeds {closure_warn_m}m") - return DriftStats( - revisit_count=len(dzs), - revisit_dz_p95=dz_p95, - closure_m=closure, - warnings=warnings, - ) diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index 1202448656..ce7ee856fd 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -14,16 +14,20 @@ """Browser point-picking for case curation, served by viser. -Opens a dark-themed local web viewer with the final map and walked path. -Shift+click picks points in start/goal pairs. Every pair gets its own panel -entry with the coordinates, a name field, geometry-suggested tag checkboxes, -custom tags, and a negative toggle. Pairs save individually or all at once, -and stay editable after saving: rename or retag and press the pair's button -again to update the manifest. Plain clicks and drags only move the camera. +Opens a dark-themed local web viewer with the final map, the walked path, +and every case already in the manifest as a collapsed, editable panel entry. +Clicking a pair's endpoint sphere in the scene highlights the pair, opens +its panel entry, and scrolls to it. The show button inside each entry +highlights its pair in the scene. Shift+click picks new start/goal pairs. +Every entry has the coordinates, a name field, geometry-suggested tag +checkboxes, custom tags, a negative toggle, and save/delete buttons, so any +case can be renamed, retagged, flipped, or removed. Plain clicks and drags +only move the camera. """ from __future__ import annotations +from dataclasses import dataclass import threading from typing import TYPE_CHECKING, Literal, cast @@ -36,18 +40,24 @@ ) if TYPE_CHECKING: - from collections.abc import Callable + from collections.abc import Callable, Sequence from numpy.typing import NDArray import viser - # (start, goal, negative, extra_tags, case_id) -> (ok, message, saved_id) + from dimos.navigation.nav_3d.evaluator.cases import Case + + # (start, goal, negative, tags, case_id) -> (ok, message, saved_id, saved_tags) SavePair = Callable[ [tuple[float, float, float], tuple[float, float, float], bool, list[str], str | None], - tuple[bool, str, str | None], + tuple[bool, str, str | None, list[str] | None], + ] + # (saved_id, new_id, negative, tags) -> (ok, message, saved_id, saved_tags) + UpdateCase = Callable[ + [str, str, bool, list[str]], tuple[bool, str, str | None, list[str] | None] ] - # (saved_id, new_id, negative, extra_tags) -> (ok, message, saved_id) - UpdateCase = Callable[[str, str, bool, list[str]], tuple[bool, str, str | None]] + # (saved_id) -> (ok, message) + DeleteCase = Callable[[str], tuple[bool, str]] # Selection cone half-angle around the click ray. Wide enough to hit a voxel # point from across a room, narrow enough to stay on the intended surface. @@ -55,9 +65,15 @@ START_COLOR = (0, 255, 255) GOAL_COLOR = (255, 140, 0) PAIR_COLOR = (255, 255, 0) +HIGHLIGHT_LINE_COLOR = (255, 255, 255) +MARKER_RADIUS = 0.09 +HIGHLIGHT_MARKER_RADIUS = 0.16 +LINE_WIDTH = 2.5 +HIGHLIGHT_LINE_WIDTH = 6.0 SUGGESTED_TAGS = ("stairs", "flat", "up", "down", "long", "doorway") INSTRUCTIONS = """**shift+click** picks START then GOAL, repeated per case. +**click** an endpoint sphere to highlight and open its case. Plain drag orbits, scroll zooms, right-drag pans. """ @@ -99,8 +115,21 @@ def suggested_tags(start: NDArray[np.float32], goal: NDArray[np.float32]) -> set return tags +@dataclass +class _Hooks: + """Manifest callbacks and shared state handed to every pair entry.""" + + save_pair: SavePair + update_case: UpdateCase + delete_case: DeleteCase + lock: threading.Lock + unregister: Callable[[_PairEntry], None] + announce: Callable[[str], None] + highlight: Callable[[_PairEntry], None] + + class _PairEntry: - """One picked start/goal pair and its editable panel widgets.""" + """One start/goal pair and its editable panel widgets and scene markers.""" def __init__( self, @@ -108,32 +137,83 @@ def __init__( n: int, start: NDArray[np.float32], goal: NDArray[np.float32], - save_pair: SavePair, - update_case: UpdateCase, - lock: threading.Lock, + hooks: _Hooks, + markers: list[viser.SceneNodeHandle], + case: Case | None = None, ) -> None: self._server = server self._n = n self.start = start self.goal = goal - self._save_pair = save_pair - self._update_case = update_case - self._lock = lock - self.saved_id: str | None = None - self._name = "" - self._checked = suggested_tags(start, goal) - self._custom = "" - self._negative = False - self._status = "unsaved" - self._build(expanded=True, order=None) - - def _build(self, *, expanded: bool, order: float | None) -> None: + self._hooks = hooks + self.markers = markers + self.preloaded = case is not None + if case is None: + self.saved_id: str | None = None + self._name = "" + self._checked = suggested_tags(start, goal) + self._custom = "" + self._negative = False + self._status = "unsaved" + else: + self.saved_id = case.id + self._name = case.id + self._sync_tags(case.tags) + self._negative = case.expect_fail + self._status = "in manifest" + self.removed = False + self._build(expanded=case is None, order=None) + for marker in markers: + if hasattr(marker, "on_click"): + marker.on_click(self._on_marker_click) + + def _on_marker_click(self, _event: object) -> None: + with self._hooks.lock: + self.reveal() + + def reveal(self) -> None: + """Announce and highlight this pair, and open its panel entry.""" + self._hooks.announce(self._label()) + self._hooks.highlight(self) + self._snapshot() + order = self.panel.order + self.panel.remove() + self._build(expanded=True, order=order, scroll=True) + + def set_highlight(self, on: bool) -> None: + if self.removed: + return + for marker in self.markers: + if hasattr(marker, "radius"): + marker.radius = HIGHLIGHT_MARKER_RADIUS if on else MARKER_RADIUS + elif hasattr(marker, "line_width"): + marker.line_width = HIGHLIGHT_LINE_WIDTH if on else LINE_WIDTH + marker.colors = np.array(HIGHLIGHT_LINE_COLOR if on else PAIR_COLOR, dtype=np.uint8) + + def _label(self) -> str: + return self.saved_id or f"pair {self._n}" + + def _sync_tags(self, tags: list[str]) -> None: + """Split a manifest tag list into checkbox and custom-text state. + + The negative tag is owned by the checkbox. Everything not in the + suggested set (auto, manual, ...) lands in the custom text so it + stays visible and round-trips verbatim. + """ + self._checked = {t for t in tags if t in SUGGESTED_TAGS} + self._custom = ", ".join(t for t in tags if t not in SUGGESTED_TAGS and t != "negative") + + def _build(self, *, expanded: bool, order: float | None, scroll: bool = False) -> None: server = self._server start, goal = self.start, self.goal - self.folder = server.gui.add_folder( - f"pair {self._n}", order=order, expand_by_default=expanded - ) - with self.folder: + self.panel = server.gui.add_folder(self._label(), order=order, expand_by_default=expanded) + with self.panel: + if scroll: + # Autofocus makes the browser scroll the side panel here. + server.gui.add_html( + '" + ) server.gui.add_markdown( f"({start[0]:.1f}, {start[1]:.1f}, {start[2]:.1f}) → " f"({goal[0]:.1f}, {goal[1]:.1f}, {goal[2]:.1f})" @@ -151,18 +231,44 @@ def _build(self, *, expanded: bool, order: float | None) -> None: ) self.negative_box = server.gui.add_checkbox("negative (must refuse)", self._negative) self.message = server.gui.add_markdown(self._status) + self.show_button = server.gui.add_button("show in scene") self.button = server.gui.add_button("save / update") + self.delete_button = server.gui.add_button("delete") + + @self.show_button.on_click + def _(_event: object) -> None: + with self._hooks.lock: + self._hooks.announce(self._label()) + self._hooks.highlight(self) @self.button.on_click def _(_event: object) -> None: # save_unsaved calls save_or_update already holding the lock; # the button path runs on a bare viser callback thread and must # take it to serialize suite/manifest mutation. - with self._lock: + with self._hooks.lock: self.save_or_update() + @self.delete_button.on_click + def _(_event: object) -> None: + with self._hooks.lock: + self.delete() + def remove(self) -> None: - self.folder.remove() + self.removed = True + self.panel.remove() + for marker in self.markers: + marker.remove() + + def delete(self) -> None: + if self.saved_id is not None: + ok, msg = self._hooks.delete_case(self.saved_id) + print(msg) + if not ok: + self.message.content = f"**FAILED**: {msg}" + return + self._hooks.unregister(self) + self.remove() def _snapshot(self) -> None: self._name = self.id_text.value @@ -178,7 +284,7 @@ def extra_tags(self) -> list[str]: def save_or_update(self) -> None: name = self.id_text.value.strip() if self.saved_id is None: - ok, msg, saved = self._save_pair( + ok, msg, saved, tags = self._hooks.save_pair( (float(self.start[0]), float(self.start[1]), float(self.start[2])), (float(self.goal[0]), float(self.goal[1]), float(self.goal[2])), self.negative_box.value, @@ -186,22 +292,23 @@ def save_or_update(self) -> None: name or None, ) else: - ok, msg, saved = self._update_case( + ok, msg, saved, tags = self._hooks.update_case( self.saved_id, name or self.saved_id, self.negative_box.value, self.extra_tags() ) print(msg) if not (ok and saved is not None): self.message.content = f"**FAILED**: {msg}" return - # Folders cannot be collapsed live in viser, expand_by_default is - # only read when the folder is first created. Rebuild it collapsed - # in place instead. + # Viser cannot collapse a live panel, so replace it with the + # collapsed button form, synced from the authoritative save. self.saved_id = saved self._snapshot() self._name = saved + if tags is not None: + self._sync_tags(tags) self._status = msg - order = self.folder.order - self.folder.remove() + order = self.panel.order + self.panel.remove() self._build(expanded=False, order=order) @@ -210,8 +317,10 @@ def pick_cases( map_points: NDArray[np.float32], map_colors: NDArray[np.uint8], walked: NDArray[np.float32], + cases: Sequence[Case], save_pair: SavePair, update_case: UpdateCase, + delete_case: DeleteCase, ) -> None: """Serve the picker until the user exits from the panel or hits ctrl-c.""" import viser @@ -243,6 +352,7 @@ def _(client: viser.ClientHandle) -> None: client.camera.look_at = tuple(center) server.gui.add_markdown(INSTRUCTIONS) + selected_line = server.gui.add_markdown("selected: —") with server.gui.add_folder("display", expand_by_default=False): size_slider = server.gui.add_slider( "point size", min=0.005, max=0.08, step=0.0025, initial_value=cloud.point_size @@ -272,9 +382,63 @@ def _(_event: object) -> None: lock = threading.Lock() stop = threading.Event() - picks: list[NDArray[np.float32]] = [] pairs: list[_PairEntry] = [] - markers: list[viser.SceneNodeHandle] = [] + + def announce(label: str) -> None: + selected_line.content = f"selected: **{label}**" + + highlighted: list[_PairEntry] = [] + + def highlight(entry: _PairEntry) -> None: + while highlighted: + highlighted.pop().set_highlight(False) + entry.set_highlight(True) + highlighted.append(entry) + + hooks = _Hooks( + save_pair, + update_case, + delete_case, + lock, + lambda entry: pairs.remove(entry), + announce, + highlight, + ) + marker_seq = 0 + + def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.SceneNodeHandle: + nonlocal marker_seq + marker_seq += 1 + return server.scene.add_icosphere( + f"/picks/m{marker_seq}", + radius=0.09, + color=color, + position=(float(point[0]), float(point[1]), float(point[2]) + 0.05), + ) + + def pair_line(start: NDArray[np.float32], goal: NDArray[np.float32]) -> viser.SceneNodeHandle: + nonlocal marker_seq + marker_seq += 1 + return server.scene.add_line_segments( + f"/picks/m{marker_seq}", + np.stack([start, goal])[None], + colors=PAIR_COLOR, + line_width=2.5, + ) + + def pair_markers( + start: NDArray[np.float32], goal: NDArray[np.float32] + ) -> list[viser.SceneNodeHandle]: + return [sphere(start, START_COLOR), sphere(goal, GOAL_COLOR), pair_line(start, goal)] + + for case in cases: + start = np.asarray(case.start, dtype=np.float32) + goal = np.asarray(case.goal, dtype=np.float32) + pairs.append( + _PairEntry(server, 0, start, goal, hooks, pair_markers(start, goal), case=case) + ) + + pending: list[tuple[viser.SceneNodeHandle, NDArray[np.float32]]] = [] pair_count = 0 @server.scene.on_click(modifier="shift") @@ -286,45 +450,27 @@ def _(event: viser.SceneClickEvent) -> None: if point is None: return with lock: - is_goal = len(picks) % 2 == 1 - picks.append(point) - n = len(picks) - markers.append( - server.scene.add_icosphere( - f"/picks/p{n}", - radius=0.09, - color=GOAL_COLOR if is_goal else START_COLOR, - position=(float(point[0]), float(point[1]), float(point[2]) + 0.05), - ) - ) - if is_goal: - start, goal = picks[-2], picks[-1] - markers.append( - server.scene.add_line_segments( - f"/picks/l{n}", - np.stack([start, goal])[None], - colors=PAIR_COLOR, - line_width=2.5, - ) - ) - pair_count += 1 - pairs.append( - _PairEntry(server, pair_count, start, goal, save_pair, update_case, lock) - ) + if not pending: + pending.append((sphere(point, START_COLOR), point)) + return + start_marker, start = pending.pop() + markers = [start_marker, sphere(point, GOAL_COLOR), pair_line(start, point)] + pair_count += 1 + pairs.append(_PairEntry(server, pair_count, start, point, hooks, markers)) @undo_button.on_click def _(_event: object) -> None: with lock: - if not picks: - return - if len(picks) % 2 == 0: - # Completing pick of the last pair. Saved cases stay in the - # manifest, only the panel entry and markers go away. - pair = pairs.pop() - pair.remove() - markers.pop().remove() # pair line - picks.pop() - markers.pop().remove() + if pending: + pending.pop()[0].remove() + elif pairs and not pairs[-1].preloaded: + # Saved cases stay in the manifest, only the panel entry and + # markers go away. Deleting from the manifest is the per-pair + # delete button. + entry = pairs.pop() + entry.remove() + if entry.saved_id is not None: + print(f"{entry.saved_id} stays in the manifest; use delete to remove it") def save_unsaved() -> None: with lock: diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 4fb4914342..b05be854be 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -23,6 +23,7 @@ import numpy as np import pytest +from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.navigation.nav_3d.evaluator import metrics, tripwire from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig @@ -39,7 +40,6 @@ Candidate, GenerationParams, _select_diverse, - drift_stats, generate_cases, snap_to_surface, ) @@ -240,20 +240,6 @@ def test_snap_to_surface() -> None: assert snap_to_surface(np.array([1.0, 1.0, 5.0], dtype=np.float32), surface, 1.0) is None -def test_drift_stats_flags_z_mismatch() -> None: - n = 400 - ts = np.linspace(0, 120, n) - out = np.stack([np.linspace(0, 20, n // 2), np.zeros(n // 2), np.zeros(n // 2)], axis=1) - back = np.stack([np.linspace(20, 0, n // 2), np.zeros(n // 2), np.full(n // 2, 0.6)], axis=1) - drifty = Trajectory(ts=ts, positions=np.concatenate([out, back]).astype(np.float32)) - stats = drift_stats(drifty) - assert stats.revisit_dz_p95 > 0.3 - assert stats.warnings - - clean = Trajectory(ts=ts, positions=np.concatenate([out, out[::-1]]).astype(np.float32)) - assert not drift_stats(clean).warnings - - def test_checkpoint_deltas_roundtrip() -> None: snapshots = [ np.array([1, 2, 3], dtype=np.int64), @@ -284,7 +270,7 @@ def test_checkpoint_deltas_roundtrip() -> None: def test_replay_frames_snapshots_grow_with_time() -> None: """Each checkpoint must contain exactly the frames seen up to its time.""" - cfg = EvalConfig(voxel_size=VOXEL, support_min=1) + mapper = VoxelRayMapper(voxel_size=VOXEL, max_range=30.0, support_min=1) def frame_at(ts: float, x: float) -> Frame: return Frame(ts=ts, points=_wall(x), origin=(x - 2.0, 0.0, 0.5)) @@ -299,7 +285,7 @@ def frame_at(ts: float, x: float) -> Frame: frame_at(2.1, 11.0), ] times = np.array([0.5, 1.5, np.inf]) - final, snapshots, observed = replay_frames(frames, cfg.make_mapper(), VOXEL, times) + final, snapshots, observed = replay_frames(frames, mapper, VOXEL, times) assert final.frames == 6 sizes = [len(s) for s in snapshots] assert 0 < sizes[0] < sizes[1] < sizes[2] @@ -575,6 +561,19 @@ def test_tripwire_outcomes() -> None: assert d.fixed == [] and d.broke == [] and d.added == [] and d.removed == [] +def test_tripwire_perf_violations() -> None: + report = _tripwire_report({"office": {"a": (True, True)}}) + report["config"] = {"plan_p95_budget_ms": 60.0, "map_update_p95_budget_ms": 3000.0} + report["plan_ms"] = {"p95": 30.0} + report["map_update_ms"] = {"p95": 1500.0} + assert tripwire.perf_violations(report) == [] + report["plan_ms"] = {"p95": 61.0} + violations = tripwire.perf_violations(report) + assert len(violations) == 1 and "plan_ms" in violations[0] + # Reports predating the budgets pass. + assert tripwire.perf_violations({"datasets": []}) == [] + + def test_tripwire_exact_differences() -> None: report = _tripwire_report({"office": {"a": (True, False)}}) assert tripwire.exact_differences(report, report) == [] diff --git a/dimos/navigation/nav_3d/evaluator/tripwire.py b/dimos/navigation/nav_3d/evaluator/tripwire.py index cf5d25952d..9da7ae7b39 100644 --- a/dimos/navigation/nav_3d/evaluator/tripwire.py +++ b/dimos/navigation/nav_3d/evaluator/tripwire.py @@ -118,6 +118,22 @@ def _walk(path: str, old: object, new: object, out: list[str]) -> None: out.append(f"{path}: {old!r} != {new!r}") +def perf_violations(report: dict[str, object]) -> list[str]: + """Timing stats that exceed the budgets recorded in the report's config.""" + config = cast("dict[str, float]", report.get("config") or {}) + out: list[str] = [] + for stat_key, budget_key in ( + ("plan_ms", "plan_p95_budget_ms"), + ("map_update_ms", "map_update_p95_budget_ms"), + ): + stats = cast("dict[str, float]", report.get(stat_key) or {}) + budget = config.get(budget_key) + p95 = stats.get("p95") + if budget is not None and p95 is not None and p95 > budget: + out.append(f"{stat_key} p95 {p95:.1f}ms exceeds budget {budget:.0f}ms") + return out + + def exact_differences(old_report: dict[str, object], new_report: dict[str, object]) -> list[str]: """Every non-timing field that differs between two reports, at full precision. diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 47f47433d5..72f587681d 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -62,6 +62,8 @@ UNREACHED_PATH_COLOR = [255, 200, 0] CLEARANCE_CLAMP_M = 1.0 +# Cells colored red as too close to a wall. Display threshold only. +CLEARANCE_RED_M = 0.1 def turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: @@ -100,7 +102,7 @@ def _log_planner(entity: str, artifacts: PlannerArtifacts | None, cfg: EvalConfi f"{entity}/surface", rr.Points3D( surface[:, :3], - colors=_clearance_colors(surface[:, 3], cfg.wall_clearance_m), + colors=_clearance_colors(surface[:, 3], CLEARANCE_RED_M), radii=cfg.voxel_size / 4, ), static=True, From f4591eda577dd4007245dcfff38284bdf2e2ef51 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 17 Jul 2026 12:57:37 -0700 Subject: [PATCH 13/29] SF cases --- dimos/navigation/nav_3d/evaluator/PROGRAM.md | 92 +++++ dimos/navigation/nav_3d/evaluator/cases.py | 13 + .../nav_3d/evaluator/cases/sf_office.yaml | 318 ++++++++++++++++++ dimos/navigation/nav_3d/evaluator/cli.py | 50 ++- dimos/navigation/nav_3d/evaluator/runner.py | 3 +- .../nav_3d/evaluator/test_evaluator.py | 3 + dimos/navigation/nav_3d/evaluator/viz.py | 3 +- 7 files changed, 463 insertions(+), 19 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/PROGRAM.md create mode 100644 dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml diff --git a/dimos/navigation/nav_3d/evaluator/PROGRAM.md b/dimos/navigation/nav_3d/evaluator/PROGRAM.md new file mode 100644 index 0000000000..219478865e --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/PROGRAM.md @@ -0,0 +1,92 @@ +# Nav-3D Autonomous Improvement Protocol + +You are improving the 3D navigation stack: the MLS planner and the voxel ray +mapper. The evaluator in this package is your loss function. It replays real +robot recordings and scores planning on the maps the robot would have had at +the time. Higher is better. The score only counts if every gate below holds. + +## Boundary + +Editable: + +- `dimos/navigation/nav_3d/mls_planner/` (Python and Rust) +- `dimos/mapping/ray_tracing/` (Python and Rust) +- `results.tsv` at the repo root (your journal) + +Everything else is off limits, in particular `dimos/navigation/nav_3d/evaluator/` +(including `cases/` and this file). The gate parameters in `EvalConfig` are +physical measurements of the Unitree Go2 and its demonstrated capabilities, +not tuning knobs. Algorithm parameters are the constructor defaults of +`MLSPlanner` and `VoxelRayMapper`; tune them by editing those defaults. + +The human reviewer verifies the boundary mechanically against the commit +the session branched from (BASE = the parent branch, e.g. +andrew/feat/nav-evaluator): + + git diff --name-only $(git merge-base BASE HEAD) \ + | grep -vE '^(dimos/navigation/nav_3d/mls_planner/|dimos/mapping/ray_tracing/|results\.tsv)' + +Any output fails the whole session. + +## Setup + +Work on a fresh branch. Confirm the recordings exist (`data/*.db`), then run +the suite once and save the report as your first kept baseline: + + python -m dimos.navigation.nav_3d.evaluator run --json data/reports/kept.json + +## Iteration cycle + +1. Form one hypothesis and make one focused change. +2. Rebuild whichever Rust module you touched, always in release: + + uv run maturin develop --uv --release -m dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml + uv run maturin develop --uv --release -m dimos/mapping/ray_tracing/rust/Cargo.toml + + If you touched the mapper (code or defaults), also wipe the replay caches: + + rm -rf data/.final + + A mapper change makes the next run rebuild the maps (~5 minutes). Planner + changes skip that, so planner experiments are much cheaper. +3. Commit, then evaluate: + + python -m dimos.navigation.nav_3d.evaluator run --json data/reports/candidate.json + python -m dimos.navigation.nav_3d.evaluator diff data/reports/kept.json data/reports/candidate.json + +4. Keep or discard: + - Keep iff the score improved AND `diff` exits 0 (no case regressed). A + BROKE line means a start/goal pair the stack used to handle now fails; + a higher average does not excuse it. Perf budget lines on this parallel + run are advisory only; wall-clock under parallel contention runs about + 20 percent hot. + - Keep: pass the confirmation check below, then + `cp data/reports/candidate.json data/reports/kept.json` and keep the + commit. + - Discard: `git reset --hard HEAD^`. If you discarded a mapper change, + wipe `data/.final` again so caches match the reverted code. +5. Confirmation check, required before every keep. Rerun serially and + require bit-identical results and in-budget timings: + + python -m dimos.navigation.nav_3d.evaluator run --workers 1 --json data/reports/serial.json + python -m dimos.navigation.nav_3d.evaluator diff data/reports/candidate.json data/reports/serial.json --exact + + The serial run is slower but its timings are the binding perf gate: + uncontended wall-clock is what the robot experiences. A nonzero exit + means the change is non-reproducible or over budget. Fix it or discard. +6. Append one row to `results.tsv` and continue: + + commit_hashscorefinal_scoreplan_p95_msfixedbrokekeep|discard|crashone-line description + +## Rules + +- Crashes are experiments too: fix trivial bugs, abandon flawed ideas, always + journal the row. +- Never edit the evaluator, the case manifests, or the gate parameters. +- Never weaken determinism to gain score. +- The unit tests of the modules you edit must pass: + + python -m pytest dimos/navigation/nav_3d/mls_planner/ dimos/mapping/ray_tracing/ -q + +- Do not add dependencies. +- Run continuously without asking for approval until interrupted. diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index 1c2fa06906..8e5efab5bf 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -25,6 +25,8 @@ import yaml +from dimos.utils.data import resolve_named_path + CASES_DIR = Path(__file__).parent / "cases" @@ -47,8 +49,16 @@ class Suite: cases: list[Case] lidar_stream: str = "pointlio_lidar" odom_stream: str = "pointlio_odometry" + # Recording location override. Default is data/.db; set this to + # keep a recording outside data/, e.g. a private or holdout recording. + db: str | None = None path: Path | None = None + def db_path(self) -> Path: + if self.db is not None: + return Path(self.db).expanduser() + return resolve_named_path(self.dataset, ".db") + def load_suite(path: Path) -> Suite: raw = yaml.safe_load(path.read_text()) @@ -79,6 +89,7 @@ def load_suite(path: Path) -> Suite: cases=cases, lidar_stream=str(raw.get("lidar_stream", "pointlio_lidar")), odom_stream=str(raw.get("odom_stream", "pointlio_odometry")), + db=str(raw["db"]) if "db" in raw else None, path=path, ) @@ -96,6 +107,8 @@ def save_suite(suite: Suite, path: Path | None = None) -> Path: """Write the suite manifest as YAML. Defaults to cases/.yaml.""" path = path or suite.path or CASES_DIR / f"{suite.dataset}.yaml" doc: dict[str, object] = {"dataset": suite.dataset} + if suite.db is not None: + doc["db"] = suite.db if suite.lidar_stream != "pointlio_lidar": doc["lidar_stream"] = suite.lidar_stream if suite.odom_stream != "pointlio_odometry": diff --git a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml new file mode 100644 index 0000000000..a5b6446c96 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml @@ -0,0 +1,318 @@ +dataset: sf_office +db: ~/nav_recordings/sf_office.db +cases: +- id: auto_00_flat + start: [-1.64, -2.52, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_01_flat + start: [-2.84, 8.44, -0.24] + goal: [3.64, 7.64, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_02_flat + start: [6.44, 0.44, -0.16] + goal: [6.68, -3.0, 0.08] + weight: 1.0 + tags: [auto, flat] +- id: auto_03_flat + start: [8.76, 4.36, -0.16] + goal: [10.84, -0.84, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_04_flat + start: [-2.2, 3.0, -0.16] + goal: [3.56, -1.32, 0.08] + weight: 1.0 + tags: [auto, flat] +- id: auto_05_flat + start: [-0.04, -0.04, -0.16] + goal: [8.92, -0.52, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_06_flat + start: [-0.2, 4.2, -0.08] + goal: [0.04, 8.04, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_07_flat + start: [-2.84, 8.44, -0.24] + goal: [-0.68, 8.2, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_08_flat + start: [3.72, 2.28, -0.16] + goal: [12.6, -0.36, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_09_flat + start: [11.72, 3.88, -0.08] + goal: [4.92, 4.76, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_10_flat + start: [13.4, 0.2, 0.24] + goal: [2.12, 0.2, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_11_flat + start: [7.0, 3.8, -0.16] + goal: [5.0, -3.56, 0.08] + weight: 1.0 + tags: [auto, flat] +- id: auto_12_flat + start: [-3.56, 5.16, -0.24] + goal: [13.96, 2.28, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_13_flat + start: [13.8, 1.56, 0.32] + goal: [6.04, 2.28, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_14_flat + start: [9.8, -0.68, 0.24] + goal: [5.16, 6.68, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_15_flat + start: [11.64, -0.76, 0.24] + goal: [1.96, 7.72, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_16_flat + start: [7.96, -0.2, 0.08] + goal: [15.64, 1.32, -0.08] + weight: 1.0 + tags: [auto, flat] +- id: auto_17_flat + start: [5.32, 2.84, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_18_flat + start: [10.6, 4.04, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_19_flat + start: [-0.44, 0.76, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_20_flat + start: [14.28, 3.48, -0.08] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_21_flat + start: [7.96, 4.12, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_22_flat + start: [13.96, 2.28, 0.24] + goal: [3.8, 0.36, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_23_flat + start: [1.88, 0.84, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_24_flat + start: [4.36, 1.48, -0.16] + goal: [8.92, -0.52, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_25_flat + start: [12.68, 3.56, -0.08] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_26_flat + start: [3.64, 1.72, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_27_flat + start: [13.88, 1.96, 0.24] + goal: [-3.24, 6.2, -0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_28_flat + start: [5.16, 2.04, -0.16] + goal: [10.84, -0.84, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_29_flat + start: [12.6, -0.36, 0.24] + goal: [-1.64, 5.0, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_30_flat + start: [1.0, 3.88, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_31_flat + start: [6.36, 3.56, -0.16] + goal: [8.92, -0.52, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_32_flat + start: [-0.44, -0.04, -0.16] + goal: [3.64, -2.36, 0.08] + weight: 1.0 + tags: [auto, flat] +- id: auto_33_flat + start: [-1.8, 2.2, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_34_flat + start: [6.92, 4.36, -0.16] + goal: [10.84, -0.84, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_35_flat + start: [11.64, -0.76, 0.24] + goal: [3.0, -0.68, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_36_flat + start: [7.08, -0.12, 0.0] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_37_flat + start: [13.8, 1.56, 0.32] + goal: [8.92, 4.44, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_38_flat + start: [-2.36, 4.04, -0.16] + goal: [13.8, 1.56, 0.32] + weight: 1.0 + tags: [auto, flat] +- id: auto_39_flat + start: [13.8, 1.56, 0.32] + goal: [6.44, 0.44, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_40_flat + start: [-1.64, -2.52, -0.16] + goal: [11.96, -0.68, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_41_flat + start: [13.88, 1.96, 0.24] + goal: [-3.64, 4.36, -0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_42_flat + start: [-1.64, -2.52, -0.16] + goal: [9.56, -0.76, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_43_flat + start: [13.8, 1.56, 0.32] + goal: [-1.16, 4.68, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_44_flat + start: [13.8, 1.56, 0.32] + goal: [11.32, 3.72, -0.08] + weight: 1.0 + tags: [auto, flat] +- id: auto_45_flat + start: [0.12, 0.04, -0.16] + goal: [10.84, -0.84, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_46_flat + start: [-0.44, 0.76, -0.16] + goal: [12.76, -0.36, 0.24] + weight: 1.0 + tags: [auto, flat] +- id: auto_47_flat + start: [13.88, 1.96, 0.24] + goal: [-1.32, 7.64, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_48_flat + start: [13.8, 1.56, 0.32] + goal: [0.36, 4.28, -0.16] + weight: 1.0 + tags: [auto, flat] +- id: auto_49_flat + start: [6.04, 1.32, -0.16] + goal: [7.88, -0.36, 0.08] + weight: 1.0 + tags: [auto, flat] +- id: neg_00 + start: [-1.72, 4.52, -0.16] + goal: [-1.32, 2.76, 0.88] + weight: 1.0 + tags: [manual, negative, stairs, up] + expect_fail: true +- id: neg_01 + start: [-1.0, 8.6, -0.24] + goal: [-0.84, 6.68, 0.48] + weight: 1.0 + tags: [manual, negative, stairs, up] + expect_fail: true +- id: neg_02 + start: [5.08, 1.32, -0.16] + goal: [7.64, 1.4, 0.56] + weight: 1.0 + tags: [manual, negative, stairs, up] + expect_fail: true +- id: manual_00 + start: [9.72, -2.84, 0.24] + goal: [12.6, 0.04, 0.24] + weight: 1.0 + tags: [manual, flat] +- id: neg_03 + start: [2.12, 3.8, -0.16] + goal: [2.12, 4.44, 1.12] + weight: 1.0 + tags: [manual, negative, stairs, up] + expect_fail: true +- id: neg_04 + start: [0.76, 6.44, -0.16] + goal: [0.04, 6.92, -0.24] + weight: 1.0 + tags: [manual, negative, flat] + expect_fail: true +- id: neg_05 + start: [-1.56, -0.2, -0.16] + goal: [-2.36, -0.76, 0.8] + weight: 1.0 + tags: [manual, negative, stairs, up] + expect_fail: true +- id: neg_06 + start: [-1.08, -2.6, 2.24] + goal: [-1.88, 0.52, -0.16] + weight: 1.0 + tags: [manual, negative, stairs, down, long] + expect_fail: true +- id: neg_07 + start: [4.28, -3.48, 0.0] + goal: [4.68, -1.32, 0.08] + weight: 1.0 + tags: [manual, negative, flat] + expect_fail: true +- id: neg_08 + start: [3.48, -2.84, 0.0] + goal: [4.68, -1.8, 1.12] + weight: 1.0 + tags: [manual, negative, stairs, up] + expect_fail: true +- id: neg_09 + start: [4.6, -2.2, 1.12] + goal: [6.52, -4.28, 0.08] + weight: 1.0 + tags: [stairs, down, manual, negative] + expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 287802eb38..96aeda4ae9 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -57,7 +57,7 @@ ) from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate -from dimos.utils.data import get_data_dir, resolve_named_path +from dimos.utils.data import get_data_dir if TYPE_CHECKING: from numpy.typing import NDArray @@ -132,8 +132,11 @@ def diff_reports( """Name every case whose pass/fail flipped between two runs. Exits 1 when any case regressed, so a keep/discard loop can gate on it. - With --exact, exits 1 on any non-timing difference at all; running the - suite twice and exact-diffing the reports is the determinism check. + Perf budget breaches always print but only exit 1 with --exact: parallel + runs inflate wall-clock ~20 percent, so the binding perf check belongs on + the serial confirmation run. With --exact, also exits 1 on any non-timing + difference at all; running the suite twice and exact-diffing the reports + is the determinism check. """ old_report = json.loads(old.read_text()) new_report = json.loads(new.read_text()) @@ -154,6 +157,8 @@ def diff_reports( violations = tripwire.perf_violations(new_report) for violation in violations: print(f"PERF BUDGET EXCEEDED: {violation}") + if violations and not exact: + print(" advisory here; the --exact confirmation run is the binding perf gate") if exact: differences = tripwire.exact_differences(old_report, new_report) if differences: @@ -165,7 +170,7 @@ def diff_reports( print(f" ... and {len(differences) - shown} more") raise typer.Exit(code=1) print("exact: reports identical") - if d.broke or violations: + if d.broke or (exact and violations): raise typer.Exit(code=1) @@ -257,6 +262,12 @@ def ingest( cases: int = typer.Option( 0, "--cases", help="Exact auto-generated case count; 0 scales with recording length" ), + external: bool = typer.Option( + False, + "--external", + help="Reference the recording in place instead of copying it into data/; " + "keeps it out of the LFS flow", + ), force: bool = typer.Option(False, "--force", help="Overwrite dataset and manifest"), ) -> None: """Register a recording as a dataset: copy, map, generate cases.""" @@ -266,14 +277,23 @@ def ingest( manifest = CASES_DIR / f"{name}.yaml" if manifest.exists() and not force: raise typer.BadParameter(f"{manifest} already exists; pass --force to regenerate") - dest = get_data_dir() / f"{name}.db" - if src.resolve() != dest.resolve(): - if dest.exists() and not force: - raise typer.BadParameter(f"{dest} already exists; pass --force to overwrite") - print(f"copying {src} -> {dest}") - _copy_recording(src, dest) - - suite = Suite(dataset=name, cases=[], lidar_stream=lidar_stream, odom_stream=odom_stream) + if external: + dest = src.resolve() + else: + dest = get_data_dir() / f"{name}.db" + if src.resolve() != dest.resolve(): + if dest.exists() and not force: + raise typer.BadParameter(f"{dest} already exists; pass --force to overwrite") + print(f"copying {src} -> {dest}") + _copy_recording(src, dest) + + suite = Suite( + dataset=name, + cases=[], + lidar_stream=lidar_stream, + odom_stream=odom_stream, + db=str(dest) if external else None, + ) trajectory = load_trajectory(dest, odom_stream) arcs = trajectory.arc_lengths() print( @@ -348,7 +368,7 @@ def _load_for_curation(dataset: str) -> tuple[Suite, Path, NDArray[np.float32], raise typer.BadParameter(f"no manifest {manifest}; run ingest first") suite = load_suite(manifest) cfg = EvalConfig() - final = load_or_build_final_map(resolve_named_path(dataset, ".db"), suite, cfg) + final = load_or_build_final_map(suite.db_path(), suite, cfg) planner = cfg.make_planner() planner.update_global_map(final.occupied) return suite, manifest, planner.surface_map(), cfg @@ -402,8 +422,8 @@ def pick_case( from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height suite, manifest, surface, cfg = _load_for_curation(dataset) - final = load_or_build_final_map(resolve_named_path(dataset, ".db"), suite, cfg) - trajectory = load_trajectory(resolve_named_path(dataset, ".db"), suite.odom_stream) + final = load_or_build_final_map(suite.db_path(), suite, cfg) + trajectory = load_trajectory(suite.db_path(), suite.odom_stream) foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) def full_tags(negative: bool, extra: list[str]) -> list[str]: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 6437453b3f..6cdef98ce7 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -42,7 +42,6 @@ load_or_build_final_map, ) from dimos.navigation.nav_3d.evaluator.recording import load_trajectory -from dimos.utils.data import resolve_named_path from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -266,7 +265,7 @@ def _snapshot(planner: MLSPlanner) -> PlannerArtifacts: def run_suite(suite: Suite, cfg: EvalConfig, threads: int = 1) -> DatasetResult: - db_path = resolve_named_path(suite.dataset, ".db") + db_path = suite.db_path() trajectory = load_trajectory(db_path, suite.odom_stream) final = load_or_build_final_map(db_path, suite, cfg) obstacle_keys = final.occupied_keys diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index b05be854be..1485158a7a 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -462,12 +462,15 @@ def test_save_suite_roundtrip(tmp_path: Path) -> None: Case(id="neg", start=(0.0, 0.0, 0.0), goal=(5.0, 5.0, 5.0), expect_fail=True), ], lidar_stream="other_lidar", + db="~/recordings/demo.db", ) path = save_suite(suite, tmp_path / "demo.yaml") loaded = load_suite(path) assert loaded.dataset == "demo" assert loaded.lidar_stream == "other_lidar" assert loaded.odom_stream == "pointlio_odometry" + assert loaded.db == "~/recordings/demo.db" + assert loaded.db_path() == Path.home() / "recordings/demo.db" assert loaded.cases[0].goal == (1.0, 2.0, 3.0) assert loaded.cases[0].tags == ["x"] assert not loaded.cases[0].expect_fail diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 72f587681d..8c36d7f44f 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -38,7 +38,6 @@ from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.recording import load_trajectory -from dimos.utils.data import resolve_named_path if TYPE_CHECKING: from pathlib import Path @@ -174,7 +173,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - suites_by_dataset = {suite.dataset: suite for suite in suites} for dataset in report.datasets: suite = suites_by_dataset[dataset.dataset] - db_path = resolve_named_path(suite.dataset, ".db") + db_path = suite.db_path() final = load_or_build_final_map(db_path, suite, cfg) trajectory = load_trajectory(db_path, suite.odom_stream) root = dataset.dataset From 03e95f72da24a1c6c953e53026e6b7e0ffdf054c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 18 Jul 2026 15:21:14 -0700 Subject: [PATCH 14/29] Better visualization --- dimos/navigation/nav_3d/evaluator/cli.py | 1 + dimos/navigation/nav_3d/evaluator/picker.py | 131 +++++++++++++++----- 2 files changed, 101 insertions(+), 31 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 96aeda4ae9..63f4d7d515 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -483,6 +483,7 @@ def delete_case(saved_id: str) -> tuple[bool, str]: dataset, final.occupied, turbo_by_height(final.occupied), + final.voxel_size, foot, suite.cases, save_pair, diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index ce7ee856fd..bbcefd459e 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -29,7 +29,7 @@ from dataclasses import dataclass import threading -from typing import TYPE_CHECKING, Literal, cast +from typing import TYPE_CHECKING import numpy as np @@ -77,6 +77,62 @@ Plain drag orbits, scroll zooms, right-drag pans. """ +# three.js ACES filmic tone mapping, the fitted curve and its color +# matrices. Applied by the viewer to mesh materials but not to the point +# and line shaders. +_ACES_INPUT = np.array( + [ + [0.59719, 0.35458, 0.04823], + [0.07600, 0.90834, 0.13383], + [0.02840, 0.01566, 0.83777], + ] +) +_ACES_OUTPUT = np.array( + [ + [1.60475, -0.53108, -0.07367], + [-0.10208, 1.10813, -0.00605], + [-0.00327, -0.07276, 1.07602], + ] +) +# White scene lights, bright enough that inverse-tone-mapped albedos fit +# in [0, 1]. LIGHT_REFERENCE is the ambient plus directional total on a +# typical face; faces above or below it shade brighter or darker. +_AMBIENT_INTENSITY = 3.5 +_DIRECTIONAL_INTENSITY = 2.0 +_LIGHT_REFERENCE = 4.6 + + +def _prelit_albedo(srgb: NDArray[np.uint8]) -> NDArray[np.float64]: + """Linear albedo that tone-maps back to the wanted sRGB color when lit. + + Voxel cubes are lit meshes, so the viewer runs them through ACES tone + mapping and would desaturate the height colormap. Feeding the inverse + curve through the material albedo cancels that out at the reference + light level. + """ + c = srgb.astype(np.float64) / 255.0 + lin = np.where(c <= 0.04045, c / 12.92, ((c + 0.055) / 1.055) ** 2.4) + t = np.clip(lin @ np.linalg.inv(_ACES_OUTPUT).T, 0.0, 0.99) + a2 = 1.0 - 0.983729 * t + a1 = 0.0245786 - 0.4329510 * t + a0 = -0.000090537 - 0.238081 * t + v = (-a1 + np.sqrt(a1 * a1 - 4.0 * a2 * a0)) / (2.0 * a2) + x = 0.6 * (v @ np.linalg.inv(_ACES_INPUT).T) + return np.clip(x / _LIGHT_REFERENCE, 0.0, 1.0) + + +def _cube_colors(srgb: NDArray[np.uint8]) -> NDArray[np.uint8]: + """Per-instance cube colors. The viewer reads these as linear RGB.""" + return np.asarray((_prelit_albedo(srgb) * 255.0).round(), dtype=np.uint8) + + +def _marker_color(srgb: tuple[int, int, int]) -> tuple[int, int, int]: + """Marker mesh color. The viewer reads this as sRGB.""" + albedo = _prelit_albedo(np.array(srgb, dtype=np.uint8)) + out = np.where(albedo <= 0.0031308, albedo * 12.92, 1.055 * albedo ** (1 / 2.4) - 0.055) + r, g, b = (out * 255.0).round().astype(int) + return int(r), int(g), int(b) + def pick_along_ray( points: NDArray[np.float32], @@ -316,6 +372,7 @@ def pick_cases( dataset: str, map_points: NDArray[np.float32], map_colors: NDArray[np.uint8], + voxel_size: float, walked: NDArray[np.float32], cases: Sequence[Case], save_pair: SavePair, @@ -329,13 +386,48 @@ def pick_cases( server.gui.configure_theme(dark_mode=True) server.scene.set_background_image(np.full((1, 1, 3), 14, dtype=np.uint8)) server.scene.set_up_direction("+z") - cloud = server.scene.add_point_cloud( + # Neutral white lighting instead of the default HDRI environment map, + # which tints the height colormap. + server.scene.configure_environment_map(None) + server.scene.configure_default_lights(enabled=False) + server.scene.add_light_ambient("/lights/ambient", intensity=_AMBIENT_INTENSITY) + server.scene.add_light_directional( + "/lights/sun", intensity=_DIRECTIONAL_INTENSITY, position=(1.0, 2.0, 3.0) + ) + # Cubes sit slightly under the voxel size so neighbors show a seam + # instead of z-fighting, keeping individual voxels distinguishable. + half = 0.42 * voxel_size + corners = half * np.array( + [[x, y, z] for x in (-1, 1) for y in (-1, 1) for z in (-1, 1)], dtype=np.float32 + ) + cube_faces = np.array( + [ + [0, 1, 3], + [0, 3, 2], + [4, 6, 7], + [4, 7, 5], + [0, 4, 5], + [0, 5, 1], + [2, 3, 7], + [2, 7, 6], + [0, 2, 6], + [0, 6, 4], + [1, 5, 7], + [1, 7, 3], + ] + ) + identity_quats = np.zeros((len(map_points), 4), dtype=np.float32) + identity_quats[:, 0] = 1.0 + server.scene.add_batched_meshes_simple( "/map", - map_points, - map_colors, - point_size=0.025, - point_shape="circle", - precision="float32", + corners, + cube_faces, + batched_wxyzs=identity_quats, + batched_positions=map_points, + batched_colors=_cube_colors(map_colors), + flat_shading=True, + cast_shadow=False, + receive_shadow=False, ) if len(walked) >= 2: segments = np.stack([walked[:-1], walked[1:]], axis=1) @@ -353,29 +445,6 @@ def _(client: viser.ClientHandle) -> None: server.gui.add_markdown(INSTRUCTIONS) selected_line = server.gui.add_markdown("selected: —") - with server.gui.add_folder("display", expand_by_default=False): - size_slider = server.gui.add_slider( - "point size", min=0.005, max=0.08, step=0.0025, initial_value=cloud.point_size - ) - shape_dropdown = server.gui.add_dropdown( - "shape", ("circle", "rounded", "square", "diamond"), initial_value="circle" - ) - shaded_box = server.gui.add_checkbox("shaded", True) - - @size_slider.on_update - def _(_event: object) -> None: - cloud.point_size = size_slider.value - - @shape_dropdown.on_update - def _(_event: object) -> None: - cloud.point_shape = cast( - "Literal['circle', 'rounded', 'square', 'diamond']", shape_dropdown.value - ) - - @shaded_box.on_update - def _(_event: object) -> None: - cloud.point_shading = "gradient" if shaded_box.value else "flat" - undo_button = server.gui.add_button("undo last pick") save_all_button = server.gui.add_button("save all unsaved") exit_button = server.gui.add_button("save all & exit") @@ -412,7 +481,7 @@ def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.Sce return server.scene.add_icosphere( f"/picks/m{marker_seq}", radius=0.09, - color=color, + color=_marker_color(color), position=(float(point[0]), float(point[1]), float(point[2]) + 0.05), ) From 5caf34227258ccd6de07c3cb7c2fac43ffd31be7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 20 Jul 2026 12:34:11 -0700 Subject: [PATCH 15/29] Better collision checking, hide more rerun things --- dimos/navigation/nav_3d/evaluator/cases.py | 15 +- .../evaluator/cases/mid360_athens_stairs.yaml | 5 + dimos/navigation/nav_3d/evaluator/cli.py | 59 ++++++- dimos/navigation/nav_3d/evaluator/config.py | 13 +- dimos/navigation/nav_3d/evaluator/generate.py | 3 +- dimos/navigation/nav_3d/evaluator/metrics.py | 83 +++++++-- dimos/navigation/nav_3d/evaluator/runner.py | 157 +++++++++++++----- .../nav_3d/evaluator/test_evaluator.py | 135 ++++++++++++++- dimos/navigation/nav_3d/evaluator/viz.py | 124 ++++++++++++-- 9 files changed, 508 insertions(+), 86 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index 8e5efab5bf..a13f36cc99 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -41,6 +41,10 @@ class Case: # Human-certified infeasible pair: the correct answer is to refuse. # Evaluated on the final map only, scored 1.0 for refusal. expect_fail: bool = False + # A route the robot walked that a later dynamic obstacle blocked, e.g. a + # door that closed. The online plan is expected to succeed, the final + # plan is expected to refuse and is scored 1.0 for refusal. + expect_final_fail: bool = False @dataclass @@ -71,6 +75,12 @@ def load_suite(path: Path) -> Suite: raise ValueError(f"{path}: case {entry['id']}: start/goal must be xyz") sx, sy, sz = (float(v) for v in entry["start"]) gx, gy, gz = (float(v) for v in entry["goal"]) + expect_fail = bool(entry.get("expect_fail", False)) + expect_final_fail = bool(entry.get("expect_final_fail", False)) + if expect_fail and expect_final_fail: + raise ValueError( + f"{path}: case {entry['id']}: expect_fail and expect_final_fail are exclusive" + ) case = Case( id=str(entry["id"]), start=(sx, sy, sz), @@ -78,7 +88,8 @@ def load_suite(path: Path) -> Suite: weight=float(entry.get("weight", 1.0)), tags=[str(t) for t in entry.get("tags", [])], l_ref=float(entry["l_ref"]) if "l_ref" in entry else None, - expect_fail=bool(entry.get("expect_fail", False)), + expect_fail=expect_fail, + expect_final_fail=expect_final_fail, ) if case.id in seen: raise ValueError(f"{path}: duplicate case id {case.id}") @@ -126,6 +137,8 @@ def save_suite(suite: Suite, path: Path | None = None) -> Path: entry["l_ref"] = round(case.l_ref, 3) if case.expect_fail: entry["expect_fail"] = True + if case.expect_final_fail: + entry["expect_final_fail"] = True entries.append(entry) doc["cases"] = entries path.write_text(yaml.safe_dump(doc, sort_keys=False, default_flow_style=None)) diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index dff5df679f..f7c9307410 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -50,3 +50,8 @@ cases: goal: [7.24, -3.96, -6.08] weight: 1.0 tags: [auto, flat] +- id: manual_00 + start: [1.8, -4.44, -0.32] + goal: [5.24, -4.2, -0.32] + weight: 1.0 + tags: [manual, flat, doorway] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 63f4d7d515..7003be462d 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -24,6 +24,7 @@ New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a Pick cases by click: python -m dimos.navigation.nav_3d.evaluator pick-case office_a Curate by coords: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z +Flag dynamic route: python -m dimos.navigation.nav_3d.evaluator tag office_a auto_03 --final-fail """ from __future__ import annotations @@ -116,6 +117,21 @@ def _print_report(report: Report) -> None: f"plan p95 {report.plan_ms['p95']:.1f}ms | " f"map update p95 {report.map_update_ms['p95']:.0f}ms" ) + inc_only = [ + f"{c.dataset}/{c.id}" + for d in report.datasets + for c in d.cases + if c.online.success and not c.final.success + ] + if inc_only: + candidates = set(report.dynamic_candidates) + others = [x for x in inc_only if x not in candidates] + print(f"\nincremental-only ({len(inc_only)}) — passed online, failed final:") + if report.dynamic_candidates: + print(f" dynamic-obstacle candidates: {', '.join(report.dynamic_candidates)}") + print(" review with --rrd, confirm: evaluator tag --final-fail") + if others: + print(f" not explained by a new obstacle, inspect final map: {', '.join(others)}") @app.command("diff") @@ -135,7 +151,7 @@ def diff_reports( Perf budget breaches always print but only exit 1 with --exact: parallel runs inflate wall-clock ~20 percent, so the binding perf check belongs on the serial confirmation run. With --exact, also exits 1 on any non-timing - difference at all; running the suite twice and exact-diffing the reports + difference at all. Running the suite twice and exact-diffing the reports is the determinism check. """ old_report = json.loads(old.read_text()) @@ -213,7 +229,7 @@ def run( if not suites: raise typer.BadParameter(f"no cases carry all tags {tag}") cfg = _apply_overrides(EvalConfig(), set_ or []) - report = evaluate(suites, cfg, workers=workers) + report = evaluate(suites, cfg, workers=workers, keep_artifacts=rrd_out is not None) _print_report(report) for violation in tripwire.perf_violations(report.to_dict()): print(f"PERF BUDGET EXCEEDED: {violation}") @@ -404,6 +420,41 @@ def add_case( ) +@app.command("tag") +def tag_case( + dataset: str = typer.Argument(..., help="Dataset whose manifest holds the case"), + case_id: str = typer.Argument(..., help="Case id to edit"), + final_fail: bool = typer.Option( + True, + "--final-fail/--no-final-fail", + help="Mark a dynamic-obstacle route: online path expected, final path expected refused", + ), +) -> None: + """Flag an auto case as a dynamic-obstacle route, e.g. a door that closed. + + The online plan is still scored normally, the final plan is scored 1.0 + for refusing. Use it on a case that shows up as incremental-only because a + real obstacle blocked the route by the final map, not a planner bug. + """ + manifest = CASES_DIR / f"{dataset}.yaml" + if not manifest.exists(): + raise typer.BadParameter(f"no manifest {manifest}") + suite = load_suite(manifest) + case = next((c for c in suite.cases if c.id == case_id), None) + if case is None: + raise typer.BadParameter(f"case {case_id!r} not found in {manifest}") + if final_fail and case.expect_fail: + raise typer.BadParameter( + f"case {case_id!r} is expect_fail; a case cannot be both infeasible and dynamic" + ) + case.expect_final_fail = final_fail + tags = [t for t in case.tags if t != "dynamic"] + case.tags = [*tags, "dynamic"] if final_fail else tags + save_suite(suite, manifest) + flag = "expect_final_fail" if final_fail else "cleared expect_final_fail" + print(f"{case.id}: {flag} [{', '.join(case.tags)}]") + + @app.command("pick-case") def pick_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the cases"), @@ -414,8 +465,8 @@ def pick_case( Serves the final map, the walked path, and every case already in the manifest as an editable panel entry. Shift+click picks new start/goal - pairs. Any case can be renamed, retagged, flipped negative, or deleted; - new pairs save to the manifest snapped like add-case. + pairs. Any case can be renamed, retagged, flipped negative, or deleted. + New pairs save to the manifest snapped like add-case. """ # Lazy: picker/viz pull in viser and matplotlib, only needed for pick-case. from dimos.navigation.nav_3d.evaluator.picker import pick_cases diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 12387beaa6..b35dd82dab 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -37,11 +37,14 @@ class EvalConfig: max_range: float = 30.0 robot_height: float = 0.3 - # Physical body envelope for the collision gate. The gate catches paths - # that penetrate obstacles, not near-grazes, so the radius is the true - # body half-width. The ground margin over the radius bounds the terrain - # slope the gate tolerates. Keep margin/radius above the steepest stairs. - robot_radius: float = 0.16 + # Physical body envelope for the collision gate: a box the robot's length + # and width, oriented along the path and pitched with the slope. The gate + # catches paths that drive the body through obstacles. Only the elevated + # body is checked, from ground_margin to body_clearance up the tilted body + # axis, so the legs and the terrain they stand on never count. Length and + # width match the Go2 collision box. + robot_length: float = 0.7 + robot_width: float = 0.31 ground_margin: float = 0.25 body_clearance: float = 0.45 goal_tolerance: float = 0.5 diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 35a305c441..d5b8e9d037 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -168,7 +168,8 @@ def generate_cases( line, obstacle_keys, cfg.voxel_size, - cfg.robot_radius, + cfg.robot_length, + cfg.robot_width, cfg.ground_margin, cfg.body_clearance, ).valid diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 87dcfd3fd0..b3676fd817 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -65,27 +65,71 @@ class GateResult: min_clearance_m: float +def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.float64]: + """Unit direction from a point span/2 behind each sample to a point span/2 + ahead, measured along the path. + + This is the rigid body's heading: the chord from the rear feet to the front + feet, not the local tangent between two points under the body center, which + on stepped terrain flips between flat treads and vertical risers. + """ + if len(samples) < 2: + return np.tile(np.array([1.0, 0.0, 0.0]), (len(samples), 1)) + pts = samples.astype(np.float64) + arc = np.concatenate([[0.0], np.cumsum(np.linalg.norm(np.diff(pts, axis=0), axis=1))]) + half = span / 2.0 + back_arc = np.clip(arc - half, arc[0], arc[-1]) + front_arc = np.clip(arc + half, arc[0], arc[-1]) + back = np.column_stack([np.interp(back_arc, arc, pts[:, c]) for c in range(3)]) + front = np.column_stack([np.interp(front_arc, arc, pts[:, c]) for c in range(3)]) + fwd = front - back + return fwd / np.maximum(np.linalg.norm(fwd, axis=1, keepdims=True), 1e-9) + + +def body_frames( + samples: NDArray[np.float32], robot_length: float +) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]: + """Per-sample body axes: forward along the robot-length chord, lateral + horizontal, up tilted with the slope, so the box yaws and pitches with the + body rather than the terrain right under its center.""" + fwd = chord_directions(samples, robot_length) + lateral = np.cross(np.array([0.0, 0.0, 1.0]), fwd) + ln = np.linalg.norm(lateral, axis=1, keepdims=True) + lateral = np.where(ln > 1e-6, lateral / np.maximum(ln, 1e-9), np.array([0.0, 1.0, 0.0])) + up = np.cross(fwd, lateral) + return fwd, lateral, up + + def check_path( waypoints: NDArray[np.float32], obstacle_keys: NDArray[np.int64], voxel_size: float, - robot_radius: float, + robot_length: float, + robot_width: float, ground_margin: float, body_clearance: float, ) -> GateResult: - """Sweep the robot body along foot-level waypoints against obstacles. - - The checked volume at each sample is a cylinder from ground_margin above - the foot (so the supporting floor never counts) up to body_clearance. - Candidate voxels come from a padded voxelized cylinder and are then - verified against the exact continuous bounds, so quantization never pulls - ground voxels into the check. + """Sweep the robot body box along foot-level waypoints against obstacles. + + At each sample the body is a box of the robot's length and width, centered + over the path point and rotated in place: yawed and pitched along the + robot-length chord, so it stays over the path rather than sliding onto the + chord. Its vertical span is the ground_margin to body_clearance band up the + tilted body axis, so the legs and the ground below never count, only the + elevated body. Candidate voxels come from a padded voxelized cylinder that + covers the box at any orientation and are tested against the exact box. """ samples = densify(waypoints, voxel_size / 2) + fwd, lateral, up = body_frames(samples, robot_length) + half_len = robot_length / 2.0 + half_wid = robot_width / 2.0 + half_band = (body_clearance - ground_margin) / 2.0 + mid = np.array([0.0, 0.0, (ground_margin + body_clearance) / 2.0]) + circ = float(np.hypot(half_len, half_wid)) offsets = cylinder_offsets( - robot_radius + MARGIN_CAP_M + voxel_size, - ground_margin - voxel_size, - body_clearance + voxel_size, + circ + MARGIN_CAP_M + voxel_size, + -(half_len + MARGIN_CAP_M + voxel_size), + body_clearance + half_len + MARGIN_CAP_M + voxel_size, voxel_size, ) keys = offset_keys(samples, offsets, voxel_size) @@ -93,11 +137,18 @@ def check_path( s_idx, o_idx = np.nonzero(candidate) if len(s_idx) == 0: return GateResult(valid=True, collision_points=samples[:0], min_clearance_m=MARGIN_CAP_M) - delta = key_centers(keys[s_idx, o_idx], voxel_size) - samples[s_idx] - hd = np.linalg.norm(delta[:, :2], axis=1) - in_band = (delta[:, 2] >= ground_margin) & (delta[:, 2] <= body_clearance) - exact = in_band & (hd <= robot_radius) - clearance = float(hd[in_band].min() - robot_radius) if in_band.any() else MARGIN_CAP_M + # Offset from the box center, which sits mid-band directly over the sample. + delta = key_centers(keys[s_idx, o_idx], voxel_size) - samples[s_idx] - mid + along = (delta * fwd[s_idx]).sum(1) + across = (delta * lateral[s_idx]).sum(1) + vertical = (delta * up[s_idx]).sum(1) + # Signed distance to the oriented footprint rectangle, negative inside. + qx = np.abs(along) - half_len + qy = np.abs(across) - half_wid + sdf = np.hypot(np.maximum(qx, 0.0), np.maximum(qy, 0.0)) + np.minimum(np.maximum(qx, qy), 0.0) + in_band = np.abs(vertical) <= half_band + exact = in_band & (sdf <= 0.0) + clearance = float(sdf[in_band].min()) if in_band.any() else MARGIN_CAP_M colliding = np.unique(s_idx[exact]) return GateResult( valid=len(colliding) == 0, diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 6cdef98ce7..1118ef4846 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -17,10 +17,11 @@ Every case is planned twice: online on the incremental map built up to the case's start time, and final on the map fed the whole recording. The final map is not ground truth, only the most complete map the pipeline produces. -The final path is gated against full final occupancy, the online path only -against obstacles the sensor had returns from by plan time. Every path must -also stand on final-map occupancy and stay within the climb envelope. The -headline score is validity-gated SPL on the incremental map. +The final path is gated against full final occupancy, the online path against +the incremental map the planner had at plan time, so obstacles the sensor had +not yet mapped never count. Every path must also stand on final-map occupancy +and stay within the climb envelope. The headline score is validity-gated SPL +on the incremental map. """ from __future__ import annotations @@ -113,8 +114,16 @@ class CaseResult: online: PlanOutcome final: PlanOutcome soft_progress: float - # Planner graph on the incremental map, kept only for failed cases. + # The online plan succeeded but a new obstacle in the final map blocks its + # route, so the case looks like a dynamic obstacle rather than a bug. + dynamic_candidate: bool = False + # Where the online route is blocked by that newly-appeared occupancy. + blocking_points: list[list[float]] = field(default_factory=list) + # Planner graph on the incremental map, kept for the rerun recording. online_artifacts: PlannerArtifacts | None = None + # Occupied voxel centers of the incremental map at plan time, kept for the + # rerun recording. + online_occupied: NDArray[np.float32] | None = None @dataclass @@ -156,6 +165,9 @@ class Report: plan_ms: dict[str, float] map_update_ms: dict[str, float] datasets: list[DatasetResult] + # dataset/id of cases whose online route a new final obstacle blocks, the + # candidates for an expect_final_fail label. + dynamic_candidates: list[str] = field(default_factory=list) config: dict[str, float | int] = field(default_factory=dict) def to_dict(self) -> dict[str, object]: @@ -164,6 +176,7 @@ def to_dict(self) -> dict[str, object]: dataset.pop("final_artifacts") for case in dataset["cases"]: case.pop("online_artifacts") + case.pop("online_occupied") return out @@ -186,7 +199,8 @@ def _run_plan( waypoints, obstacle_keys, cfg.voxel_size, - cfg.robot_radius, + cfg.robot_length, + cfg.robot_width, cfg.ground_margin, cfg.body_clearance, ) @@ -257,6 +271,41 @@ def _goal_seen(online_points: NDArray[np.float32], goal: tuple[float, float, flo return bool(d.min() <= GOAL_SEEN_RADIUS_M) +def _dynamic_candidate( + online: PlanOutcome, + final: PlanOutcome, + online_wp: NDArray[np.float32] | None, + online_keys: NDArray[np.int64], + final_keys: NDArray[np.int64], + cfg: EvalConfig, +) -> tuple[bool, list[list[float]]]: + """Flag a case whose online route is blocked only by new final occupancy. + + An online success paired with a final failure is either a dynamic obstacle + that appeared after the robot passed or a planner or mapping bug. Gating + the online path against the voxels the final map gained since plan time + tells the two apart. If that newly-occupied set alone blocks the route, a + real obstacle appeared. A human still confirms before labeling the case. + """ + if online_wp is None or not online.success or final.success: + return False, [] + new_keys = np.setdiff1d(final_keys, online_keys) + if not len(new_keys): + return False, [] + gate = metrics.check_path( + online_wp, + new_keys, + cfg.voxel_size, + cfg.robot_length, + cfg.robot_width, + cfg.ground_margin, + cfg.body_clearance, + ) + if gate.valid: + return False, [] + return True, gate.collision_points[:MAX_COLLISIONS_KEPT].tolist() + + def _snapshot(planner: MLSPlanner) -> PlannerArtifacts: return PlannerArtifacts( surface_clearance=planner.surface_clearance_map(), @@ -264,7 +313,9 @@ def _snapshot(planner: MLSPlanner) -> PlannerArtifacts: ) -def run_suite(suite: Suite, cfg: EvalConfig, threads: int = 1) -> DatasetResult: +def run_suite( + suite: Suite, cfg: EvalConfig, threads: int = 1, keep_artifacts: bool = False +) -> DatasetResult: db_path = suite.db_path() trajectory = load_trajectory(db_path, suite.odom_stream) final = load_or_build_final_map(db_path, suite, cfg) @@ -333,7 +384,6 @@ def run_suite(suite: Suite, cfg: EvalConfig, threads: int = 1) -> DatasetResult: def process_checkpoint( k: int, keys: NDArray[np.int64], - online_gate_keys: NDArray[np.int64], online_planner: MLSPlanner, ) -> None: online_points = key_centers(keys, cfg.voxel_size) @@ -346,15 +396,29 @@ def process_checkpoint( final_out, _ = _run_plan( final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg ) + if case.expect_final_fail: + # A dynamic obstacle blocked the route by the final map, so the + # planner is right to refuse it there while the online plan, + # made before the closure, is scored normally. + final_out = score_negative(final_out) if len(online_points): + # Collisions are checked against the incremental map the planner + # actually had at plan time (keys), not the final map. Support + # still uses the final map, since the ground exists whether or + # not it was mapped yet. online_out, online_wp = _run_plan( - online_planner, case, ref.length, online_gate_keys, obstacle_keys, cfg + online_planner, case, ref.length, keys, obstacle_keys, cfg ) else: online_out = _no_plan(case, 0.0) online_wp = None end = online_wp[-1] if online_wp is not None and len(online_wp) else None goal_seen = _goal_seen(online_points, case.goal) + dynamic_candidate, blocking = ( + (False, []) + if case.expect_final_fail + else _dynamic_candidate(online_out, final_out, online_wp, keys, obstacle_keys, cfg) + ) results[ci] = CaseResult( id=case.id, dataset=suite.dataset, @@ -372,34 +436,33 @@ def process_checkpoint( online=online_out, final=final_out, soft_progress=metrics.soft_progress(end, case.start, case.goal), - online_artifacts=None - if online_out.success or not len(online_points) - else _snapshot(online_planner), + dynamic_candidate=dynamic_candidate, + blocking_points=blocking, + online_artifacts=_snapshot(online_planner) + if keep_artifacts and len(online_points) + else None, + online_occupied=online_points if keep_artifacts and len(online_points) else None, ) active = {int(k) for k in case_ckpt} tls = threading.local() - def task(k: int, keys: NDArray[np.int64], gate: NDArray[np.int64]) -> None: + def task(k: int, keys: NDArray[np.int64]) -> None: planner = getattr(tls, "planner", None) if planner is None: planner = tls.planner = cfg.make_planner() try: - process_checkpoint(k, keys, gate, planner) + process_checkpoint(k, keys, planner) finally: in_flight.release() - def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64], NDArray[np.int64]]]: - """Walk the delta chain once. The online gate only holds obstacles the - sensor had returns from by plan time. Obstacles never observed are not - the planner's fault.""" - gate = np.array([], dtype=np.int64) - for k, (keys, observed_new) in enumerate(checkpoints.iter_snapshots()): - fresh = np.intersect1d(obstacle_keys, observed_new, assume_unique=True) - if len(fresh): - gate = np.union1d(gate, fresh) + def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64]]]: + """Walk the delta chain once, yielding the incremental occupancy at each + case's plan time. Only voxels mapped by then are present, so obstacles + the sensor never saw are naturally excluded from the online check.""" + for k, (keys, _observed) in enumerate(checkpoints.iter_snapshots()): if k in active: - yield k, keys, gate + yield k, keys # The planner releases the GIL and parallelizes updates internally via a # shared rayon pool, so a few worker threads interleave the serial parts @@ -416,8 +479,8 @@ def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64], NDArray[np.int64 future.result() else: online_planner = cfg.make_planner() - for k, keys, gate in snapshot_stream(): - process_checkpoint(k, keys, gate, online_planner) + for k, keys in snapshot_stream(): + process_checkpoint(k, keys, online_planner) done = [r for r in results if r is not None] if len(done) != len(suite.cases): @@ -429,22 +492,37 @@ def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64], NDArray[np.int64 map_build_ms=final.build_ms, add_frame_ms=final.add_frame_ms, frames=final.frames, - final_artifacts=_snapshot(final_planner), + final_artifacts=_snapshot(final_planner) if keep_artifacts else None, ) -def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = 1) -> Report: +def evaluate( + suites: list[Suite], + cfg: EvalConfig | None = None, + workers: int = 1, + keep_artifacts: bool = False, +) -> Report: """Score every suite. workers is total parallelism: datasets spread over - processes and each dataset's checkpoints over threads.""" + processes and each dataset's checkpoints over threads. keep_artifacts + snapshots each planner graph for the rerun recording.""" cfg = cfg or EvalConfig() if workers > 1 and len(suites) > 1: threads = max(1, workers // len(suites)) with ProcessPoolExecutor(max_workers=min(workers, len(suites))) as pool: datasets = list( - pool.map(run_suite, suites, itertools.repeat(cfg), itertools.repeat(threads)) + pool.map( + run_suite, + suites, + itertools.repeat(cfg), + itertools.repeat(threads), + itertools.repeat(keep_artifacts), + ) ) else: - datasets = [run_suite(suite, cfg, threads=workers) for suite in suites] + datasets = [ + run_suite(suite, cfg, threads=workers, keep_artifacts=keep_artifacts) + for suite in suites + ] cases = [c for d in datasets for c in d.cases] if not cases: raise ValueError("no cases to evaluate") @@ -453,15 +531,15 @@ def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = online_spl = np.array([c.online.spl for c in cases]) final_spl = np.array([c.final.spl for c in cases]) soft = np.array([c.soft_progress if not c.online.success else c.online.spl for c in cases]) - outcome_counts = {"both": 0, "final_only": 0, "incremental_only": 0, "neither": 0} + outcome_names = { + (True, True): "both", + (False, True): "final_only", + (True, False): "incremental_only", + (False, False): "neither", + } + outcome_counts = dict.fromkeys(outcome_names.values(), 0) for c in cases: - key = { - (True, True): "both", - (False, True): "final_only", - (True, False): "incremental_only", - (False, False): "neither", - }[(c.online.success, c.final.success)] - outcome_counts[key] += 1 + outcome_counts[outcome_names[c.online.success, c.final.success]] += 1 by_tag: dict[str, TagStats] = {} for tag in sorted({t for c in cases for t in c.tags}): @@ -487,5 +565,6 @@ def evaluate(suites: list[Suite], cfg: EvalConfig | None = None, workers: int = plan_ms=metrics.timing_stats([c.online.plan_ms for c in cases]), map_update_ms=metrics.timing_stats([c.map_update_ms for c in cases]), datasets=datasets, + dynamic_candidates=[f"{c.dataset}/{c.id}" for c in cases if c.dynamic_candidate], config=asdict(cfg), ) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 1485158a7a..7849e33a35 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -48,6 +48,7 @@ CaseResult, DatasetResult, Report, + _dynamic_candidate, _no_plan, _run_plan, score_negative, @@ -80,7 +81,13 @@ def _wall(x: float) -> np.ndarray: def _gate(waypoints: np.ndarray, obstacles: np.ndarray) -> metrics.GateResult: keys = np.unique(voxel_keys(obstacles, VOXEL)) return metrics.check_path( - waypoints, keys, VOXEL, robot_radius=0.16, ground_margin=0.25, body_clearance=0.45 + waypoints, + keys, + VOXEL, + robot_length=0.7, + robot_width=0.31, + ground_margin=0.25, + body_clearance=0.45, ) @@ -89,7 +96,19 @@ def test_gate_blocks_wall_crossing() -> None: result = _gate(path, _wall(0.0)) assert not result.valid assert len(result.collision_points) > 0 - assert np.all(np.abs(result.collision_points[:, 0]) < 0.3) + # Collisions fall within the box half-length (0.35) of the wall. + assert np.all(np.abs(result.collision_points[:, 0]) < 0.45) + + +def test_gate_box_uses_travel_orientation() -> None: + """The body box is long along travel (0.7) and narrow across it (0.31).""" + path = np.array([[-0.5, 0, 0], [0.5, 0, 0]], dtype=np.float32) + # 0.25 m ahead along travel is inside the 0.35 m half-length. + ahead = np.array([[0.25, 0.0, 0.35]], dtype=np.float32) + assert not _gate(path, ahead).valid + # The same 0.25 m offset to the side is outside the 0.155 m half-width. + beside = np.array([[0.0, 0.25, 0.35]], dtype=np.float32) + assert _gate(path, beside).valid def test_gate_passes_clear_path() -> None: @@ -104,8 +123,31 @@ def test_gate_ignores_ground() -> None: assert _gate(path, floor).valid +def test_chord_direction_spans_robot_length() -> None: + """Heading comes from the body-length chord, not the local step, so it is + steady across stepped terrain instead of flipping tread-to-riser.""" + xs = np.arange(0, 3.0, 0.1) + zs = np.floor(xs / 0.2) * 0.1 # stairs: 0.1 m rise every 0.2 m, mean slope 0.5 + path = np.stack([xs, np.zeros_like(xs), zs], axis=1).astype(np.float32) + fwd = metrics.chord_directions(path, span=0.7) + local = np.diff(path.astype(np.float64), axis=0) + local /= np.linalg.norm(local, axis=1, keepdims=True) + assert fwd[5:-5, 2].std() < 0.5 * local[:, 2].std() + assert fwd[5:-5, 2].mean() > 0.2 # steadily pitched up the stairs + + +def test_gate_pitch_clears_rising_step() -> None: + """A voxel ahead-and-up is inside a flat box but beyond the pitched one.""" + step = np.array([[0.3, 0.0, 0.4]], dtype=np.float32) + # Level travel: the step sits in the horizontal body band and collides. + assert not _gate(np.array([[0, 0, 0], [1, 0, 0]], dtype=np.float32), step).valid + # Climbing at 45 degrees: the box pitches up, so the same voxel falls beyond + # the tilted body and clears. + assert _gate(np.array([[0, 0, 0], [1, 0, 1]], dtype=np.float32), step).valid + + def test_gate_tolerates_stair_slope() -> None: - """Terrain rising at stair slope inside the disc must not trigger the gate.""" + """Terrain rising at stair slope inside the body box must not trigger the gate.""" xs, ys = np.meshgrid(np.arange(-1, 2, VOXEL), np.arange(-1, 1, VOXEL)) slope = np.stack([xs.ravel(), ys.ravel(), xs.ravel() * 0.7 - 0.05], axis=1, dtype=np.float32) path = np.stack( @@ -119,7 +161,9 @@ def test_gate_reports_clearance_margin() -> None: graze = np.array([[9.7, -0.5, 0], [9.7, 0.5, 0]], dtype=np.float32) result = _gate(graze, wall) assert result.valid - assert result.min_clearance_m == pytest.approx(0.35 - 0.16, abs=0.02) + # Travel is +y here, so the wall 0.35 m away in x sits off the box's + # 0.155 m half-width. + assert result.min_clearance_m == pytest.approx(0.35 - 0.155, abs=0.02) crossing = _gate(np.array([[9, 0, 0], [11, 0, 0]], dtype=np.float32), wall) assert not crossing.valid assert crossing.min_clearance_m < 0 @@ -417,6 +461,60 @@ def test_meta_negative_case_scoring() -> None: assert score_negative(partial).success +def test_expect_final_fail_scores_online_normally_and_refuses_final() -> None: + """A door-closed route: the online plan earns SPL, the final plan must refuse. + + Mirrors the runner, which inverts only the final outcome for an + expect_final_fail case and scores the online outcome as usual. + """ + keys, cfg, case = _meta_scene() + open_keys = np.unique(voxel_keys(_floor(), VOXEL)) + route = _u_route() + l_ref = metrics.path_length(route) + + # Online, before the door closed: the wall is absent and the walked route + # is clean, so it scores in full. + online, _ = _run_plan(_stub(route), case, l_ref, open_keys, open_keys, cfg) + assert online.success + assert online.spl == pytest.approx(1.0) + + # Final, after the door closed: the wall is present and refusing is right. + refused, _ = _run_plan(_stub(None), case, l_ref, keys, keys, cfg) + final = score_negative(refused) + assert final.success + assert final.spl == 1.0 + + # Claiming a route straight through the closed door is a false positive. + line = np.array([case.start, case.goal], dtype=np.float32) + claimed, _ = _run_plan(_stub(line), case, l_ref, keys, keys, cfg) + assert score_negative(claimed).spl == 0.0 + + +def test_dynamic_candidate_flags_route_blocked_by_new_occupancy() -> None: + """Online success with a final failure from newly-appeared occupancy flags.""" + _, cfg, case = _meta_scene() + open_keys = np.unique(voxel_keys(_floor(), VOXEL)) + final_keys = np.unique(voxel_keys(np.concatenate([_floor(), _wall(10.0)]), VOXEL)) + line = np.array([case.start, case.goal], dtype=np.float32) + + online, wp = _run_plan(_stub(line), case, 24.0, open_keys, open_keys, cfg) + assert online.success + final, _ = _run_plan(_stub(line), case, 24.0, final_keys, final_keys, cfg) + assert not final.success + + flagged, blocking = _dynamic_candidate(online, final, wp, open_keys, final_keys, cfg) + assert flagged + assert blocking + + # No occupancy appeared between the two maps, so the final failure is not a + # dynamic obstacle and must not be flagged. + unflagged, _ = _dynamic_candidate(online, final, wp, final_keys, final_keys, cfg) + assert not unflagged + + # A clean final plan is never a candidate. + assert not _dynamic_candidate(online, online, wp, open_keys, final_keys, cfg)[0] + + def test_check_kinematics_rejects_cliff_jumps() -> None: stairs = np.array([[0, 0, 0], [0.4, 0, 0.16], [0.8, 0, 0.32]], dtype=np.float32) assert metrics.check_kinematics(stairs, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid @@ -502,6 +600,35 @@ def test_load_suite(tmp_path: Path) -> None: load_suite(manifest) +def test_expect_final_fail_roundtrips(tmp_path: Path) -> None: + suite = Suite( + dataset="demo", + cases=[ + Case( + id="dyn", + start=(0.0, 0.0, 0.0), + goal=(3.0, 0.0, 0.0), + tags=["auto", "dynamic"], + expect_final_fail=True, + ) + ], + ) + loaded = load_suite(save_suite(suite, tmp_path / "demo.yaml")) + assert loaded.cases[0].expect_final_fail + assert not loaded.cases[0].expect_fail + + +def test_expect_fail_and_final_fail_are_exclusive(tmp_path: Path) -> None: + manifest = tmp_path / "demo.yaml" + manifest.write_text( + "dataset: demo\ncases:\n" + " - {id: bad, start: [0, 0, 0], goal: [1, 0, 0], " + "expect_fail: true, expect_final_fail: true}\n" + ) + with pytest.raises(ValueError, match="exclusive"): + load_suite(manifest) + + def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, object]: """Report JSON from a {dataset: {case_id: (inc, fin)}} pass/fail spec.""" datasets = [] diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 8c36d7f44f..88880c74bf 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -18,14 +18,17 @@ - map/obstacles: final voxels, turbo colormap by height - walked_path: the recorded foot path (white) - planner_final: the planner graph the full aggregated map produced. - Surface cells colored by wall clearance (red inside the hard clearance), + Surface cells colored by wall clearance (gray inside the hard clearance), edges colored white to red by log traversal cost. - cases/: start (cyan), goal (orange), online and final planned paths colored by verdict (green valid, red gate-invalid, yellow unreached), the - gate's collision samples (red dots), unsupported samples (magenta), and - too-steep waypoints (purple). Failed cases also get a thin red - start-to-goal intent line and a known/ layer: the planner graph on the - incremental map at plan time, i.e. what the robot knew when it failed. + gate's body box at each collision (semi-transparent red, pitched with the + slope and elevated over the legs), unsupported samples (magenta), and + too-steep waypoints (purple). Every case carries a known/ layer: the + incremental voxel map (known/voxels, turbo by height) and planner graph + at plan time, what the robot knew then. Failed cases also get a thin red + start-to-goal intent line. Dynamic obstacle candidates get a new_obstacle + layer marking the final occupancy that blocks the online route. """ from __future__ import annotations @@ -35,6 +38,7 @@ import numpy as np import rerun as rr import rerun.blueprint as rrb +from scipy.spatial.transform import Rotation from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.recording import load_trajectory @@ -52,17 +56,20 @@ START_COLOR = [0, 255, 255] GOAL_COLOR = [255, 140, 0] COLLISION_COLOR = [255, 0, 0] +COLLISION_FILL_ALPHA = 90 UNSUPPORTED_COLOR = [255, 0, 255] STEEP_COLOR = [160, 32, 240] NEGATIVE_INTENT_COLOR = [255, 255, 0] +NEAR_WALL_COLOR = [120, 120, 120] VALID_PATH_COLOR = [0, 220, 0] INVALID_PATH_COLOR = [255, 0, 0] UNREACHED_PATH_COLOR = [255, 200, 0] +DYNAMIC_BLOCK_COLOR = [255, 20, 147] CLEARANCE_CLAMP_M = 1.0 -# Cells colored red as too close to a wall. Display threshold only. -CLEARANCE_RED_M = 0.1 +# Cells colored gray as too close to a wall. Display threshold only. +CLEARANCE_NEAR_WALL_M = 0.1 def turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: @@ -80,7 +87,7 @@ def _clearance_colors(clearance: NDArray[np.float32], hard_clearance: float) -> blocked = np.array([4.0, 8.0, 48.0]) clear = np.array([150.0, 200.0, 255.0]) out = np.asarray(blocked + norm[:, None] * (clear - blocked), dtype=np.uint8) - out[clearance < hard_clearance] = (255, 0, 0) + out[clearance < hard_clearance] = NEAR_WALL_COLOR return out @@ -101,7 +108,7 @@ def _log_planner(entity: str, artifacts: PlannerArtifacts | None, cfg: EvalConfi f"{entity}/surface", rr.Points3D( surface[:, :3], - colors=_clearance_colors(surface[:, 3], CLEARANCE_RED_M), + colors=_clearance_colors(surface[:, 3], CLEARANCE_NEAR_WALL_M), radii=cfg.voxel_size / 4, ), static=True, @@ -127,7 +134,53 @@ def _outcome_color(outcome: PlanOutcome) -> list[int]: return UNREACHED_PATH_COLOR -def _log_path(entity: str, outcome: PlanOutcome, radius: float) -> None: +def _travel_dirs( + points: NDArray[np.float32], waypoints: NDArray[np.float32], span: float +) -> NDArray[np.float64]: + """Rigid-body heading at each point: the chord from span/2 behind it to + span/2 ahead along the path, the rear-feet to front-feet direction.""" + if len(waypoints) < 2: + return np.tile(np.array([1.0, 0.0, 0.0]), (len(points), 1)) + wp = waypoints.astype(np.float64) + seg = np.linalg.norm(np.diff(wp, axis=0), axis=1) + arc = np.concatenate([[0.0], np.cumsum(seg)]) + a2, v2 = wp[:-1, :2], np.diff(wp[:, :2], axis=0) + hlen2 = np.maximum((v2 * v2).sum(1), 1e-12) + half = span / 2.0 + dirs = np.empty((len(points), 3)) + for i, p in enumerate(points): + t = np.clip(((p[:2] - a2) * v2).sum(1) / hlen2, 0.0, 1.0) + s = int(np.argmin(((a2 + t[:, None] * v2 - p[:2]) ** 2).sum(1))) + pos = arc[s] + t[s] * seg[s] + lo, hi = np.clip([pos - half, pos + half], arc[0], arc[-1]) + d = np.array( + [np.interp(hi, arc, wp[:, c]) - np.interp(lo, arc, wp[:, c]) for c in range(3)] + ) + dirs[i] = d / max(float(np.linalg.norm(d)), 1e-9) + return dirs + + +def _thin_by_gap(points: NDArray[np.float32], gap: float) -> NDArray[np.float32]: + """Keep points at least gap apart along the sequence.""" + kept: list[NDArray[np.float32]] = [] + for p in points: + if not kept or float(np.linalg.norm(p - kept[-1])) >= gap: + kept.append(p) + return np.asarray(kept, dtype=np.float32) + + +def _body_box_quat(direction: NDArray[np.float64]) -> rr.Quaternion: + """Box orientation for a body travelling along direction: yaw and pitch from + the chord, no roll.""" + fwd = direction + lateral = np.cross([0.0, 0.0, 1.0], fwd) + ln = float(np.linalg.norm(lateral)) + lateral = lateral / ln if ln > 1e-6 else np.array([0.0, 1.0, 0.0]) + up = np.cross(fwd, lateral) + return rr.Quaternion(xyzw=Rotation.from_matrix(np.column_stack([fwd, lateral, up])).as_quat()) + + +def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) -> None: if not outcome.waypoints: return rr.log( @@ -136,9 +189,30 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float) -> None: static=True, ) if outcome.collisions: + # The gate's body box at each colliding foot sample: the robot length + # and width, centered over the path point and rotated in place (yaw and + # pitch from the chord), elevated over the legs into the ground-margin + # to body-clearance band. Thinned to about a body length apart so the + # boxes read as distinct bodies instead of one overlapping smear. + feet = _thin_by_gap(np.asarray(outcome.collisions, dtype=np.float32), cfg.robot_length) + waypoints = np.asarray(outcome.waypoints, dtype=np.float32) + mid = np.array([0.0, 0.0, (cfg.ground_margin + cfg.body_clearance) / 2.0]) + half = [ + cfg.robot_length / 2.0, + cfg.robot_width / 2.0, + (cfg.body_clearance - cfg.ground_margin) / 2.0, + ] rr.log( f"{entity}/collisions", - rr.Points3D(outcome.collisions, colors=[COLLISION_COLOR], radii=radius * 3), + rr.Boxes3D( + half_sizes=np.tile(half, (len(feet), 1)), + centers=feet + mid, + quaternions=[ + _body_box_quat(d) for d in _travel_dirs(feet, waypoints, cfg.robot_length) + ], + colors=[[*COLLISION_COLOR, COLLISION_FILL_ALPHA]], + fill_mode=rr.components.FillMode.Solid, + ), static=True, ) if outcome.unsupported: @@ -156,9 +230,10 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float) -> None: def _dataset_view(root: str, case_ids: list[str]) -> rrb.Spatial3DView: - """One view per dataset, planner graph edges hidden until toggled on.""" + """One view per dataset. Every case is hidden until toggled on, and the + final planner graph edges start off.""" hidden = [f"{root}/planner_final/edges"] - hidden += [f"{root}/cases/{cid}/known/edges" for cid in case_ids] + hidden += [f"{root}/cases/{cid}" for cid in case_ids] return rrb.Spatial3DView( origin=f"/{root}", name=root, @@ -225,9 +300,26 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - ), static=True, ) - _log_planner(f"{base}/known", case.online_artifacts, cfg) - _log_path(f"{base}/online", case.online, radius=0.04) - _log_path(f"{base}/final", case.final, radius=0.02) + # The incremental map at plan time, saved for every case. + _log_planner(f"{base}/known", case.online_artifacts, cfg) + if case.online_occupied is not None and len(case.online_occupied): + rr.log( + f"{base}/known/voxels", + rr.Points3D( + case.online_occupied, + colors=turbo_by_height(case.online_occupied), + radii=cfg.voxel_size / 4, + ), + static=True, + ) + if case.blocking_points: + rr.log( + f"{base}/new_obstacle", + rr.Points3D(case.blocking_points, colors=[DYNAMIC_BLOCK_COLOR], radii=0.06), + static=True, + ) + _log_path(f"{base}/online", case.online, radius=0.04, cfg=cfg) + _log_path(f"{base}/final", case.final, radius=0.02, cfg=cfg) views = [_dataset_view(d.dataset, [c.id for c in d.cases]) for d in report.datasets] rr.send_blueprint(rrb.Blueprint(rrb.Tabs(*views) if len(views) > 1 else views[0])) From d577f065c60256730e5ab3615ce6a050155a8fc3 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 20 Jul 2026 13:00:57 -0700 Subject: [PATCH 16/29] Final only for manual tests --- .../evaluator/cases/mid360_athens_stairs.yaml | 3 +- dimos/navigation/nav_3d/evaluator/cli.py | 24 +++- dimos/navigation/nav_3d/evaluator/runner.py | 130 +++++++++++------- .../nav_3d/evaluator/test_evaluator.py | 20 ++- 4 files changed, 115 insertions(+), 62 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index f7c9307410..9ff66fbf90 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -54,4 +54,5 @@ cases: start: [1.8, -4.44, -0.32] goal: [5.24, -4.2, -0.32] weight: 1.0 - tags: [manual, flat, doorway] + tags: [manual, flat, doorway, negative] + expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 7003be462d..00865360a6 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -63,6 +63,8 @@ if TYPE_CHECKING: from numpy.typing import NDArray + from dimos.navigation.nav_3d.evaluator.runner import PlanOutcome + app = typer.Typer(no_args_is_help=True, add_completion=False) @@ -79,23 +81,32 @@ def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: return cfg +def _score_cell(outcome: PlanOutcome) -> str: + """No path at all shows x. A planned path shows its SPL, which is 0.00 when + the path is invalid and higher when it is valid.""" + return "x" if not outcome.planned and not outcome.success else f"{outcome.spl:.2f}" + + def _print_report(report: Report) -> None: header = ( f"{'case':<28} {'dataset':<22} {'inc':>5} {'fin':>5} " - f"{'len':>6} {'ref':>6} {'miss':>6} {'clr':>6} {'vox':>8} {'ms':>7}" + f"{'len':>6} {'ref':>6} {'clr':>6} {'vox':>8} {'ms':>7}" ) print(header) print("-" * len(header)) for d in report.datasets: for c in d.cases: + inc = "-" if c.final_only else _score_cell(c.online) + no_path = not c.online.planned and not c.online.success + length = "x" if no_path else f"{c.online.length:.1f}" clr = ( f"{c.online.min_clearance:>6.2f}" if c.online.min_clearance is not None else " " * 6 ) print( f"{c.id:<28} {c.dataset:<22} " - f"{c.online.spl:>5.2f} {c.final.spl:>5.2f} " - f"{c.online.length:>6.1f} {c.l_ref:>6.1f} " - f"{c.online.goal_miss:>6.1f} {clr} " + f"{inc:>5} {_score_cell(c.final):>5} " + f"{length:>6} {c.l_ref:>6.1f} " + f"{clr} " f"{c.online_voxels:>8d} {c.online.plan_ms:>7.1f}" ) print("-" * len(header)) @@ -107,11 +118,12 @@ def _print_report(report: Report) -> None: ) print(f"\n{'by tag':<12} {'inc':>5} {'fin':>5} {'n':>4}") for tag, s in report.by_tag.items(): - print(f"{tag:<12} {s.inc_score:>5.2f} {s.fin_score:>5.2f} {s.n:>4}") + inc = f"{s.inc_score:.2f}" if s.n_online else "-" + print(f"{tag:<12} {inc:>5} {s.fin_score:>5.2f} {s.n:>4}") print( f"\nscore {report.score:.3f} | soft {report.score_soft:.3f} | " f"final {report.final_score:.3f} | " - f"success inc {report.n_success}/{report.n_cases} " + f"success inc {report.n_success}/{report.n_online} " f"fin {report.n_success_final}/{report.n_cases} | " f"outcomes {report.outcome_counts} | " f"plan p95 {report.plan_ms['p95']:.1f}ms | " diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 1118ef4846..05876b18bb 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -14,9 +14,11 @@ """Run case suites through the ray tracer and MLS planner and score them. -Every case is planned twice: online on the incremental map built up to the -case's start time, and final on the map fed the whole recording. The final -map is not ground truth, only the most complete map the pipeline produces. +Auto cases, derived from the walked trajectory, are planned twice: online on +the incremental map built up to the case's start time, and final on the map +fed the whole recording. Manual and certified-infeasible cases have no plan +time on the recording, so they are planned once, on the final map only. The +final map is not ground truth, only the most complete map the pipeline makes. The final path is gated against full final occupancy, the online path against the incremental map the planner had at plan time, so obstacles the sensor had not yet mapped never count. Every path must also stand on final-map occupancy @@ -76,9 +78,6 @@ class PlanOutcome: length: float plan_ms: float spl: float - # How far the path end is from the goal. Start-to-goal distance when no - # path was planned. Smooth counterpart to the binary reached flag. - goal_miss: float # Gate margin along the path (see GateResult.min_clearance_m). None when # no path was planned. min_clearance: float | None @@ -114,6 +113,8 @@ class CaseResult: online: PlanOutcome final: PlanOutcome soft_progress: float + # Scored on the final map only, so there is no distinct online score. + final_only: bool = False # The online plan succeeded but a new obstacle in the final map blocks its # route, so the case looks like a dynamic obstacle rather than a bug. dynamic_candidate: bool = False @@ -142,6 +143,8 @@ class TagStats: """Aggregate scores over every case carrying a given tag.""" n: int + # Cases with an online phase (excludes final-only manual/infeasible cases). + n_online: int inc_score: float fin_score: float inc_success: int @@ -154,6 +157,8 @@ class Report: score_soft: float final_score: float n_cases: int + # Cases with an online phase; the incremental score is over these only. + n_online: int n_success: int n_success_final: int # The incremental and final runs are independent tests per case. These @@ -192,7 +197,7 @@ def _run_plan( waypoints = planner.plan(case.start, case.goal) plan_ms = (perf_counter() - t0) * 1000 if waypoints is None or len(waypoints) == 0: - return _no_plan(case, plan_ms), None + return _no_plan(plan_ms), None reached = metrics.goal_reached(waypoints, case.goal, cfg.goal_tolerance) gate = metrics.check_path( @@ -222,7 +227,6 @@ def _run_plan( length=length, plan_ms=plan_ms, spl=metrics.spl(success, l_ref, length), - goal_miss=float(np.linalg.norm(waypoints[-1] - np.asarray(case.goal, dtype=np.float32))), min_clearance=gate.min_clearance_m, waypoints=waypoints.tolist(), collisions=gate.collision_points[:MAX_COLLISIONS_KEPT].tolist(), @@ -232,8 +236,7 @@ def _run_plan( return outcome, waypoints -def _no_plan(case: Case, plan_ms: float) -> PlanOutcome: - miss = float(np.linalg.norm(np.asarray(case.goal) - np.asarray(case.start))) +def _no_plan(plan_ms: float) -> PlanOutcome: return PlanOutcome( planned=False, reached=False, @@ -244,7 +247,6 @@ def _no_plan(case: Case, plan_ms: float) -> PlanOutcome: length=0.0, plan_ms=plan_ms, spl=0.0, - goal_miss=miss, min_clearance=None, waypoints=[], collisions=[], @@ -313,6 +315,16 @@ def _snapshot(planner: MLSPlanner) -> PlannerArtifacts: ) +def _final_only(case: Case) -> bool: + """Whether a case is scored on the final map only, with no online phase. + + Manual and certified-infeasible cases have hand-placed endpoints that are + not tied to the recording timeline, so there is no meaningful incremental + map at plan time to replay against. They are pure final-map tests. + """ + return case.expect_fail or "manual" in case.tags + + def run_suite( suite: Suite, cfg: EvalConfig, threads: int = 1, keep_artifacts: bool = False ) -> DatasetResult: @@ -321,47 +333,55 @@ def run_suite( final = load_or_build_final_map(db_path, suite, cfg) obstacle_keys = final.occupied_keys + final_only = np.array([_final_only(c) for c in suite.cases]) refs: list[metrics.Reference] = [] - for case in suite.cases: + for i, case in enumerate(suite.cases): if case.expect_fail: # Infeasible by certification: no demonstrated route, no plan time. miss = float(np.linalg.norm(np.asarray(case.goal) - np.asarray(case.start))) refs.append(metrics.Reference(miss, False, float("inf"), False)) continue ref = metrics.reference_length(trajectory, case.start, case.goal, cfg.robot_height) - if not ref.snapped: - logger.warning( - "%s/%s: start or goal is off the walked trajectory; " - "using straight-line reference and the full map", - suite.dataset, - case.id, - ) - elif not ref.causal: - logger.warning( - "%s/%s: goal is never visited before the start; planning on the full map", - suite.dataset, - case.id, - ) + if not final_only[i]: + # Only online-replayed cases need a causal snap onto the trajectory. + if not ref.snapped: + logger.warning( + "%s/%s: start or goal is off the walked trajectory; " + "using straight-line reference and the full map", + suite.dataset, + case.id, + ) + elif not ref.causal: + logger.warning( + "%s/%s: goal is never visited before the start; planning on the full map", + suite.dataset, + case.id, + ) if case.l_ref is not None: ref = metrics.Reference(case.l_ref, ref.snapped, ref.start_ts, ref.causal) refs.append(ref) - start_ts = np.array([r.start_ts for r in refs], dtype=np.float64) + # Final-only cases never replay online, so they take no checkpoint and their + # plan time drops out of the schedule. + start_ts = np.array( + [float("inf") if final_only[i] else r.start_ts for i, r in enumerate(refs)], + dtype=np.float64, + ) checkpoints = load_or_build_checkpoints(db_path, suite, cfg, start_ts) case_ckpt = np.searchsorted(checkpoints.times, start_ts) - negative = np.array([c.expect_fail for c in suite.cases]) - case_ckpt[negative] = -1 + case_ckpt[final_only] = -1 final_planner = cfg.make_planner() final_planner.update_global_map(final.occupied) results: list[CaseResult | None] = [None] * len(suite.cases) - for ci in np.flatnonzero(negative): + for ci in np.flatnonzero(final_only): case, ref = suite.cases[ci], refs[ci] - outcome = score_negative( - _run_plan(final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg)[0] - ) + outcome, _ = _run_plan(final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg) + if case.expect_fail: + # An infeasible case is passed by refusing it. + outcome = score_negative(outcome) results[ci] = CaseResult( id=case.id, dataset=suite.dataset, @@ -370,15 +390,16 @@ def run_suite( weight=case.weight, tags=case.tags, l_ref=ref.length, - l_ref_snapped=False, + l_ref_snapped=ref.snapped, plan_ts=float("inf"), online_voxels=len(final.occupied), map_update_ms=0.0, goal_seen=True, - expect_fail=True, + expect_fail=case.expect_fail, online=outcome, final=outcome, soft_progress=outcome.spl, + final_only=True, ) def process_checkpoint( @@ -410,7 +431,7 @@ def process_checkpoint( online_planner, case, ref.length, keys, obstacle_keys, cfg ) else: - online_out = _no_plan(case, 0.0) + online_out = _no_plan(0.0) online_wp = None end = online_wp[-1] if online_wp is not None and len(online_wp) else None goal_seen = _goal_seen(online_points, case.goal) @@ -527,10 +548,15 @@ def evaluate( if not cases: raise ValueError("no cases to evaluate") - weights = np.array([c.weight for c in cases]) - online_spl = np.array([c.online.spl for c in cases]) - final_spl = np.array([c.final.spl for c in cases]) - soft = np.array([c.soft_progress if not c.online.success else c.online.spl for c in cases]) + # Manual and infeasible cases have no online phase, so every incremental + # aggregate is over the online cases only. Final aggregates cover them all. + online = [c for c in cases if not c.final_only] + + def wmean(values: list[float], items: list[CaseResult]) -> float: + if not items: + return 0.0 + return float(np.average(values, weights=[c.weight for c in items])) + outcome_names = { (True, True): "both", (False, True): "final_only", @@ -538,32 +564,36 @@ def evaluate( (False, False): "neither", } outcome_counts = dict.fromkeys(outcome_names.values(), 0) - for c in cases: + for c in online: outcome_counts[outcome_names[c.online.success, c.final.success]] += 1 by_tag: dict[str, TagStats] = {} for tag in sorted({t for c in cases for t in c.tags}): tc = [c for c in cases if tag in c.tags] - w = np.array([c.weight for c in tc]) + oc = [c for c in tc if not c.final_only] by_tag[tag] = TagStats( n=len(tc), - inc_score=float(np.average([c.online.spl for c in tc], weights=w)), - fin_score=float(np.average([c.final.spl for c in tc], weights=w)), - inc_success=sum(c.online.success for c in tc), + n_online=len(oc), + inc_score=wmean([c.online.spl for c in oc], oc), + fin_score=wmean([c.final.spl for c in tc], tc), + inc_success=sum(c.online.success for c in oc), fin_success=sum(c.final.success for c in tc), ) return Report( - score=float(np.average(online_spl, weights=weights)), - score_soft=float(np.average(soft, weights=weights)), - final_score=float(np.average(final_spl, weights=weights)), + score=wmean([c.online.spl for c in online], online), + score_soft=wmean( + [c.soft_progress if not c.online.success else c.online.spl for c in online], online + ), + final_score=wmean([c.final.spl for c in cases], cases), n_cases=len(cases), - n_success=sum(c.online.success for c in cases), + n_online=len(online), + n_success=sum(c.online.success for c in online), n_success_final=sum(c.final.success for c in cases), outcome_counts=outcome_counts, by_tag=by_tag, - plan_ms=metrics.timing_stats([c.online.plan_ms for c in cases]), - map_update_ms=metrics.timing_stats([c.map_update_ms for c in cases]), + plan_ms=metrics.timing_stats([c.online.plan_ms for c in online]), + map_update_ms=metrics.timing_stats([c.map_update_ms for c in online]), datasets=datasets, dynamic_candidates=[f"{c.dataset}/{c.id}" for c in cases if c.dynamic_candidate], config=asdict(cfg), diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 7849e33a35..69e45aa899 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -49,6 +49,7 @@ DatasetResult, Report, _dynamic_candidate, + _final_only, _no_plan, _run_plan, score_negative, @@ -394,12 +395,11 @@ def test_meta_straight_line_cheat_scores_zero() -> None: assert out.min_clearance is not None and out.min_clearance < 0 -def test_meta_no_path_scores_zero_with_miss() -> None: +def test_meta_no_path_scores_zero() -> None: keys, cfg, case = _meta_scene() out, _ = _run_plan(_stub(None), case, 24.0, keys, keys, cfg) assert not out.planned assert out.spl == 0.0 - assert out.goal_miss == pytest.approx(16.0) assert out.min_clearance is None @@ -411,7 +411,6 @@ def test_meta_demonstrated_route_scores_full() -> None: out, _ = _run_plan(_stub(route), case, l_ref, keys, keys, cfg) assert out.success assert out.spl == pytest.approx(1.0) - assert out.goal_miss == 0.0 assert out.min_clearance == metrics.MARGIN_CAP_M @@ -600,6 +599,16 @@ def test_load_suite(tmp_path: Path) -> None: load_suite(manifest) +def test_final_only_covers_manual_and_infeasible() -> None: + """Manual and infeasible cases skip the online phase; auto cases keep it.""" + xyz = (0.0, 0.0, 0.0) + assert not _final_only(Case(id="a", start=xyz, goal=xyz, tags=["auto", "flat"])) + assert _final_only(Case(id="m", start=xyz, goal=xyz, tags=["manual", "flat"])) + assert _final_only(Case(id="n", start=xyz, goal=xyz, tags=["manual"], expect_fail=True)) + # A dynamic-obstacle auto case still replays online. + assert not _final_only(Case(id="d", start=xyz, goal=xyz, tags=["auto"], expect_final_fail=True)) + + def test_expect_final_fail_roundtrips(tmp_path: Path) -> None: suite = Suite( dataset="demo", @@ -651,8 +660,8 @@ def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, map_update_ms=0.0, goal_seen=True, expect_fail=False, - online=replace(_no_plan(case, 0.0), success=inc), - final=replace(_no_plan(case, 0.0), success=fin), + online=replace(_no_plan(0.0), success=inc), + final=replace(_no_plan(0.0), success=fin), soft_progress=0.0, ) ) @@ -671,6 +680,7 @@ def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, score_soft=0.0, final_score=0.0, n_cases=0, + n_online=0, n_success=0, n_success_final=0, outcome_counts={}, From e67d96fe7995e5ca6bf4874d1076c0fb02e86122 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 20 Jul 2026 15:24:29 -0700 Subject: [PATCH 17/29] Better auto tagging --- dimos/navigation/nav_3d/evaluator/cases.py | 4 +- .../nav_3d/evaluator/cases/china_office.yaml | 2 +- .../evaluator/cases/mid360_athens_stairs.yaml | 2 +- .../nav_3d/evaluator/cases/sf_office.yaml | 10 +- dimos/navigation/nav_3d/evaluator/cli.py | 61 ++++- dimos/navigation/nav_3d/evaluator/config.py | 8 +- .../navigation/nav_3d/evaluator/final_map.py | 3 +- dimos/navigation/nav_3d/evaluator/generate.py | 29 ++- dimos/navigation/nav_3d/evaluator/metrics.py | 49 +++- dimos/navigation/nav_3d/evaluator/picker.py | 36 ++- dimos/navigation/nav_3d/evaluator/runner.py | 29 +-- dimos/navigation/nav_3d/evaluator/tagging.py | 225 ++++++++++++++++++ .../nav_3d/evaluator/test_evaluator.py | 121 +++++++++- dimos/navigation/nav_3d/evaluator/viz.py | 24 +- 14 files changed, 504 insertions(+), 99 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/tagging.py diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index a13f36cc99..79fe533bf8 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -53,8 +53,8 @@ class Suite: cases: list[Case] lidar_stream: str = "pointlio_lidar" odom_stream: str = "pointlio_odometry" - # Recording location override. Default is data/.db; set this to - # keep a recording outside data/, e.g. a private or holdout recording. + # Recording location override, defaulting to data/.db. + # Set this to keep a recording outside data/. db: str | None = None path: Path | None = None diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml index 051522b168..3545c94d6b 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -109,7 +109,7 @@ cases: start: [-7.4, 11.96, 3.04] goal: [8.2, -17.32, -0.4] weight: 1.0 - tags: [auto, stairs, down, long] + tags: [auto, stairs, down, long, doorway, narrow] - id: auto_22_up start: [-0.36, -4.44, -0.4] goal: [11.24, -6.76, 4.16] diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index 9ff66fbf90..ca71e87124 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -39,7 +39,7 @@ cases: start: [6.68, -5.56, 2.08] goal: [5.72, -5.48, -1.04] weight: 1.0 - tags: [auto, stairs, down, long] + tags: [auto, stairs, down, long, narrow] - id: auto_08_down start: [-0.2, -3.0, 2.56] goal: [7.16, -3.8, -3.12] diff --git a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml index a5b6446c96..e6a9a47e95 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml @@ -5,7 +5,7 @@ cases: start: [-1.64, -2.52, -0.16] goal: [13.8, 1.56, 0.32] weight: 1.0 - tags: [auto, flat] + tags: [auto, flat, doorway, narrow] - id: auto_01_flat start: [-2.84, 8.44, -0.24] goal: [3.64, 7.64, -0.16] @@ -45,7 +45,7 @@ cases: start: [3.72, 2.28, -0.16] goal: [12.6, -0.36, 0.24] weight: 1.0 - tags: [auto, flat] + tags: [auto, flat, narrow] - id: auto_09_flat start: [11.72, 3.88, -0.08] goal: [4.92, 4.76, -0.16] @@ -205,7 +205,7 @@ cases: start: [-1.64, -2.52, -0.16] goal: [11.96, -0.68, 0.24] weight: 1.0 - tags: [auto, flat] + tags: [auto, flat, doorway, narrow] - id: auto_41_flat start: [13.88, 1.96, 0.24] goal: [-3.64, 4.36, -0.24] @@ -230,12 +230,12 @@ cases: start: [0.12, 0.04, -0.16] goal: [10.84, -0.84, 0.24] weight: 1.0 - tags: [auto, flat] + tags: [auto, flat, narrow] - id: auto_46_flat start: [-0.44, 0.76, -0.16] goal: [12.76, -0.36, 0.24] weight: 1.0 - tags: [auto, flat] + tags: [auto, flat, narrow] - id: auto_47_flat start: [13.88, 1.96, 0.24] goal: [-1.32, 7.64, -0.16] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 00865360a6..3d67f891d3 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -25,12 +25,14 @@ Pick cases by click: python -m dimos.navigation.nav_3d.evaluator pick-case office_a Curate by coords: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z Flag dynamic route: python -m dimos.navigation.nav_3d.evaluator tag office_a auto_03 --final-fail +Recompute tags: python -m dimos.navigation.nav_3d.evaluator retag office_a """ from __future__ import annotations import contextlib import dataclasses +import itertools import json import os from pathlib import Path @@ -56,8 +58,10 @@ generate_cases, snap_to_surface, ) +from dimos.navigation.nav_3d.evaluator.metrics import ground_truth_route from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate +from dimos.navigation.nav_3d.evaluator.tagging import GEOMETRIC_TAGS, route_tags from dimos.utils.data import get_data_dir if TYPE_CHECKING: @@ -160,11 +164,9 @@ def diff_reports( """Name every case whose pass/fail flipped between two runs. Exits 1 when any case regressed, so a keep/discard loop can gate on it. - Perf budget breaches always print but only exit 1 with --exact: parallel - runs inflate wall-clock ~20 percent, so the binding perf check belongs on - the serial confirmation run. With --exact, also exits 1 on any non-timing - difference at all. Running the suite twice and exact-diffing the reports - is the determinism check. + Perf budget breaches always print but only exit 1 with --exact, which also + exits 1 on any non-timing difference. Running the suite twice and + exact-diffing the reports is the determinism check. """ old_report = json.loads(old.read_text()) new_report = json.loads(new.read_text()) @@ -373,8 +375,13 @@ def _append_case( print(f"note: goal {goal} is off any standable surface; keeping it as picked") else: raise typer.BadParameter(f"goal {goal} is more than {snap_max}m from standable surface") + if case_id is None: + existing = {c.id for c in suite.cases} + case_id = next( + f"{prefix}_{n:02d}" for n in itertools.count() if f"{prefix}_{n:02d}" not in existing + ) case = Case( - id=case_id or f"{prefix}_{sum(c.id.startswith(f'{prefix}_') for c in suite.cases):02d}", + id=case_id, start=_snap_or_fail("start", start, surface, snap_max), goal=goal, weight=weight, @@ -467,6 +474,44 @@ def tag_case( print(f"{case.id}: {flag} [{', '.join(case.tags)}]") +@app.command("retag") +def retag( + dataset: str = typer.Argument(..., help="Dataset whose manifest gets retagged"), +) -> None: + """Recompute geometric tags for auto-generated cases from the final map. + + Only the geometric tags (flat, stairs, narrow, switchback, and the rest) + are replaced, so improving the tagger never churns start/goal pairs. The + auto provenance tag survives. Manually curated cases are left untouched: + their tags are human intent, not something to recompute. + """ + manifest = CASES_DIR / f"{dataset}.yaml" + if not manifest.exists(): + raise typer.BadParameter(f"no manifest {manifest}; run ingest first") + suite = load_suite(manifest) + cfg = EvalConfig() + final = load_or_build_final_map(suite.db_path(), suite, cfg) + trajectory = load_trajectory(suite.db_path(), suite.odom_stream) + changed = 0 + for case in suite.cases: + if "auto" not in case.tags: + print(f" {case.id}: curated, tags kept [{', '.join(case.tags)}]") + continue + route = ground_truth_route(trajectory, case.start, case.goal, cfg.robot_height) + if route is None: + print(f" {case.id}: off-trajectory, tags kept [{', '.join(case.tags)}]") + continue + provenance = [t for t in case.tags if t not in GEOMETRIC_TAGS] + geo = route_tags(case.start, case.goal, route, final.occupied_keys, cfg) + new_tags = provenance + [t for t in geo if t not in provenance] + if new_tags != case.tags: + changed += 1 + print(f" {case.id}: [{', '.join(case.tags)}] -> [{', '.join(new_tags)}]") + case.tags = new_tags + save_suite(suite, manifest) + print(f"\nretagged {changed} case(s) in {manifest.name}") + + @app.command("pick-case") def pick_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the cases"), @@ -526,11 +571,13 @@ def update_case( if new_id != saved_id and any(c.id == new_id for c in suite.cases): return False, f"case id {new_id!r} already exists", None, None case.id = new_id - # Tags round-trip verbatim; the negative checkbox owns only the + # Tags round-trip verbatim. The negative checkbox owns only the # negative tag, so auto/manual provenance survives edits. plain = [t for t in tags if t != "negative"] case.tags = plain + (["negative"] if negative else []) case.expect_fail = negative + if negative: + case.expect_final_fail = False save_suite(suite, manifest) return True, f"updated {case.id} [{', '.join(case.tags)}]", case.id, list(case.tags) diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index b35dd82dab..9eec530ce1 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -26,11 +26,9 @@ class EvalConfig: (0.31m wide, 0.40m tall, ~0.16m stair risers.) - Algorithm tuning lives in the algorithm packages as their constructor - defaults. The evaluator fixes only the shared voxel resolution, the - sensor range, the robot's sensor height, and the physical body and - capability bounds it gates against. Improving the algorithm means - changing the algorithm, never this file. + Fixes only the shared voxel resolution, sensor range, sensor height, and + the physical body and capability bounds it gates against. Algorithm tuning + lives in the algorithm packages as their constructor defaults. """ voxel_size: float = 0.08 diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index f35337235e..6db5ed4e3d 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -139,7 +139,8 @@ def iter_snapshots(self) -> Iterator[tuple[NDArray[np.int64], NDArray[np.int64]] """ keys = np.array([], dtype=np.int64) for add, rem, obs in zip(self.added, self.removed, self.observed_added, strict=True): - keys = np.union1d(np.setdiff1d(keys, rem, assume_unique=True), add) + keys = np.delete(keys, np.searchsorted(keys, rem)) + keys = np.insert(keys, np.searchsorted(keys, add), add) yield keys, obs diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index d5b8e9d037..f6adcd967d 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -32,6 +32,7 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.cases import Case +from dimos.navigation.nav_3d.evaluator.tagging import STAIRS_DZ_M, route_tags if TYPE_CHECKING: from numpy.typing import NDArray @@ -40,10 +41,6 @@ from dimos.navigation.nav_3d.evaluator.final_map import FinalMap from dimos.navigation.nav_3d.evaluator.recording import Trajectory -STAIRS_DZ_M = 0.5 -LONG_STAIRS_DZ_M = 1.5 -LONG_STAIRS_WALKED_M = 20.0 - @dataclass class GenerationParams: @@ -199,7 +196,13 @@ def generate_cases( ranked = sorted(candidates.values(), key=lambda c: (-c.priority, c.start, c.goal)) selected = _select_diverse(ranked, params, params.resolve_max_cases(float(arcs[-1]))) - cases = [_to_case(cand, n) for n, cand in enumerate(selected)] + cases = [] + for n, cand in enumerate(selected): + route = metrics.ground_truth_route( + trajectory, cand.start, cand.goal, cfg.robot_height, params.snap_max_m + ) + tags = route_tags(cand.start, cand.goal, route, obstacle_keys, cfg) + cases.append(_to_case(cand, n, tags)) return cases @@ -255,7 +258,7 @@ def fill(target: int, relax: bool) -> None: spread = np.full(len(ranked), 2.0 * spread_cap, dtype=np.float32) score = priorities + 0.4 * spread score[~alive] = -np.inf - if not relax and len(stairs) + 1 >= stairs_cap: + if not relax and len(stairs) >= stairs_cap: score[is_stairs] = -np.inf if not np.isfinite(score).any(): break @@ -287,21 +290,17 @@ def fill(target: int, relax: bool) -> None: return (stairs + flats)[:max_cases] -def _to_case(cand: Candidate, n: int) -> Case: +def _to_case(cand: Candidate, n: int, tags: list[str]) -> Case: if cand.dz >= STAIRS_DZ_M: - kind, tags = "up", ["auto", "stairs", "up"] + kind = "up" elif cand.dz <= -STAIRS_DZ_M: - kind, tags = "down", ["auto", "stairs", "down"] + kind = "down" else: - kind, tags = "flat", ["auto", "flat"] - if kind != "flat" and ( - abs(cand.dz) >= LONG_STAIRS_DZ_M or cand.walked_m >= LONG_STAIRS_WALKED_M - ): - tags.append("long") + kind = "flat" return Case( id=f"auto_{n:02d}_{kind}", start=cand.start, goal=cand.goal, weight=1.0, - tags=tags, + tags=["auto", *tags], ) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index b3676fd817..99ccaf64d0 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -252,13 +252,10 @@ def reference_length( ) -> Reference: """Shortest walked length the trajectory demonstrates between start and goal. - The robot usually passes each spot several times, so the reference is the - minimum route length over every combination of start and goal visits, not - the route between single nearest poses, which would include any wandering - in between. Only causal pairs count when one exists: the goal visited - before the start, so an incremental map at the start time has seen the - goal and the demonstrated route. When either endpoint is farther than - max_snap_m from the trajectory, falls back to the straight-line distance. + Minimizes route length over every combination of start and goal visits, + not the route between single nearest poses. Only causal pairs count when + one exists: the goal visited before the start. Falls back to straight-line + distance when either endpoint is farther than max_snap_m from the trajectory. """ foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) s = np.asarray(start, dtype=np.float32) @@ -285,6 +282,44 @@ def reference_length( return Reference(max(float(totals[best]), 1e-6), True, start_ts, causal) +def ground_truth_route( + trajectory: Trajectory, + start: tuple[float, float, float], + goal: tuple[float, float, float], + robot_height: float, + max_snap_m: float = 1.0, +) -> NDArray[np.float32] | None: + """Foot-level polyline of the shortest walk the robot took between start and + goal, or None when either endpoint is off the trajectory. + + Unlike reference_length this ignores causality: it describes the terrain + between two places, so the nearest-in-time pass is what we want, not the + causal one. Picking the visit pair with the least trajectory between them + keeps the route local instead of a building-spanning detour. + """ + foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) + s = np.asarray(start, dtype=np.float32) + g = np.asarray(goal, dtype=np.float32) + ds = np.linalg.norm(foot - s, axis=1) + dg = np.linalg.norm(foot - g, axis=1) + if ds.min() > max_snap_m or dg.min() > max_snap_m: + return None + arcs = trajectory.arc_lengths() + near_s = np.flatnonzero(ds <= max_snap_m) + near_g = np.flatnonzero(dg <= max_snap_m) + totals = ( + np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) + + ds[near_s][:, None] + + dg[near_g][None, :] + ) + best = np.unravel_index(totals.argmin(), totals.shape) + i = int(near_s[best[0]]) + j = int(near_g[best[1]]) + # Orient the slice start-to-goal so the route reads in the case's direction. + route = foot[i : j + 1] if i <= j else foot[j : i + 1][::-1] + return route.astype(np.float32) + + def spl(success: bool, l_ref: float, p_len: float) -> float: if not success: return 0.0 diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index bbcefd459e..93f8902fef 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -14,15 +14,10 @@ """Browser point-picking for case curation, served by viser. -Opens a dark-themed local web viewer with the final map, the walked path, -and every case already in the manifest as a collapsed, editable panel entry. -Clicking a pair's endpoint sphere in the scene highlights the pair, opens -its panel entry, and scrolls to it. The show button inside each entry -highlights its pair in the scene. Shift+click picks new start/goal pairs. -Every entry has the coordinates, a name field, geometry-suggested tag -checkboxes, custom tags, a negative toggle, and save/delete buttons, so any -case can be renamed, retagged, flipped, or removed. Plain clicks and drags -only move the camera. +Serves the final map, the walked path, and every existing case as an editable +panel entry. Shift+click picks new start/goal pairs. Each entry exposes the +coordinates, a name field, tag checkboxes, a negative toggle, and save/delete +buttons, so any case can be renamed, retagged, flipped, or removed. """ from __future__ import annotations @@ -33,7 +28,7 @@ import numpy as np -from dimos.navigation.nav_3d.evaluator.generate import ( +from dimos.navigation.nav_3d.evaluator.tagging import ( LONG_STAIRS_DZ_M, LONG_STAIRS_WALKED_M, STAIRS_DZ_M, @@ -96,7 +91,7 @@ ) # White scene lights, bright enough that inverse-tone-mapped albedos fit # in [0, 1]. LIGHT_REFERENCE is the ambient plus directional total on a -# typical face; faces above or below it shade brighter or darker. +# typical face. Faces above or below it shade brighter or darker. _AMBIENT_INTENSITY = 3.5 _DIRECTIONAL_INTENSITY = 2.0 _LIGHT_REFERENCE = 4.6 @@ -299,8 +294,8 @@ def _(_event: object) -> None: @self.button.on_click def _(_event: object) -> None: - # save_unsaved calls save_or_update already holding the lock; - # the button path runs on a bare viser callback thread and must + # save_unsaved calls save_or_update already holding the lock. + # The button path runs on a bare viser callback thread and must # take it to serialize suite/manifest mutation. with self._hooks.lock: self.save_or_update() @@ -337,7 +332,7 @@ def extra_tags(self) -> list[str]: tags += [t.strip() for t in self.custom_text.value.split(",") if t.strip()] return tags - def save_or_update(self) -> None: + def save_or_update(self) -> bool: name = self.id_text.value.strip() if self.saved_id is None: ok, msg, saved, tags = self._hooks.save_pair( @@ -354,7 +349,7 @@ def save_or_update(self) -> None: print(msg) if not (ok and saved is not None): self.message.content = f"**FAILED**: {msg}" - return + return False # Viser cannot collapse a live panel, so replace it with the # collapsed button form, synced from the authoritative save. self.saved_id = saved @@ -366,6 +361,7 @@ def save_or_update(self) -> None: order = self.panel.order self.panel.remove() self._build(expanded=False, order=order) + return True def pick_cases( @@ -541,11 +537,9 @@ def _(_event: object) -> None: if entry.saved_id is not None: print(f"{entry.saved_id} stays in the manifest; use delete to remove it") - def save_unsaved() -> None: + def save_unsaved() -> int: with lock: - for pair in pairs: - if pair.saved_id is None: - pair.save_or_update() + return sum(not pair.save_or_update() for pair in pairs if pair.saved_id is None) @save_all_button.on_click def _(_event: object) -> None: @@ -553,8 +547,8 @@ def _(_event: object) -> None: @exit_button.on_click def _(_event: object) -> None: - save_unsaved() - stop.set() + if save_unsaved() == 0: + stop.set() print("picker running; ctrl-c to exit (unsaved pairs are discarded)") try: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 05876b18bb..5c9de7a0ac 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -14,16 +14,12 @@ """Run case suites through the ray tracer and MLS planner and score them. -Auto cases, derived from the walked trajectory, are planned twice: online on -the incremental map built up to the case's start time, and final on the map -fed the whole recording. Manual and certified-infeasible cases have no plan -time on the recording, so they are planned once, on the final map only. The -final map is not ground truth, only the most complete map the pipeline makes. -The final path is gated against full final occupancy, the online path against -the incremental map the planner had at plan time, so obstacles the sensor had -not yet mapped never count. Every path must also stand on final-map occupancy -and stay within the climb envelope. The headline score is validity-gated SPL -on the incremental map. +Auto cases plan twice: online on the incremental map at the case start time, +and final on the map fed the whole recording. Manual and infeasible cases plan +once on the final map. The final path is gated against full final occupancy, +the online path against the incremental map at plan time. Every path must stand +on final-map occupancy and stay within the climb envelope. The headline score +is validity-gated SPL on the incremental map. """ from __future__ import annotations @@ -157,7 +153,7 @@ class Report: score_soft: float final_score: float n_cases: int - # Cases with an online phase; the incremental score is over these only. + # Cases with an online phase. The incremental score is over these only. n_online: int n_success: int n_success_final: int @@ -283,11 +279,10 @@ def _dynamic_candidate( ) -> tuple[bool, list[list[float]]]: """Flag a case whose online route is blocked only by new final occupancy. - An online success paired with a final failure is either a dynamic obstacle - that appeared after the robot passed or a planner or mapping bug. Gating - the online path against the voxels the final map gained since plan time - tells the two apart. If that newly-occupied set alone blocks the route, a - real obstacle appeared. A human still confirms before labeling the case. + An online success with a final failure is either a dynamic obstacle that + appeared after the robot passed or a planner or mapping bug. Gating the + online path against the voxels gained since plan time tells them apart. A + human confirms before labeling the case. """ if online_wp is None or not online.success or final.success: return False, [] @@ -333,7 +328,7 @@ def run_suite( final = load_or_build_final_map(db_path, suite, cfg) obstacle_keys = final.occupied_keys - final_only = np.array([_final_only(c) for c in suite.cases]) + final_only = np.array([_final_only(c) for c in suite.cases], dtype=bool) refs: list[metrics.Reference] = [] for i, case in enumerate(suite.cases): if case.expect_fail: diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py new file mode 100644 index 0000000000..7707e15310 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -0,0 +1,225 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deterministic geometric tags for a case's demonstrated route. + +Every threshold is a physical measure of the robot from EvalConfig, not a +difficulty knob. The tags mark the terrain a case exercises so a run can filter +to the hard ones (stairs, narrow passages, doorways) while keeping flat +coverage. Elevation comes from the endpoints. Path-shape tags describe the local +terrain and only apply when the demonstrated route runs roughly straight from +start to goal, so a long building detour cannot mislabel a case. Tagging never +touches a case's weight. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.final_map import ( + densify, + keys_contain, + voxel_keys, +) +from dimos.navigation.nav_3d.evaluator.metrics import MARGIN_CAP_M, body_frames + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + +# A pair of endpoints this far apart in z or beyond is a climb, not a flat +# traverse. Half the body height, the smallest step the elevation tags care to +# call stairs. +STAIRS_DZ_M = 0.5 +# A climb earns "long" past either bound: a tall total rise or a long walk. +LONG_STAIRS_DZ_M = 1.5 +LONG_STAIRS_WALKED_M = 20.0 +# A sustained narrow stretch this long is a corridor, not a doorway. +CORRIDOR_RUN_M = 2.0 +# A doorway pinch is no longer than this. Beyond it the passage is a corridor. +DOORWAY_MAX_RUN_M = 1.2 +# Open space must reappear within this arc on both sides of a pinch for it to be +# a doorway rather than a dead-end narrowing. +DOORWAY_FLANK_M = 1.4 +# A door frame makes the width wobble across the threshold, splitting one pinch +# into fragments. Merge narrow runs separated by gaps this small so a sharp +# doorway reads as one passage, not several. +NARROW_MERGE_GAP_M = 0.2 +# A narrow run shorter than this is a single stray voxel, not a passage. +NARROW_MIN_RUN_M = 0.15 +# Path-shape tags describe the local terrain between the endpoints, so they only +# apply when the demonstrated route runs roughly straight from start to goal. A +# route far longer than the straight line is a detour through the building, and +# one far shorter is a stub that never spans the endpoints. Neither describes +# the case. Kept strict for precision: a doorway tag the filter can trust is +# worth more than catching every winding-route doorway. +LOCAL_DETOUR_MAX = 2.0 +LOCAL_SPAN_MIN_FRAC = 0.8 + +# The tags this module owns and recomputes. Everything else on a case, such as +# auto, manual, negative, or dynamic provenance, is left untouched by a retag. +GEOMETRIC_TAGS = frozenset( + {"flat", "up", "down", "stairs", "long", "narrow", "doorway", "corridor"} +) + + +def _elevation_tags( + start: tuple[float, float, float], goal: tuple[float, float, float] +) -> list[str]: + """Elevation from the case endpoints, matching how generation labels them. + + Endpoints, not the recovered route: the route can wander far off the + straight line, and a case's climb is defined by where it starts and ends. + """ + dz = goal[2] - start[2] + euclid = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) + if abs(dz) < STAIRS_DZ_M: + return ["flat"] + tags = ["stairs", "up" if dz > 0 else "down"] + if abs(dz) >= LONG_STAIRS_DZ_M or euclid >= LONG_STAIRS_WALKED_M: + tags.append("long") + return tags + + +def _corridor_width( + route: NDArray[np.float32], occupied_keys: NDArray[np.int64], cfg: EvalConfig +) -> NDArray[np.float64]: + """Free lateral width at each densified sample, at body height. + + Probes outward along both body-lateral directions from a point chest-high + over the path and returns left-plus-right distance to the nearest occupied + voxel, capped when the passage is open. This is the room the body has to + pass, not the room the feet have to stand. + """ + samples = densify(route, cfg.voxel_size) + _, lateral, _ = body_frames(samples, cfg.robot_length) + mid_z = (cfg.ground_margin + cfg.body_clearance) / 2.0 + origin = samples.astype(np.float64) + np.array([0.0, 0.0, mid_z]) + max_probe = cfg.robot_length + MARGIN_CAP_M + steps = np.arange(cfg.voxel_size, max_probe + cfg.voxel_size, cfg.voxel_size) + + def side_dist(sign: float) -> NDArray[np.float64]: + pts = origin[:, None, :] + sign * steps[None, :, None] * lateral[:, None, :] + hit = keys_contain(occupied_keys, voxel_keys(pts.reshape(-1, 3), cfg.voxel_size)) + hit = hit.reshape(len(samples), len(steps)) + any_hit = hit.any(axis=1) + first = np.where(any_hit, steps[hit.argmax(axis=1)], max_probe) + return first + + return side_dist(1.0) + side_dist(-1.0) + + +def _runs(mask: NDArray[np.bool_], arc: NDArray[np.float64]) -> list[tuple[float, float]]: + """Arc-length (start, end) of every maximal True run in mask.""" + out: list[tuple[float, float]] = [] + i, n = 0, len(mask) + while i < n: + if mask[i]: + j = i + 1 + while j < n and mask[j]: + j += 1 + out.append((float(arc[i]), float(arc[j - 1]))) + i = j + else: + i += 1 + return out + + +def _merge(runs: list[tuple[float, float]], gap: float) -> list[tuple[float, float]]: + """Join runs separated by less than gap of arc length.""" + merged: list[tuple[float, float]] = [] + for lo, hi in runs: + if merged and lo - merged[-1][1] <= gap: + merged[-1] = (merged[-1][0], hi) + else: + merged.append((lo, hi)) + return merged + + +def _corridor_tags( + route: NDArray[np.float32], occupied_keys: NDArray[np.int64], cfg: EvalConfig +) -> list[str]: + if len(occupied_keys) == 0 or len(route) < 2: + return [] + width = _corridor_width(route, occupied_keys, cfg) + tight = cfg.robot_width + 2.0 * MARGIN_CAP_M + roomy = cfg.robot_width + 2.0 * cfg.robot_length + samples = densify(route, cfg.voxel_size) + arc = np.concatenate([[0.0], np.cumsum(np.linalg.norm(np.diff(samples, axis=0), axis=1))]) + # A real passage is at least the robot's own body wide. Anything tighter is + # furniture or map noise the robot could not have walked through, so it does + # not count as a passage. + narrow = (width >= cfg.robot_width) & (width < tight) + runs = [ + (lo, hi) + for lo, hi in _merge(_runs(narrow, arc), NARROW_MERGE_GAP_M) + if hi - lo >= NARROW_MIN_RUN_M + ] + if not runs: + return [] + if max(hi - lo for lo, hi in runs) >= CORRIDOR_RUN_M: + return ["corridor", "narrow"] + # A doorway is a short pinch with open space reappearing on both sides. + for lo, hi in runs: + before = width[(arc >= lo - DOORWAY_FLANK_M) & (arc < lo)] + after = width[(arc > hi) & (arc <= hi + DOORWAY_FLANK_M)] + if ( + hi - lo <= DOORWAY_MAX_RUN_M + and bool((before >= roomy).any()) + and bool((after >= roomy).any()) + ): + return ["doorway", "narrow"] + return ["narrow"] + + +def _is_local( + route: NDArray[np.float32], + start: tuple[float, float, float], + goal: tuple[float, float, float], + cfg: EvalConfig, +) -> bool: + """True when the route runs roughly straight from start to goal. + + Endpoints closer than a body length have no terrain to describe. Otherwise + the walked route must span the straight line without detouring far past it. + """ + eucl = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) + if eucl < cfg.robot_length: + return False + arc = float(np.linalg.norm(np.diff(route, axis=0), axis=1).sum()) + return LOCAL_SPAN_MIN_FRAC * eucl <= arc <= LOCAL_DETOUR_MAX * eucl + cfg.robot_length + + +def route_tags( + start: tuple[float, float, float], + goal: tuple[float, float, float], + route: NDArray[np.float32] | None, + occupied_keys: NDArray[np.int64], + cfg: EvalConfig, +) -> list[str]: + """Geometric tags for a case, in a stable order. + + Elevation comes from the endpoints. Path-shape tags describe the local + terrain between them and need a walked route that runs roughly straight from + start to goal, so they are skipped when the route is off the trajectory or a + long detour. Deterministic and free of provenance: the caller prepends auto + or manual. + """ + tags = _elevation_tags(start, goal) + if route is not None and len(route) >= 2 and _is_local(route, start, goal, cfg): + tags += _corridor_tags(route, occupied_keys, cfg) + return tags diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 69e45aa899..85c7d4d60f 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -54,6 +54,7 @@ _run_plan, score_negative, ) +from dimos.navigation.nav_3d.evaluator.tagging import GEOMETRIC_TAGS, route_tags if TYPE_CHECKING: from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner @@ -600,7 +601,7 @@ def test_load_suite(tmp_path: Path) -> None: def test_final_only_covers_manual_and_infeasible() -> None: - """Manual and infeasible cases skip the online phase; auto cases keep it.""" + """Manual and infeasible cases skip the online phase. Auto cases keep it.""" xyz = (0.0, 0.0, 0.0) assert not _final_only(Case(id="a", start=xyz, goal=xyz, tags=["auto", "flat"])) assert _final_only(Case(id="m", start=xyz, goal=xyz, tags=["manual", "flat"])) @@ -725,6 +726,124 @@ def test_tripwire_exact_differences() -> None: assert "length" in diffs[0] and "12.34" in diffs[0] +def _ywall(y: float, x_lo: float, x_hi: float) -> np.ndarray: + """A wall parallel to +x travel, at constant y, spanning body height.""" + xs, zs = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(0.05, 1.5, VOXEL)) + return np.stack([xs.ravel(), np.full(xs.size, y), zs.ravel()], axis=1, dtype=np.float32) + + +def _tag_cfg() -> EvalConfig: + return EvalConfig(voxel_size=VOXEL) + + +def _tags(route: np.ndarray, keys: np.ndarray) -> list[str]: + """Tag a synthetic route, taking its endpoints for elevation.""" + start = (float(route[0, 0]), float(route[0, 1]), float(route[0, 2])) + goal = (float(route[-1, 0]), float(route[-1, 1]), float(route[-1, 2])) + return route_tags(start, goal, route, keys, _tag_cfg()) + + +def test_ground_truth_route_returns_walked_slice() -> None: + """The route is the shortest walked slice between the endpoints, not a line.""" + out = np.stack([np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1) + detour = np.stack([np.full(101, 10.0), np.linspace(0, 6, 101), np.full(101, 0.3)], axis=1) + positions = np.concatenate([out, detour]).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 20, len(positions)), positions=positions) + route = metrics.ground_truth_route(traj, (0, 0, 0), (10, 6, 0), robot_height=0.3) + assert route is not None + # Foot level (sensor height removed) and it walks the full out-and-detour. + assert abs(route[0, 2]) < 1e-5 + assert metrics.path_length(route) == pytest.approx(16.0, abs=0.2) + assert metrics.ground_truth_route(traj, (0, 20, 0), (10, 6, 0), robot_height=0.3) is None + + +def test_ground_truth_route_orients_start_to_goal() -> None: + """The route runs start-to-goal even when the start was walked later, so its + elevation is not read backward.""" + xs = np.linspace(0, 10, 101) + positions = np.stack([xs, np.zeros(101), xs * 0.25], axis=1).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) + # Start high at x=8, goal low at x=2: a downhill traverse. + route = metrics.ground_truth_route(traj, (8, 0, 1.7), (2, 0, 0.2), robot_height=0.3) + assert route is not None + # Runs start-to-goal: x decreasing from ~8 to ~2, so elevation reads downhill. + assert route[0, 0] > 6.0 and route[-1, 0] < 4.0 + assert route[0, 0] > route[-1, 0] + assert route[-1, 2] < route[0, 2] + + +def test_route_tags_flat_wide_has_no_shape_tags() -> None: + """A wide flat traverse is just flat: no narrow, doorway, or corridor.""" + route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + walls = np.concatenate([_ywall(-2.0, 0, 4), _ywall(2.0, 0, 4)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + tags = _tags(route, keys) + assert "flat" in tags + assert "narrow" not in tags and "doorway" not in tags and "corridor" not in tags + + +def test_route_tags_narrow_passage() -> None: + """Walls under a body-plus-clearance apart the whole way make a corridor.""" + route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + tags = _tags(route, keys) + assert "narrow" in tags and "corridor" in tags + assert "doorway" not in tags # sustained squeeze, not a short pinch + assert "open" not in tags + + +def test_route_tags_doorway_is_a_short_pinch() -> None: + """An open corridor that pinches briefly and reopens is a doorway.""" + route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) + far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) + pinch = np.concatenate([_ywall(-0.35, 2.0, 2.6), _ywall(0.35, 2.0, 2.6)]) + keys = np.unique(voxel_keys(np.concatenate([far, pinch]), VOXEL)) + tags = _tags(route, keys) + assert "doorway" in tags and "narrow" in tags + + +def test_route_tags_sharp_doorway() -> None: + """A door frame is a sharp pinch: the narrow stretch is only a couple of + voxels, but flanked by open space it is still a doorway.""" + route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) + far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) + frame = np.concatenate([_ywall(-0.35, 2.4, 2.6), _ywall(0.35, 2.4, 2.6)]) + keys = np.unique(voxel_keys(np.concatenate([far, frame]), VOXEL)) + assert "doorway" in _tags(route, keys) + + +def test_route_tags_stairs_from_endpoints() -> None: + """Elevation comes from the endpoints: a climb past the threshold is stairs, + and a big rise is long, whatever the route in between does.""" + up = np.stack([np.linspace(0, 4, 40), np.zeros(40), np.linspace(0, 2.0, 40)], axis=1).astype( + np.float32 + ) + tags = _tags(up, np.array([], dtype=np.int64)) + assert "stairs" in tags and "up" in tags and "long" in tags + assert "down" in _tags(up[::-1].copy(), np.array([], dtype=np.int64)) + + +def test_route_tags_gate_excludes_detour_routes() -> None: + """Shape tags need a near-direct route. Between the same endpoints, a + straight walk through the corridor is tagged, but a long detour is not: + its terrain cannot be attributed to the case, so it gets elevation only.""" + walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + direct = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + assert "corridor" in route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), direct, keys, _tag_cfg()) + detour = np.array([[0, 0, 0], [2, 6, 0], [4, 0, 0]], dtype=np.float32) + assert route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), detour, keys, _tag_cfg()) == ["flat"] + + +def test_route_tags_are_all_geometric() -> None: + """Every tag the tagger emits is one a retag is allowed to recompute.""" + route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + assert set(_tags(route, keys)) <= GEOMETRIC_TAGS + + def test_tripwire_diff_names_every_flip() -> None: old = _tripwire_report( {"office": {"a": (False, True), "b": (True, True), "gone": (True, True)}} diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 88880c74bf..97135ed308 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -14,21 +14,11 @@ """Write an evaluation report into a rerun recording. -One static scene per dataset: -- map/obstacles: final voxels, turbo colormap by height -- walked_path: the recorded foot path (white) -- planner_final: the planner graph the full aggregated map produced. - Surface cells colored by wall clearance (gray inside the hard clearance), - edges colored white to red by log traversal cost. -- cases/: start (cyan), goal (orange), online and final planned paths - colored by verdict (green valid, red gate-invalid, yellow unreached), the - gate's body box at each collision (semi-transparent red, pitched with the - slope and elevated over the legs), unsupported samples (magenta), and - too-steep waypoints (purple). Every case carries a known/ layer: the - incremental voxel map (known/voxels, turbo by height) and planner graph - at plan time, what the robot knew then. Failed cases also get a thin red - start-to-goal intent line. Dynamic obstacle candidates get a new_obstacle - layer marking the final occupancy that blocks the online route. +One static scene per dataset: the final voxel map, the walked path, the +planner graph over the aggregated map, and per-case start/goal with the +online and final planned paths colored by verdict and the gate's collision +boxes. Each case also carries a known/ layer holding the incremental map and +planner graph at plan time. """ from __future__ import annotations @@ -76,8 +66,10 @@ def turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: # Lazy: matplotlib is a heavy viz-only dependency. import matplotlib.pyplot as plt + if len(points) == 0: + return np.zeros((0, 3), dtype=np.uint8) z = points[:, 2].astype(np.float64) - span = float(z.max() - z.min()) if len(z) else 0.0 + span = float(z.max() - z.min()) t = (z - z.min()) / max(span, 1e-6) return np.asarray(plt.get_cmap("turbo")(t)[:, :3] * 255, dtype=np.uint8) From b1507ddf4eaddcd115a30d1dfa759b33abcc657c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 20 Jul 2026 15:33:57 -0700 Subject: [PATCH 18/29] Remove bad code --- dimos/navigation/nav_3d/evaluator/PROGRAM.md | 92 -------------------- dimos/navigation/nav_3d/evaluator/cli.py | 8 +- dimos/navigation/nav_3d/evaluator/tagging.py | 14 ++- 3 files changed, 9 insertions(+), 105 deletions(-) delete mode 100644 dimos/navigation/nav_3d/evaluator/PROGRAM.md diff --git a/dimos/navigation/nav_3d/evaluator/PROGRAM.md b/dimos/navigation/nav_3d/evaluator/PROGRAM.md deleted file mode 100644 index 219478865e..0000000000 --- a/dimos/navigation/nav_3d/evaluator/PROGRAM.md +++ /dev/null @@ -1,92 +0,0 @@ -# Nav-3D Autonomous Improvement Protocol - -You are improving the 3D navigation stack: the MLS planner and the voxel ray -mapper. The evaluator in this package is your loss function. It replays real -robot recordings and scores planning on the maps the robot would have had at -the time. Higher is better. The score only counts if every gate below holds. - -## Boundary - -Editable: - -- `dimos/navigation/nav_3d/mls_planner/` (Python and Rust) -- `dimos/mapping/ray_tracing/` (Python and Rust) -- `results.tsv` at the repo root (your journal) - -Everything else is off limits, in particular `dimos/navigation/nav_3d/evaluator/` -(including `cases/` and this file). The gate parameters in `EvalConfig` are -physical measurements of the Unitree Go2 and its demonstrated capabilities, -not tuning knobs. Algorithm parameters are the constructor defaults of -`MLSPlanner` and `VoxelRayMapper`; tune them by editing those defaults. - -The human reviewer verifies the boundary mechanically against the commit -the session branched from (BASE = the parent branch, e.g. -andrew/feat/nav-evaluator): - - git diff --name-only $(git merge-base BASE HEAD) \ - | grep -vE '^(dimos/navigation/nav_3d/mls_planner/|dimos/mapping/ray_tracing/|results\.tsv)' - -Any output fails the whole session. - -## Setup - -Work on a fresh branch. Confirm the recordings exist (`data/*.db`), then run -the suite once and save the report as your first kept baseline: - - python -m dimos.navigation.nav_3d.evaluator run --json data/reports/kept.json - -## Iteration cycle - -1. Form one hypothesis and make one focused change. -2. Rebuild whichever Rust module you touched, always in release: - - uv run maturin develop --uv --release -m dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml - uv run maturin develop --uv --release -m dimos/mapping/ray_tracing/rust/Cargo.toml - - If you touched the mapper (code or defaults), also wipe the replay caches: - - rm -rf data/.final - - A mapper change makes the next run rebuild the maps (~5 minutes). Planner - changes skip that, so planner experiments are much cheaper. -3. Commit, then evaluate: - - python -m dimos.navigation.nav_3d.evaluator run --json data/reports/candidate.json - python -m dimos.navigation.nav_3d.evaluator diff data/reports/kept.json data/reports/candidate.json - -4. Keep or discard: - - Keep iff the score improved AND `diff` exits 0 (no case regressed). A - BROKE line means a start/goal pair the stack used to handle now fails; - a higher average does not excuse it. Perf budget lines on this parallel - run are advisory only; wall-clock under parallel contention runs about - 20 percent hot. - - Keep: pass the confirmation check below, then - `cp data/reports/candidate.json data/reports/kept.json` and keep the - commit. - - Discard: `git reset --hard HEAD^`. If you discarded a mapper change, - wipe `data/.final` again so caches match the reverted code. -5. Confirmation check, required before every keep. Rerun serially and - require bit-identical results and in-budget timings: - - python -m dimos.navigation.nav_3d.evaluator run --workers 1 --json data/reports/serial.json - python -m dimos.navigation.nav_3d.evaluator diff data/reports/candidate.json data/reports/serial.json --exact - - The serial run is slower but its timings are the binding perf gate: - uncontended wall-clock is what the robot experiences. A nonzero exit - means the change is non-reproducible or over budget. Fix it or discard. -6. Append one row to `results.tsv` and continue: - - commit_hashscorefinal_scoreplan_p95_msfixedbrokekeep|discard|crashone-line description - -## Rules - -- Crashes are experiments too: fix trivial bugs, abandon flawed ideas, always - journal the row. -- Never edit the evaluator, the case manifests, or the gate parameters. -- Never weaken determinism to gain score. -- The unit tests of the modules you edit must pass: - - python -m pytest dimos/navigation/nav_3d/mls_planner/ dimos/mapping/ray_tracing/ -q - -- Do not add dependencies. -- Run continuously without asking for approval until interrupted. diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 3d67f891d3..65f4f61871 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -163,10 +163,10 @@ def diff_reports( ) -> None: """Name every case whose pass/fail flipped between two runs. - Exits 1 when any case regressed, so a keep/discard loop can gate on it. - Perf budget breaches always print but only exit 1 with --exact, which also - exits 1 on any non-timing difference. Running the suite twice and - exact-diffing the reports is the determinism check. + Exits 1 when any case regressed, so a CI check or before/after comparison + can gate on it. Perf budget breaches always print but only exit 1 with + --exact, which also exits 1 on any non-timing difference. Running the suite + twice and exact-diffing the reports is the determinism check. """ old_report = json.loads(old.read_text()) new_report = json.loads(new.read_text()) diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py index 7707e15310..7f9b66311e 100644 --- a/dimos/navigation/nav_3d/evaluator/tagging.py +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -12,15 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Deterministic geometric tags for a case's demonstrated route. - -Every threshold is a physical measure of the robot from EvalConfig, not a -difficulty knob. The tags mark the terrain a case exercises so a run can filter -to the hard ones (stairs, narrow passages, doorways) while keeping flat -coverage. Elevation comes from the endpoints. Path-shape tags describe the local -terrain and only apply when the demonstrated route runs roughly straight from -start to goal, so a long building detour cannot mislabel a case. Tagging never -touches a case's weight. +"""Geometric tags for a case: elevation, narrow, doorway, corridor. + +Elevation tags come from the endpoints. Shape tags measure corridor width along +the demonstrated route and apply only when it runs roughly straight from start +to goal. Thresholds come from the robot's dimensions in EvalConfig. """ from __future__ import annotations From f8d918e4735541124c95441eb25c2aea4121a402 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 20 Jul 2026 16:13:46 -0700 Subject: [PATCH 19/29] Refactor and remove unused code --- dimos/navigation/nav_3d/evaluator/cases.py | 15 +- .../nav_3d/evaluator/cases/china_office.yaml | 35 ---- .../evaluator/cases/mid360_athens_stairs.yaml | 11 - .../nav_3d/evaluator/cases/sf_office.yaml | 63 +----- dimos/navigation/nav_3d/evaluator/cli.py | 197 +++--------------- dimos/navigation/nav_3d/evaluator/config.py | 14 +- dimos/navigation/nav_3d/evaluator/curation.py | 145 +++++++++++++ .../navigation/nav_3d/evaluator/final_map.py | 134 ++---------- dimos/navigation/nav_3d/evaluator/generate.py | 34 +-- dimos/navigation/nav_3d/evaluator/metrics.py | 170 ++++++++------- dimos/navigation/nav_3d/evaluator/picker.py | 118 ++++------- dimos/navigation/nav_3d/evaluator/runner.py | 126 +++++------ dimos/navigation/nav_3d/evaluator/tagging.py | 42 ++-- .../nav_3d/evaluator/test_evaluator.py | 90 +++----- dimos/navigation/nav_3d/evaluator/viz.py | 70 ++----- .../navigation/nav_3d/evaluator/voxel_keys.py | 74 +++++++ 16 files changed, 552 insertions(+), 786 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/curation.py create mode 100644 dimos/navigation/nav_3d/evaluator/voxel_keys.py diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index 79fe533bf8..9fe578af57 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -35,15 +35,11 @@ class Case: id: str start: tuple[float, float, float] goal: tuple[float, float, float] - weight: float = 1.0 tags: list[str] = field(default_factory=list) - l_ref: float | None = None - # Human-certified infeasible pair: the correct answer is to refuse. - # Evaluated on the final map only, scored 1.0 for refusal. + # If the pair should not have a valid path, this is assigned by humans expect_fail: bool = False - # A route the robot walked that a later dynamic obstacle blocked, e.g. a - # door that closed. The online plan is expected to succeed, the final - # plan is expected to refuse and is scored 1.0 for refusal. + # Some routes are passable with the incremental map but not passable in the final. + # For example a door is open, robot walks through, then it gets closed. expect_final_fail: bool = False @@ -85,9 +81,7 @@ def load_suite(path: Path) -> Suite: id=str(entry["id"]), start=(sx, sy, sz), goal=(gx, gy, gz), - weight=float(entry.get("weight", 1.0)), tags=[str(t) for t in entry.get("tags", [])], - l_ref=float(entry["l_ref"]) if "l_ref" in entry else None, expect_fail=expect_fail, expect_final_fail=expect_final_fail, ) @@ -130,11 +124,8 @@ def save_suite(suite: Suite, path: Path | None = None) -> Path: "id": case.id, "start": [round(float(v), 3) for v in case.start], "goal": [round(float(v), 3) for v in case.goal], - "weight": case.weight, "tags": case.tags, } - if case.l_ref is not None: - entry["l_ref"] = round(case.l_ref, 3) if case.expect_fail: entry["expect_fail"] = True if case.expect_final_fail: diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml index 3545c94d6b..f22f67d7de 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml @@ -3,176 +3,141 @@ cases: - id: auto_00_up start: [-10.2, 15.8, -0.48] goal: [-8.12, 14.6, 2.56] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_01_down start: [-3.72, -4.6, 2.96] goal: [0.04, 0.04, -0.4] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_02_up start: [4.12, -21.72, -0.48] goal: [9.56, -14.28, 4.24] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_03_down start: [2.52, 10.84, 3.12] goal: [5.64, 23.64, -0.24] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_04_down start: [10.44, -4.12, 4.16] goal: [16.52, -32.28, -0.96] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_05_up start: [18.2, -20.6, -1.36] goal: [-1.4, -14.2, 2.96] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_06_down start: [-4.6, 4.52, 6.32] goal: [11.72, 6.36, -0.96] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_07_down start: [6.36, 2.6, 3.2] goal: [15.4, -9.88, -1.28] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_08_up start: [7.72, -33.8, -0.72] goal: [-3.32, 11.32, 2.88] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_09_down start: [12.2, -9.24, 4.16] goal: [3.56, -7.72, -0.4] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_10_down start: [-7.4, 7.32, 2.8] goal: [9.8, 14.04, -0.56] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_11_down start: [-2.52, 0.68, 4.0] goal: [13.4, -1.16, -1.2] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_12_down start: [6.28, -2.04, 4.16] goal: [3.32, 16.12, -0.24] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_13_down start: [-1.72, -9.72, 2.96] goal: [5.64, -13.56, -0.4] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_14_up start: [7.48, -27.48, -0.48] goal: [-6.2, 3.48, 2.88] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_15_up start: [19.8, -27.8, -1.12] goal: [2.6, -16.2, 2.08] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_16_up start: [0.6, 6.6, -0.4] goal: [2.04, -0.28, 3.12] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_17_down start: [4.28, 5.96, 3.12] goal: [17.0, -15.48, -1.28] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_18_down start: [-4.6, 8.2, 6.56] goal: [8.84, 18.84, -0.4] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_19_up start: [-8.84, 10.76, -0.48] goal: [-1.4, -2.04, 3.12] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_20_up start: [14.6, -27.88, -0.24] goal: [1.0, 13.56, 2.96] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_21_down start: [-7.4, 11.96, 3.04] goal: [8.2, -17.32, -0.4] - weight: 1.0 tags: [auto, stairs, down, long, doorway, narrow] - id: auto_22_up start: [-0.36, -4.44, -0.4] goal: [11.24, -6.76, 4.16] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_23_up start: [-0.28, 10.92, -0.4] goal: [0.6, -12.52, 3.12] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_24_flat start: [12.52, -33.4, -0.8] goal: [14.52, -5.96, -1.28] - weight: 1.0 tags: [auto, flat] - id: auto_25_flat start: [10.68, 9.88, -0.72] goal: [5.24, 12.52, -0.24] - weight: 1.0 tags: [auto, flat] - id: auto_26_flat start: [19.16, -24.44, -1.36] goal: [12.6, 2.68, -1.04] - weight: 1.0 tags: [auto, flat] - id: auto_27_flat start: [3.56, -18.36, 0.16] goal: [8.92, -13.32, -0.32] - weight: 1.0 tags: [auto, flat] - id: auto_28_flat start: [-4.92, 11.0, -0.4] goal: [10.6, -34.92, -0.8] - weight: 1.0 tags: [auto, flat] - id: auto_29_flat start: [-0.12, -17.08, 2.08] goal: [2.76, 7.4, 1.6] - weight: 1.0 tags: [auto, flat] - id: auto_30_flat start: [6.04, -24.2, -0.48] goal: [7.0, 14.12, -0.08] - weight: 1.0 tags: [auto, flat] - id: auto_31_flat start: [8.36, -30.44, -0.48] goal: [18.28, -32.28, -0.96] - weight: 1.0 tags: [auto, flat] - id: auto_32_flat start: [11.32, 8.12, -0.88] goal: [5.72, -9.72, -0.4] - weight: 1.0 tags: [auto, flat] - id: neg_00 start: [5.0, 2.44, 3.2] goal: [9.32, 5.24, 6.4] - weight: 1.0 tags: [manual, negative] expect_fail: true - id: manual_00 start: [13.88, 1.48, -1.04] goal: [12.28, 8.6, -0.8] - weight: 1.0 tags: [manual, flat] diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index ca71e87124..3fa8eeec83 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -3,56 +3,45 @@ cases: - id: auto_00_up start: [-0.12, -0.6, -0.32] goal: [1.32, -0.84, 2.72] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_01_up start: [7.24, -3.96, -6.08] goal: [6.44, -5.56, -1.44] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_02_down start: [8.04, -0.76, 3.04] goal: [-2.36, -4.36, -0.32] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_03_down start: [5.88, -4.52, 2.96] goal: [2.28, -6.52, -0.32] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_04_down start: [-0.2, -3.0, 2.56] goal: [-2.04, 3.16, -0.48] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_05_down start: [0.52, -4.68, -0.32] goal: [5.96, -3.64, -3.52] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_06_up start: [6.36, -5.4, -4.48] goal: [-2.36, 0.44, -0.4] - weight: 1.0 tags: [auto, stairs, up, long] - id: auto_07_down start: [6.68, -5.56, 2.08] goal: [5.72, -5.48, -1.04] - weight: 1.0 tags: [auto, stairs, down, long, narrow] - id: auto_08_down start: [-0.2, -3.0, 2.56] goal: [7.16, -3.8, -3.12] - weight: 1.0 tags: [auto, stairs, down, long] - id: auto_09_flat start: [5.08, -3.8, -6.48] goal: [7.24, -3.96, -6.08] - weight: 1.0 tags: [auto, flat] - id: manual_00 start: [1.8, -4.44, -0.32] goal: [5.24, -4.2, -0.32] - weight: 1.0 tags: [manual, flat, doorway, negative] expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml index e6a9a47e95..6ca7199e84 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml @@ -4,315 +4,254 @@ cases: - id: auto_00_flat start: [-1.64, -2.52, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat, doorway, narrow] - id: auto_01_flat start: [-2.84, 8.44, -0.24] goal: [3.64, 7.64, -0.16] - weight: 1.0 - tags: [auto, flat] + tags: [manual, flat, doorway, auto] - id: auto_02_flat start: [6.44, 0.44, -0.16] goal: [6.68, -3.0, 0.08] - weight: 1.0 tags: [auto, flat] - id: auto_03_flat start: [8.76, 4.36, -0.16] goal: [10.84, -0.84, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_04_flat start: [-2.2, 3.0, -0.16] goal: [3.56, -1.32, 0.08] - weight: 1.0 tags: [auto, flat] - id: auto_05_flat start: [-0.04, -0.04, -0.16] goal: [8.92, -0.52, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_06_flat start: [-0.2, 4.2, -0.08] goal: [0.04, 8.04, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_07_flat start: [-2.84, 8.44, -0.24] goal: [-0.68, 8.2, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_08_flat start: [3.72, 2.28, -0.16] goal: [12.6, -0.36, 0.24] - weight: 1.0 tags: [auto, flat, narrow] - id: auto_09_flat start: [11.72, 3.88, -0.08] goal: [4.92, 4.76, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_10_flat start: [13.4, 0.2, 0.24] goal: [2.12, 0.2, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_11_flat start: [7.0, 3.8, -0.16] goal: [5.0, -3.56, 0.08] - weight: 1.0 tags: [auto, flat] - id: auto_12_flat start: [-3.56, 5.16, -0.24] goal: [13.96, 2.28, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_13_flat start: [13.8, 1.56, 0.32] goal: [6.04, 2.28, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_14_flat start: [9.8, -0.68, 0.24] goal: [5.16, 6.68, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_15_flat start: [11.64, -0.76, 0.24] goal: [1.96, 7.72, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_16_flat start: [7.96, -0.2, 0.08] goal: [15.64, 1.32, -0.08] - weight: 1.0 tags: [auto, flat] - id: auto_17_flat start: [5.32, 2.84, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_18_flat start: [10.6, 4.04, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_19_flat start: [-0.44, 0.76, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_20_flat start: [14.28, 3.48, -0.08] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_21_flat start: [7.96, 4.12, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_22_flat start: [13.96, 2.28, 0.24] goal: [3.8, 0.36, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_23_flat start: [1.88, 0.84, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_24_flat start: [4.36, 1.48, -0.16] goal: [8.92, -0.52, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_25_flat start: [12.68, 3.56, -0.08] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_26_flat start: [3.64, 1.72, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_27_flat start: [13.88, 1.96, 0.24] goal: [-3.24, 6.2, -0.24] - weight: 1.0 tags: [auto, flat] - id: auto_28_flat start: [5.16, 2.04, -0.16] goal: [10.84, -0.84, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_29_flat start: [12.6, -0.36, 0.24] goal: [-1.64, 5.0, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_30_flat start: [1.0, 3.88, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_31_flat start: [6.36, 3.56, -0.16] goal: [8.92, -0.52, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_32_flat start: [-0.44, -0.04, -0.16] goal: [3.64, -2.36, 0.08] - weight: 1.0 tags: [auto, flat] - id: auto_33_flat start: [-1.8, 2.2, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_34_flat start: [6.92, 4.36, -0.16] goal: [10.84, -0.84, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_35_flat start: [11.64, -0.76, 0.24] goal: [3.0, -0.68, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_36_flat start: [7.08, -0.12, 0.0] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_37_flat start: [13.8, 1.56, 0.32] goal: [8.92, 4.44, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_38_flat start: [-2.36, 4.04, -0.16] goal: [13.8, 1.56, 0.32] - weight: 1.0 tags: [auto, flat] - id: auto_39_flat start: [13.8, 1.56, 0.32] goal: [6.44, 0.44, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_40_flat start: [-1.64, -2.52, -0.16] goal: [11.96, -0.68, 0.24] - weight: 1.0 tags: [auto, flat, doorway, narrow] - id: auto_41_flat start: [13.88, 1.96, 0.24] goal: [-3.64, 4.36, -0.24] - weight: 1.0 tags: [auto, flat] - id: auto_42_flat start: [-1.64, -2.52, -0.16] goal: [9.56, -0.76, 0.24] - weight: 1.0 tags: [auto, flat] - id: auto_43_flat start: [13.8, 1.56, 0.32] goal: [-1.16, 4.68, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_44_flat start: [13.8, 1.56, 0.32] goal: [11.32, 3.72, -0.08] - weight: 1.0 tags: [auto, flat] - id: auto_45_flat start: [0.12, 0.04, -0.16] goal: [10.84, -0.84, 0.24] - weight: 1.0 tags: [auto, flat, narrow] - id: auto_46_flat start: [-0.44, 0.76, -0.16] goal: [12.76, -0.36, 0.24] - weight: 1.0 tags: [auto, flat, narrow] - id: auto_47_flat start: [13.88, 1.96, 0.24] goal: [-1.32, 7.64, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_48_flat start: [13.8, 1.56, 0.32] goal: [0.36, 4.28, -0.16] - weight: 1.0 tags: [auto, flat] - id: auto_49_flat start: [6.04, 1.32, -0.16] goal: [7.88, -0.36, 0.08] - weight: 1.0 tags: [auto, flat] - id: neg_00 start: [-1.72, 4.52, -0.16] goal: [-1.32, 2.76, 0.88] - weight: 1.0 tags: [manual, negative, stairs, up] expect_fail: true - id: neg_01 start: [-1.0, 8.6, -0.24] goal: [-0.84, 6.68, 0.48] - weight: 1.0 tags: [manual, negative, stairs, up] expect_fail: true - id: neg_02 start: [5.08, 1.32, -0.16] goal: [7.64, 1.4, 0.56] - weight: 1.0 tags: [manual, negative, stairs, up] expect_fail: true - id: manual_00 start: [9.72, -2.84, 0.24] goal: [12.6, 0.04, 0.24] - weight: 1.0 tags: [manual, flat] - id: neg_03 start: [2.12, 3.8, -0.16] goal: [2.12, 4.44, 1.12] - weight: 1.0 tags: [manual, negative, stairs, up] expect_fail: true - id: neg_04 start: [0.76, 6.44, -0.16] goal: [0.04, 6.92, -0.24] - weight: 1.0 tags: [manual, negative, flat] expect_fail: true - id: neg_05 start: [-1.56, -0.2, -0.16] goal: [-2.36, -0.76, 0.8] - weight: 1.0 tags: [manual, negative, stairs, up] expect_fail: true - id: neg_06 start: [-1.08, -2.6, 2.24] goal: [-1.88, 0.52, -0.16] - weight: 1.0 tags: [manual, negative, stairs, down, long] expect_fail: true - id: neg_07 start: [4.28, -3.48, 0.0] goal: [4.68, -1.32, 0.08] - weight: 1.0 tags: [manual, negative, flat] expect_fail: true - id: neg_08 start: [3.48, -2.84, 0.0] goal: [4.68, -1.8, 1.12] - weight: 1.0 tags: [manual, negative, stairs, up] expect_fail: true - id: neg_09 start: [4.6, -2.2, 1.12] goal: [6.52, -4.28, 0.08] - weight: 1.0 tags: [stairs, down, manual, negative] expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 65f4f61871..1c609e365c 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -32,7 +32,6 @@ import contextlib import dataclasses -import itertools import json import os from pathlib import Path @@ -45,19 +44,15 @@ from dimos.navigation.nav_3d.evaluator import tripwire from dimos.navigation.nav_3d.evaluator.cases import ( CASES_DIR, - Case, Suite, load_suite, load_suites, save_suite, ) from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.curation import CurationError, load_store from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map -from dimos.navigation.nav_3d.evaluator.generate import ( - GenerationParams, - generate_cases, - snap_to_surface, -) +from dimos.navigation.nav_3d.evaluator.generate import GenerationParams, generate_cases from dimos.navigation.nav_3d.evaluator.metrics import ground_truth_route from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate @@ -65,19 +60,24 @@ from dimos.utils.data import get_data_dir if TYPE_CHECKING: - from numpy.typing import NDArray - + from dimos.navigation.nav_3d.evaluator.curation import CaseStore + from dimos.navigation.nav_3d.evaluator.final_map import FinalMap from dimos.navigation.nav_3d.evaluator.runner import PlanOutcome app = typer.Typer(no_args_is_help=True, add_completion=False) def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: - fields = {f.name: f.type for f in dataclasses.fields(EvalConfig)} + fields = {f.name for f in dataclasses.fields(EvalConfig)} for spec in overrides: if "=" not in spec: raise typer.BadParameter(f"--set expects name=value, got {spec!r}") name, value = spec.split("=", 1) + if name.startswith("planner."): + # Planner constructor arguments are validated by the planner, which + # owns their names and defaults. + cfg.planner[name.removeprefix("planner.")] = float(value) + continue if name not in fields: raise typer.BadParameter(f"unknown config field {name!r}") current = getattr(cfg, name) @@ -267,20 +267,6 @@ def _copy_recording(src: Path, dest: Path) -> None: source.backup(target) -def _snap_or_fail( - label: str, - point: tuple[float, float, float], - surface: NDArray[np.float32], - snap_max_m: float, -) -> tuple[float, float, float]: - snapped = snap_to_surface(np.asarray(point, dtype=np.float32), surface, snap_max_m) - if snapped is None: - raise typer.BadParameter( - f"{label} {point} is more than {snap_max_m}m from any standable surface" - ) - return (float(snapped[0]), float(snapped[1]), float(snapped[2])) - - @app.command() def ingest( source: Path = typer.Argument( @@ -350,63 +336,15 @@ def ingest( path = save_suite(suite, manifest) print(f"\n{len(suite.cases)} cases -> {path}") for case in suite.cases: - print(f" {case.id}: w={case.weight:g} [{', '.join(case.tags)}]") + print(f" {case.id}: [{', '.join(case.tags)}]") print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {name}") -def _append_case( - suite: Suite, - manifest: Path, - surface: NDArray[np.float32], - start: tuple[float, float, float], - goal: tuple[float, float, float], - case_id: str | None, - tags: list[str], - weight: float, - snap_max: float, - expect_fail: bool, -) -> Case: - prefix = "neg" if expect_fail else "manual" - snapped_goal = snap_to_surface(np.asarray(goal, dtype=np.float32), surface, snap_max) - if snapped_goal is not None: - goal = (float(snapped_goal[0]), float(snapped_goal[1]), float(snapped_goal[2])) - elif expect_fail: - # An infeasible goal may sit on geometry with no standable surface. - print(f"note: goal {goal} is off any standable surface; keeping it as picked") - else: - raise typer.BadParameter(f"goal {goal} is more than {snap_max}m from standable surface") - if case_id is None: - existing = {c.id for c in suite.cases} - case_id = next( - f"{prefix}_{n:02d}" for n in itertools.count() if f"{prefix}_{n:02d}" not in existing - ) - case = Case( - id=case_id, - start=_snap_or_fail("start", start, surface, snap_max), - goal=goal, - weight=weight, - tags=tags, - expect_fail=expect_fail, - ) - if any(c.id == case.id for c in suite.cases): - raise typer.BadParameter(f"case id {case.id!r} already exists in {manifest}") - suite.cases.append(case) - save_suite(suite, manifest) - kind = "negative (must refuse)" if expect_fail else "positive" - print(f"added {kind} {case.id}: {case.start} -> {case.goal} to {manifest}") - return case - - -def _load_for_curation(dataset: str) -> tuple[Suite, Path, NDArray[np.float32], EvalConfig]: - manifest = CASES_DIR / f"{dataset}.yaml" - if not manifest.exists(): - raise typer.BadParameter(f"no manifest {manifest}; run ingest first") - suite = load_suite(manifest) - cfg = EvalConfig() - final = load_or_build_final_map(suite.db_path(), suite, cfg) - planner = cfg.make_planner() - planner.update_global_map(final.occupied) - return suite, manifest, planner.surface_map(), cfg +def _open(dataset: str) -> tuple[CaseStore, FinalMap]: + try: + return load_store(dataset) + except CurationError as err: + raise typer.BadParameter(str(err)) from err @app.command("add-case") @@ -416,27 +354,22 @@ def add_case( goal: tuple[float, float, float] = typer.Option(..., "--goal", help="Foot-level xyz"), case_id: str = typer.Option(None, "--id", help="Case id; default manual_ or neg_"), tags: str = typer.Option(None, "--tags", help="Comma-separated tags"), - weight: float = typer.Option(1.0, "--weight"), - snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), expect_fail: bool = typer.Option( False, "--expect-fail", help="Certified-infeasible pair; the planner must refuse" ), ) -> None: """Append a curated case, with endpoints snapped to the final surface.""" - suite, manifest, surface, _ = _load_for_curation(dataset) - default_tags = "manual,negative" if expect_fail else "manual" - _append_case( - suite, - manifest, - surface, - start, - goal, - case_id, - [t.strip() for t in (tags or default_tags).split(",") if t.strip()], - weight, - snap_max, - expect_fail, - ) + store, _ = _open(dataset) + try: + store.add( + start, + goal, + [t.strip() for t in (tags or "").split(",") if t.strip()], + case_id=case_id, + expect_fail=expect_fail, + ) + except CurationError as err: + raise typer.BadParameter(str(err)) from err @app.command("tag") @@ -497,7 +430,7 @@ def retag( if "auto" not in case.tags: print(f" {case.id}: curated, tags kept [{', '.join(case.tags)}]") continue - route = ground_truth_route(trajectory, case.start, case.goal, cfg.robot_height) + route = ground_truth_route(trajectory, case.start, case.goal, cfg) if route is None: print(f" {case.id}: off-trajectory, tags kept [{', '.join(case.tags)}]") continue @@ -515,8 +448,6 @@ def retag( @app.command("pick-case") def pick_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the cases"), - weight: float = typer.Option(1.0, "--weight"), - snap_max: float = typer.Option(1.0, "--snap-max", help="Max snap distance to surface (m)"), ) -> None: """Pick and edit cases by shift+clicking the map in a browser viewer. @@ -529,90 +460,30 @@ def pick_case( from dimos.navigation.nav_3d.evaluator.picker import pick_cases from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height - suite, manifest, surface, cfg = _load_for_curation(dataset) - final = load_or_build_final_map(suite.db_path(), suite, cfg) - trajectory = load_trajectory(suite.db_path(), suite.odom_stream) - foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) - - def full_tags(negative: bool, extra: list[str]) -> list[str]: - tags = ["manual"] + (["negative"] if negative else []) - return tags + [t for t in extra if t not in tags] - - def save_pair( - start: tuple[float, float, float], - goal: tuple[float, float, float], - negative: bool, - tags: list[str], - case_id: str | None, - ) -> tuple[bool, str, str | None, list[str] | None]: - try: - case = _append_case( - suite, - manifest, - surface, - start, - goal, - case_id, - full_tags(negative, tags), - weight, - snap_max, - negative, - ) - except typer.BadParameter as err: - return False, str(err), None, None - return True, f"saved {case.id} [{', '.join(case.tags)}]", case.id, list(case.tags) - - def update_case( - saved_id: str, new_id: str, negative: bool, tags: list[str] - ) -> tuple[bool, str, str | None, list[str] | None]: - case = next((c for c in suite.cases if c.id == saved_id), None) - if case is None: - return False, f"case {saved_id!r} not found in manifest", None, None - if new_id != saved_id and any(c.id == new_id for c in suite.cases): - return False, f"case id {new_id!r} already exists", None, None - case.id = new_id - # Tags round-trip verbatim. The negative checkbox owns only the - # negative tag, so auto/manual provenance survives edits. - plain = [t for t in tags if t != "negative"] - case.tags = plain + (["negative"] if negative else []) - case.expect_fail = negative - if negative: - case.expect_final_fail = False - save_suite(suite, manifest) - return True, f"updated {case.id} [{', '.join(case.tags)}]", case.id, list(case.tags) - - def delete_case(saved_id: str) -> tuple[bool, str]: - case = next((c for c in suite.cases if c.id == saved_id), None) - if case is None: - return False, f"case {saved_id!r} not found in manifest" - suite.cases.remove(case) - save_suite(suite, manifest) - return True, f"deleted {saved_id} from {manifest.name}" - + store, final = _open(dataset) + trajectory = load_trajectory(store.suite.db_path(), store.suite.odom_stream) + foot = trajectory.positions - np.array([0.0, 0.0, store.cfg.robot_height], dtype=np.float32) pick_cases( dataset, final.occupied, turbo_by_height(final.occupied), final.voxel_size, foot, - suite.cases, - save_pair, - update_case, - delete_case, + store, ) print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {dataset}") @app.command("list") def list_cases() -> None: - """Print every dataset's cases with endpoints, weights, and tags.""" + """Print every dataset's cases with endpoints and tags.""" for suite in load_suites(): print(f"{suite.dataset} ({suite.path.name if suite.path else '?'})") for case in suite.cases: tags = f" [{', '.join(case.tags)}]" if case.tags else "" print( f" {case.id}: {tuple(round(v, 2) for v in case.start)} -> " - f"{tuple(round(v, 2) for v in case.goal)} w={case.weight:g}{tags}" + f"{tuple(round(v, 2) for v in case.goal)}{tags}" ) diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 9eec530ce1..78bbf02fdb 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -14,7 +14,7 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner @@ -59,16 +59,26 @@ class EvalConfig: max_slope: float = 1.2 max_step_m: float = 0.2 kinematic_window_m: float = 0.5 + # How far an endpoint may sit from a standable surface before it counts as + # off the map, for both case generation and curation snapping. + snap_max_m: float = 1.0 # An improvement must not buy score with compute. p95 over the suite. plan_p95_budget_ms: float = 50.0 map_update_p95_budget_ms: float = 1000.0 + # Planner constructor overrides, e.g. --set planner.wall_clearance_m=0.0. + # Omitted keys keep the planner's own defaults, so nothing is duplicated + # here, and the report records whatever was swept. + planner: dict[str, float] = field(default_factory=dict) + def make_mapper(self) -> VoxelRayMapper: return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) def make_planner(self) -> MLSPlanner: - return MLSPlanner(voxel_size=self.voxel_size, robot_height=self.robot_height) + return MLSPlanner( + voxel_size=self.voxel_size, robot_height=self.robot_height, **self.planner + ) def mapper_fingerprint(self) -> dict[str, float | int]: """Cache key parameters for the final map. diff --git a/dimos/navigation/nav_3d/evaluator/curation.py b/dimos/navigation/nav_3d/evaluator/curation.py new file mode 100644 index 0000000000..c304287b3c --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/curation.py @@ -0,0 +1,145 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Editing a case manifest: add, update, and delete curated cases. + +Every mutation snaps endpoints to the final map's standable surface and writes +the manifest, so the CLI and the browser picker share one implementation and +one set of rules. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import itertools +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.cases import CASES_DIR, Case, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map +from dimos.navigation.nav_3d.evaluator.generate import snap_to_surface + +if TYPE_CHECKING: + from pathlib import Path + + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.cases import Suite + from dimos.navigation.nav_3d.evaluator.final_map import FinalMap + +Point = tuple[float, float, float] + + +class CurationError(Exception): + """A curation request the manifest cannot accept.""" + + +@dataclass +class CaseStore: + """Mutable view of one dataset's manifest, saved after every change.""" + + suite: Suite + manifest: Path + surface: NDArray[np.float32] + cfg: EvalConfig + + def _snap(self, label: str, point: Point, *, required: bool = True) -> Point: + snapped = snap_to_surface( + np.asarray(point, dtype=np.float32), self.surface, self.cfg.snap_max_m + ) + if snapped is None: + if required: + raise CurationError( + f"{label} {point} is more than {self.cfg.snap_max_m}m from a standable surface" + ) + # An infeasible goal may sit on geometry with no standable surface. + print(f"note: {label} {point} is off any standable surface; keeping it as picked") + return point + return (float(snapped[0]), float(snapped[1]), float(snapped[2])) + + def _next_id(self, prefix: str) -> str: + existing = {c.id for c in self.suite.cases} + return next( + f"{prefix}_{n:02d}" for n in itertools.count() if f"{prefix}_{n:02d}" not in existing + ) + + def add( + self, + start: Point, + goal: Point, + tags: list[str], + case_id: str | None = None, + expect_fail: bool = False, + ) -> Case: + case = Case( + id=case_id or self._next_id("neg" if expect_fail else "manual"), + start=self._snap("start", start), + goal=self._snap("goal", goal, required=not expect_fail), + tags=_curated_tags(tags, expect_fail), + expect_fail=expect_fail, + ) + if any(c.id == case.id for c in self.suite.cases): + raise CurationError(f"case id {case.id!r} already exists in {self.manifest}") + self.suite.cases.append(case) + self.save() + kind = "negative (must refuse)" if expect_fail else "positive" + print(f"added {kind} {case.id}: {case.start} -> {case.goal} to {self.manifest}") + return case + + def update(self, case_id: str, new_id: str, tags: list[str], expect_fail: bool) -> Case: + case = self.get(case_id) + if new_id != case_id and any(c.id == new_id for c in self.suite.cases): + raise CurationError(f"case id {new_id!r} already exists") + case.id = new_id + case.tags = _curated_tags(tags, expect_fail) + case.expect_fail = expect_fail + if expect_fail: + case.expect_final_fail = False + self.save() + return case + + def delete(self, case_id: str) -> None: + self.suite.cases.remove(self.get(case_id)) + self.save() + + def get(self, case_id: str) -> Case: + case = next((c for c in self.suite.cases if c.id == case_id), None) + if case is None: + raise CurationError(f"case {case_id!r} not found in {self.manifest}") + return case + + def save(self) -> None: + save_suite(self.suite, self.manifest) + + +def _curated_tags(tags: list[str], expect_fail: bool) -> list[str]: + """Curated cases always carry manual provenance. The negative tag tracks + expect_fail rather than being editable text, so the two cannot drift.""" + keep = [t for t in tags if t not in ("manual", "negative")] + return ["manual", *(["negative"] if expect_fail else []), *keep] + + +def load_store(dataset: str) -> tuple[CaseStore, FinalMap]: + """Open a dataset's manifest with the final map and surface it snaps to.""" + manifest = CASES_DIR / f"{dataset}.yaml" + if not manifest.exists(): + raise CurationError(f"no manifest {manifest}; run ingest first") + suite = load_suite(manifest) + cfg = EvalConfig() + final = load_or_build_final_map(suite.db_path(), suite, cfg) + planner = cfg.make_planner() + planner.update_global_map(final.occupied) + return CaseStore(suite, manifest, planner.surface_map(), cfg), final diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index 6db5ed4e3d..e857d812ce 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -24,14 +24,15 @@ from dataclasses import dataclass import hashlib -import itertools import json from time import perf_counter from typing import TYPE_CHECKING import numpy as np +from dimos.navigation.nav_3d.evaluator.metrics import timing_stats from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames +from dimos.navigation.nav_3d.evaluator.voxel_keys import voxel_keys from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -47,64 +48,6 @@ logger = setup_logger() -_KEY_OFFSET = 1 << 20 - - -def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: - """Pack voxel indices into sortable int64 keys, one per point.""" - idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - return (idx[:, 0] << 42) | (idx[:, 1] << 21) | idx[:, 2] - - -def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: - """Voxel center positions for packed keys, the inverse of voxel_keys.""" - mask = (1 << 21) - 1 - idx = np.stack([keys >> 42, (keys >> 21) & mask, keys & mask], axis=1) - _KEY_OFFSET - return ((idx + 0.5) * voxel_size).astype(np.float32) - - -def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: - if len(sorted_keys) == 0: - return np.zeros(len(query), dtype=bool) - pos = np.clip(np.searchsorted(sorted_keys, query), 0, len(sorted_keys) - 1) - return np.asarray(sorted_keys[pos] == query) - - -def cylinder_offsets( - radius: float, z_lo: float, z_hi: float, voxel_size: float -) -> NDArray[np.int64]: - """Integer voxel offsets forming a vertical cylinder.""" - r_vox = int(np.ceil(radius / voxel_size)) - span = np.arange(-r_vox, r_vox + 1) - dx, dy = np.meshgrid(span, span, indexing="ij") - in_disc = (dx * voxel_size) ** 2 + (dy * voxel_size) ** 2 <= radius**2 - dz = np.arange(int(np.floor(z_lo / voxel_size)), int(np.ceil(z_hi / voxel_size)) + 1) - disc = np.stack([dx[in_disc], dy[in_disc]], axis=1) - out = np.concatenate([np.hstack([disc, np.full((len(disc), 1), z)]) for z in dz]) - return np.asarray(out, dtype=np.int64) - - -def offset_keys( - points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float -) -> NDArray[np.int64]: - """Keys of every (point voxel + offset) pair, shape (P * O,).""" - idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - swept = idx[:, None, :] + offsets[None, :, :] - return np.asarray((swept[..., 0] << 42) | (swept[..., 1] << 21) | swept[..., 2]) - - -def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: - """Resample a polyline so consecutive samples are at most step apart.""" - if len(points) < 2: - return points.astype(np.float32) - out = [points[:1]] - for a, b in itertools.pairwise(points): - seg = np.linalg.norm(b - a) - n = max(int(np.ceil(seg / step)), 1) - t = np.linspace(0.0, 1.0, n + 1)[1:, None] - out.append(a[None, :] * (1 - t) + b[None, :] * t) - return np.concatenate(out).astype(np.float32) - @dataclass class FinalMap: @@ -118,34 +61,23 @@ class FinalMap: @dataclass class MapCheckpoints: - """Map state at increasing times, delta-encoded between snapshots. - - occupied tracks the mapper's healthy voxels. observed tracks every voxel a - raw lidar return ever landed in, mapper-independent, so it only grows. - """ + """The mapper's occupied set at increasing times, delta-encoded.""" times: NDArray[np.float64] added: list[NDArray[np.int64]] removed: list[NDArray[np.int64]] - observed_added: list[NDArray[np.int64]] - - def iter_snapshots(self) -> Iterator[tuple[NDArray[np.int64], NDArray[np.int64]]]: - """Yield (occupied_keys, newly_observed_keys) in time order. - occupied comes as the full sorted set. observed only ever grows and - gets intersected by its consumer, so it arrives as the delta since - the previous checkpoint instead of the accumulated multi-million-key - set. - """ + def iter_snapshots(self) -> Iterator[NDArray[np.int64]]: + """Yield the full sorted occupied key set at each time, in order.""" keys = np.array([], dtype=np.int64) - for add, rem, obs in zip(self.added, self.removed, self.observed_added, strict=True): + for add, rem in zip(self.added, self.removed, strict=True): keys = np.delete(keys, np.searchsorted(keys, rem)) keys = np.insert(keys, np.searchsorted(keys, add), add) - yield keys, obs + yield keys CACHE_VERSION = 3 -CHECKPOINT_CACHE_VERSION = 2 +CHECKPOINT_CACHE_VERSION = 3 def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: @@ -168,57 +100,34 @@ def replay_frames( mapper: VoxelRayMapper, voxel_size: float, times: NDArray[np.float64], -) -> tuple[FinalMap, list[NDArray[np.int64]], list[NDArray[np.int64]]]: +) -> tuple[FinalMap, list[NDArray[np.int64]]]: """Feed frames through the mapper in order, snapshotting at each requested time. A snapshot holds exactly the frames with ts <= its time. Returns the - final map, the occupied-key snapshots, and the observed-key snapshots. + final map and the occupied-key snapshots. """ snapshots: list[NDArray[np.int64]] = [] - observed_snapshots: list[NDArray[np.int64]] = [] - observed = np.array([], dtype=np.int64) - pending: list[NDArray[np.int64]] = [] - - def merged() -> NDArray[np.int64]: - nonlocal observed - if pending: - observed = np.union1d(observed, np.concatenate(pending)) - pending.clear() - return observed - add_ms: list[float] = [] t0 = perf_counter() for frame in frames: while len(snapshots) < len(times) and frame.ts > times[len(snapshots)]: snapshots.append(np.unique(voxel_keys(mapper.global_map(), voxel_size))) - observed_snapshots.append(merged()) t1 = perf_counter() mapper.add_frame(frame.points, frame.origin) add_ms.append((perf_counter() - t1) * 1000) - pts = frame.points[np.isfinite(frame.points).all(axis=1)] - pending.append(np.unique(voxel_keys(pts, voxel_size))) - if sum(len(p) for p in pending) > 4_000_000: - merged() build_ms = (perf_counter() - t0) * 1000 occupied = mapper.global_map() occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) - final_observed = merged() while len(snapshots) < len(times): snapshots.append(occupied_keys) - observed_snapshots.append(final_observed) - add_arr = np.asarray(add_ms) if add_ms else np.zeros(1) final = FinalMap( voxel_size=voxel_size, occupied=occupied, occupied_keys=occupied_keys, frames=len(add_ms), - add_frame_ms={ - "p50": float(np.percentile(add_arr, 50)), - "p95": float(np.percentile(add_arr, 95)), - "max": float(add_arr.max()), - }, + add_frame_ms=timing_stats(add_ms), build_ms=build_ms, ) - return final, snapshots, observed_snapshots + return final, snapshots def _save_final(cache: Path, final: FinalMap) -> None: @@ -228,9 +137,7 @@ def _save_final(cache: Path, final: FinalMap) -> None: occupied=final.occupied, occupied_keys=final.occupied_keys, frames=final.frames, - add_p50=final.add_frame_ms["p50"], - add_p95=final.add_frame_ms["p95"], - add_max=final.add_frame_ms["max"], + **{f"add_{k}": v for k, v in final.add_frame_ms.items()}, ) logger.info("final map cached: %s (%d voxels)", cache.name, len(final.occupied)) @@ -244,16 +151,12 @@ def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> Fin occupied=data["occupied"], occupied_keys=data["occupied_keys"], frames=int(data["frames"]), - add_frame_ms={ - "p50": float(data["add_p50"]), - "p95": float(data["add_p95"]), - "max": float(data["add_max"]), - }, + add_frame_ms={k: float(data[f"add_{k}"]) for k in ("p50", "p95", "max")}, build_ms=0.0, ) logger.info("building final map for %s (cache miss)", db_path.name) - final, _, _ = replay_frames( + final, _ = replay_frames( iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol), cfg.make_mapper(), cfg.voxel_size, @@ -299,11 +202,10 @@ def load_or_build_checkpoints( times=data["times"], added=[data[f"add_{i}"] for i in range(n)], removed=[data[f"rem_{i}"] for i in range(n)], - observed_added=[data[f"obs_{i}"] for i in range(n)], ) logger.info("building %d map checkpoints for %s (cache miss)", len(times), db_path.name) - final, snapshots, observed = replay_frames( + final, snapshots = replay_frames( iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol), cfg.make_mapper(), cfg.voxel_size, @@ -313,12 +215,10 @@ def load_or_build_checkpoints( if not final_cache.exists(): _save_final(final_cache, final) added, removed = encode_deltas(snapshots) - observed_added, _ = encode_deltas(observed) arrays: dict[str, NDArray[np.int64] | NDArray[np.float64]] = {"times": times} arrays |= {f"add_{i}": a for i, a in enumerate(added)} arrays |= {f"rem_{i}": r for i, r in enumerate(removed)} - arrays |= {f"obs_{i}": o for i, o in enumerate(observed_added)} cache.parent.mkdir(exist_ok=True) np.savez_compressed(cache, **arrays) # type: ignore[arg-type] logger.info("checkpoints cached: %s", cache.name) - return MapCheckpoints(times=times, added=added, removed=removed, observed_added=observed_added) + return MapCheckpoints(times=times, added=added, removed=removed) diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index f6adcd967d..ad0350671b 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -47,7 +47,6 @@ class GenerationParams: min_separation_m: float = 3.0 min_euclid_m: float = 2.0 detour_ratio_min: float = 1.3 - snap_max_m: float = 1.0 bin_size_m: float = 2.0 waypoint_spacing_m: float = 1.0 # None scales the case count with the walked distance. @@ -130,7 +129,7 @@ def generate_cases( idx = _subsample_indices(trajectory, params.waypoint_spacing_m) snaps = np.full((len(idx), 3), np.nan, dtype=np.float32) for n, i in enumerate(idx): - hit = snap_to_surface(foot[i], surface, params.snap_max_m) + hit = snap_to_surface(foot[i], surface, cfg.snap_max_m) if hit is not None: snaps[n] = hit ok = np.isfinite(snaps[:, 0]) @@ -141,7 +140,7 @@ def generate_cases( if not ok[ai]: continue sa = snaps[ai] - near_a = np.linalg.norm(foot - sa, axis=1) <= params.snap_max_m + near_a = np.linalg.norm(foot - sa, axis=1) <= cfg.snap_max_m last_visit_a = float(trajectory.ts[near_a].max()) if near_a.any() else -np.inf later = np.arange(ai + 1, len(idx)) later = later[ok[later]] @@ -161,15 +160,7 @@ def generate_cases( continue # Only pairs not already qualified pay for the line sweep. line = np.stack([sa, sb]) - blocked = not metrics.check_path( - line, - obstacle_keys, - cfg.voxel_size, - cfg.robot_length, - cfg.robot_width, - cfg.ground_margin, - cfg.body_clearance, - ).valid + blocked = not metrics.check_path(line, obstacle_keys, cfg).valid if not blocked: continue # Backward in time is always causal. Forward only when the start @@ -198,9 +189,7 @@ def generate_cases( selected = _select_diverse(ranked, params, params.resolve_max_cases(float(arcs[-1]))) cases = [] for n, cand in enumerate(selected): - route = metrics.ground_truth_route( - trajectory, cand.start, cand.goal, cfg.robot_height, params.snap_max_m - ) + route = metrics.ground_truth_route(trajectory, cand.start, cand.goal, cfg) tags = route_tags(cand.start, cand.goal, route, obstacle_keys, cfg) cases.append(_to_case(cand, n, tags)) return cases @@ -291,16 +280,5 @@ def fill(target: int, relax: bool) -> None: def _to_case(cand: Candidate, n: int, tags: list[str]) -> Case: - if cand.dz >= STAIRS_DZ_M: - kind = "up" - elif cand.dz <= -STAIRS_DZ_M: - kind = "down" - else: - kind = "flat" - return Case( - id=f"auto_{n:02d}_{kind}", - start=cand.start, - goal=cand.goal, - weight=1.0, - tags=["auto", *tags], - ) + kind = "up" if cand.dz >= STAIRS_DZ_M else "down" if cand.dz <= -STAIRS_DZ_M else "flat" + return Case(id=f"auto_{n:02d}_{kind}", start=cand.start, goal=cand.goal, tags=["auto", *tags]) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 99ccaf64d0..642c7fa891 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -21,9 +21,8 @@ import numpy as np -from dimos.navigation.nav_3d.evaluator.final_map import ( +from dimos.navigation.nav_3d.evaluator.voxel_keys import ( cylinder_offsets, - densify, key_centers, keys_contain, offset_keys, @@ -32,6 +31,7 @@ if TYPE_CHECKING: from numpy.typing import NDArray + from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.recording import Trajectory @@ -41,6 +41,25 @@ def path_length(waypoints: NDArray[np.float32]) -> float: return float(np.linalg.norm(np.diff(waypoints, axis=0), axis=1).sum()) +def arc_lengths(points: NDArray[np.float32]) -> NDArray[np.float64]: + """Cumulative 3D arc length at each point, starting at zero.""" + steps = np.linalg.norm(np.diff(points, axis=0), axis=1) + return np.concatenate([[0.0], np.cumsum(steps)]) + + +def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: + """Resample a polyline so consecutive samples are at most step apart.""" + if len(points) < 2: + return points.astype(np.float32) + seg = np.linalg.norm(np.diff(points, axis=0), axis=1) + n = np.maximum(np.ceil(seg / step).astype(np.int64), 1) + idx = np.repeat(np.arange(len(n)), n) + starts = np.concatenate([[0], np.cumsum(n)[:-1]]) + t = ((np.arange(n.sum()) - starts[idx] + 1) / n[idx])[:, None] + body = points[idx] * (1 - t) + points[idx + 1] * t + return np.concatenate([points[:1], body]).astype(np.float32) + + def goal_reached( waypoints: NDArray[np.float32], goal: tuple[float, float, float], tolerance: float ) -> bool: @@ -58,6 +77,9 @@ class GateResult: valid: bool collision_points: NDArray[np.float32] + # Indices of the colliding samples in densify(waypoints, voxel_size / 2), + # so a viewer can recover the exact body frames the gate tested. + collision_indices: NDArray[np.int64] # Horizontal distance from the body surface to the nearest obstacle in # the gate's z band, minimized along the path. Negative is penetration # depth, capped at MARGIN_CAP_M when nothing is near. Gives a smooth @@ -76,7 +98,7 @@ def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.fl if len(samples) < 2: return np.tile(np.array([1.0, 0.0, 0.0]), (len(samples), 1)) pts = samples.astype(np.float64) - arc = np.concatenate([[0.0], np.cumsum(np.linalg.norm(np.diff(pts, axis=0), axis=1))]) + arc = arc_lengths(pts) half = span / 2.0 back_arc = np.clip(arc - half, arc[0], arc[-1]) front_arc = np.clip(arc + half, arc[0], arc[-1]) @@ -101,13 +123,7 @@ def body_frames( def check_path( - waypoints: NDArray[np.float32], - obstacle_keys: NDArray[np.int64], - voxel_size: float, - robot_length: float, - robot_width: float, - ground_margin: float, - body_clearance: float, + waypoints: NDArray[np.float32], obstacle_keys: NDArray[np.int64], cfg: EvalConfig ) -> GateResult: """Sweep the robot body box along foot-level waypoints against obstacles. @@ -119,24 +135,30 @@ def check_path( elevated body. Candidate voxels come from a padded voxelized cylinder that covers the box at any orientation and are tested against the exact box. """ + voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size / 2) - fwd, lateral, up = body_frames(samples, robot_length) - half_len = robot_length / 2.0 - half_wid = robot_width / 2.0 - half_band = (body_clearance - ground_margin) / 2.0 - mid = np.array([0.0, 0.0, (ground_margin + body_clearance) / 2.0]) + fwd, lateral, up = body_frames(samples, cfg.robot_length) + half_len = cfg.robot_length / 2.0 + half_wid = cfg.robot_width / 2.0 + half_band = (cfg.body_clearance - cfg.ground_margin) / 2.0 + mid = np.array([0.0, 0.0, (cfg.ground_margin + cfg.body_clearance) / 2.0]) circ = float(np.hypot(half_len, half_wid)) offsets = cylinder_offsets( circ + MARGIN_CAP_M + voxel_size, -(half_len + MARGIN_CAP_M + voxel_size), - body_clearance + half_len + MARGIN_CAP_M + voxel_size, + cfg.body_clearance + half_len + MARGIN_CAP_M + voxel_size, voxel_size, ) keys = offset_keys(samples, offsets, voxel_size) candidate = keys_contain(obstacle_keys, keys.ravel()).reshape(keys.shape) s_idx, o_idx = np.nonzero(candidate) if len(s_idx) == 0: - return GateResult(valid=True, collision_points=samples[:0], min_clearance_m=MARGIN_CAP_M) + return GateResult( + valid=True, + collision_points=samples[:0], + collision_indices=np.empty(0, dtype=np.int64), + min_clearance_m=MARGIN_CAP_M, + ) # Offset from the box center, which sits mid-band directly over the sample. delta = key_centers(keys[s_idx, o_idx], voxel_size) - samples[s_idx] - mid along = (delta * fwd[s_idx]).sum(1) @@ -153,6 +175,7 @@ def check_path( return GateResult( valid=len(colliding) == 0, collision_points=samples[colliding], + collision_indices=colliding, min_clearance_m=min(clearance, MARGIN_CAP_M), ) @@ -166,11 +189,7 @@ class SupportResult: def check_support( - waypoints: NDArray[np.float32], - support_keys: NDArray[np.int64], - voxel_size: float, - radius: float, - depth: float, + waypoints: NDArray[np.float32], support_keys: NDArray[np.int64], cfg: EvalConfig ) -> SupportResult: """Require occupied voxels beneath every path sample. @@ -179,8 +198,9 @@ def check_support( one occupied voxel within radius horizontally and from depth below the foot up to one voxel above it. """ + voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size) - offsets = cylinder_offsets(radius, -depth, voxel_size, voxel_size) + offsets = cylinder_offsets(cfg.support_radius_m, -cfg.support_depth_m, voxel_size, voxel_size) keys = offset_keys(samples, offsets, voxel_size) supported = keys_contain(support_keys, keys.ravel()).reshape(keys.shape).any(axis=1) return SupportResult(bool(supported.all()), samples[~supported]) @@ -196,8 +216,7 @@ class KinematicsResult: def _resample(waypoints: NDArray[np.float32], spacing: float) -> NDArray[np.float32]: """Points every spacing meters of 3D arc length along the polyline.""" - steps = np.linalg.norm(np.diff(waypoints, axis=0), axis=1) - arc = np.concatenate([[0.0], np.cumsum(steps)]) + arc = arc_lengths(waypoints) if arc[-1] <= spacing: return waypoints[[0, -1]] s = np.append(np.arange(0.0, arc[-1], spacing), arc[-1]) @@ -206,12 +225,7 @@ def _resample(waypoints: NDArray[np.float32], spacing: float) -> NDArray[np.floa ) -def check_kinematics( - waypoints: NDArray[np.float32], - max_slope: float, - max_step_m: float, - window_m: float, -) -> KinematicsResult: +def check_kinematics(waypoints: NDArray[np.float32], cfg: EvalConfig) -> KinematicsResult: """Reject paths that climb steeper than the robot can. The profile is resampled at window_m of arc length so single-cell @@ -221,11 +235,11 @@ def check_kinematics( """ if len(waypoints) < 2: return KinematicsResult(True, waypoints[:0]) - profile = _resample(waypoints, window_m) + profile = _resample(waypoints, cfg.kinematic_window_m) d = np.diff(profile, axis=0) rise = np.abs(d[:, 2]) run = np.linalg.norm(d[:, :2], axis=1) - bad = rise > np.maximum(run * max_slope, max_step_m) + bad = rise > np.maximum(run * cfg.max_slope, cfg.max_step_m) return KinematicsResult(not bad.any(), profile[1:][bad]) @@ -243,41 +257,64 @@ class Reference: causal: bool +@dataclass +class _Visits: + """Every trajectory pose near the start and near the goal, with the walked + length of each start-visit to goal-visit pairing.""" + + foot: NDArray[np.float32] + near_s: NDArray[np.int64] + near_g: NDArray[np.int64] + totals: NDArray[np.float64] + + +def _visits( + trajectory: Trajectory, + start: tuple[float, float, float], + goal: tuple[float, float, float], + cfg: EvalConfig, +) -> _Visits | None: + """None when either endpoint is farther than snap_max_m from the trajectory.""" + foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + ds = np.linalg.norm(foot - np.asarray(start, dtype=np.float32), axis=1) + dg = np.linalg.norm(foot - np.asarray(goal, dtype=np.float32), axis=1) + if ds.min() > cfg.snap_max_m or dg.min() > cfg.snap_max_m: + return None + arcs = trajectory.arc_lengths() + near_s = np.flatnonzero(ds <= cfg.snap_max_m) + near_g = np.flatnonzero(dg <= cfg.snap_max_m) + totals = ( + np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) + + ds[near_s][:, None] + + dg[near_g][None, :] + ) + return _Visits(foot, near_s, near_g, totals) + + def reference_length( trajectory: Trajectory, start: tuple[float, float, float], goal: tuple[float, float, float], - robot_height: float, - max_snap_m: float = 1.0, + cfg: EvalConfig, ) -> Reference: """Shortest walked length the trajectory demonstrates between start and goal. Minimizes route length over every combination of start and goal visits, not the route between single nearest poses. Only causal pairs count when one exists: the goal visited before the start. Falls back to straight-line - distance when either endpoint is farther than max_snap_m from the trajectory. + distance when either endpoint is off the trajectory. """ - foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) - s = np.asarray(start, dtype=np.float32) - g = np.asarray(goal, dtype=np.float32) - ds = np.linalg.norm(foot - s, axis=1) - dg = np.linalg.norm(foot - g, axis=1) - if ds.min() > max_snap_m or dg.min() > max_snap_m: - return Reference(float(np.linalg.norm(g - s)), False, float("inf"), False) - arcs = trajectory.arc_lengths() - near_s = np.flatnonzero(ds <= max_snap_m) - near_g = np.flatnonzero(dg <= max_snap_m) - totals = ( - np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) - + ds[near_s][:, None] - + dg[near_g][None, :] - ) - backward = trajectory.ts[near_g][None, :] <= trajectory.ts[near_s][:, None] + visits = _visits(trajectory, start, goal, cfg) + if visits is None: + straight = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) + return Reference(straight, False, float("inf"), False) + totals = visits.totals + backward = trajectory.ts[visits.near_g][None, :] <= trajectory.ts[visits.near_s][:, None] causal = bool(backward.any()) if causal: totals = np.where(backward, totals, np.inf) best = np.unravel_index(totals.argmin(), totals.shape) - i = int(near_s[best[0]]) + i = int(visits.near_s[best[0]]) start_ts = float(trajectory.ts[i]) if causal else float("inf") return Reference(max(float(totals[best]), 1e-6), True, start_ts, causal) @@ -286,8 +323,7 @@ def ground_truth_route( trajectory: Trajectory, start: tuple[float, float, float], goal: tuple[float, float, float], - robot_height: float, - max_snap_m: float = 1.0, + cfg: EvalConfig, ) -> NDArray[np.float32] | None: """Foot-level polyline of the shortest walk the robot took between start and goal, or None when either endpoint is off the trajectory. @@ -297,26 +333,14 @@ def ground_truth_route( causal one. Picking the visit pair with the least trajectory between them keeps the route local instead of a building-spanning detour. """ - foot = trajectory.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) - s = np.asarray(start, dtype=np.float32) - g = np.asarray(goal, dtype=np.float32) - ds = np.linalg.norm(foot - s, axis=1) - dg = np.linalg.norm(foot - g, axis=1) - if ds.min() > max_snap_m or dg.min() > max_snap_m: + visits = _visits(trajectory, start, goal, cfg) + if visits is None: return None - arcs = trajectory.arc_lengths() - near_s = np.flatnonzero(ds <= max_snap_m) - near_g = np.flatnonzero(dg <= max_snap_m) - totals = ( - np.abs(arcs[near_s][:, None] - arcs[near_g][None, :]) - + ds[near_s][:, None] - + dg[near_g][None, :] - ) - best = np.unravel_index(totals.argmin(), totals.shape) - i = int(near_s[best[0]]) - j = int(near_g[best[1]]) + best = np.unravel_index(visits.totals.argmin(), visits.totals.shape) + i = int(visits.near_s[best[0]]) + j = int(visits.near_g[best[1]]) # Orient the slice start-to-goal so the route reads in the case's direction. - route = foot[i : j + 1] if i <= j else foot[j : i + 1][::-1] + route = visits.foot[i : j + 1] if i <= j else visits.foot[j : i + 1][::-1] return route.astype(np.float32) diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index 93f8902fef..04d7a6f6f1 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -28,31 +28,17 @@ import numpy as np -from dimos.navigation.nav_3d.evaluator.tagging import ( - LONG_STAIRS_DZ_M, - LONG_STAIRS_WALKED_M, - STAIRS_DZ_M, -) +from dimos.navigation.nav_3d.evaluator.curation import CurationError +from dimos.navigation.nav_3d.evaluator.tagging import elevation_tags if TYPE_CHECKING: - from collections.abc import Callable, Sequence + from collections.abc import Callable from numpy.typing import NDArray import viser from dimos.navigation.nav_3d.evaluator.cases import Case - - # (start, goal, negative, tags, case_id) -> (ok, message, saved_id, saved_tags) - SavePair = Callable[ - [tuple[float, float, float], tuple[float, float, float], bool, list[str], str | None], - tuple[bool, str, str | None, list[str] | None], - ] - # (saved_id, new_id, negative, tags) -> (ok, message, saved_id, saved_tags) - UpdateCase = Callable[ - [str, str, bool, list[str]], tuple[bool, str, str | None, list[str] | None] - ] - # (saved_id) -> (ok, message) - DeleteCase = Callable[[str], tuple[bool, str]] + from dimos.navigation.nav_3d.evaluator.curation import CaseStore # Selection cone half-angle around the click ray. Wide enough to hit a voxel # point from across a room, narrow enough to stay on the intended surface. @@ -72,6 +58,7 @@ Plain drag orbits, scroll zooms, right-drag pans. """ + # three.js ACES filmic tone mapping, the fitted curve and its color # matrices. Applied by the viewer to mesh materials but not to the point # and line shaders. @@ -129,6 +116,10 @@ def _marker_color(srgb: tuple[int, int, int]) -> tuple[int, int, int]: return int(r), int(g), int(b) +def _point(p: NDArray[np.float32]) -> tuple[float, float, float]: + return (float(p[0]), float(p[1]), float(p[2])) + + def pick_along_ray( points: NDArray[np.float32], origin: NDArray[np.float64], @@ -152,27 +143,11 @@ def pick_along_ray( return None -def suggested_tags(start: NDArray[np.float32], goal: NDArray[np.float32]) -> set[str]: - """Geometry-derived tag suggestions, mirroring auto-generation's rules.""" - dz = float(goal[2] - start[2]) - euclid = float(np.linalg.norm(goal - start)) - tags: set[str] = set() - if abs(dz) >= STAIRS_DZ_M: - tags |= {"stairs", "up" if dz > 0 else "down"} - else: - tags.add("flat") - if abs(dz) >= LONG_STAIRS_DZ_M or euclid >= LONG_STAIRS_WALKED_M: - tags.add("long") - return tags - - @dataclass class _Hooks: - """Manifest callbacks and shared state handed to every pair entry.""" + """Manifest store and shared state handed to every pair entry.""" - save_pair: SavePair - update_case: UpdateCase - delete_case: DeleteCase + store: CaseStore lock: threading.Lock unregister: Callable[[_PairEntry], None] announce: Callable[[str], None] @@ -202,7 +177,7 @@ def __init__( if case is None: self.saved_id: str | None = None self._name = "" - self._checked = suggested_tags(start, goal) + self._checked = set(elevation_tags(_point(start), _point(goal))) self._custom = "" self._negative = False self._status = "unsaved" @@ -313,11 +288,13 @@ def remove(self) -> None: def delete(self) -> None: if self.saved_id is not None: - ok, msg = self._hooks.delete_case(self.saved_id) - print(msg) - if not ok: - self.message.content = f"**FAILED**: {msg}" + try: + self._hooks.store.delete(self.saved_id) + except CurationError as err: + self.message.content = f"**FAILED**: {err}" + print(err) return + print(f"deleted {self.saved_id}") self._hooks.unregister(self) self.remove() @@ -334,29 +311,33 @@ def extra_tags(self) -> list[str]: def save_or_update(self) -> bool: name = self.id_text.value.strip() - if self.saved_id is None: - ok, msg, saved, tags = self._hooks.save_pair( - (float(self.start[0]), float(self.start[1]), float(self.start[2])), - (float(self.goal[0]), float(self.goal[1]), float(self.goal[2])), - self.negative_box.value, - self.extra_tags(), - name or None, - ) - else: - ok, msg, saved, tags = self._hooks.update_case( - self.saved_id, name or self.saved_id, self.negative_box.value, self.extra_tags() - ) - print(msg) - if not (ok and saved is not None): - self.message.content = f"**FAILED**: {msg}" + store = self._hooks.store + negative = self.negative_box.value + try: + if self.saved_id is None: + case = store.add( + _point(self.start), + _point(self.goal), + self.extra_tags(), + case_id=name or None, + expect_fail=negative, + ) + else: + case = store.update( + self.saved_id, name or self.saved_id, self.extra_tags(), negative + ) + except CurationError as err: + print(err) + self.message.content = f"**FAILED**: {err}" return False + msg = f"saved {case.id} [{', '.join(case.tags)}]" + print(msg) # Viser cannot collapse a live panel, so replace it with the # collapsed button form, synced from the authoritative save. - self.saved_id = saved + self.saved_id = case.id self._snapshot() - self._name = saved - if tags is not None: - self._sync_tags(tags) + self._name = case.id + self._sync_tags(case.tags) self._status = msg order = self.panel.order self.panel.remove() @@ -370,10 +351,7 @@ def pick_cases( map_colors: NDArray[np.uint8], voxel_size: float, walked: NDArray[np.float32], - cases: Sequence[Case], - save_pair: SavePair, - update_case: UpdateCase, - delete_case: DeleteCase, + store: CaseStore, ) -> None: """Serve the picker until the user exits from the panel or hits ctrl-c.""" import viser @@ -460,15 +438,7 @@ def highlight(entry: _PairEntry) -> None: entry.set_highlight(True) highlighted.append(entry) - hooks = _Hooks( - save_pair, - update_case, - delete_case, - lock, - lambda entry: pairs.remove(entry), - announce, - highlight, - ) + hooks = _Hooks(store, lock, lambda entry: pairs.remove(entry), announce, highlight) marker_seq = 0 def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.SceneNodeHandle: @@ -496,7 +466,7 @@ def pair_markers( ) -> list[viser.SceneNodeHandle]: return [sphere(start, START_COLOR), sphere(goal, GOAL_COLOR), pair_line(start, goal)] - for case in cases: + for case in store.suite.cases: start = np.asarray(case.start, dtype=np.float32) goal = np.asarray(case.goal, dtype=np.float32) pairs.append( diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 5c9de7a0ac..b5d262cf5d 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -36,11 +36,11 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.final_map import ( - key_centers, load_or_build_checkpoints, load_or_build_final_map, ) from dimos.navigation.nav_3d.evaluator.recording import load_trajectory +from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -79,6 +79,9 @@ class PlanOutcome: min_clearance: float | None waypoints: list[list[float]] collisions: list[list[float]] + # Indices of the colliding samples along the densified path, so a viewer + # can redraw the exact body boxes the gate rejected. + collision_indices: list[int] unsupported: list[list[float]] steep: list[list[float]] @@ -97,7 +100,6 @@ class CaseResult: dataset: str start: tuple[float, float, float] goal: tuple[float, float, float] - weight: float tags: list[str] l_ref: float l_ref_snapped: bool @@ -169,7 +171,7 @@ class Report: # dataset/id of cases whose online route a new final obstacle blocks, the # candidates for an expect_final_fail label. dynamic_candidates: list[str] = field(default_factory=list) - config: dict[str, float | int] = field(default_factory=dict) + config: dict[str, object] = field(default_factory=dict) def to_dict(self) -> dict[str, object]: out = asdict(self) @@ -196,21 +198,9 @@ def _run_plan( return _no_plan(plan_ms), None reached = metrics.goal_reached(waypoints, case.goal, cfg.goal_tolerance) - gate = metrics.check_path( - waypoints, - obstacle_keys, - cfg.voxel_size, - cfg.robot_length, - cfg.robot_width, - cfg.ground_margin, - cfg.body_clearance, - ) - support = metrics.check_support( - waypoints, support_keys, cfg.voxel_size, cfg.support_radius_m, cfg.support_depth_m - ) - kinematics = metrics.check_kinematics( - waypoints, cfg.max_slope, cfg.max_step_m, cfg.kinematic_window_m - ) + gate = metrics.check_path(waypoints, obstacle_keys, cfg) + support = metrics.check_support(waypoints, support_keys, cfg) + kinematics = metrics.check_kinematics(waypoints, cfg) length = metrics.path_length(waypoints) success = reached and gate.valid and support.valid and kinematics.valid outcome = PlanOutcome( @@ -226,6 +216,7 @@ def _run_plan( min_clearance=gate.min_clearance_m, waypoints=waypoints.tolist(), collisions=gate.collision_points[:MAX_COLLISIONS_KEPT].tolist(), + collision_indices=gate.collision_indices[:MAX_COLLISIONS_KEPT].tolist(), unsupported=support.unsupported_points[:MAX_COLLISIONS_KEPT].tolist(), steep=kinematics.violation_points[:MAX_COLLISIONS_KEPT].tolist(), ) @@ -246,6 +237,7 @@ def _no_plan(plan_ms: float) -> PlanOutcome: min_clearance=None, waypoints=[], collisions=[], + collision_indices=[], unsupported=[], steep=[], ) @@ -286,18 +278,11 @@ def _dynamic_candidate( """ if online_wp is None or not online.success or final.success: return False, [] - new_keys = np.setdiff1d(final_keys, online_keys) + # Both come from np.unique, so the sort in setdiff1d is pure waste. + new_keys = np.setdiff1d(final_keys, online_keys, assume_unique=True) if not len(new_keys): return False, [] - gate = metrics.check_path( - online_wp, - new_keys, - cfg.voxel_size, - cfg.robot_length, - cfg.robot_width, - cfg.ground_margin, - cfg.body_clearance, - ) + gate = metrics.check_path(online_wp, new_keys, cfg) if gate.valid: return False, [] return True, gate.collision_points[:MAX_COLLISIONS_KEPT].tolist() @@ -336,7 +321,7 @@ def run_suite( miss = float(np.linalg.norm(np.asarray(case.goal) - np.asarray(case.start))) refs.append(metrics.Reference(miss, False, float("inf"), False)) continue - ref = metrics.reference_length(trajectory, case.start, case.goal, cfg.robot_height) + ref = metrics.reference_length(trajectory, case.start, case.goal, cfg) if not final_only[i]: # Only online-replayed cases need a causal snap onto the trajectory. if not ref.snapped: @@ -352,8 +337,6 @@ def run_suite( suite.dataset, case.id, ) - if case.l_ref is not None: - ref = metrics.Reference(case.l_ref, ref.snapped, ref.start_ts, ref.causal) refs.append(ref) # Final-only cases never replay online, so they take no checkpoint and their @@ -371,21 +354,28 @@ def run_suite( results: list[CaseResult | None] = [None] * len(suite.cases) - for ci in np.flatnonzero(final_only): - case, ref = suite.cases[ci], refs[ci] - outcome, _ = _run_plan(final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg) - if case.expect_fail: - # An infeasible case is passed by refusing it. - outcome = score_negative(outcome) - results[ci] = CaseResult( + def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: + """Fill the fields every case copies straight from its case and reference.""" + return CaseResult( id=case.id, dataset=suite.dataset, start=case.start, goal=case.goal, - weight=case.weight, tags=case.tags, l_ref=ref.length, l_ref_snapped=ref.snapped, + **rest, # type: ignore[arg-type] + ) + + for ci in np.flatnonzero(final_only): + case, ref = suite.cases[ci], refs[ci] + outcome, _ = _run_plan(final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg) + if case.expect_fail: + # An infeasible case is passed by refusing it. + outcome = score_negative(outcome) + results[ci] = _result( + case, + ref, plan_ts=float("inf"), online_voxels=len(final.occupied), map_update_ms=0.0, @@ -435,15 +425,9 @@ def process_checkpoint( if case.expect_final_fail else _dynamic_candidate(online_out, final_out, online_wp, keys, obstacle_keys, cfg) ) - results[ci] = CaseResult( - id=case.id, - dataset=suite.dataset, - start=case.start, - goal=case.goal, - weight=case.weight, - tags=case.tags, - l_ref=ref.length, - l_ref_snapped=ref.snapped, + results[ci] = _result( + case, + ref, plan_ts=float(checkpoints.times[k]), online_voxels=len(keys), map_update_ms=map_update_ms, @@ -476,27 +460,19 @@ def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64]]]: """Walk the delta chain once, yielding the incremental occupancy at each case's plan time. Only voxels mapped by then are present, so obstacles the sensor never saw are naturally excluded from the online check.""" - for k, (keys, _observed) in enumerate(checkpoints.iter_snapshots()): + for k, keys in enumerate(checkpoints.iter_snapshots()): if k in active: yield k, keys - # The planner releases the GIL and parallelizes updates internally via a - # shared rayon pool, so a few worker threads interleave the serial parts - # of checkpoint updates without oversubscribing. The semaphore bounds how - # many reconstructed snapshots are held in memory at once. - if threads > 1: - in_flight = threading.BoundedSemaphore(threads * 2) - with ThreadPoolExecutor(max_workers=threads) as pool: - futures = [] - for item in snapshot_stream(): - in_flight.acquire() - futures.append(pool.submit(task, *item)) - for future in futures: - future.result() - else: - online_planner = cfg.make_planner() - for k, keys in snapshot_stream(): - process_checkpoint(k, keys, online_planner) + # The semaphore caps how many reconstructed snapshots are held in memory. + in_flight = threading.BoundedSemaphore(max(1, threads) * 2) + with ThreadPoolExecutor(max_workers=max(1, threads)) as pool: + futures = [] + for item in snapshot_stream(): + in_flight.acquire() + futures.append(pool.submit(task, *item)) + for future in futures: + future.result() done = [r for r in results if r is not None] if len(done) != len(suite.cases): @@ -547,10 +523,8 @@ def evaluate( # aggregate is over the online cases only. Final aggregates cover them all. online = [c for c in cases if not c.final_only] - def wmean(values: list[float], items: list[CaseResult]) -> float: - if not items: - return 0.0 - return float(np.average(values, weights=[c.weight for c in items])) + def mean(values: list[float]) -> float: + return float(np.mean(values)) if values else 0.0 outcome_names = { (True, True): "both", @@ -569,18 +543,18 @@ def wmean(values: list[float], items: list[CaseResult]) -> float: by_tag[tag] = TagStats( n=len(tc), n_online=len(oc), - inc_score=wmean([c.online.spl for c in oc], oc), - fin_score=wmean([c.final.spl for c in tc], tc), + inc_score=mean([c.online.spl for c in oc]), + fin_score=mean([c.final.spl for c in tc]), inc_success=sum(c.online.success for c in oc), fin_success=sum(c.final.success for c in tc), ) return Report( - score=wmean([c.online.spl for c in online], online), - score_soft=wmean( - [c.soft_progress if not c.online.success else c.online.spl for c in online], online + score=mean([c.online.spl for c in online]), + score_soft=mean( + [c.soft_progress if not c.online.success else c.online.spl for c in online] ), - final_score=wmean([c.final.spl for c in cases], cases), + final_score=mean([c.final.spl for c in cases]), n_cases=len(cases), n_online=len(online), n_success=sum(c.online.success for c in online), diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py index 7f9b66311e..e7f5bbfd44 100644 --- a/dimos/navigation/nav_3d/evaluator/tagging.py +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -21,16 +21,19 @@ from __future__ import annotations +import itertools from typing import TYPE_CHECKING import numpy as np -from dimos.navigation.nav_3d.evaluator.final_map import ( +from dimos.navigation.nav_3d.evaluator.metrics import ( + MARGIN_CAP_M, + arc_lengths, + body_frames, densify, - keys_contain, - voxel_keys, + path_length, ) -from dimos.navigation.nav_3d.evaluator.metrics import MARGIN_CAP_M, body_frames +from dimos.navigation.nav_3d.evaluator.voxel_keys import keys_contain, voxel_keys if TYPE_CHECKING: from numpy.typing import NDArray @@ -73,7 +76,7 @@ ) -def _elevation_tags( +def elevation_tags( start: tuple[float, float, float], goal: tuple[float, float, float] ) -> list[str]: """Elevation from the case endpoints, matching how generation labels them. @@ -92,7 +95,7 @@ def _elevation_tags( def _corridor_width( - route: NDArray[np.float32], occupied_keys: NDArray[np.int64], cfg: EvalConfig + samples: NDArray[np.float32], occupied_keys: NDArray[np.int64], cfg: EvalConfig ) -> NDArray[np.float64]: """Free lateral width at each densified sample, at body height. @@ -101,7 +104,6 @@ def _corridor_width( voxel, capped when the passage is open. This is the room the body has to pass, not the room the feet have to stand. """ - samples = densify(route, cfg.voxel_size) _, lateral, _ = body_frames(samples, cfg.robot_length) mid_z = (cfg.ground_margin + cfg.body_clearance) / 2.0 origin = samples.astype(np.float64) + np.array([0.0, 0.0, mid_z]) @@ -122,16 +124,12 @@ def side_dist(sign: float) -> NDArray[np.float64]: def _runs(mask: NDArray[np.bool_], arc: NDArray[np.float64]) -> list[tuple[float, float]]: """Arc-length (start, end) of every maximal True run in mask.""" out: list[tuple[float, float]] = [] - i, n = 0, len(mask) - while i < n: - if mask[i]: - j = i + 1 - while j < n and mask[j]: - j += 1 - out.append((float(arc[i]), float(arc[j - 1]))) - i = j - else: - i += 1 + i = 0 + for value, group in itertools.groupby(mask): + n = sum(1 for _ in group) + if value: + out.append((float(arc[i]), float(arc[i + n - 1]))) + i += n return out @@ -151,11 +149,11 @@ def _corridor_tags( ) -> list[str]: if len(occupied_keys) == 0 or len(route) < 2: return [] - width = _corridor_width(route, occupied_keys, cfg) + samples = densify(route, cfg.voxel_size) + width = _corridor_width(samples, occupied_keys, cfg) tight = cfg.robot_width + 2.0 * MARGIN_CAP_M roomy = cfg.robot_width + 2.0 * cfg.robot_length - samples = densify(route, cfg.voxel_size) - arc = np.concatenate([[0.0], np.cumsum(np.linalg.norm(np.diff(samples, axis=0), axis=1))]) + arc = arc_lengths(samples) # A real passage is at least the robot's own body wide. Anything tighter is # furniture or map noise the robot could not have walked through, so it does # not count as a passage. @@ -196,7 +194,7 @@ def _is_local( eucl = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) if eucl < cfg.robot_length: return False - arc = float(np.linalg.norm(np.diff(route, axis=0), axis=1).sum()) + arc = path_length(route) return LOCAL_SPAN_MIN_FRAC * eucl <= arc <= LOCAL_DETOUR_MAX * eucl + cfg.robot_length @@ -215,7 +213,7 @@ def route_tags( long detour. Deterministic and free of provenance: the caller prepends auto or manual. """ - tags = _elevation_tags(start, goal) + tags = elevation_tags(start, goal) if route is not None and len(route) >= 2 and _is_local(route, start, goal, cfg): tags += _corridor_tags(route, occupied_keys, cfg) return tags diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 85c7d4d60f..ee1a1e8ed3 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -31,10 +31,7 @@ FinalMap, MapCheckpoints, encode_deltas, - key_centers, - keys_contain, replay_frames, - voxel_keys, ) from dimos.navigation.nav_3d.evaluator.generate import ( Candidate, @@ -55,6 +52,7 @@ score_negative, ) from dimos.navigation.nav_3d.evaluator.tagging import GEOMETRIC_TAGS, route_tags +from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys if TYPE_CHECKING: from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner @@ -80,17 +78,13 @@ def _wall(x: float) -> np.ndarray: return np.stack([np.full(ys.size, x), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) +def _cfg(**overrides: float) -> EvalConfig: + return replace(EvalConfig(voxel_size=VOXEL), **overrides) + + def _gate(waypoints: np.ndarray, obstacles: np.ndarray) -> metrics.GateResult: keys = np.unique(voxel_keys(obstacles, VOXEL)) - return metrics.check_path( - waypoints, - keys, - VOXEL, - robot_length=0.7, - robot_width=0.31, - ground_margin=0.25, - body_clearance=0.45, - ) + return metrics.check_path(waypoints, keys, _cfg()) def test_gate_blocks_wall_crossing() -> None: @@ -186,16 +180,16 @@ def test_reference_length_snaps_to_trajectory() -> None: ).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) # Walking toward a never-yet-visited goal is not causal. - ref = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) + ref = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), _cfg()) assert ref.snapped assert ref.length == pytest.approx(10.0, abs=0.01) assert not ref.causal assert ref.start_ts == float("inf") # Returning to the walk's origin is causal. - ref = metrics.reference_length(traj, (10, 0, 0), (0, 0, 0), robot_height=0.3) + ref = metrics.reference_length(traj, (10, 0, 0), (0, 0, 0), _cfg()) assert ref.causal assert 9.0 <= ref.start_ts <= 10.0 - ref = metrics.reference_length(traj, (0, 5, 0), (10, 0, 0), robot_height=0.3) + ref = metrics.reference_length(traj, (0, 5, 0), (10, 0, 0), _cfg()) assert not ref.snapped assert ref.start_ts == float("inf") @@ -207,7 +201,7 @@ def test_reference_length_uses_shortest_revisit() -> None: back = detour[::-1] positions = np.concatenate([out, detour, back]).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 30, len(positions)), positions=positions) - ref = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), robot_height=0.3) + ref = metrics.reference_length(traj, (0, 0, 0), (10, 0, 0), _cfg()) assert ref.snapped assert ref.length == pytest.approx(10.0, abs=0.2) @@ -292,26 +286,10 @@ def test_checkpoint_deltas_roundtrip() -> None: np.array([2, 3, 4, 5], dtype=np.int64), np.array([4, 5], dtype=np.int64), ] - observed = [ - np.array([1, 2, 3], dtype=np.int64), - np.array([1, 2, 3, 4, 5], dtype=np.int64), - np.array([1, 2, 3, 4, 5, 9], dtype=np.int64), - ] added, removed = encode_deltas(snapshots) - observed_added, _ = encode_deltas(observed) - ckpt = MapCheckpoints( - times=np.arange(3, dtype=np.float64), - added=added, - removed=removed, - observed_added=observed_added, - ) - seen = np.array([], dtype=np.int64) - for (orig_keys, orig_obs), (keys, obs_new) in zip( - zip(snapshots, observed, strict=True), ckpt.iter_snapshots(), strict=True - ): - assert np.array_equal(orig_keys, keys) - seen = np.union1d(seen, obs_new) - assert np.array_equal(orig_obs, seen) + ckpt = MapCheckpoints(times=np.arange(3, dtype=np.float64), added=added, removed=removed) + for original, keys in zip(snapshots, ckpt.iter_snapshots(), strict=True): + assert np.array_equal(original, keys) def test_replay_frames_snapshots_grow_with_time() -> None: @@ -331,20 +309,20 @@ def frame_at(ts: float, x: float) -> Frame: frame_at(2.1, 11.0), ] times = np.array([0.5, 1.5, np.inf]) - final, snapshots, observed = replay_frames(frames, mapper, VOXEL, times) + final, snapshots = replay_frames(frames, mapper, VOXEL, times) assert final.frames == 6 sizes = [len(s) for s in snapshots] assert 0 < sizes[0] < sizes[1] < sizes[2] assert np.array_equal(snapshots[2], final.occupied_keys) for earlier, later in itertools.pairwise(snapshots): assert keys_contain(later, earlier).all() - # The observed set holds raw returns causally: wall 1 by the first - # checkpoint, wall 3 only at the end. + # Each checkpoint holds the walls mapped by its time: wall 1 by the first, + # wall 3 only at the end. wall1 = np.unique(voxel_keys(_wall(5.0), VOXEL)) wall3 = np.unique(voxel_keys(_wall(11.0), VOXEL)) - assert keys_contain(observed[0], wall1).all() - assert not keys_contain(observed[0], wall3).any() - assert keys_contain(observed[2], wall3).all() + assert keys_contain(snapshots[0], wall1).all() + assert not keys_contain(snapshots[0], wall3).any() + assert keys_contain(snapshots[2], wall3).all() class _StubPlanner: @@ -517,16 +495,16 @@ def test_dynamic_candidate_flags_route_blocked_by_new_occupancy() -> None: def test_check_kinematics_rejects_cliff_jumps() -> None: stairs = np.array([[0, 0, 0], [0.4, 0, 0.16], [0.8, 0, 0.32]], dtype=np.float32) - assert metrics.check_kinematics(stairs, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid + assert metrics.check_kinematics(stairs, _cfg(max_slope=1.0)).valid riser = np.array([[0, 0, 0], [0.08, 0, 0.16]], dtype=np.float32) - assert metrics.check_kinematics(riser, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid + assert metrics.check_kinematics(riser, _cfg(max_slope=1.0)).valid # A double riser between adjacent cells is quantization, not a cliff. quantized = np.array( [[0, 0, 0], [0.4, 0, 0.08], [0.56, 0, 0.4], [0.96, 0, 0.48]], dtype=np.float32 ) - assert metrics.check_kinematics(quantized, max_slope=1.0, max_step_m=0.2, window_m=0.5).valid + assert metrics.check_kinematics(quantized, _cfg(max_slope=1.0)).valid cliff = np.array([[0, 0, 0], [0.2, 0, 0.9], [1, 0, 0.9]], dtype=np.float32) - result = metrics.check_kinematics(cliff, max_slope=1.0, max_step_m=0.2, window_m=0.5) + result = metrics.check_kinematics(cliff, _cfg(max_slope=1.0)) assert not result.valid assert len(result.violation_points) >= 1 @@ -556,7 +534,7 @@ def test_save_suite_roundtrip(tmp_path: Path) -> None: suite = Suite( dataset="demo", cases=[ - Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), weight=2.0, tags=["x"]), + Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), tags=["x"]), Case(id="neg", start=(0.0, 0.0, 0.0), goal=(5.0, 5.0, 5.0), expect_fail=True), ], lidar_stream="other_lidar", @@ -583,13 +561,12 @@ def test_load_suite(tmp_path: Path) -> None: " - id: a\n" " start: [0, 0, 0]\n" " goal: [1, 2, 3]\n" - " weight: 2\n" " tags: [stairs]\n" ) suite = load_suite(manifest) assert suite.dataset == "demo" assert suite.cases[0].goal == (1.0, 2.0, 3.0) - assert suite.cases[0].weight == 2.0 + assert suite.cases[0].tags == ["stairs"] manifest.write_text( "dataset: demo\ncases:\n" @@ -652,7 +629,6 @@ def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, dataset=dataset, start=case.start, goal=case.goal, - weight=1.0, tags=[], l_ref=1.0, l_ref_snapped=False, @@ -732,15 +708,11 @@ def _ywall(y: float, x_lo: float, x_hi: float) -> np.ndarray: return np.stack([xs.ravel(), np.full(xs.size, y), zs.ravel()], axis=1, dtype=np.float32) -def _tag_cfg() -> EvalConfig: - return EvalConfig(voxel_size=VOXEL) - - def _tags(route: np.ndarray, keys: np.ndarray) -> list[str]: """Tag a synthetic route, taking its endpoints for elevation.""" start = (float(route[0, 0]), float(route[0, 1]), float(route[0, 2])) goal = (float(route[-1, 0]), float(route[-1, 1]), float(route[-1, 2])) - return route_tags(start, goal, route, keys, _tag_cfg()) + return route_tags(start, goal, route, keys, _cfg()) def test_ground_truth_route_returns_walked_slice() -> None: @@ -749,12 +721,12 @@ def test_ground_truth_route_returns_walked_slice() -> None: detour = np.stack([np.full(101, 10.0), np.linspace(0, 6, 101), np.full(101, 0.3)], axis=1) positions = np.concatenate([out, detour]).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 20, len(positions)), positions=positions) - route = metrics.ground_truth_route(traj, (0, 0, 0), (10, 6, 0), robot_height=0.3) + route = metrics.ground_truth_route(traj, (0, 0, 0), (10, 6, 0), _cfg()) assert route is not None # Foot level (sensor height removed) and it walks the full out-and-detour. assert abs(route[0, 2]) < 1e-5 assert metrics.path_length(route) == pytest.approx(16.0, abs=0.2) - assert metrics.ground_truth_route(traj, (0, 20, 0), (10, 6, 0), robot_height=0.3) is None + assert metrics.ground_truth_route(traj, (0, 20, 0), (10, 6, 0), _cfg()) is None def test_ground_truth_route_orients_start_to_goal() -> None: @@ -764,7 +736,7 @@ def test_ground_truth_route_orients_start_to_goal() -> None: positions = np.stack([xs, np.zeros(101), xs * 0.25], axis=1).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) # Start high at x=8, goal low at x=2: a downhill traverse. - route = metrics.ground_truth_route(traj, (8, 0, 1.7), (2, 0, 0.2), robot_height=0.3) + route = metrics.ground_truth_route(traj, (8, 0, 1.7), (2, 0, 0.2), _cfg()) assert route is not None # Runs start-to-goal: x decreasing from ~8 to ~2, so elevation reads downhill. assert route[0, 0] > 6.0 and route[-1, 0] < 4.0 @@ -831,9 +803,9 @@ def test_route_tags_gate_excludes_detour_routes() -> None: walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) keys = np.unique(voxel_keys(walls, VOXEL)) direct = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) - assert "corridor" in route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), direct, keys, _tag_cfg()) + assert "corridor" in route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), direct, keys, _cfg()) detour = np.array([[0, 0, 0], [2, 6, 0], [4, 0, 0]], dtype=np.float32) - assert route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), detour, keys, _tag_cfg()) == ["flat"] + assert route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), detour, keys, _cfg()) == ["flat"] def test_route_tags_are_all_geometric() -> None: diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 97135ed308..d5580ff262 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -30,6 +30,7 @@ import rerun.blueprint as rrb from scipy.spatial.transform import Rotation +from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.recording import load_trajectory @@ -126,50 +127,13 @@ def _outcome_color(outcome: PlanOutcome) -> list[int]: return UNREACHED_PATH_COLOR -def _travel_dirs( - points: NDArray[np.float32], waypoints: NDArray[np.float32], span: float -) -> NDArray[np.float64]: - """Rigid-body heading at each point: the chord from span/2 behind it to - span/2 ahead along the path, the rear-feet to front-feet direction.""" - if len(waypoints) < 2: - return np.tile(np.array([1.0, 0.0, 0.0]), (len(points), 1)) - wp = waypoints.astype(np.float64) - seg = np.linalg.norm(np.diff(wp, axis=0), axis=1) - arc = np.concatenate([[0.0], np.cumsum(seg)]) - a2, v2 = wp[:-1, :2], np.diff(wp[:, :2], axis=0) - hlen2 = np.maximum((v2 * v2).sum(1), 1e-12) - half = span / 2.0 - dirs = np.empty((len(points), 3)) +def _thin_by_gap(points: NDArray[np.float32], gap: float) -> NDArray[np.int64]: + """Indices of points at least gap apart along the sequence.""" + kept: list[int] = [] for i, p in enumerate(points): - t = np.clip(((p[:2] - a2) * v2).sum(1) / hlen2, 0.0, 1.0) - s = int(np.argmin(((a2 + t[:, None] * v2 - p[:2]) ** 2).sum(1))) - pos = arc[s] + t[s] * seg[s] - lo, hi = np.clip([pos - half, pos + half], arc[0], arc[-1]) - d = np.array( - [np.interp(hi, arc, wp[:, c]) - np.interp(lo, arc, wp[:, c]) for c in range(3)] - ) - dirs[i] = d / max(float(np.linalg.norm(d)), 1e-9) - return dirs - - -def _thin_by_gap(points: NDArray[np.float32], gap: float) -> NDArray[np.float32]: - """Keep points at least gap apart along the sequence.""" - kept: list[NDArray[np.float32]] = [] - for p in points: - if not kept or float(np.linalg.norm(p - kept[-1])) >= gap: - kept.append(p) - return np.asarray(kept, dtype=np.float32) - - -def _body_box_quat(direction: NDArray[np.float64]) -> rr.Quaternion: - """Box orientation for a body travelling along direction: yaw and pitch from - the chord, no roll.""" - fwd = direction - lateral = np.cross([0.0, 0.0, 1.0], fwd) - ln = float(np.linalg.norm(lateral)) - lateral = lateral / ln if ln > 1e-6 else np.array([0.0, 1.0, 0.0]) - up = np.cross(fwd, lateral) - return rr.Quaternion(xyzw=Rotation.from_matrix(np.column_stack([fwd, lateral, up])).as_quat()) + if not kept or float(np.linalg.norm(p - points[kept[-1]])) >= gap: + kept.append(i) + return np.asarray(kept, dtype=np.int64) def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) -> None: @@ -180,14 +144,18 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) rr.LineStrips3D([outcome.waypoints], colors=[_outcome_color(outcome)], radii=radius), static=True, ) - if outcome.collisions: + if outcome.collision_indices: # The gate's body box at each colliding foot sample: the robot length # and width, centered over the path point and rotated in place (yaw and # pitch from the chord), elevated over the legs into the ground-margin - # to body-clearance band. Thinned to about a body length apart so the - # boxes read as distinct bodies instead of one overlapping smear. - feet = _thin_by_gap(np.asarray(outcome.collisions, dtype=np.float32), cfg.robot_length) + # to body-clearance band. Rebuilt from the gate's own sample indices so + # the drawn boxes are the boxes it rejected. Thinned to about a body + # length apart so they read as distinct bodies, not one smear. waypoints = np.asarray(outcome.waypoints, dtype=np.float32) + samples = metrics.densify(waypoints, cfg.voxel_size / 2) + axes = np.stack(metrics.body_frames(samples, cfg.robot_length), axis=-1) + idx = np.asarray(outcome.collision_indices, dtype=np.int64) + idx = idx[_thin_by_gap(samples[idx], cfg.robot_length)] mid = np.array([0.0, 0.0, (cfg.ground_margin + cfg.body_clearance) / 2.0]) half = [ cfg.robot_length / 2.0, @@ -197,11 +165,9 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) rr.log( f"{entity}/collisions", rr.Boxes3D( - half_sizes=np.tile(half, (len(feet), 1)), - centers=feet + mid, - quaternions=[ - _body_box_quat(d) for d in _travel_dirs(feet, waypoints, cfg.robot_length) - ], + half_sizes=np.tile(half, (len(idx), 1)), + centers=samples[idx] + mid, + quaternions=Rotation.from_matrix(axes[idx]).as_quat(), colors=[[*COLLISION_COLOR, COLLISION_FILL_ALPHA]], fill_mode=rr.components.FillMode.Solid, ), diff --git a/dimos/navigation/nav_3d/evaluator/voxel_keys.py b/dimos/navigation/nav_3d/evaluator/voxel_keys.py new file mode 100644 index 0000000000..76dd721400 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/voxel_keys.py @@ -0,0 +1,74 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Voxel indices packed into sortable int64 keys. + +Membership tests over a map run as a sorted-array search on these keys rather +than a per-point spatial query, which is what makes the gates cheap enough to +sweep a whole path. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np + +if TYPE_CHECKING: + from numpy.typing import NDArray + +_KEY_OFFSET = 1 << 20 + + +def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: + """Pack voxel indices into sortable int64 keys, one per point.""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET + return (idx[:, 0] << 42) | (idx[:, 1] << 21) | idx[:, 2] + + +def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: + """Voxel center positions for packed keys, the inverse of voxel_keys.""" + mask = (1 << 21) - 1 + idx = np.stack([keys >> 42, (keys >> 21) & mask, keys & mask], axis=1) - _KEY_OFFSET + return ((idx + 0.5) * voxel_size).astype(np.float32) + + +def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: + if len(sorted_keys) == 0: + return np.zeros(len(query), dtype=bool) + pos = np.clip(np.searchsorted(sorted_keys, query), 0, len(sorted_keys) - 1) + return np.asarray(sorted_keys[pos] == query) + + +def cylinder_offsets( + radius: float, z_lo: float, z_hi: float, voxel_size: float +) -> NDArray[np.int64]: + """Integer voxel offsets forming a vertical cylinder.""" + r_vox = int(np.ceil(radius / voxel_size)) + span = np.arange(-r_vox, r_vox + 1) + dx, dy = np.meshgrid(span, span, indexing="ij") + in_disc = (dx * voxel_size) ** 2 + (dy * voxel_size) ** 2 <= radius**2 + dz = np.arange(int(np.floor(z_lo / voxel_size)), int(np.ceil(z_hi / voxel_size)) + 1) + disc = np.stack([dx[in_disc], dy[in_disc]], axis=1) + out = np.concatenate([np.hstack([disc, np.full((len(disc), 1), z)]) for z in dz]) + return np.asarray(out, dtype=np.int64) + + +def offset_keys( + points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float +) -> NDArray[np.int64]: + """Keys of every (point voxel + offset) pair, shape (P * O,).""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET + swept = idx[:, None, :] + offsets[None, :, :] + return np.asarray((swept[..., 0] << 42) | (swept[..., 1] << 21) | swept[..., 2]) From aff2a84c11cce90134f540dc5089d53f8f936049 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 20 Jul 2026 17:43:15 -0700 Subject: [PATCH 20/29] Add end time stamp, mostly hack around door closing --- dimos/navigation/nav_3d/evaluator/cases.py | 11 +- .../evaluator/cases/mid360_athens_stairs.yaml | 194 +++++- dimos/navigation/nav_3d/evaluator/cli.py | 6 +- .../navigation/nav_3d/evaluator/final_map.py | 13 +- .../navigation/nav_3d/evaluator/recording.py | 12 +- dimos/navigation/nav_3d/evaluator/runner.py | 18 +- .../nav_3d/evaluator/test_evaluator.py | 556 +----------------- dimos/navigation/nav_3d/evaluator/viz.py | 2 +- 8 files changed, 240 insertions(+), 572 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index 9fe578af57..ac74a13ee1 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -50,8 +50,10 @@ class Suite: lidar_stream: str = "pointlio_lidar" odom_stream: str = "pointlio_odometry" # Recording location override, defaulting to data/.db. - # Set this to keep a recording outside data/. + # Set this to keep a recording outside data/, in case you don't want it to be tracked. db: str | None = None + # Discard frames after this timestamp + end_ts: int | None = None path: Path | None = None def db_path(self) -> Path: @@ -59,6 +61,10 @@ def db_path(self) -> Path: return Path(self.db).expanduser() return resolve_named_path(self.dataset, ".db") + def end_ts_seconds(self) -> float | None: + """end_ts in the recording's second-based observation timestamps.""" + return None if self.end_ts is None else self.end_ts / 1e9 + def load_suite(path: Path) -> Suite: raw = yaml.safe_load(path.read_text()) @@ -95,6 +101,7 @@ def load_suite(path: Path) -> Suite: lidar_stream=str(raw.get("lidar_stream", "pointlio_lidar")), odom_stream=str(raw.get("odom_stream", "pointlio_odometry")), db=str(raw["db"]) if "db" in raw else None, + end_ts=int(raw["end_ts"]) if "end_ts" in raw else None, path=path, ) @@ -118,6 +125,8 @@ def save_suite(suite: Suite, path: Path | None = None) -> Path: doc["lidar_stream"] = suite.lidar_stream if suite.odom_stream != "pointlio_odometry": doc["odom_stream"] = suite.odom_stream + if suite.end_ts is not None: + doc["end_ts"] = suite.end_ts entries = [] for case in suite.cases: entry: dict[str, object] = { diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml index 3fa8eeec83..ca53f2a91a 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml @@ -1,47 +1,203 @@ dataset: mid360_athens_stairs +end_ts: 1783527459453386006 cases: -- id: auto_00_up - start: [-0.12, -0.6, -0.32] - goal: [1.32, -0.84, 2.72] - tags: [auto, stairs, up, long] +- id: auto_00_down + start: [1.32, -0.84, 2.72] + goal: [-2.52, -0.52, -0.32] + tags: [auto, stairs, down, long] - id: auto_01_up start: [7.24, -3.96, -6.08] goal: [6.44, -5.56, -1.44] tags: [auto, stairs, up, long] -- id: auto_02_down - start: [8.04, -0.76, 3.04] - goal: [-2.36, -4.36, -0.32] - tags: [auto, stairs, down, long] +- id: auto_02_up + start: [0.68, -4.12, -0.32] + goal: [8.04, -0.76, 3.04] + tags: [auto, stairs, up, long] - id: auto_03_down start: [5.88, -4.52, 2.96] - goal: [2.28, -6.52, -0.32] + goal: [-2.36, -4.36, -0.32] tags: [auto, stairs, down, long] - id: auto_04_down start: [-0.2, -3.0, 2.56] goal: [-2.04, 3.16, -0.48] tags: [auto, stairs, down, long] - id: auto_05_down - start: [0.52, -4.68, -0.32] + start: [7.4, -4.04, 0.56] goal: [5.96, -3.64, -3.52] tags: [auto, stairs, down, long] - id: auto_06_up start: [6.36, -5.4, -4.48] - goal: [-2.36, 0.44, -0.4] + goal: [-0.04, 0.04, -0.32] tags: [auto, stairs, up, long] - id: auto_07_down start: [6.68, -5.56, 2.08] goal: [5.72, -5.48, -1.04] - tags: [auto, stairs, down, long, narrow] + tags: [auto, stairs, down, long] - id: auto_08_down - start: [-0.2, -3.0, 2.56] + start: [-0.04, -1.96, 2.56] goal: [7.16, -3.8, -3.12] tags: [auto, stairs, down, long] -- id: auto_09_flat +- id: auto_09_down + start: [5.0, -1.88, 2.96] + goal: [2.28, -6.52, -0.32] + tags: [auto, stairs, down, long] +- id: auto_10_down + start: [2.68, -2.28, 2.72] + goal: [3.64, -4.12, -0.32] + tags: [auto, stairs, down, long] +- id: auto_11_down + start: [6.92, -2.12, 3.04] + goal: [-2.44, -2.44, -0.32] + tags: [auto, stairs, down, long] +- id: auto_12_down + start: [5.8, -3.56, 2.96] + goal: [0.44, -2.2, -0.32] + tags: [auto, stairs, down, long] +- id: auto_13_down + start: [3.16, -1.0, 2.88] + goal: [-0.12, -0.6, -0.32] + tags: [auto, stairs, down, long] +- id: auto_14_down + start: [5.56, -2.6, 2.96] + goal: [-2.36, 0.44, -0.4] + tags: [auto, stairs, down, long] +- id: auto_15_down + start: [5.64, -3.88, -0.32] + goal: [6.44, -3.72, -3.52] + tags: [auto, stairs, down, long] +- id: auto_16_down + start: [4.36, -1.24, 2.96] + goal: [-2.44, 2.36, -0.4] + tags: [auto, stairs, down, long] +- id: auto_17_down + start: [2.76, -4.2, -0.32] + goal: [4.84, -4.28, -3.44] + tags: [auto, stairs, down, long] +- id: auto_18_up + start: [0.52, -3.16, -0.32] + goal: [3.48, -1.72, 2.88] + tags: [auto, stairs, up, long] +- id: auto_19_down + start: [6.52, -1.16, 3.04] + goal: [0.2, -1.32, -0.32] + tags: [auto, stairs, down, long] +- id: auto_20_down + start: [1.96, -2.84, 2.56] + goal: [-1.96, 0.12, -0.4] + tags: [auto, stairs, down, long] +- id: auto_21_down + start: [0.44, -1.24, 2.64] + goal: [-2.44, 2.36, -0.4] + tags: [auto, stairs, down, long] +- id: auto_22_down + start: [7.96, -0.12, 3.04] + goal: [-0.12, -0.6, -0.32] + tags: [auto, stairs, down, long] +- id: auto_23_down + start: [-0.04, -4.04, -0.32] + goal: [6.36, -5.56, -4.48] + tags: [auto, stairs, down, long] +- id: auto_24_up + start: [4.52, -4.36, -0.32] + goal: [5.88, -4.52, 2.96] + tags: [auto, stairs, up, long] +- id: auto_25_down + start: [4.04, -1.4, 2.96] + goal: [-2.44, -2.44, -0.32] + tags: [auto, stairs, down, long] +- id: auto_26_down + start: [0.52, -4.68, -0.32] + goal: [6.44, -3.72, -3.52] + tags: [auto, stairs, down, long] +- id: auto_27_down + start: [0.2, -3.64, -0.32] + goal: [4.84, -4.28, -3.44] + tags: [auto, stairs, down, long] +- id: auto_28_down + start: [0.28, -3.64, 2.56] + goal: [-2.52, -0.52, -0.32] + tags: [auto, stairs, down, long] +- id: auto_29_down + start: [6.36, -3.8, -0.24] + goal: [6.36, -5.56, -4.48] + tags: [auto, stairs, down, long] +- id: auto_30_down + start: [-0.2, -3.0, 2.56] + goal: [5.16, -5.08, -0.56] + tags: [auto, stairs, down, long] +- id: auto_31_down + start: [1.32, -0.84, 2.72] + goal: [-2.44, -2.44, -0.32] + tags: [auto, stairs, down, long] +- id: auto_32_down + start: [1.32, -0.84, 2.72] + goal: [-2.36, -4.36, -0.32] + tags: [auto, stairs, down, long] +- id: auto_33_down + start: [1.32, -0.84, 2.72] + goal: [-0.12, -0.6, -0.32] + tags: [auto, stairs, down, long] +- id: auto_34_down + start: [2.68, -2.28, 2.72] + goal: [-2.44, -2.44, -0.32] + tags: [auto, stairs, down, long] +- id: auto_35_down + start: [2.68, -2.28, 2.72] + goal: [-2.36, -4.36, -0.32] + tags: [auto, stairs, down, long] +- id: auto_36_down + start: [3.16, -1.0, 2.88] + goal: [-2.52, -0.52, -0.32] + tags: [auto, stairs, down, long] +- id: auto_37_down + start: [4.36, -1.24, 2.96] + goal: [-2.36, -4.36, -0.32] + tags: [auto, stairs, down, long] +- id: auto_38_down + start: [5.56, -2.6, 2.96] + goal: [-2.44, 2.36, -0.4] + tags: [auto, stairs, down, long] +- id: auto_39_down + start: [5.56, -2.6, 2.96] + goal: [-2.36, -4.36, -0.32] + tags: [auto, stairs, down, long] +- id: auto_40_down + start: [5.56, -2.6, 2.96] + goal: [-0.12, -0.6, -0.32] + tags: [auto, stairs, down, long] +- id: auto_41_down + start: [5.88, -4.52, 2.96] + goal: [-2.52, -0.52, -0.32] + tags: [auto, stairs, down, long] +- id: auto_42_down + start: [5.88, -4.52, 2.96] + goal: [-2.44, -2.44, -0.32] + tags: [auto, stairs, down, long] +- id: auto_43_down + start: [5.88, -4.52, 2.96] + goal: [-2.44, 2.36, -0.4] + tags: [auto, stairs, down, long] +- id: auto_44_down + start: [5.88, -4.52, 2.96] + goal: [-0.12, -0.6, -0.32] + tags: [auto, stairs, down, long] +- id: auto_45_down + start: [2.68, -2.28, 2.72] + goal: [-2.44, 2.36, -0.4] + tags: [auto, stairs, down, long] +- id: auto_46_down + start: [6.52, -1.16, 3.04] + goal: [-2.52, -0.52, -0.32] + tags: [auto, stairs, down, long] +- id: auto_47_down + start: [8.04, -0.76, 3.04] + goal: [-2.52, -0.52, -0.32] + tags: [auto, stairs, down, long] +- id: auto_48_up + start: [0.52, -3.16, -0.32] + goal: [5.88, -4.52, 2.96] + tags: [auto, stairs, up, long] +- id: auto_49_flat start: [5.08, -3.8, -6.48] goal: [7.24, -3.96, -6.08] tags: [auto, flat] -- id: manual_00 - start: [1.8, -4.44, -0.32] - goal: [5.24, -4.2, -0.32] - tags: [manual, flat, doorway, negative] - expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 1c609e365c..88d870f3ee 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -424,7 +424,7 @@ def retag( suite = load_suite(manifest) cfg = EvalConfig() final = load_or_build_final_map(suite.db_path(), suite, cfg) - trajectory = load_trajectory(suite.db_path(), suite.odom_stream) + trajectory = load_trajectory(suite.db_path(), suite.odom_stream, suite.end_ts_seconds()) changed = 0 for case in suite.cases: if "auto" not in case.tags: @@ -461,7 +461,9 @@ def pick_case( from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height store, final = _open(dataset) - trajectory = load_trajectory(store.suite.db_path(), store.suite.odom_stream) + trajectory = load_trajectory( + store.suite.db_path(), store.suite.odom_stream, store.suite.end_ts_seconds() + ) foot = trajectory.positions - np.array([0.0, 0.0, store.cfg.robot_height], dtype=np.float32) pick_cases( dataset, diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index e857d812ce..ae489cab63 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -86,13 +86,16 @@ def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: def _final_params(suite: Suite, cfg: EvalConfig) -> dict[str, float | int | str]: - return { + params: dict[str, float | int | str] = { **cfg.mapper_fingerprint(), "align_tol": cfg.align_tol, "lidar_stream": suite.lidar_stream, "odom_stream": suite.odom_stream, "cache_version": CACHE_VERSION, } + if suite.end_ts is not None: + params["end_ts"] = suite.end_ts + return params def replay_frames( @@ -157,7 +160,9 @@ def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> Fin logger.info("building final map for %s (cache miss)", db_path.name) final, _ = replay_frames( - iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol), + iter_world_frames( + db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() + ), cfg.make_mapper(), cfg.voxel_size, np.array([], dtype=np.float64), @@ -206,7 +211,9 @@ def load_or_build_checkpoints( logger.info("building %d map checkpoints for %s (cache miss)", len(times), db_path.name) final, snapshots = replay_frames( - iter_world_frames(db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol), + iter_world_frames( + db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() + ), cfg.make_mapper(), cfg.voxel_size, times, diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py index 384ebcd5c1..1981154081 100644 --- a/dimos/navigation/nav_3d/evaluator/recording.py +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -60,11 +60,13 @@ def iter_world_frames( lidar_stream: str, odom_stream: str, align_tol: float = 0.05, + end_ts: float | None = None, ) -> Iterator[Frame]: """Yield lidar frames registered into the world by their aligned odometry pose. - Clouds must be sensor-frame. Legacy recordings with pre-registered - world-frame clouds are rejected. Re-record them. + Frames at or after end_ts (seconds) are skipped. Clouds must be sensor-frame. + Legacy recordings with pre-registered world-frame clouds are rejected. + Re-record them. """ store = SqliteStore(path=str(db_path)) with store: @@ -72,6 +74,8 @@ def iter_world_frames( odom = store.stream(odom_stream, Odometry).order_by("ts") for pair_obs in lidar.align(odom, tolerance=align_tol): lidar_obs, odom_obs = pair_obs.data + if end_ts is not None and lidar_obs.ts >= end_ts: + break if lidar_obs.data.frame_id == "world": raise ValueError( f"{db_path}: stream {lidar_stream!r} has pre-registered world-frame " @@ -94,12 +98,14 @@ def iter_world_frames( ) -def load_trajectory(db_path: Path, odom_stream: str) -> Trajectory: +def load_trajectory(db_path: Path, odom_stream: str, end_ts: float | None = None) -> Trajectory: store = SqliteStore(path=str(db_path)) ts: list[float] = [] positions: list[tuple[float, float, float]] = [] with store: for obs in store.stream(odom_stream, Odometry).order_by("ts"): + if end_ts is not None and obs.ts >= end_ts: + break o = obs.data ts.append(obs.ts) positions.append((float(o.position.x), float(o.position.y), float(o.position.z))) diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index b5d262cf5d..b0a1191067 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -54,9 +54,6 @@ logger = setup_logger() MAX_COLLISIONS_KEPT = 50 -# The goal counts as seen when the incremental map has an occupied voxel -# within this distance of it at plan time. -GOAL_SEEN_RADIUS_M = 1.0 @dataclass @@ -102,11 +99,9 @@ class CaseResult: goal: tuple[float, float, float] tags: list[str] l_ref: float - l_ref_snapped: bool plan_ts: float online_voxels: int map_update_ms: float - goal_seen: bool expect_fail: bool online: PlanOutcome final: PlanOutcome @@ -254,13 +249,6 @@ def score_negative(raw: PlanOutcome) -> PlanOutcome: return replace(raw, success=refused, spl=1.0 if refused else 0.0) -def _goal_seen(online_points: NDArray[np.float32], goal: tuple[float, float, float]) -> bool: - if len(online_points) == 0: - return False - d = np.linalg.norm(online_points - np.asarray(goal, dtype=np.float32), axis=1) - return bool(d.min() <= GOAL_SEEN_RADIUS_M) - - def _dynamic_candidate( online: PlanOutcome, final: PlanOutcome, @@ -309,7 +297,7 @@ def run_suite( suite: Suite, cfg: EvalConfig, threads: int = 1, keep_artifacts: bool = False ) -> DatasetResult: db_path = suite.db_path() - trajectory = load_trajectory(db_path, suite.odom_stream) + trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) final = load_or_build_final_map(db_path, suite, cfg) obstacle_keys = final.occupied_keys @@ -363,7 +351,6 @@ def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: goal=case.goal, tags=case.tags, l_ref=ref.length, - l_ref_snapped=ref.snapped, **rest, # type: ignore[arg-type] ) @@ -379,7 +366,6 @@ def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: plan_ts=float("inf"), online_voxels=len(final.occupied), map_update_ms=0.0, - goal_seen=True, expect_fail=case.expect_fail, online=outcome, final=outcome, @@ -419,7 +405,6 @@ def process_checkpoint( online_out = _no_plan(0.0) online_wp = None end = online_wp[-1] if online_wp is not None and len(online_wp) else None - goal_seen = _goal_seen(online_points, case.goal) dynamic_candidate, blocking = ( (False, []) if case.expect_final_fail @@ -431,7 +416,6 @@ def process_checkpoint( plan_ts=float(checkpoints.times[k]), online_voxels=len(keys), map_update_ms=map_update_ms, - goal_seen=goal_seen, expect_fail=False, online=online_out, final=final_out, diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index ee1a1e8ed3..3663b5d3cd 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -16,42 +16,18 @@ from dataclasses import replace import itertools -import json -from pathlib import Path from typing import TYPE_CHECKING, cast import numpy as np import pytest from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper -from dimos.navigation.nav_3d.evaluator import metrics, tripwire -from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator import metrics +from dimos.navigation.nav_3d.evaluator.cases import Case from dimos.navigation.nav_3d.evaluator.config import EvalConfig -from dimos.navigation.nav_3d.evaluator.final_map import ( - FinalMap, - MapCheckpoints, - encode_deltas, - replay_frames, -) -from dimos.navigation.nav_3d.evaluator.generate import ( - Candidate, - GenerationParams, - _select_diverse, - generate_cases, - snap_to_surface, -) +from dimos.navigation.nav_3d.evaluator.final_map import MapCheckpoints, encode_deltas, replay_frames from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory -from dimos.navigation.nav_3d.evaluator.runner import ( - CaseResult, - DatasetResult, - Report, - _dynamic_candidate, - _final_only, - _no_plan, - _run_plan, - score_negative, -) -from dimos.navigation.nav_3d.evaluator.tagging import GEOMETRIC_TAGS, route_tags +from dimos.navigation.nav_3d.evaluator.runner import _run_plan, score_negative from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys if TYPE_CHECKING: @@ -60,6 +36,15 @@ VOXEL = 0.1 +def _cfg(**overrides: float) -> EvalConfig: + return replace(EvalConfig(voxel_size=VOXEL), **overrides) + + +def _wall(x: float) -> np.ndarray: + ys, zs = np.meshgrid(np.arange(-1, 1, VOXEL), np.arange(0.05, 1.5, VOXEL)) + return np.stack([np.full(ys.size, x), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) + + def test_voxel_key_roundtrip() -> None: pts = np.array([[0.05, 0.05, 0.05], [-3.21, 4.7, -0.09], [80.0, -80.0, 12.3]], dtype=np.float32) centers = key_centers(voxel_keys(pts, VOXEL), VOXEL) @@ -73,15 +58,6 @@ def test_keys_contain() -> None: assert keys_contain(np.array([], dtype=np.int64), query).tolist() == [False, False] -def _wall(x: float) -> np.ndarray: - ys, zs = np.meshgrid(np.arange(-1, 1, VOXEL), np.arange(0.05, 1.5, VOXEL)) - return np.stack([np.full(ys.size, x), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) - - -def _cfg(**overrides: float) -> EvalConfig: - return replace(EvalConfig(voxel_size=VOXEL), **overrides) - - def _gate(waypoints: np.ndarray, obstacles: np.ndarray) -> metrics.GateResult: keys = np.unique(voxel_keys(obstacles, VOXEL)) return metrics.check_path(waypoints, keys, _cfg()) @@ -119,19 +95,6 @@ def test_gate_ignores_ground() -> None: assert _gate(path, floor).valid -def test_chord_direction_spans_robot_length() -> None: - """Heading comes from the body-length chord, not the local step, so it is - steady across stepped terrain instead of flipping tread-to-riser.""" - xs = np.arange(0, 3.0, 0.1) - zs = np.floor(xs / 0.2) * 0.1 # stairs: 0.1 m rise every 0.2 m, mean slope 0.5 - path = np.stack([xs, np.zeros_like(xs), zs], axis=1).astype(np.float32) - fwd = metrics.chord_directions(path, span=0.7) - local = np.diff(path.astype(np.float64), axis=0) - local /= np.linalg.norm(local, axis=1, keepdims=True) - assert fwd[5:-5, 2].std() < 0.5 * local[:, 2].std() - assert fwd[5:-5, 2].mean() > 0.2 # steadily pitched up the stairs - - def test_gate_pitch_clears_rising_step() -> None: """A voxel ahead-and-up is inside a flat box but beyond the pitched one.""" step = np.array([[0.3, 0.0, 0.4]], dtype=np.float32) @@ -167,13 +130,6 @@ def test_gate_reports_clearance_margin() -> None: assert far.min_clearance_m == metrics.MARGIN_CAP_M -def test_spl() -> None: - assert metrics.spl(False, 10.0, 10.0) == 0.0 - assert metrics.spl(True, 10.0, 10.0) == 1.0 - assert metrics.spl(True, 10.0, 20.0) == pytest.approx(0.5) - assert metrics.spl(True, 10.0, 5.0) == 1.0 - - def test_reference_length_snaps_to_trajectory() -> None: positions = np.stack( [np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1 @@ -206,80 +162,6 @@ def test_reference_length_uses_shortest_revisit() -> None: assert ref.length == pytest.approx(10.0, abs=0.2) -def test_path_length_and_goal() -> None: - path = np.array([[0, 0, 0], [3, 4, 0]], dtype=np.float32) - assert metrics.path_length(path) == pytest.approx(5.0) - assert metrics.goal_reached(path, (3, 4, 0.2), tolerance=0.5) - assert not metrics.goal_reached(path, (3, 4, 1.0), tolerance=0.5) - - -def test_generate_cases_around_wall() -> None: - """A U-shaped walk around a wall must yield non-trivial cases spanning it.""" - wall_pts = _wall(10.0) - wall_keys = np.unique(voxel_keys(wall_pts, VOXEL)) - final = FinalMap( - voxel_size=VOXEL, - occupied=wall_pts, - occupied_keys=wall_keys, - frames=1, - add_frame_ms={"p50": 0.0, "p95": 0.0, "max": 0.0}, - build_ms=0.0, - ) - xs, ys = np.meshgrid(np.arange(0, 20, VOXEL), np.arange(-3, 6, VOXEL)) - surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) - - legs = [ - np.stack([np.linspace(2, 8, 40), np.zeros(40)], axis=1), - np.stack([np.full(40, 8.0), np.linspace(0, 4, 40)], axis=1), - np.stack([np.linspace(8, 12, 40), np.full(40, 4.0)], axis=1), - np.stack([np.full(40, 12.0), np.linspace(4, 0, 40)], axis=1), - np.stack([np.linspace(12, 18, 40), np.zeros(40)], axis=1), - ] - xy = np.concatenate(legs) - positions = np.column_stack([xy, np.full(len(xy), 0.3)]).astype(np.float32) - traj = Trajectory(ts=np.linspace(0, 60, len(positions)), positions=positions) - - cfg = EvalConfig(voxel_size=VOXEL) - cases = generate_cases(traj, final, surface, cfg, GenerationParams(max_cases=10)) - assert cases - assert len({c.id for c in cases}) == len(cases) - spans_wall = [c for c in cases if (c.start[0] - 10) * (c.goal[0] - 10) < 0] - assert spans_wall - for c in cases: - assert abs(c.start[2]) < 1e-5 and abs(c.goal[2]) < 1e-5 - assert "flat" in c.tags - - -def test_select_diverse_backfills_to_min_cases() -> None: - """Sector caps must not starve a dataset below the case floor.""" - candidates = [ - Candidate( - start=(float(x), 0.0, 0.0), - goal=(float(x), 20.0, 0.0), - walked_m=30.0, - detour_ratio=1.5, - dz=0.0, - ) - for x in np.arange(0.0, 16.0, 2.0) - ] - strict = _select_diverse(candidates, GenerationParams(min_cases=0), max_cases=12) - assert len(strict) == 4 - backfilled = _select_diverse(candidates, GenerationParams(min_cases=10), max_cases=12) - assert len(backfilled) == 8 - assert len({(c.start, c.goal) for c in backfilled}) == 8 - - -def test_snap_to_surface() -> None: - xs, ys = np.meshgrid(np.arange(0, 2, VOXEL), np.arange(0, 2, VOXEL)) - surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) - snapped = snap_to_surface(np.array([1.0, 1.0, 0.4], dtype=np.float32), surface, 1.0) - assert snapped is not None - assert abs(snapped[2]) < 1e-6 - assert np.linalg.norm(snapped[:2] - [1.0, 1.0]) < VOXEL - assert snap_to_surface(np.array([9.0, 9.0, 0.0], dtype=np.float32), surface, 1.0) is None - assert snap_to_surface(np.array([1.0, 1.0, 5.0], dtype=np.float32), surface, 1.0) is None - - def test_checkpoint_deltas_roundtrip() -> None: snapshots = [ np.array([1, 2, 3], dtype=np.int64), @@ -325,6 +207,22 @@ def frame_at(ts: float, x: float) -> Frame: assert keys_contain(snapshots[2], wall3).all() +def test_check_kinematics_rejects_cliff_jumps() -> None: + stairs = np.array([[0, 0, 0], [0.4, 0, 0.16], [0.8, 0, 0.32]], dtype=np.float32) + assert metrics.check_kinematics(stairs, _cfg(max_slope=1.0)).valid + riser = np.array([[0, 0, 0], [0.08, 0, 0.16]], dtype=np.float32) + assert metrics.check_kinematics(riser, _cfg(max_slope=1.0)).valid + # A double riser between adjacent cells is quantization, not a cliff. + quantized = np.array( + [[0, 0, 0], [0.4, 0, 0.08], [0.56, 0, 0.4], [0.96, 0, 0.48]], dtype=np.float32 + ) + assert metrics.check_kinematics(quantized, _cfg(max_slope=1.0)).valid + cliff = np.array([[0, 0, 0], [0.2, 0, 0.9], [1, 0, 0.9]], dtype=np.float32) + result = metrics.check_kinematics(cliff, _cfg(max_slope=1.0)) + assert not result.valid + assert len(result.violation_points) >= 1 + + class _StubPlanner: """Returns a fixed path regardless of the map, for gaming the scorer.""" @@ -356,10 +254,7 @@ def _meta_scene() -> tuple[np.ndarray, EvalConfig, Case]: def _u_route() -> np.ndarray: - return np.array( - [[2, 0, 0], [2, 4, 0], [18, 4, 0], [18, 0, 0]], - dtype=np.float32, - ) + return np.array([[2, 0, 0], [2, 4, 0], [18, 4, 0], [18, 0, 0]], dtype=np.float32) def test_meta_straight_line_cheat_scores_zero() -> None: @@ -437,394 +332,3 @@ def test_meta_negative_case_scoring() -> None: wander = np.array([case.start, [4.0, 2.0, 0.0]], dtype=np.float32) partial, _ = _run_plan(_stub(wander), case, 16.0, keys, keys, cfg) assert score_negative(partial).success - - -def test_expect_final_fail_scores_online_normally_and_refuses_final() -> None: - """A door-closed route: the online plan earns SPL, the final plan must refuse. - - Mirrors the runner, which inverts only the final outcome for an - expect_final_fail case and scores the online outcome as usual. - """ - keys, cfg, case = _meta_scene() - open_keys = np.unique(voxel_keys(_floor(), VOXEL)) - route = _u_route() - l_ref = metrics.path_length(route) - - # Online, before the door closed: the wall is absent and the walked route - # is clean, so it scores in full. - online, _ = _run_plan(_stub(route), case, l_ref, open_keys, open_keys, cfg) - assert online.success - assert online.spl == pytest.approx(1.0) - - # Final, after the door closed: the wall is present and refusing is right. - refused, _ = _run_plan(_stub(None), case, l_ref, keys, keys, cfg) - final = score_negative(refused) - assert final.success - assert final.spl == 1.0 - - # Claiming a route straight through the closed door is a false positive. - line = np.array([case.start, case.goal], dtype=np.float32) - claimed, _ = _run_plan(_stub(line), case, l_ref, keys, keys, cfg) - assert score_negative(claimed).spl == 0.0 - - -def test_dynamic_candidate_flags_route_blocked_by_new_occupancy() -> None: - """Online success with a final failure from newly-appeared occupancy flags.""" - _, cfg, case = _meta_scene() - open_keys = np.unique(voxel_keys(_floor(), VOXEL)) - final_keys = np.unique(voxel_keys(np.concatenate([_floor(), _wall(10.0)]), VOXEL)) - line = np.array([case.start, case.goal], dtype=np.float32) - - online, wp = _run_plan(_stub(line), case, 24.0, open_keys, open_keys, cfg) - assert online.success - final, _ = _run_plan(_stub(line), case, 24.0, final_keys, final_keys, cfg) - assert not final.success - - flagged, blocking = _dynamic_candidate(online, final, wp, open_keys, final_keys, cfg) - assert flagged - assert blocking - - # No occupancy appeared between the two maps, so the final failure is not a - # dynamic obstacle and must not be flagged. - unflagged, _ = _dynamic_candidate(online, final, wp, final_keys, final_keys, cfg) - assert not unflagged - - # A clean final plan is never a candidate. - assert not _dynamic_candidate(online, online, wp, open_keys, final_keys, cfg)[0] - - -def test_check_kinematics_rejects_cliff_jumps() -> None: - stairs = np.array([[0, 0, 0], [0.4, 0, 0.16], [0.8, 0, 0.32]], dtype=np.float32) - assert metrics.check_kinematics(stairs, _cfg(max_slope=1.0)).valid - riser = np.array([[0, 0, 0], [0.08, 0, 0.16]], dtype=np.float32) - assert metrics.check_kinematics(riser, _cfg(max_slope=1.0)).valid - # A double riser between adjacent cells is quantization, not a cliff. - quantized = np.array( - [[0, 0, 0], [0.4, 0, 0.08], [0.56, 0, 0.4], [0.96, 0, 0.48]], dtype=np.float32 - ) - assert metrics.check_kinematics(quantized, _cfg(max_slope=1.0)).valid - cliff = np.array([[0, 0, 0], [0.2, 0, 0.9], [1, 0, 0.9]], dtype=np.float32) - result = metrics.check_kinematics(cliff, _cfg(max_slope=1.0)) - assert not result.valid - assert len(result.violation_points) >= 1 - - -def test_pick_along_ray() -> None: - from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray - - wall = _wall(10.0) - origin = np.array([0.0, 0.0, 0.5]) - target = np.array([10.0, 0.35, 0.75]) - direction = target - origin - direction /= np.linalg.norm(direction) - picked = pick_along_ray(wall, origin, direction) - assert picked is not None - assert np.linalg.norm(picked - target) < 0.15 - # The nearest surface along the ray wins over one behind it. - two_walls = np.concatenate([_wall(10.0), _wall(15.0)]) - picked = pick_along_ray(two_walls, origin, direction) - assert picked is not None - assert abs(picked[0] - 10.0) < 0.2 - # A ray into empty space picks nothing. - up = np.array([0.0, 0.0, 1.0]) - assert pick_along_ray(wall, origin, up) is None - - -def test_save_suite_roundtrip(tmp_path: Path) -> None: - suite = Suite( - dataset="demo", - cases=[ - Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), tags=["x"]), - Case(id="neg", start=(0.0, 0.0, 0.0), goal=(5.0, 5.0, 5.0), expect_fail=True), - ], - lidar_stream="other_lidar", - db="~/recordings/demo.db", - ) - path = save_suite(suite, tmp_path / "demo.yaml") - loaded = load_suite(path) - assert loaded.dataset == "demo" - assert loaded.lidar_stream == "other_lidar" - assert loaded.odom_stream == "pointlio_odometry" - assert loaded.db == "~/recordings/demo.db" - assert loaded.db_path() == Path.home() / "recordings/demo.db" - assert loaded.cases[0].goal == (1.0, 2.0, 3.0) - assert loaded.cases[0].tags == ["x"] - assert not loaded.cases[0].expect_fail - assert loaded.cases[1].expect_fail - - -def test_load_suite(tmp_path: Path) -> None: - manifest = tmp_path / "demo.yaml" - manifest.write_text( - "dataset: demo\n" - "cases:\n" - " - id: a\n" - " start: [0, 0, 0]\n" - " goal: [1, 2, 3]\n" - " tags: [stairs]\n" - ) - suite = load_suite(manifest) - assert suite.dataset == "demo" - assert suite.cases[0].goal == (1.0, 2.0, 3.0) - assert suite.cases[0].tags == ["stairs"] - - manifest.write_text( - "dataset: demo\ncases:\n" - " - {id: a, start: [0, 0, 0], goal: [1, 2, 3]}\n" - " - {id: a, start: [0, 0, 0], goal: [4, 5, 6]}\n" - ) - with pytest.raises(ValueError, match="duplicate"): - load_suite(manifest) - - -def test_final_only_covers_manual_and_infeasible() -> None: - """Manual and infeasible cases skip the online phase. Auto cases keep it.""" - xyz = (0.0, 0.0, 0.0) - assert not _final_only(Case(id="a", start=xyz, goal=xyz, tags=["auto", "flat"])) - assert _final_only(Case(id="m", start=xyz, goal=xyz, tags=["manual", "flat"])) - assert _final_only(Case(id="n", start=xyz, goal=xyz, tags=["manual"], expect_fail=True)) - # A dynamic-obstacle auto case still replays online. - assert not _final_only(Case(id="d", start=xyz, goal=xyz, tags=["auto"], expect_final_fail=True)) - - -def test_expect_final_fail_roundtrips(tmp_path: Path) -> None: - suite = Suite( - dataset="demo", - cases=[ - Case( - id="dyn", - start=(0.0, 0.0, 0.0), - goal=(3.0, 0.0, 0.0), - tags=["auto", "dynamic"], - expect_final_fail=True, - ) - ], - ) - loaded = load_suite(save_suite(suite, tmp_path / "demo.yaml")) - assert loaded.cases[0].expect_final_fail - assert not loaded.cases[0].expect_fail - - -def test_expect_fail_and_final_fail_are_exclusive(tmp_path: Path) -> None: - manifest = tmp_path / "demo.yaml" - manifest.write_text( - "dataset: demo\ncases:\n" - " - {id: bad, start: [0, 0, 0], goal: [1, 0, 0], " - "expect_fail: true, expect_final_fail: true}\n" - ) - with pytest.raises(ValueError, match="exclusive"): - load_suite(manifest) - - -def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, object]: - """Report JSON from a {dataset: {case_id: (inc, fin)}} pass/fail spec.""" - datasets = [] - for dataset, cases in spec.items(): - results = [] - for case_id, (inc, fin) in cases.items(): - case = Case(id=case_id, start=(0.0, 0.0, 0.0), goal=(1.0, 0.0, 0.0)) - results.append( - CaseResult( - id=case_id, - dataset=dataset, - start=case.start, - goal=case.goal, - tags=[], - l_ref=1.0, - l_ref_snapped=False, - plan_ts=0.0, - online_voxels=0, - map_update_ms=0.0, - goal_seen=True, - expect_fail=False, - online=replace(_no_plan(0.0), success=inc), - final=replace(_no_plan(0.0), success=fin), - soft_progress=0.0, - ) - ) - datasets.append( - DatasetResult( - dataset=dataset, - cases=results, - final_voxels=0, - map_build_ms=0.0, - add_frame_ms={}, - frames=0, - ) - ) - report = Report( - score=0.0, - score_soft=0.0, - final_score=0.0, - n_cases=0, - n_online=0, - n_success=0, - n_success_final=0, - outcome_counts={}, - by_tag={}, - plan_ms={}, - map_update_ms={}, - datasets=datasets, - ) - return json.loads(json.dumps(report.to_dict())) - - -def test_tripwire_outcomes() -> None: - report = _tripwire_report({"office": {"a": (True, False), "b": (False, True)}}) - assert tripwire.outcomes(report) == { - "office": {"a": {"inc": True, "fin": False}, "b": {"inc": False, "fin": True}} - } - d = tripwire.diff(report, report) - assert d.fixed == [] and d.broke == [] and d.added == [] and d.removed == [] - - -def test_tripwire_perf_violations() -> None: - report = _tripwire_report({"office": {"a": (True, True)}}) - report["config"] = {"plan_p95_budget_ms": 60.0, "map_update_p95_budget_ms": 3000.0} - report["plan_ms"] = {"p95": 30.0} - report["map_update_ms"] = {"p95": 1500.0} - assert tripwire.perf_violations(report) == [] - report["plan_ms"] = {"p95": 61.0} - violations = tripwire.perf_violations(report) - assert len(violations) == 1 and "plan_ms" in violations[0] - # Reports predating the budgets pass. - assert tripwire.perf_violations({"datasets": []}) == [] - - -def test_tripwire_exact_differences() -> None: - report = _tripwire_report({"office": {"a": (True, False)}}) - assert tripwire.exact_differences(report, report) == [] - changed = json.loads(json.dumps(report)) - changed["datasets"][0]["cases"][0]["online"]["length"] = 12.34 - changed["datasets"][0]["cases"][0]["online"]["plan_ms"] = 99.0 - diffs = tripwire.exact_differences(report, changed) - assert len(diffs) == 1 - assert "length" in diffs[0] and "12.34" in diffs[0] - - -def _ywall(y: float, x_lo: float, x_hi: float) -> np.ndarray: - """A wall parallel to +x travel, at constant y, spanning body height.""" - xs, zs = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(0.05, 1.5, VOXEL)) - return np.stack([xs.ravel(), np.full(xs.size, y), zs.ravel()], axis=1, dtype=np.float32) - - -def _tags(route: np.ndarray, keys: np.ndarray) -> list[str]: - """Tag a synthetic route, taking its endpoints for elevation.""" - start = (float(route[0, 0]), float(route[0, 1]), float(route[0, 2])) - goal = (float(route[-1, 0]), float(route[-1, 1]), float(route[-1, 2])) - return route_tags(start, goal, route, keys, _cfg()) - - -def test_ground_truth_route_returns_walked_slice() -> None: - """The route is the shortest walked slice between the endpoints, not a line.""" - out = np.stack([np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1) - detour = np.stack([np.full(101, 10.0), np.linspace(0, 6, 101), np.full(101, 0.3)], axis=1) - positions = np.concatenate([out, detour]).astype(np.float32) - traj = Trajectory(ts=np.linspace(0, 20, len(positions)), positions=positions) - route = metrics.ground_truth_route(traj, (0, 0, 0), (10, 6, 0), _cfg()) - assert route is not None - # Foot level (sensor height removed) and it walks the full out-and-detour. - assert abs(route[0, 2]) < 1e-5 - assert metrics.path_length(route) == pytest.approx(16.0, abs=0.2) - assert metrics.ground_truth_route(traj, (0, 20, 0), (10, 6, 0), _cfg()) is None - - -def test_ground_truth_route_orients_start_to_goal() -> None: - """The route runs start-to-goal even when the start was walked later, so its - elevation is not read backward.""" - xs = np.linspace(0, 10, 101) - positions = np.stack([xs, np.zeros(101), xs * 0.25], axis=1).astype(np.float32) - traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) - # Start high at x=8, goal low at x=2: a downhill traverse. - route = metrics.ground_truth_route(traj, (8, 0, 1.7), (2, 0, 0.2), _cfg()) - assert route is not None - # Runs start-to-goal: x decreasing from ~8 to ~2, so elevation reads downhill. - assert route[0, 0] > 6.0 and route[-1, 0] < 4.0 - assert route[0, 0] > route[-1, 0] - assert route[-1, 2] < route[0, 2] - - -def test_route_tags_flat_wide_has_no_shape_tags() -> None: - """A wide flat traverse is just flat: no narrow, doorway, or corridor.""" - route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) - walls = np.concatenate([_ywall(-2.0, 0, 4), _ywall(2.0, 0, 4)]) - keys = np.unique(voxel_keys(walls, VOXEL)) - tags = _tags(route, keys) - assert "flat" in tags - assert "narrow" not in tags and "doorway" not in tags and "corridor" not in tags - - -def test_route_tags_narrow_passage() -> None: - """Walls under a body-plus-clearance apart the whole way make a corridor.""" - route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) - walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) - keys = np.unique(voxel_keys(walls, VOXEL)) - tags = _tags(route, keys) - assert "narrow" in tags and "corridor" in tags - assert "doorway" not in tags # sustained squeeze, not a short pinch - assert "open" not in tags - - -def test_route_tags_doorway_is_a_short_pinch() -> None: - """An open corridor that pinches briefly and reopens is a doorway.""" - route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) - far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) - pinch = np.concatenate([_ywall(-0.35, 2.0, 2.6), _ywall(0.35, 2.0, 2.6)]) - keys = np.unique(voxel_keys(np.concatenate([far, pinch]), VOXEL)) - tags = _tags(route, keys) - assert "doorway" in tags and "narrow" in tags - - -def test_route_tags_sharp_doorway() -> None: - """A door frame is a sharp pinch: the narrow stretch is only a couple of - voxels, but flanked by open space it is still a doorway.""" - route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) - far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) - frame = np.concatenate([_ywall(-0.35, 2.4, 2.6), _ywall(0.35, 2.4, 2.6)]) - keys = np.unique(voxel_keys(np.concatenate([far, frame]), VOXEL)) - assert "doorway" in _tags(route, keys) - - -def test_route_tags_stairs_from_endpoints() -> None: - """Elevation comes from the endpoints: a climb past the threshold is stairs, - and a big rise is long, whatever the route in between does.""" - up = np.stack([np.linspace(0, 4, 40), np.zeros(40), np.linspace(0, 2.0, 40)], axis=1).astype( - np.float32 - ) - tags = _tags(up, np.array([], dtype=np.int64)) - assert "stairs" in tags and "up" in tags and "long" in tags - assert "down" in _tags(up[::-1].copy(), np.array([], dtype=np.int64)) - - -def test_route_tags_gate_excludes_detour_routes() -> None: - """Shape tags need a near-direct route. Between the same endpoints, a - straight walk through the corridor is tagged, but a long detour is not: - its terrain cannot be attributed to the case, so it gets elevation only.""" - walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) - keys = np.unique(voxel_keys(walls, VOXEL)) - direct = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) - assert "corridor" in route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), direct, keys, _cfg()) - detour = np.array([[0, 0, 0], [2, 6, 0], [4, 0, 0]], dtype=np.float32) - assert route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), detour, keys, _cfg()) == ["flat"] - - -def test_route_tags_are_all_geometric() -> None: - """Every tag the tagger emits is one a retag is allowed to recompute.""" - route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) - walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) - keys = np.unique(voxel_keys(walls, VOXEL)) - assert set(_tags(route, keys)) <= GEOMETRIC_TAGS - - -def test_tripwire_diff_names_every_flip() -> None: - old = _tripwire_report( - {"office": {"a": (False, True), "b": (True, True), "gone": (True, True)}} - ) - new = _tripwire_report( - {"office": {"a": (True, True), "b": (False, False), "fresh": (True, True)}} - ) - d = tripwire.diff(old, new) - assert [(f.key, f.test) for f in d.fixed] == [("office/a", "inc")] - assert [(f.key, f.test) for f in d.broke] == [("office/b", "inc"), ("office/b", "fin")] - assert d.added == ["office/fresh"] - assert d.removed == ["office/gone"] diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index d5580ff262..2990228f30 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -208,7 +208,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - suite = suites_by_dataset[dataset.dataset] db_path = suite.db_path() final = load_or_build_final_map(db_path, suite, cfg) - trajectory = load_trajectory(db_path, suite.odom_stream) + trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) root = dataset.dataset rr.log( From c4e75f269da6458d2ad7915403dc8c0b0a6b7a1c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 22 Jul 2026 09:57:09 -0700 Subject: [PATCH 21/29] Wip --- dimos/navigation/nav_3d/evaluator/generate.py | 2 ++ dimos/navigation/nav_3d/evaluator/runner.py | 14 -------------- .../navigation/nav_3d/evaluator/test_evaluator.py | 1 - dimos/navigation/nav_3d/evaluator/tripwire.py | 3 +-- dimos/navigation/nav_3d/evaluator/viz.py | 1 + 5 files changed, 4 insertions(+), 17 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index ad0350671b..15147f9e5e 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -99,6 +99,8 @@ def snap_to_surface( Horizontal distance dominates so drift in z between passes does not pull the snap onto another floor. """ + if len(surface) == 0: + return None hd = np.linalg.norm(surface[:, :2] - point[:2], axis=1) zd = np.abs(surface[:, 2] - point[2]) score = hd + np.where(zd < 1.0, zd * 0.5, np.inf) diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index b0a1191067..1a9c5e6067 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -63,8 +63,6 @@ class PlanOutcome: valid: bool # Every sample stands on final-map occupancy. Fabricated bridges fail. supported: bool - # No segment rises steeper than the robot can climb. - kinematic: bool # For an ordinary case: all of the above. For an expect_fail case: the # planner correctly refused the infeasible goal. success: bool @@ -75,7 +73,6 @@ class PlanOutcome: # no path was planned. min_clearance: float | None waypoints: list[list[float]] - collisions: list[list[float]] # Indices of the colliding samples along the densified path, so a viewer # can redraw the exact body boxes the gate rejected. collision_indices: list[int] @@ -99,7 +96,6 @@ class CaseResult: goal: tuple[float, float, float] tags: list[str] l_ref: float - plan_ts: float online_voxels: int map_update_ms: float expect_fail: bool @@ -140,8 +136,6 @@ class TagStats: n_online: int inc_score: float fin_score: float - inc_success: int - fin_success: int @dataclass @@ -203,14 +197,12 @@ def _run_plan( reached=reached, valid=gate.valid, supported=support.valid, - kinematic=kinematics.valid, success=success, length=length, plan_ms=plan_ms, spl=metrics.spl(success, l_ref, length), min_clearance=gate.min_clearance_m, waypoints=waypoints.tolist(), - collisions=gate.collision_points[:MAX_COLLISIONS_KEPT].tolist(), collision_indices=gate.collision_indices[:MAX_COLLISIONS_KEPT].tolist(), unsupported=support.unsupported_points[:MAX_COLLISIONS_KEPT].tolist(), steep=kinematics.violation_points[:MAX_COLLISIONS_KEPT].tolist(), @@ -224,14 +216,12 @@ def _no_plan(plan_ms: float) -> PlanOutcome: reached=False, valid=False, supported=True, - kinematic=True, success=False, length=0.0, plan_ms=plan_ms, spl=0.0, min_clearance=None, waypoints=[], - collisions=[], collision_indices=[], unsupported=[], steep=[], @@ -363,7 +353,6 @@ def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: results[ci] = _result( case, ref, - plan_ts=float("inf"), online_voxels=len(final.occupied), map_update_ms=0.0, expect_fail=case.expect_fail, @@ -413,7 +402,6 @@ def process_checkpoint( results[ci] = _result( case, ref, - plan_ts=float(checkpoints.times[k]), online_voxels=len(keys), map_update_ms=map_update_ms, expect_fail=False, @@ -529,8 +517,6 @@ def mean(values: list[float]) -> float: n_online=len(oc), inc_score=mean([c.online.spl for c in oc]), fin_score=mean([c.final.spl for c in tc]), - inc_success=sum(c.online.success for c in oc), - fin_success=sum(c.final.success for c in tc), ) return Report( diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 3663b5d3cd..a21b7989d8 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -265,7 +265,6 @@ def test_meta_straight_line_cheat_scores_zero() -> None: assert out.planned and out.reached and out.supported assert not out.valid assert out.spl == 0.0 - assert out.collisions assert out.min_clearance is not None and out.min_clearance < 0 diff --git a/dimos/navigation/nav_3d/evaluator/tripwire.py b/dimos/navigation/nav_3d/evaluator/tripwire.py index 9da7ae7b39..3a0bcde758 100644 --- a/dimos/navigation/nav_3d/evaluator/tripwire.py +++ b/dimos/navigation/nav_3d/evaluator/tripwire.py @@ -36,7 +36,6 @@ class Flip: key: str test: str - passed: bool @dataclass @@ -79,7 +78,7 @@ def diff(old_report: dict[str, object], new_report: dict[str, object]) -> Report for test in TESTS: was, now = old_cases[case_id][test], tests[test] if was != now: - (fixed if now else broke).append(Flip(key, test, now)) + (fixed if now else broke).append(Flip(key, test)) removed = [ f"{dataset}/{case_id}" for dataset, cases in old.items() diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 2990228f30..e732ed2eb9 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -201,6 +201,7 @@ def _dataset_view(root: str, case_ids: list[str]) -> rrb.Spatial3DView: def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) -> None: rr.init("nav3d_eval", recording_id="nav3d_eval") + out.parent.mkdir(parents=True, exist_ok=True) rr.save(str(out)) suites_by_dataset = {suite.dataset: suite for suite in suites} From 2558e42aaaf2d8df26b305a25f8650a873432ca4 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 22 Jul 2026 11:18:54 -0700 Subject: [PATCH 22/29] Better case pick stuff --- dimos/navigation/nav_3d/evaluator/picker.py | 76 ++++++++++++++------- 1 file changed, 51 insertions(+), 25 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index 04d7a6f6f1..1661a7d700 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -40,17 +40,23 @@ from dimos.navigation.nav_3d.evaluator.cases import Case from dimos.navigation.nav_3d.evaluator.curation import CaseStore -# Selection cone half-angle around the click ray. Wide enough to hit a voxel -# point from across a room, narrow enough to stay on the intended surface. -PICK_CONE_RAD = 0.008 START_COLOR = (0, 255, 255) GOAL_COLOR = (255, 140, 0) PAIR_COLOR = (255, 255, 0) HIGHLIGHT_LINE_COLOR = (255, 255, 255) -MARKER_RADIUS = 0.09 -HIGHLIGHT_MARKER_RADIUS = 0.16 +MARKER_RADIUS = 0.14 +HIGHLIGHT_MARKER_RADIUS = 0.22 +# Pick markers float this far above their point so the voxel cube they sit on +# does not hide them. +MARKER_LIFT = 0.12 LINE_WIDTH = 2.5 HIGHLIGHT_LINE_WIDTH = 6.0 +# A pair being picked but not yet saved wears distinct colors and a thicker line +# so it stands out from the cases already in the manifest, until it is saved. +NEW_START_COLOR = (0, 255, 128) +NEW_GOAL_COLOR = (255, 0, 200) +NEW_PAIR_COLOR = (0, 255, 128) +NEW_LINE_WIDTH = 5.0 SUGGESTED_TAGS = ("stairs", "flat", "up", "down", "long", "doorway") INSTRUCTIONS = """**shift+click** picks START then GOAL, repeated per case. @@ -124,9 +130,14 @@ def pick_along_ray( points: NDArray[np.float32], origin: NDArray[np.float64], direction: NDArray[np.float64], - cone_rad: float = PICK_CONE_RAD, + radius: float, ) -> NDArray[np.float32] | None: - """Nearest cloud point inside a small cone around the click ray.""" + """Nearest cloud point inside a tube of the given radius around the click ray. + + A physical perpendicular radius, not an angular cone: an angular cone widens + with distance, so it would pick a voxel far down the ray over the near one + the click landed on. With a fixed tube the nearest voxel along the ray wins. + """ rel = points.astype(np.float64) - origin t = rel @ direction ahead = t > 0.05 @@ -134,9 +145,8 @@ def pick_along_ray( return None t = t[ahead] perp = np.linalg.norm(rel[ahead] - t[:, None] * direction, axis=1) - angle = perp / t - for widen in (1.0, 4.0): - hit = angle < cone_rad * widen + for r in (radius, 3.0 * radius): + hit = perp < r if hit.any(): idx = np.flatnonzero(ahead)[hit] return np.asarray(points[idx[np.argmin(t[hit])]]) @@ -216,6 +226,19 @@ def set_highlight(self, on: bool) -> None: marker.line_width = HIGHLIGHT_LINE_WIDTH if on else LINE_WIDTH marker.colors = np.array(HIGHLIGHT_LINE_COLOR if on else PAIR_COLOR, dtype=np.uint8) + def _mark_saved(self) -> None: + """Repaint the pick markers to the standard saved look, so only pairs + not yet in the manifest wear the distinct new-pick colors and line.""" + sphere_colors = [START_COLOR, GOAL_COLOR] + for marker in self.markers: + if hasattr(marker, "radius"): + color = _marker_color(sphere_colors.pop(0)) + if hasattr(marker, "color"): + marker.color = color + elif hasattr(marker, "line_width"): + marker.line_width = LINE_WIDTH + marker.colors = np.array(PAIR_COLOR, dtype=np.uint8) + def _label(self) -> str: return self.saved_id or f"pair {self._n}" @@ -257,16 +280,9 @@ def _build(self, *, expanded: bool, order: float | None, scroll: bool = False) - ) self.negative_box = server.gui.add_checkbox("negative (must refuse)", self._negative) self.message = server.gui.add_markdown(self._status) - self.show_button = server.gui.add_button("show in scene") self.button = server.gui.add_button("save / update") self.delete_button = server.gui.add_button("delete") - @self.show_button.on_click - def _(_event: object) -> None: - with self._hooks.lock: - self._hooks.announce(self._label()) - self._hooks.highlight(self) - @self.button.on_click def _(_event: object) -> None: # save_unsaved calls save_or_update already holding the lock. @@ -332,6 +348,7 @@ def save_or_update(self) -> bool: return False msg = f"saved {case.id} [{', '.join(case.tags)}]" print(msg) + self._mark_saved() # Viser cannot collapse a live panel, so replace it with the # collapsed button form, synced from the authoritative save. self.saved_id = case.id @@ -446,19 +463,24 @@ def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.Sce marker_seq += 1 return server.scene.add_icosphere( f"/picks/m{marker_seq}", - radius=0.09, + radius=MARKER_RADIUS, color=_marker_color(color), - position=(float(point[0]), float(point[1]), float(point[2]) + 0.05), + position=(float(point[0]), float(point[1]), float(point[2]) + MARKER_LIFT), ) - def pair_line(start: NDArray[np.float32], goal: NDArray[np.float32]) -> viser.SceneNodeHandle: + def pair_line( + start: NDArray[np.float32], + goal: NDArray[np.float32], + color: tuple[int, int, int] = PAIR_COLOR, + width: float = LINE_WIDTH, + ) -> viser.SceneNodeHandle: nonlocal marker_seq marker_seq += 1 return server.scene.add_line_segments( f"/picks/m{marker_seq}", np.stack([start, goal])[None], - colors=PAIR_COLOR, - line_width=2.5, + colors=color, + line_width=width, ) def pair_markers( @@ -480,16 +502,20 @@ def pair_markers( def _(event: viser.SceneClickEvent) -> None: nonlocal pair_count point = pick_along_ray( - map_points, np.asarray(event.ray_origin), np.asarray(event.ray_direction) + map_points, np.asarray(event.ray_origin), np.asarray(event.ray_direction), voxel_size ) if point is None: return with lock: if not pending: - pending.append((sphere(point, START_COLOR), point)) + pending.append((sphere(point, NEW_START_COLOR), point)) return start_marker, start = pending.pop() - markers = [start_marker, sphere(point, GOAL_COLOR), pair_line(start, point)] + markers = [ + start_marker, + sphere(point, NEW_GOAL_COLOR), + pair_line(start, point, NEW_PAIR_COLOR, NEW_LINE_WIDTH), + ] pair_count += 1 pairs.append(_PairEntry(server, pair_count, start, point, hooks, markers)) From 51646b37a852613ce7c0fd209f3a4920845d5f1d Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 3 Aug 2026 16:11:49 -0700 Subject: [PATCH 23/29] Add back some tests: --- .../nav_3d/evaluator/test_evaluator.py | 433 +++++++++++++++++- 1 file changed, 429 insertions(+), 4 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index a21b7989d8..1a625e02e7 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -16,18 +16,42 @@ from dataclasses import replace import itertools +import json +from pathlib import Path from typing import TYPE_CHECKING, cast import numpy as np import pytest from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper -from dimos.navigation.nav_3d.evaluator import metrics -from dimos.navigation.nav_3d.evaluator.cases import Case +from dimos.navigation.nav_3d.evaluator import metrics, tripwire +from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig -from dimos.navigation.nav_3d.evaluator.final_map import MapCheckpoints, encode_deltas, replay_frames +from dimos.navigation.nav_3d.evaluator.final_map import ( + FinalMap, + MapCheckpoints, + encode_deltas, + replay_frames, +) +from dimos.navigation.nav_3d.evaluator.generate import ( + Candidate, + GenerationParams, + _select_diverse, + generate_cases, + snap_to_surface, +) from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory -from dimos.navigation.nav_3d.evaluator.runner import _run_plan, score_negative +from dimos.navigation.nav_3d.evaluator.runner import ( + CaseResult, + DatasetResult, + Report, + _dynamic_candidate, + _final_only, + _no_plan, + _run_plan, + score_negative, +) +from dimos.navigation.nav_3d.evaluator.tagging import route_tags from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys if TYPE_CHECKING: @@ -45,6 +69,12 @@ def _wall(x: float) -> np.ndarray: return np.stack([np.full(ys.size, x), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) +def _ywall(y: float, x_lo: float, x_hi: float) -> np.ndarray: + """A wall parallel to +x travel, at constant y, spanning body height.""" + xs, zs = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(0.05, 1.5, VOXEL)) + return np.stack([xs.ravel(), np.full(xs.size, y), zs.ravel()], axis=1, dtype=np.float32) + + def test_voxel_key_roundtrip() -> None: pts = np.array([[0.05, 0.05, 0.05], [-3.21, 4.7, -0.09], [80.0, -80.0, 12.3]], dtype=np.float32) centers = key_centers(voxel_keys(pts, VOXEL), VOXEL) @@ -331,3 +361,398 @@ def test_meta_negative_case_scoring() -> None: wander = np.array([case.start, [4.0, 2.0, 0.0]], dtype=np.float32) partial, _ = _run_plan(_stub(wander), case, 16.0, keys, keys, cfg) assert score_negative(partial).success + + +def test_expect_final_fail_scores_online_normally_and_refuses_final() -> None: + """A door-closed route: the online plan earns SPL, the final plan must refuse. + + Mirrors the runner, which inverts only the final outcome for an + expect_final_fail case and scores the online outcome as usual. + """ + keys, cfg, case = _meta_scene() + open_keys = np.unique(voxel_keys(_floor(), VOXEL)) + route = _u_route() + l_ref = metrics.path_length(route) + + # Online, before the door closed: the wall is absent and the walked route + # is clean, so it scores in full. + online, _ = _run_plan(_stub(route), case, l_ref, open_keys, open_keys, cfg) + assert online.success + assert online.spl == pytest.approx(1.0) + + # Final, after the door closed: the wall is present and refusing is right. + refused, _ = _run_plan(_stub(None), case, l_ref, keys, keys, cfg) + final = score_negative(refused) + assert final.success + assert final.spl == 1.0 + + # Claiming a route straight through the closed door is a false positive. + line = np.array([case.start, case.goal], dtype=np.float32) + claimed, _ = _run_plan(_stub(line), case, l_ref, keys, keys, cfg) + assert score_negative(claimed).spl == 0.0 + + # Unlike a manual or infeasible case, it still replays online. + assert not _final_only(replace(case, tags=["auto"], expect_final_fail=True)) + + +def test_dynamic_candidate_flags_route_blocked_by_new_occupancy() -> None: + """Online success with a final failure from newly-appeared occupancy flags.""" + _, cfg, case = _meta_scene() + open_keys = np.unique(voxel_keys(_floor(), VOXEL)) + final_keys = np.unique(voxel_keys(np.concatenate([_floor(), _wall(10.0)]), VOXEL)) + line = np.array([case.start, case.goal], dtype=np.float32) + + online, wp = _run_plan(_stub(line), case, 24.0, open_keys, open_keys, cfg) + assert online.success + final, _ = _run_plan(_stub(line), case, 24.0, final_keys, final_keys, cfg) + assert not final.success + + flagged, blocking = _dynamic_candidate(online, final, wp, open_keys, final_keys, cfg) + assert flagged + assert blocking + + # No occupancy appeared between the two maps, so the final failure is not a + # dynamic obstacle and must not be flagged. + unflagged, _ = _dynamic_candidate(online, final, wp, final_keys, final_keys, cfg) + assert not unflagged + + # A clean final plan is never a candidate. + assert not _dynamic_candidate(online, online, wp, open_keys, final_keys, cfg)[0] + + +def test_chord_direction_spans_robot_length() -> None: + """Heading comes from the body-length chord, not the local step, so it is + steady across stepped terrain instead of flipping tread-to-riser.""" + xs = np.arange(0, 3.0, 0.1) + zs = np.floor(xs / 0.2) * 0.1 # stairs: 0.1 m rise every 0.2 m, mean slope 0.5 + path = np.stack([xs, np.zeros_like(xs), zs], axis=1).astype(np.float32) + fwd = metrics.chord_directions(path, span=0.7) + local = np.diff(path.astype(np.float64), axis=0) + local /= np.linalg.norm(local, axis=1, keepdims=True) + assert fwd[5:-5, 2].std() < 0.5 * local[:, 2].std() + assert fwd[5:-5, 2].mean() > 0.2 # steadily pitched up the stairs + + +def test_ground_truth_route_returns_walked_slice() -> None: + """The route is the shortest walked slice between the endpoints, not a line.""" + out = np.stack([np.linspace(0, 10, 101), np.zeros(101), np.full(101, 0.3)], axis=1) + detour = np.stack([np.full(101, 10.0), np.linspace(0, 6, 101), np.full(101, 0.3)], axis=1) + positions = np.concatenate([out, detour]).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 20, len(positions)), positions=positions) + route = metrics.ground_truth_route(traj, (0, 0, 0), (10, 6, 0), _cfg()) + assert route is not None + # Foot level (sensor height removed) and it walks the full out-and-detour. + assert abs(route[0, 2]) < 1e-5 + assert metrics.path_length(route) == pytest.approx(16.0, abs=0.2) + assert metrics.ground_truth_route(traj, (0, 20, 0), (10, 6, 0), _cfg()) is None + + +def test_ground_truth_route_orients_start_to_goal() -> None: + """The route runs start-to-goal even when the start was walked later, so its + elevation is not read backward.""" + xs = np.linspace(0, 10, 101) + positions = np.stack([xs, np.zeros(101), xs * 0.25], axis=1).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 10, 101), positions=positions) + # Start high at x=8, goal low at x=2: a downhill traverse. + route = metrics.ground_truth_route(traj, (8, 0, 1.7), (2, 0, 0.2), _cfg()) + assert route is not None + assert route[0, 0] > 6.0 and route[-1, 0] < 4.0 + assert route[-1, 2] < route[0, 2] + + +def _tags(route: np.ndarray, keys: np.ndarray) -> list[str]: + """Tag a synthetic route, taking its endpoints for elevation.""" + start = (float(route[0, 0]), float(route[0, 1]), float(route[0, 2])) + goal = (float(route[-1, 0]), float(route[-1, 1]), float(route[-1, 2])) + return route_tags(start, goal, route, keys, _cfg()) + + +def test_route_tags_flat_wide_has_no_shape_tags() -> None: + """A wide flat traverse is just flat: no narrow, doorway, or corridor.""" + route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + walls = np.concatenate([_ywall(-2.0, 0, 4), _ywall(2.0, 0, 4)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + tags = _tags(route, keys) + assert "flat" in tags + assert "narrow" not in tags and "doorway" not in tags and "corridor" not in tags + + +def test_route_tags_narrow_passage() -> None: + """Walls under a body-plus-clearance apart the whole way make a corridor.""" + route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + walls = np.concatenate([_ywall(-0.4, 0, 4), _ywall(0.4, 0, 4)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + tags = _tags(route, keys) + assert "narrow" in tags and "corridor" in tags + assert "doorway" not in tags # sustained squeeze, not a short pinch + + +def test_route_tags_doorway_is_a_short_pinch() -> None: + """An open corridor that pinches briefly and reopens is a doorway.""" + route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) + far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) + pinch = np.concatenate([_ywall(-0.35, 2.0, 2.6), _ywall(0.35, 2.0, 2.6)]) + keys = np.unique(voxel_keys(np.concatenate([far, pinch]), VOXEL)) + tags = _tags(route, keys) + assert "doorway" in tags and "narrow" in tags + + +def test_route_tags_sharp_doorway() -> None: + """A door frame is a sharp pinch: the narrow stretch is only a couple of + voxels, but flanked by open space it is still a doorway.""" + route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) + far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) + frame = np.concatenate([_ywall(-0.35, 2.4, 2.6), _ywall(0.35, 2.4, 2.6)]) + keys = np.unique(voxel_keys(np.concatenate([far, frame]), VOXEL)) + assert "doorway" in _tags(route, keys) + + +def test_route_tags_stairs_from_endpoints() -> None: + """Elevation comes from the endpoints: a climb past the threshold is stairs, + and a big rise is long, whatever the route in between does.""" + up = np.stack([np.linspace(0, 4, 40), np.zeros(40), np.linspace(0, 2.0, 40)], axis=1).astype( + np.float32 + ) + tags = _tags(up, np.array([], dtype=np.int64)) + assert "stairs" in tags and "up" in tags and "long" in tags + assert "down" in _tags(up[::-1].copy(), np.array([], dtype=np.int64)) + + +def test_route_tags_gate_excludes_detour_routes() -> None: + """Shape tags need a near-direct route. Between the same endpoints, a + straight walk through the corridor is tagged, but a long detour is not: + its terrain cannot be attributed to the case, so it gets elevation only.""" + walls = np.concatenate([_ywall(-0.4, 0, 20), _ywall(0.4, 0, 20)]) + keys = np.unique(voxel_keys(walls, VOXEL)) + direct = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) + assert "corridor" in route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), direct, keys, _cfg()) + # The detour runs the same corridor, so its terrain would tag identically. + # Only the arc-length gate can tell the two apart. + detour = np.array([[0, 0, 0], [20, 0, 0], [4, 0, 0]], dtype=np.float32) + assert route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), detour, keys, _cfg()) == ["flat"] + + +def test_snap_to_surface() -> None: + xs, ys = np.meshgrid(np.arange(0, 2, VOXEL), np.arange(0, 2, VOXEL)) + surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) + snapped = snap_to_surface(np.array([1.0, 1.0, 0.4], dtype=np.float32), surface, 1.0) + assert snapped is not None + assert abs(snapped[2]) < 1e-6 + assert np.linalg.norm(snapped[:2] - [1.0, 1.0]) < VOXEL + assert snap_to_surface(np.array([9.0, 9.0, 0.0], dtype=np.float32), surface, 1.0) is None + assert snap_to_surface(np.array([1.0, 1.0, 5.0], dtype=np.float32), surface, 1.0) is None + + +def test_generate_cases_around_wall() -> None: + """A U-shaped walk around a wall must yield non-trivial cases spanning it.""" + wall_pts = _wall(10.0) + final = FinalMap( + voxel_size=VOXEL, + occupied=wall_pts, + occupied_keys=np.unique(voxel_keys(wall_pts, VOXEL)), + frames=1, + add_frame_ms={"p50": 0.0, "p95": 0.0, "max": 0.0}, + build_ms=0.0, + ) + xs, ys = np.meshgrid(np.arange(0, 20, VOXEL), np.arange(-3, 6, VOXEL)) + surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) + + legs = [ + np.stack([np.linspace(2, 8, 40), np.zeros(40)], axis=1), + np.stack([np.full(40, 8.0), np.linspace(0, 4, 40)], axis=1), + np.stack([np.linspace(8, 12, 40), np.full(40, 4.0)], axis=1), + np.stack([np.full(40, 12.0), np.linspace(4, 0, 40)], axis=1), + np.stack([np.linspace(12, 18, 40), np.zeros(40)], axis=1), + ] + xy = np.concatenate(legs) + positions = np.column_stack([xy, np.full(len(xy), 0.3)]).astype(np.float32) + traj = Trajectory(ts=np.linspace(0, 60, len(positions)), positions=positions) + + cases = generate_cases(traj, final, surface, _cfg(), GenerationParams(max_cases=10)) + assert cases + assert len({c.id for c in cases}) == len(cases) + assert [c for c in cases if (c.start[0] - 10) * (c.goal[0] - 10) < 0] + for c in cases: + assert abs(c.start[2]) < 1e-5 and abs(c.goal[2]) < 1e-5 + assert "flat" in c.tags + + +def test_select_diverse_backfills_to_min_cases() -> None: + """Sector caps must not starve a dataset below the case floor.""" + candidates = [ + Candidate( + start=(float(x), 0.0, 0.0), + goal=(float(x), 20.0, 0.0), + walked_m=30.0, + detour_ratio=1.5, + dz=0.0, + ) + for x in np.arange(0.0, 16.0, 2.0) + ] + strict = _select_diverse(candidates, GenerationParams(min_cases=0), max_cases=12) + assert len(strict) == 4 + backfilled = _select_diverse(candidates, GenerationParams(min_cases=10), max_cases=12) + assert len(backfilled) == 8 + assert len({(c.start, c.goal) for c in backfilled}) == 8 + + +def test_pick_along_ray() -> None: + from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray + + wall = _wall(10.0) + origin = np.array([0.0, 0.0, 0.5]) + target = np.array([10.0, 0.35, 0.75]) + direction = target - origin + direction /= np.linalg.norm(direction) + picked = pick_along_ray(wall, origin, direction, VOXEL) + assert picked is not None + assert np.linalg.norm(picked - target) < 0.15 + # The nearest surface along the ray wins over one behind it. + two_walls = np.concatenate([_wall(10.0), _wall(15.0)]) + picked = pick_along_ray(two_walls, origin, direction, VOXEL) + assert picked is not None + assert abs(picked[0] - 10.0) < 0.2 + # A ray into empty space picks nothing. + up = np.array([0.0, 0.0, 1.0]) + assert pick_along_ray(wall, origin, up, VOXEL) is None + + +def test_load_suite_rejects_malformed_manifests(tmp_path: Path) -> None: + """Manifests are hand-edited, so a wrong one must fail loudly, not silently.""" + manifest = tmp_path / "demo.yaml" + manifest.write_text( + "dataset: demo\n" + "cases:\n" + " - id: a\n" + " start: [0, 0, 0]\n" + " goal: [1, 2, 3]\n" + " tags: [stairs]\n" + ) + suite = load_suite(manifest) + assert suite.dataset == "demo" + assert suite.cases[0].goal == (1.0, 2.0, 3.0) + assert suite.cases[0].tags == ["stairs"] + + manifest.write_text( + "dataset: demo\ncases:\n" + " - {id: a, start: [0, 0, 0], goal: [1, 2, 3]}\n" + " - {id: a, start: [0, 0, 0], goal: [4, 5, 6]}\n" + ) + with pytest.raises(ValueError, match="duplicate"): + load_suite(manifest) + + manifest.write_text( + "dataset: demo\ncases:\n" + " - {id: bad, start: [0, 0, 0], goal: [1, 0, 0], " + "expect_fail: true, expect_final_fail: true}\n" + ) + with pytest.raises(ValueError, match="exclusive"): + load_suite(manifest) + + +def test_save_suite_roundtrip(tmp_path: Path) -> None: + suite = Suite( + dataset="demo", + cases=[ + Case(id="a", start=(0.0, 0.0, 0.0), goal=(1.0, 2.0, 3.0), tags=["x"]), + Case(id="neg", start=(0.0, 0.0, 0.0), goal=(5.0, 5.0, 5.0), expect_fail=True), + Case(id="dyn", start=(0.0, 0.0, 0.0), goal=(3.0, 0.0, 0.0), expect_final_fail=True), + ], + lidar_stream="other_lidar", + db="~/recordings/demo.db", + ) + loaded = load_suite(save_suite(suite, tmp_path / "demo.yaml")) + assert loaded.dataset == "demo" + assert loaded.lidar_stream == "other_lidar" + assert loaded.odom_stream == "pointlio_odometry" + assert loaded.db_path() == Path.home() / "recordings/demo.db" + assert loaded.cases[0].goal == (1.0, 2.0, 3.0) + assert loaded.cases[0].tags == ["x"] + assert not loaded.cases[0].expect_fail and not loaded.cases[0].expect_final_fail + assert loaded.cases[1].expect_fail + assert loaded.cases[2].expect_final_fail + + +def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, object]: + """Report JSON from a {dataset: {case_id: (inc, fin)}} pass/fail spec.""" + datasets = [ + DatasetResult( + dataset=dataset, + cases=[ + CaseResult( + id=case_id, + dataset=dataset, + start=(0.0, 0.0, 0.0), + goal=(1.0, 0.0, 0.0), + tags=[], + l_ref=1.0, + online_voxels=0, + map_update_ms=0.0, + expect_fail=False, + online=replace(_no_plan(0.0), success=inc), + final=replace(_no_plan(0.0), success=fin), + soft_progress=0.0, + ) + for case_id, (inc, fin) in cases.items() + ], + final_voxels=0, + map_build_ms=0.0, + add_frame_ms={}, + frames=0, + ) + for dataset, cases in spec.items() + ] + report = Report( + score=0.0, + score_soft=0.0, + final_score=0.0, + n_cases=0, + n_online=0, + n_success=0, + n_success_final=0, + outcome_counts={}, + by_tag={}, + plan_ms={}, + map_update_ms={}, + datasets=datasets, + ) + return json.loads(json.dumps(report.to_dict())) + + +def test_tripwire_diff_names_every_flip() -> None: + old = _tripwire_report( + {"office": {"a": (False, True), "b": (True, True), "gone": (True, True)}} + ) + new = _tripwire_report( + {"office": {"a": (True, True), "b": (False, False), "fresh": (True, True)}} + ) + d = tripwire.diff(old, new) + assert [(f.key, f.test) for f in d.fixed] == [("office/a", "inc")] + assert [(f.key, f.test) for f in d.broke] == [("office/b", "inc"), ("office/b", "fin")] + assert d.added == ["office/fresh"] + assert d.removed == ["office/gone"] + + +def test_tripwire_exact_differences() -> None: + """Wall-clock fields must be ignored while any result field is caught.""" + report = _tripwire_report({"office": {"a": (True, False)}}) + assert tripwire.exact_differences(report, report) == [] + changed = json.loads(json.dumps(report)) + changed["datasets"][0]["cases"][0]["online"]["length"] = 12.34 + changed["datasets"][0]["cases"][0]["online"]["plan_ms"] = 99.0 + diffs = tripwire.exact_differences(report, changed) + assert len(diffs) == 1 + assert "length" in diffs[0] and "12.34" in diffs[0] + + +def test_tripwire_perf_violations() -> None: + report = _tripwire_report({"office": {"a": (True, True)}}) + report["config"] = {"plan_p95_budget_ms": 60.0, "map_update_p95_budget_ms": 3000.0} + report["plan_ms"] = {"p95": 30.0} + report["map_update_ms"] = {"p95": 1500.0} + assert tripwire.perf_violations(report) == [] + report["plan_ms"] = {"p95": 61.0} + violations = tripwire.perf_violations(report) + assert len(violations) == 1 and "plan_ms" in violations[0] + # Reports predating the budgets pass. + assert tripwire.perf_violations({"datasets": []}) == [] From 25fd70f0799dc09af3722885ee632be2e63b39c2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 3 Aug 2026 16:54:48 -0700 Subject: [PATCH 24/29] Switch to arbitrary planner pipeline so it's actually modular --- dimos/navigation/nav_3d/evaluator/cli.py | 9 +- dimos/navigation/nav_3d/evaluator/config.py | 30 ++- dimos/navigation/nav_3d/evaluator/curation.py | 4 +- .../navigation/nav_3d/evaluator/final_map.py | 16 +- dimos/navigation/nav_3d/evaluator/pipeline.py | 107 ++++++++ dimos/navigation/nav_3d/evaluator/runner.py | 240 ++++++++---------- .../nav_3d/evaluator/test_evaluator.py | 82 +++++- 7 files changed, 333 insertions(+), 155 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/pipeline.py diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 88d870f3ee..752a5b5d10 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -131,7 +131,7 @@ def _print_report(report: Report) -> None: f"fin {report.n_success_final}/{report.n_cases} | " f"outcomes {report.outcome_counts} | " f"plan p95 {report.plan_ms['p95']:.1f}ms | " - f"map update p95 {report.map_update_ms['p95']:.0f}ms" + f"ingest p95 {report.map_update_ms['p95']:.1f}ms/frame" ) inc_only = [ f"{c.dataset}/{c.id}" @@ -218,7 +218,7 @@ def run( workers: int = typer.Option( os.cpu_count() or 1, "--workers", - help="Total parallelism: dataset processes x checkpoint threads", + help="Datasets evaluated in parallel processes", ), set_: list[str] = typer.Option( None, "--set", help="Repeatable EvalConfig override, e.g. goal_tolerance=0.4" @@ -319,12 +319,11 @@ def ingest( ) cfg = EvalConfig() final = load_or_build_final_map(dest, suite, cfg) - planner = cfg.make_planner() - planner.update_global_map(final.occupied) gen = GenerationParams(max_cases=cases or None) if cases: gen.min_cases = cases - suite.cases = generate_cases(trajectory, final, planner.surface_map(), cfg, gen) + surface = final.standable_surface(cfg.robot_height) + suite.cases = generate_cases(trajectory, final, surface, cfg, gen) if not suite.cases: raise typer.Exit(code=1) floor = min(gen.min_cases, gen.resolve_max_cases(float(arcs[-1]))) diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 78bbf02fdb..9d2b4178da 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -15,9 +15,12 @@ from __future__ import annotations from dataclasses import dataclass, field +from typing import TYPE_CHECKING from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper -from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + +if TYPE_CHECKING: + from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline @dataclass @@ -64,21 +67,30 @@ class EvalConfig: snap_max_m: float = 1.0 # An improvement must not buy score with compute. p95 over the suite. - plan_p95_budget_ms: float = 50.0 - map_update_p95_budget_ms: float = 1000.0 + # A plan is timed end to end. A pipeline is opaque, so map work it defers + # until asked for a route is charged here rather than hidden, and the + # budget has to cover both the rebuild and the search. + plan_p95_budget_ms: float = 200.0 + # Per lidar frame, so a pipeline that cannot keep up with the sensor fails + # regardless of how it scores. + map_update_p95_budget_ms: float = 100.0 - # Planner constructor overrides, e.g. --set planner.wall_clearance_m=0.0. - # Omitted keys keep the planner's own defaults, so nothing is duplicated + # Which pipeline is under test, by registry name. + pipeline: str = "mls" + # Pipeline constructor overrides, e.g. --set planner.wall_clearance_m=0.0. + # Omitted keys keep the pipeline's own defaults, so nothing is duplicated # here, and the report records whatever was swept. planner: dict[str, float] = field(default_factory=dict) def make_mapper(self) -> VoxelRayMapper: + """The evaluator's own mapper, which builds the occupancy every pipeline + is graded against. Pipelines may use it too, but nothing requires them to.""" return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) - def make_planner(self) -> MLSPlanner: - return MLSPlanner( - voxel_size=self.voxel_size, robot_height=self.robot_height, **self.planner - ) + def make_pipeline(self) -> NavPipeline: + from dimos.navigation.nav_3d.evaluator.pipeline import make_pipeline + + return make_pipeline(self.pipeline, self) def mapper_fingerprint(self) -> dict[str, float | int]: """Cache key parameters for the final map. diff --git a/dimos/navigation/nav_3d/evaluator/curation.py b/dimos/navigation/nav_3d/evaluator/curation.py index c304287b3c..e63796544e 100644 --- a/dimos/navigation/nav_3d/evaluator/curation.py +++ b/dimos/navigation/nav_3d/evaluator/curation.py @@ -140,6 +140,4 @@ def load_store(dataset: str) -> tuple[CaseStore, FinalMap]: suite = load_suite(manifest) cfg = EvalConfig() final = load_or_build_final_map(suite.db_path(), suite, cfg) - planner = cfg.make_planner() - planner.update_global_map(final.occupied) - return CaseStore(suite, manifest, planner.surface_map(), cfg), final + return CaseStore(suite, manifest, final.standable_surface(cfg.robot_height), cfg), final diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index ae489cab63..0f29aae85c 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -32,7 +32,7 @@ from dimos.navigation.nav_3d.evaluator.metrics import timing_stats from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames -from dimos.navigation.nav_3d.evaluator.voxel_keys import voxel_keys +from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -58,6 +58,20 @@ class FinalMap: add_frame_ms: dict[str, float] build_ms: float + def standable_surface(self, robot_height: float) -> NDArray[np.float32]: + """Occupied cells with robot_height of free space directly above them. + + The evaluator's own account of where a robot could stand, so case + geometry is fixed by the recording rather than by whichever planner is + under test. Deliberately cruder than a planner's surface extraction: + it decides where an endpoint may sit, not where a path may go. + """ + keys = self.occupied_keys + blocked = np.zeros(len(keys), dtype=bool) + for dz in range(1, int(np.ceil(robot_height / self.voxel_size)) + 1): + blocked |= keys_contain(keys, keys + dz) + return key_centers(keys[~blocked], self.voxel_size) + @dataclass class MapCheckpoints: diff --git a/dimos/navigation/nav_3d/evaluator/pipeline.py b/dimos/navigation/nav_3d/evaluator/pipeline.py new file mode 100644 index 0000000000..40b3e20fa5 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/pipeline.py @@ -0,0 +1,107 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The unit under evaluation: lidar and odometry in, paths out. + +A pipeline owns whatever mapping it needs and the evaluator never looks inside +it. Frames arrive in recording order exactly as a LIO stack produced them, and +a plan is asked for at the point in the stream the case starts. The occupancy +the evaluator grades against is built separately by its own mapper, so what a +pipeline chose to keep does not decide whether it passed. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Protocol, runtime_checkable + +if TYPE_CHECKING: + from collections.abc import Callable + + import numpy as np + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + +Point = tuple[float, float, float] + + +class NavPipeline(Protocol): + """Everything the evaluator requires of a navigation stack.""" + + def add_frame(self, points: NDArray[np.float32], origin: Point, ts: float) -> None: + """Take one world-frame lidar cloud and the sensor origin it was shot from.""" + ... + + def plan(self, start: Point, goal: Point) -> NDArray[np.float32] | None: + """Foot-level waypoints from start to goal, or None when there is no route.""" + ... + + +@runtime_checkable +class PipelineIntrospection(Protocol): + """Optional graph layers, drawn into the rerun recording when a pipeline + offers them. Nothing in scoring depends on these.""" + + def surface_clearance_map(self) -> NDArray[np.float32]: ... + + def node_edges(self) -> NDArray[np.float32]: ... + + +class MLSPipeline: + """The voxel ray-tracing mapper feeding the MLS planner. + + The accumulated map is handed to the planner on the first plan after new + frames rather than on every frame, so a plan following a long stretch of + ingest pays for the rebuild it triggers. + """ + + def __init__(self, cfg: EvalConfig) -> None: + from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + + self._mapper = cfg.make_mapper() + self._planner = MLSPlanner( + voxel_size=cfg.voxel_size, robot_height=cfg.robot_height, **cfg.planner + ) + self._pending = False + self._mapped = False + + def add_frame(self, points: NDArray[np.float32], origin: Point, ts: float) -> None: + self._mapper.add_frame(points, origin) + self._pending = True + + def plan(self, start: Point, goal: Point) -> NDArray[np.float32] | None: + if self._pending: + occupied = self._mapper.global_map() + if len(occupied): + self._planner.update_global_map(occupied) + self._mapped = True + self._pending = False + if not self._mapped: + return None + return self._planner.plan(start, goal) + + def surface_clearance_map(self) -> NDArray[np.float32]: + return self._planner.surface_clearance_map() + + def node_edges(self) -> NDArray[np.float32]: + return self._planner.node_edges() + + +PIPELINES: dict[str, Callable[[EvalConfig], NavPipeline]] = {"mls": MLSPipeline} + + +def make_pipeline(name: str, cfg: EvalConfig) -> NavPipeline: + if name not in PIPELINES: + raise ValueError(f"unknown pipeline {name!r}; known: {sorted(PIPELINES)}") + return PIPELINES[name](cfg) diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 1a9c5e6067..1dfa7bd2b3 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -24,10 +24,9 @@ from __future__ import annotations -from concurrent.futures import ProcessPoolExecutor, ThreadPoolExecutor +from concurrent.futures import ProcessPoolExecutor from dataclasses import asdict, dataclass, field, replace import itertools -import threading from time import perf_counter from typing import TYPE_CHECKING @@ -39,17 +38,16 @@ load_or_build_checkpoints, load_or_build_final_map, ) -from dimos.navigation.nav_3d.evaluator.recording import load_trajectory +from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection +from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from collections.abc import Iterator - from numpy.typing import NDArray from dimos.navigation.nav_3d.evaluator.cases import Case, Suite - from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline logger = setup_logger() @@ -82,7 +80,7 @@ class PlanOutcome: @dataclass class PlannerArtifacts: - """Graph state of one planner after its map update. Not serialized to JSON.""" + """Graph state a pipeline chose to expose. Not serialized to JSON.""" surface_clearance: NDArray[np.float32] edges: NDArray[np.float32] @@ -97,7 +95,6 @@ class CaseResult: tags: list[str] l_ref: float online_voxels: int - map_update_ms: float expect_fail: bool online: PlanOutcome final: PlanOutcome @@ -122,6 +119,8 @@ class DatasetResult: cases: list[CaseResult] final_voxels: int map_build_ms: float + # Per-frame cost of feeding the pipeline, which is what map_update_ms + # aggregates. The grading mapper's own replay cost is cached and not it. add_frame_ms: dict[str, float] frames: int final_artifacts: PlannerArtifacts | None = None @@ -173,7 +172,7 @@ def to_dict(self) -> dict[str, object]: def _run_plan( - planner: MLSPlanner, + pipeline: NavPipeline, case: Case, l_ref: float, obstacle_keys: NDArray[np.int64], @@ -181,7 +180,7 @@ def _run_plan( cfg: EvalConfig, ) -> tuple[PlanOutcome, NDArray[np.float32] | None]: t0 = perf_counter() - waypoints = planner.plan(case.start, case.goal) + waypoints = pipeline.plan(case.start, case.goal) plan_ms = (perf_counter() - t0) * 1000 if waypoints is None or len(waypoints) == 0: return _no_plan(plan_ms), None @@ -266,10 +265,14 @@ def _dynamic_candidate( return True, gate.collision_points[:MAX_COLLISIONS_KEPT].tolist() -def _snapshot(planner: MLSPlanner) -> PlannerArtifacts: +def _snapshot(pipeline: NavPipeline) -> PlannerArtifacts | None: + """Graph layers for the rerun recording, or None from a pipeline that keeps + its internals to itself.""" + if not isinstance(pipeline, PipelineIntrospection): + return None return PlannerArtifacts( - surface_clearance=planner.surface_clearance_map(), - edges=planner.node_edges(), + surface_clearance=pipeline.surface_clearance_map(), + edges=pipeline.node_edges(), ) @@ -283,9 +286,7 @@ def _final_only(case: Case) -> bool: return case.expect_fail or "manual" in case.tags -def run_suite( - suite: Suite, cfg: EvalConfig, threads: int = 1, keep_artifacts: bool = False -) -> DatasetResult: +def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> DatasetResult: db_path = suite.db_path() trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) final = load_or_build_final_map(db_path, suite, cfg) @@ -327,10 +328,11 @@ def run_suite( case_ckpt = np.searchsorted(checkpoints.times, start_ts) case_ckpt[final_only] = -1 - final_planner = cfg.make_planner() - final_planner.update_global_map(final.occupied) - + pipeline = cfg.make_pipeline() results: list[CaseResult | None] = [None] * len(suite.cases) + online: dict[int, tuple[PlanOutcome, NDArray[np.float32] | None, NDArray[np.int64]]] = {} + artifacts: dict[int, PlannerArtifacts | None] = {} + occupied_at_plan: dict[int, NDArray[np.float32] | None] = {} def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: """Fill the fields every case copies straight from its case and reference.""" @@ -344,107 +346,89 @@ def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: **rest, # type: ignore[arg-type] ) - for ci in np.flatnonzero(final_only): - case, ref = suite.cases[ci], refs[ci] - outcome, _ = _run_plan(final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg) - if case.expect_fail: - # An infeasible case is passed by refusing it. - outcome = score_negative(outcome) - results[ci] = _result( - case, - ref, - online_voxels=len(final.occupied), - map_update_ms=0.0, - expect_fail=case.expect_fail, - online=outcome, - final=outcome, - soft_progress=outcome.spl, - final_only=True, - ) - - def process_checkpoint( - k: int, - keys: NDArray[np.int64], - online_planner: MLSPlanner, - ) -> None: - online_points = key_centers(keys, cfg.voxel_size) - t0 = perf_counter() - if len(online_points): - online_planner.update_global_map(online_points) - map_update_ms = (perf_counter() - t0) * 1000 + def plan_online(k: int, keys: NDArray[np.int64]) -> None: + """Plan every case whose start time this checkpoint covers, against the + pipeline as it stands after the frames seen so far.""" for ci in np.flatnonzero(case_ckpt == k): case, ref = suite.cases[ci], refs[ci] - final_out, _ = _run_plan( - final_planner, case, ref.length, obstacle_keys, obstacle_keys, cfg - ) - if case.expect_final_fail: - # A dynamic obstacle blocked the route by the final map, so the - # planner is right to refuse it there while the online plan, - # made before the closure, is scored normally. + if not len(keys): + online[ci] = (_no_plan(0.0), None, keys) + continue + # Collisions are checked against the incremental map the evaluator + # had at plan time, not the final map. Support still uses the final + # map, since the ground exists whether or not it was mapped yet. + outcome, waypoints = _run_plan(pipeline, case, ref.length, keys, obstacle_keys, cfg) + online[ci] = (outcome, waypoints, keys) + artifacts[ci] = _snapshot(pipeline) if keep_artifacts else None + occupied_at_plan[ci] = key_centers(keys, cfg.voxel_size) if keep_artifacts else None + + # One pass. Frames go to the pipeline in recording order and each case is + # planned at the point in the stream its start time falls, so the pipeline + # has seen exactly what the robot had seen by then. A pipeline's state + # cannot be snapshotted from outside, which is why this is sequential. + snapshots = checkpoints.iter_snapshots() + add_ms: list[float] = [] + k = 0 + for frame in iter_world_frames( + db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() + ): + while k < len(checkpoints.times) and frame.ts > checkpoints.times[k]: + plan_online(k, next(snapshots)) + k += 1 + t0 = perf_counter() + pipeline.add_frame(frame.points, frame.origin, frame.ts) + add_ms.append((perf_counter() - t0) * 1000) + while k < len(checkpoints.times): + plan_online(k, next(snapshots)) + k += 1 + + # The stream is exhausted, so the pipeline now holds the whole recording. + final_artifacts = _snapshot(pipeline) if keep_artifacts else None + for ci, case in enumerate(suite.cases): + ref = refs[ci] + final_out, _ = _run_plan(pipeline, case, ref.length, obstacle_keys, obstacle_keys, cfg) + if final_only[ci]: + if case.expect_fail: + # An infeasible case is passed by refusing it. final_out = score_negative(final_out) - if len(online_points): - # Collisions are checked against the incremental map the planner - # actually had at plan time (keys), not the final map. Support - # still uses the final map, since the ground exists whether or - # not it was mapped yet. - online_out, online_wp = _run_plan( - online_planner, case, ref.length, keys, obstacle_keys, cfg - ) - else: - online_out = _no_plan(0.0) - online_wp = None - end = online_wp[-1] if online_wp is not None and len(online_wp) else None - dynamic_candidate, blocking = ( - (False, []) - if case.expect_final_fail - else _dynamic_candidate(online_out, final_out, online_wp, keys, obstacle_keys, cfg) - ) results[ci] = _result( case, ref, - online_voxels=len(keys), - map_update_ms=map_update_ms, - expect_fail=False, - online=online_out, + online_voxels=len(final.occupied), + expect_fail=case.expect_fail, + online=final_out, final=final_out, - soft_progress=metrics.soft_progress(end, case.start, case.goal), - dynamic_candidate=dynamic_candidate, - blocking_points=blocking, - online_artifacts=_snapshot(online_planner) - if keep_artifacts and len(online_points) - else None, - online_occupied=online_points if keep_artifacts and len(online_points) else None, + soft_progress=final_out.spl, + final_only=True, ) - - active = {int(k) for k in case_ckpt} - tls = threading.local() - - def task(k: int, keys: NDArray[np.int64]) -> None: - planner = getattr(tls, "planner", None) - if planner is None: - planner = tls.planner = cfg.make_planner() - try: - process_checkpoint(k, keys, planner) - finally: - in_flight.release() - - def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64]]]: - """Walk the delta chain once, yielding the incremental occupancy at each - case's plan time. Only voxels mapped by then are present, so obstacles - the sensor never saw are naturally excluded from the online check.""" - for k, keys in enumerate(checkpoints.iter_snapshots()): - if k in active: - yield k, keys - - # The semaphore caps how many reconstructed snapshots are held in memory. - in_flight = threading.BoundedSemaphore(max(1, threads) * 2) - with ThreadPoolExecutor(max_workers=max(1, threads)) as pool: - futures = [] - for item in snapshot_stream(): - in_flight.acquire() - futures.append(pool.submit(task, *item)) - for future in futures: - future.result() + continue + if case.expect_final_fail: + # A dynamic obstacle blocked the route by the final map, so the + # planner is right to refuse it there while the online plan, made + # before the closure, is scored normally. + final_out = score_negative(final_out) + online_out, online_wp, online_keys = online[ci] + end = online_wp[-1] if online_wp is not None and len(online_wp) else None + dynamic_candidate, blocking = ( + (False, []) + if case.expect_final_fail + else _dynamic_candidate( + online_out, final_out, online_wp, online_keys, obstacle_keys, cfg + ) + ) + results[ci] = _result( + case, + ref, + online_voxels=len(online_keys), + expect_fail=False, + online=online_out, + final=final_out, + soft_progress=metrics.soft_progress(end, case.start, case.goal), + dynamic_candidate=dynamic_candidate, + blocking_points=blocking, + online_artifacts=artifacts.get(ci), + online_occupied=occupied_at_plan.get(ci), + ) done = [r for r in results if r is not None] if len(done) != len(suite.cases): @@ -454,9 +438,9 @@ def snapshot_stream() -> Iterator[tuple[int, NDArray[np.int64]]]: cases=done, final_voxels=len(final.occupied), map_build_ms=final.build_ms, - add_frame_ms=final.add_frame_ms, - frames=final.frames, - final_artifacts=_snapshot(final_planner) if keep_artifacts else None, + add_frame_ms=metrics.timing_stats(add_ms), + frames=len(add_ms), + final_artifacts=final_artifacts, ) @@ -466,27 +450,17 @@ def evaluate( workers: int = 1, keep_artifacts: bool = False, ) -> Report: - """Score every suite. workers is total parallelism: datasets spread over - processes and each dataset's checkpoints over threads. keep_artifacts - snapshots each planner graph for the rerun recording.""" + """Score every suite. A dataset is one sequential pass over its recording, + so workers only spreads datasets across processes. keep_artifacts snapshots + each pipeline's graph for the rerun recording.""" cfg = cfg or EvalConfig() if workers > 1 and len(suites) > 1: - threads = max(1, workers // len(suites)) with ProcessPoolExecutor(max_workers=min(workers, len(suites))) as pool: datasets = list( - pool.map( - run_suite, - suites, - itertools.repeat(cfg), - itertools.repeat(threads), - itertools.repeat(keep_artifacts), - ) + pool.map(run_suite, suites, itertools.repeat(cfg), itertools.repeat(keep_artifacts)) ) else: - datasets = [ - run_suite(suite, cfg, threads=workers, keep_artifacts=keep_artifacts) - for suite in suites - ] + datasets = [run_suite(suite, cfg, keep_artifacts=keep_artifacts) for suite in suites] cases = [c for d in datasets for c in d.cases] if not cases: raise ValueError("no cases to evaluate") @@ -532,7 +506,11 @@ def mean(values: list[float]) -> float: outcome_counts=outcome_counts, by_tag=by_tag, plan_ms=metrics.timing_stats([c.online.plan_ms for c in online]), - map_update_ms=metrics.timing_stats([c.map_update_ms for c in online]), + # Worst dataset's per-frame ingest cost: the budget asks whether any + # pipeline failed to keep up with the sensor, not what the average was. + map_update_ms={ + k: max(d.add_frame_ms.get(k, 0.0) for d in datasets) for k in ("p50", "p95", "max") + }, datasets=datasets, dynamic_candidates=[f"{c.dataset}/{c.id}" for c in cases if c.dynamic_candidate], config=asdict(cfg), diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 1a625e02e7..642567ba42 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -40,6 +40,7 @@ generate_cases, snap_to_surface, ) +from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection, make_pipeline from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory from dimos.navigation.nav_3d.evaluator.runner import ( CaseResult, @@ -49,13 +50,14 @@ _final_only, _no_plan, _run_plan, + _snapshot, score_negative, ) from dimos.navigation.nav_3d.evaluator.tagging import route_tags from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys if TYPE_CHECKING: - from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner + from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline VOXEL = 0.1 @@ -253,11 +255,15 @@ def test_check_kinematics_rejects_cliff_jumps() -> None: assert len(result.violation_points) >= 1 -class _StubPlanner: - """Returns a fixed path regardless of the map, for gaming the scorer.""" +class _StubPipeline: + """Returns a fixed path regardless of what it was fed, for gaming the scorer.""" def __init__(self, waypoints: np.ndarray | None) -> None: self._waypoints = waypoints + self.frames = 0 + + def add_frame(self, points: np.ndarray, origin: tuple[float, float, float], ts: float) -> None: + self.frames += 1 def plan( self, start: tuple[float, float, float], goal: tuple[float, float, float] @@ -265,8 +271,29 @@ def plan( return self._waypoints -def _stub(waypoints: np.ndarray | None) -> MLSPlanner: - return cast("MLSPlanner", _StubPlanner(waypoints)) +def _stub(waypoints: np.ndarray | None) -> NavPipeline: + return cast("NavPipeline", _StubPipeline(waypoints)) + + +def test_make_pipeline_rejects_unknown_name() -> None: + assert isinstance(make_pipeline("mls", _cfg()), PipelineIntrospection) + with pytest.raises(ValueError, match="unknown pipeline"): + make_pipeline("nope", _cfg()) + + +def test_graph_layers_are_optional() -> None: + """A pipeline that keeps its internals to itself still evaluates, it just + contributes no graph layers to the recording.""" + + class _Introspective(_StubPipeline): + def surface_clearance_map(self) -> np.ndarray: + return np.zeros((1, 4), dtype=np.float32) + + def node_edges(self) -> np.ndarray: + return np.zeros((1, 7), dtype=np.float32) + + assert _snapshot(_stub(None)) is None + assert _snapshot(cast("NavPipeline", _Introspective(None))) is not None def _floor(x_lo: float = 0.0, x_hi: float = 20.0) -> np.ndarray: @@ -532,6 +559,50 @@ def test_route_tags_gate_excludes_detour_routes() -> None: assert route_tags((0.0, 0.0, 0.0), (4.0, 0.0, 0.0), detour, keys, _cfg()) == ["flat"] +def _final_map(points: np.ndarray) -> FinalMap: + return FinalMap( + voxel_size=VOXEL, + occupied=points, + occupied_keys=np.unique(voxel_keys(points, VOXEL)), + frames=1, + add_frame_ms={"p50": 0.0, "p95": 0.0, "max": 0.0}, + build_ms=0.0, + ) + + +def _surface_keys(points: np.ndarray, robot_height: float = 0.3) -> np.ndarray: + return np.unique(voxel_keys(_final_map(points).standable_surface(robot_height), VOXEL)) + + +def test_standable_surface_is_the_top_of_each_column() -> None: + """A wall contributes its cap and not its face, and the floor it stands on + stops being standable.""" + floor, wall = _floor(0.0, 2.0), _wall(1.0) + keys = _surface_keys(np.concatenate([floor, wall])) + + def probe(point: np.ndarray) -> bool: + return bool(keys_contain(keys, voxel_keys(point.reshape(1, 3), VOXEL))[0]) + + # Take one real column of the wall so the checks land on cells that exist. + top = wall[wall[:, 2].argmax()] + assert probe(top) + assert not probe(top - np.array([0, 0, 0.5], dtype=np.float32)) + # Floor buried under that column, versus floor out in the open. + assert not probe(np.array([top[0], top[1], -0.05], dtype=np.float32)) + assert probe(np.array([top[0] - 0.5, top[1], -0.05], dtype=np.float32)) + + +def test_standable_surface_keeps_floor_under_high_ceiling() -> None: + """Headroom is what matters: a ceiling above the robot's height leaves the + floor standable, one inside it does not.""" + floor = _floor(0.0, 2.0) + floor_keys = np.unique(voxel_keys(floor, VOXEL)) + high = _surface_keys(np.concatenate([floor, floor + np.array([0, 0, 1.2], dtype=np.float32)])) + assert keys_contain(high, floor_keys).all() + low = _surface_keys(np.concatenate([floor, floor + np.array([0, 0, 0.2], dtype=np.float32)])) + assert not keys_contain(low, floor_keys).any() + + def test_snap_to_surface() -> None: xs, ys = np.meshgrid(np.arange(0, 2, VOXEL), np.arange(0, 2, VOXEL)) surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) @@ -687,7 +758,6 @@ def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, tags=[], l_ref=1.0, online_voxels=0, - map_update_ms=0.0, expect_fail=False, online=replace(_no_plan(0.0), success=inc), final=replace(_no_plan(0.0), success=fin), From b1d8180cceafe48074c5496765fbf91170dec3c2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 3 Aug 2026 19:38:14 -0700 Subject: [PATCH 25/29] Fixes --- dimos/cli/dimos.py | 5 + dimos/navigation/nav_3d/evaluator/__main__.py | 17 --- dimos/navigation/nav_3d/evaluator/cases.py | 7 +- .../nav_3d/evaluator/cases/sf_office.yaml | 2 +- dimos/navigation/nav_3d/evaluator/cli.py | 22 +--- dimos/navigation/nav_3d/evaluator/config.py | 50 ++------ dimos/navigation/nav_3d/evaluator/curation.py | 39 ++++-- .../navigation/nav_3d/evaluator/final_map.py | 39 ++++-- dimos/navigation/nav_3d/evaluator/generate.py | 80 +++++++----- dimos/navigation/nav_3d/evaluator/metrics.py | 112 ++++++++-------- dimos/navigation/nav_3d/evaluator/picker.py | 5 +- dimos/navigation/nav_3d/evaluator/pipeline.py | 10 +- .../navigation/nav_3d/evaluator/recording.py | 15 ++- dimos/navigation/nav_3d/evaluator/runner.py | 52 ++++---- dimos/navigation/nav_3d/evaluator/tagging.py | 34 ++--- .../nav_3d/evaluator/test_evaluator.py | 121 +++++++++++++++++- dimos/navigation/nav_3d/evaluator/tripwire.py | 8 +- dimos/navigation/nav_3d/evaluator/viz.py | 36 ++---- .../navigation/nav_3d/evaluator/voxel_keys.py | 36 ++++-- pyproject.toml | 2 - uv.lock | 5 - 21 files changed, 381 insertions(+), 316 deletions(-) delete mode 100644 dimos/navigation/nav_3d/evaluator/__main__.py diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index d1487798c3..0febe95084 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -753,6 +753,11 @@ def send( map_app.command("global")(_map_main) +from dimos.navigation.nav_3d.evaluator.cli import app as nav_eval_app + +main.add_typer(nav_eval_app, name="nav-eval") + + dataprep_app = typer.Typer(help="Build and inspect learning datasets from recordings") main.add_typer(dataprep_app, name="dataprep") diff --git a/dimos/navigation/nav_3d/evaluator/__main__.py b/dimos/navigation/nav_3d/evaluator/__main__.py deleted file mode 100644 index d8f8c66af0..0000000000 --- a/dimos/navigation/nav_3d/evaluator/__main__.py +++ /dev/null @@ -1,17 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos.navigation.nav_3d.evaluator.cli import app - -app() diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index ac74a13ee1..db73cec7aa 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -12,11 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Case manifests for the nav-3d evaluator. - -A suite is one YAML file per dataset under cases/. Start and goal are -foot-level world coordinates, the frame the planner consumes. -""" +"""Case manifests, one YAML file per dataset. Endpoints are foot-level world +coordinates.""" from __future__ import annotations diff --git a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml index 6ca7199e84..deb4a761e1 100644 --- a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml +++ b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml @@ -8,7 +8,7 @@ cases: - id: auto_01_flat start: [-2.84, 8.44, -0.24] goal: [3.64, 7.64, -0.16] - tags: [manual, flat, doorway, auto] + tags: [auto, flat, doorway] - id: auto_02_flat start: [6.44, 0.44, -0.16] goal: [6.68, -3.0, 0.08] diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 752a5b5d10..74486c4cac 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -12,21 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Nav-3d evaluation CLI. - -Run every suite: python -m dimos.navigation.nav_3d.evaluator run -One dataset: python -m dimos.navigation.nav_3d.evaluator run --dataset mid360_athens_stairs -Only some cases: python -m dimos.navigation.nav_3d.evaluator run --tag stairs --tag up -Machine output: python -m dimos.navigation.nav_3d.evaluator run --json report.json -Override a gate: python -m dimos.navigation.nav_3d.evaluator run --set goal_tolerance=0.4 -Compare two runs: python -m dimos.navigation.nav_3d.evaluator diff old.json new.json -Determinism check: run twice with --json, then diff a.json b.json --exact -New dataset: python -m dimos.navigation.nav_3d.evaluator ingest recordings/.../mem2.db --name office_a -Pick cases by click: python -m dimos.navigation.nav_3d.evaluator pick-case office_a -Curate by coords: python -m dimos.navigation.nav_3d.evaluator add-case office_a --start x y z --goal x y z -Flag dynamic route: python -m dimos.navigation.nav_3d.evaluator tag office_a auto_03 --final-fail -Recompute tags: python -m dimos.navigation.nav_3d.evaluator retag office_a -""" +"""Nav-3d evaluation CLI, mounted as `dimos nav-eval`.""" from __future__ import annotations @@ -145,7 +131,7 @@ def _print_report(report: Report) -> None: print(f"\nincremental-only ({len(inc_only)}) — passed online, failed final:") if report.dynamic_candidates: print(f" dynamic-obstacle candidates: {', '.join(report.dynamic_candidates)}") - print(" review with --rrd, confirm: evaluator tag --final-fail") + print(" review with --rrd, confirm: dimos nav-eval tag --final-fail") if others: print(f" not explained by a new obstacle, inspect final map: {', '.join(others)}") @@ -336,7 +322,7 @@ def ingest( print(f"\n{len(suite.cases)} cases -> {path}") for case in suite.cases: print(f" {case.id}: [{', '.join(case.tags)}]") - print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {name}") + print(f"\nrun with: dimos nav-eval run --dataset {name}") def _open(dataset: str) -> tuple[CaseStore, FinalMap]: @@ -472,7 +458,7 @@ def pick_case( foot, store, ) - print(f"\nrun with: python -m dimos.navigation.nav_3d.evaluator run --dataset {dataset}") + print(f"\nrun with: dimos nav-eval run --dataset {dataset}") @app.command("list") diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 9d2b4178da..5d94baa634 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -17,59 +17,41 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING -from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper - if TYPE_CHECKING: - from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline + from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper @dataclass class EvalConfig: """Harness and gate parameters, sized for the Unitree Go2. - (0.31m wide, 0.40m tall, ~0.16m stair risers.) - - Fixes only the shared voxel resolution, sensor range, sensor height, and - the physical body and capability bounds it gates against. Algorithm tuning - lives in the algorithm packages as their constructor defaults. + Algorithm tuning lives in the algorithm packages, not here. """ voxel_size: float = 0.08 max_range: float = 30.0 robot_height: float = 0.3 - # Physical body envelope for the collision gate: a box the robot's length - # and width, oriented along the path and pitched with the slope. The gate - # catches paths that drive the body through obstacles. Only the elevated - # body is checked, from ground_margin to body_clearance up the tilted body - # axis, so the legs and the terrain they stand on never count. Length and - # width match the Go2 collision box. + # Collision-gate body box. Only the ground_margin to body_clearance band + # is checked, so the legs and the terrain under them never count. robot_length: float = 0.7 robot_width: float = 0.31 ground_margin: float = 0.25 body_clearance: float = 0.45 goal_tolerance: float = 0.5 align_tol: float = 0.05 - # Paths must stand on final-map occupancy within support_radius_m of - # each sample and support_depth_m below it. The radius models the Go2 - # straddling small scan holes (0.7m footprint), not its body width. + # Ground-support reach. The radius models straddling small scan holes. support_radius_m: float = 0.35 support_depth_m: float = 0.35 - # Climb limits, checked over a stride-scale window so planner cell - # quantization does not read as a cliff. The slope bound comes from the - # steepest climbs the Go2 demonstrated on the Athens stairs, where - # switchback corners locally exceed the spec-sheet 40 degrees. + # Climb limits, from the steepest climbs the Go2 demonstrated on stairs. max_slope: float = 1.2 max_step_m: float = 0.2 kinematic_window_m: float = 0.5 - # How far an endpoint may sit from a standable surface before it counts as - # off the map, for both case generation and curation snapping. + # How far an endpoint may sit from a standable surface before it is off the map. snap_max_m: float = 1.0 # An improvement must not buy score with compute. p95 over the suite. - # A plan is timed end to end. A pipeline is opaque, so map work it defers - # until asked for a route is charged here rather than hidden, and the - # budget has to cover both the rebuild and the search. + # A plan is timed end to end, so deferred map work is charged here too. plan_p95_budget_ms: float = 200.0 # Per lidar frame, so a pipeline that cannot keep up with the sensor fails # regardless of how it scores. @@ -78,24 +60,18 @@ class EvalConfig: # Which pipeline is under test, by registry name. pipeline: str = "mls" # Pipeline constructor overrides, e.g. --set planner.wall_clearance_m=0.0. - # Omitted keys keep the pipeline's own defaults, so nothing is duplicated - # here, and the report records whatever was swept. planner: dict[str, float] = field(default_factory=dict) def make_mapper(self) -> VoxelRayMapper: - """The evaluator's own mapper, which builds the occupancy every pipeline - is graded against. Pipelines may use it too, but nothing requires them to.""" - return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) + """The mapper that builds the occupancy every pipeline is graded against.""" + from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper - def make_pipeline(self) -> NavPipeline: - from dimos.navigation.nav_3d.evaluator.pipeline import make_pipeline - - return make_pipeline(self.pipeline, self) + return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) def mapper_fingerprint(self) -> dict[str, float | int]: """Cache key parameters for the final map. - Mapper internals are deliberately not fingerprinted. Changes to the - mapper, code or defaults, require wiping data/.final instead. + Mapper internals are not fingerprinted, so a mapper change needs + dimos cache clean rather than a new key here. """ return {"voxel_size": self.voxel_size, "max_range": self.max_range} diff --git a/dimos/navigation/nav_3d/evaluator/curation.py b/dimos/navigation/nav_3d/evaluator/curation.py index e63796544e..1558207298 100644 --- a/dimos/navigation/nav_3d/evaluator/curation.py +++ b/dimos/navigation/nav_3d/evaluator/curation.py @@ -12,12 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Editing a case manifest: add, update, and delete curated cases. - -Every mutation snaps endpoints to the final map's standable surface and writes -the manifest, so the CLI and the browser picker share one implementation and -one set of rules. -""" +"""Editing a case manifest, shared by the CLI and the browser picker.""" from __future__ import annotations @@ -31,6 +26,7 @@ from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.generate import snap_to_surface +from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from pathlib import Path @@ -40,6 +36,8 @@ from dimos.navigation.nav_3d.evaluator.cases import Suite from dimos.navigation.nav_3d.evaluator.final_map import FinalMap +logger = setup_logger() + Point = tuple[float, float, float] @@ -47,6 +45,10 @@ class CurationError(Exception): """A curation request the manifest cannot accept.""" +def _provenance(tags: list[str]) -> str: + return "auto" if "auto" in tags else "manual" + + @dataclass class CaseStore: """Mutable view of one dataset's manifest, saved after every change.""" @@ -66,7 +68,7 @@ def _snap(self, label: str, point: Point, *, required: bool = True) -> Point: f"{label} {point} is more than {self.cfg.snap_max_m}m from a standable surface" ) # An infeasible goal may sit on geometry with no standable surface. - print(f"note: {label} {point} is off any standable surface; keeping it as picked") + logger.warning("%s %s is off any standable surface; kept as picked", label, point) return point return (float(snapped[0]), float(snapped[1]), float(snapped[2])) @@ -96,7 +98,9 @@ def add( self.suite.cases.append(case) self.save() kind = "negative (must refuse)" if expect_fail else "positive" - print(f"added {kind} {case.id}: {case.start} -> {case.goal} to {self.manifest}") + logger.info( + "added %s %s: %s -> %s to %s", kind, case.id, case.start, case.goal, self.manifest + ) return case def update(self, case_id: str, new_id: str, tags: list[str], expect_fail: bool) -> Case: @@ -104,7 +108,7 @@ def update(self, case_id: str, new_id: str, tags: list[str], expect_fail: bool) if new_id != case_id and any(c.id == new_id for c in self.suite.cases): raise CurationError(f"case id {new_id!r} already exists") case.id = new_id - case.tags = _curated_tags(tags, expect_fail) + case.tags = _curated_tags(tags, expect_fail, _provenance(case.tags)) case.expect_fail = expect_fail if expect_fail: case.expect_final_fail = False @@ -125,11 +129,18 @@ def save(self) -> None: save_suite(self.suite, self.manifest) -def _curated_tags(tags: list[str], expect_fail: bool) -> list[str]: - """Curated cases always carry manual provenance. The negative tag tracks - expect_fail rather than being editable text, so the two cannot drift.""" - keep = [t for t in tags if t not in ("manual", "negative")] - return ["manual", *(["negative"] if expect_fail else []), *keep] +PROVENANCE_TAGS = ("auto", "manual") + + +def _curated_tags(tags: list[str], expect_fail: bool, provenance: str = "manual") -> list[str]: + """Rewrite a case's tags around exactly one provenance tag. + + Editing a case must not change what it measures, so an auto case keeps its + generated provenance. The negative tag tracks expect_fail rather than being + editable text, so the two cannot drift. + """ + keep = [t for t in tags if t not in (*PROVENANCE_TAGS, "negative")] + return [provenance, *(["negative"] if expect_fail else []), *keep] def load_store(dataset: str) -> tuple[CaseStore, FinalMap]: diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index 0f29aae85c..550015d9c0 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -12,12 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Final map: the mapper's output over every frame of a recording. +"""The evaluator's own map of a recording, and the checkpoints along it. -Not ground truth, just the most complete map the pipeline produces, so it -serves as the collision reference for returned paths. The same replay also -produces incremental checkpoints: the occupied set at chosen mid-recording -times, which is what the robot had seen by then. +Not ground truth, just the most complete occupancy the mapper produces, which +is what returned paths are graded against. """ from __future__ import annotations @@ -25,11 +23,13 @@ from dataclasses import dataclass import hashlib import json +import os from time import perf_counter from typing import TYPE_CHECKING import numpy as np +from dimos.constants import CACHE_DIR from dimos.navigation.nav_3d.evaluator.metrics import timing_stats from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys @@ -61,10 +61,7 @@ class FinalMap: def standable_surface(self, robot_height: float) -> NDArray[np.float32]: """Occupied cells with robot_height of free space directly above them. - The evaluator's own account of where a robot could stand, so case - geometry is fixed by the recording rather than by whichever planner is - under test. Deliberately cruder than a planner's surface extraction: - it decides where an endpoint may sit, not where a path may go. + Decides where an endpoint may sit, not where a path may go. """ keys = self.occupied_keys blocked = np.zeros(len(keys), dtype=bool) @@ -94,9 +91,25 @@ def iter_snapshots(self) -> Iterator[NDArray[np.int64]]: CHECKPOINT_CACHE_VERSION = 3 +def _save_npz(cache: Path, **arrays: object) -> None: + """Publish a cache atomically, so an interrupted build cannot poison later runs.""" + cache.parent.mkdir(parents=True, exist_ok=True) + tmp = cache.with_name(f"{cache.name}.{os.getpid()}.tmp") + try: + # Written through a handle because savez appends .npz to a bare path. + with tmp.open("wb") as fh: + np.savez_compressed(fh, **arrays) # type: ignore[arg-type] + os.replace(tmp, cache) + finally: + tmp.unlink(missing_ok=True) + + +CACHE_SUBDIR = CACHE_DIR / "nav3d_eval" + + def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: digest = hashlib.sha1(json.dumps(params, sort_keys=True).encode()).hexdigest()[:10] - return db_path.parent / ".final" / f"{db_path.stem}.{digest}.npz" + return CACHE_SUBDIR / f"{db_path.stem}.{digest}.npz" def _final_params(suite: Suite, cfg: EvalConfig) -> dict[str, float | int | str]: @@ -148,8 +161,7 @@ def replay_frames( def _save_final(cache: Path, final: FinalMap) -> None: - cache.parent.mkdir(exist_ok=True) - np.savez_compressed( + _save_npz( cache, occupied=final.occupied, occupied_keys=final.occupied_keys, @@ -239,7 +251,6 @@ def load_or_build_checkpoints( arrays: dict[str, NDArray[np.int64] | NDArray[np.float64]] = {"times": times} arrays |= {f"add_{i}": a for i, a in enumerate(added)} arrays |= {f"rem_{i}": r for i, r in enumerate(removed)} - cache.parent.mkdir(exist_ok=True) - np.savez_compressed(cache, **arrays) # type: ignore[arg-type] + _save_npz(cache, **arrays) logger.info("checkpoints cached: %s", cache.name) return MapCheckpoints(times=times, added=added, removed=removed) diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 15147f9e5e..53d10bb06a 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -14,13 +14,8 @@ """Generate evaluation cases from a recorded trajectory. -Candidate pairs are sampled along the walked path, so both endpoints are -physically proven reachable, and kept only when non-trivial (the straight -line collides, the route detours, or the pair climbs). Cases point backward -in time so an incremental map built to the start has already seen the goal -and a demonstrated route, with the forward direction emitted when the start -is revisited after the goal. Endpoints snap to the final surface so drift -cannot leave a case floating off the map. Generation is deterministic. +Endpoint pairs come off the walked path, so both are proven reachable, and are +kept only when non-trivial and causal. Deterministic. """ from __future__ import annotations @@ -42,6 +37,10 @@ from dimos.navigation.nav_3d.evaluator.recording import Trajectory +# Beyond this a near-straight flat pair is trivial whatever the map holds. +MAX_TRIVIAL_SPAN_M = 30.0 + + @dataclass class GenerationParams: min_separation_m: float = 3.0 @@ -124,9 +123,9 @@ def generate_cases( params: GenerationParams | None = None, ) -> list[Case]: params = params or GenerationParams() - obstacle_keys = final.occupied_keys + map_keys = final.occupied_keys arcs = trajectory.arc_lengths() - foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + foot = trajectory.foot(cfg.robot_height) idx = _subsample_indices(trajectory, params.waypoint_spacing_m) snaps = np.full((len(idx), 3), np.nan, dtype=np.float32) @@ -156,33 +155,39 @@ def generate_cases( sb = snaps[bi] dz = float(sb[2] - sa[2]) detour = float(w / e) - if detour < params.detour_ratio_min and abs(dz) < STAIRS_DZ_M: - # A long near-straight flat pair is trivial. Not worth a sweep. - if e > 30.0: - continue - # Only pairs not already qualified pay for the line sweep. - line = np.stack([sa, sb]) - blocked = not metrics.check_path(line, obstacle_keys, cfg).valid - if not blocked: - continue # Backward in time is always causal. Forward only when the start # spot is revisited after the goal visit. directed = [(sb, sa, -dz)] if last_visit_a >= float(trajectory.ts[idx[bi]]): directed.append((sa, sb, dz)) - for p_start, p_goal, d_dz in directed: - cand = Candidate( - start=(float(p_start[0]), float(p_start[1]), float(p_start[2])), - goal=(float(p_goal[0]), float(p_goal[1]), float(p_goal[2])), - walked_m=float(w), - detour_ratio=detour, - dz=d_dz, + proposed = [ + ( + Candidate( + start=(float(p_start[0]), float(p_start[1]), float(p_start[2])), + goal=(float(p_goal[0]), float(p_goal[1]), float(p_goal[2])), + walked_m=float(w), + detour_ratio=detour, + dz=d_dz, + ), + _bin_key(p_start, p_goal, d_dz, params.bin_size_m), ) - bins = np.floor(np.array([*p_start[:2], *p_goal[:2]]) / params.bin_size_m).astype( - int - ) - dz_sign = int(np.sign(d_dz)) if abs(d_dz) >= STAIRS_DZ_M else 0 - key = (*bins, dz_sign) + for p_start, p_goal, d_dz in directed + ] + # The sweep only decides admission, never priority, so a pair that + # cannot win any of its bins never has to pay for one. + if all( + (best := candidates.get(key)) is not None and best.priority >= cand.priority + for cand, key in proposed + ): + continue + if detour < params.detour_ratio_min and abs(dz) < STAIRS_DZ_M: + # A long near-straight flat pair is trivial. Not worth a sweep. + if e > MAX_TRIVIAL_SPAN_M: + continue + line = np.stack([sa, sb]) + if metrics.check_path(line, map_keys, cfg).valid: + continue + for cand, key in proposed: best = candidates.get(key) if best is None or cand.priority > best.priority: candidates[key] = cand @@ -192,11 +197,18 @@ def generate_cases( cases = [] for n, cand in enumerate(selected): route = metrics.ground_truth_route(trajectory, cand.start, cand.goal, cfg) - tags = route_tags(cand.start, cand.goal, route, obstacle_keys, cfg) + tags = route_tags(cand.start, cand.goal, route, map_keys, cfg) cases.append(_to_case(cand, n, tags)) return cases +def _bin_key( + start: NDArray[np.float32], goal: NDArray[np.float32], dz: float, bin_size_m: float +) -> tuple[int, ...]: + bins = np.floor(np.array([*start[:2], *goal[:2]]) / bin_size_m).astype(int) + return (*bins, int(np.sign(dz)) if abs(dz) >= STAIRS_DZ_M else 0) + + def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> bool: a = np.array([*cand.start, *cand.goal]) for other in accepted: @@ -209,10 +221,8 @@ def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> def _select_diverse( ranked: list[Candidate], params: GenerationParams, max_cases: int ) -> list[Candidate]: - """Spread-greedy selection scored by priority plus endpoint distance from - already-used points, with a sector cap and flat quota. A relaxed pass - backfills to min_cases when the strict pass falls short. - """ + """Spread-greedy selection under a sector cap and flat quota, with a + relaxed pass to reach min_cases.""" if not ranked: return [] flat_target = int(max_cases * params.flat_fraction) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 642c7fa891..625860a32a 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -25,7 +25,9 @@ cylinder_offsets, key_centers, keys_contain, + offset_deltas, offset_keys, + voxel_keys, ) if TYPE_CHECKING: @@ -70,30 +72,29 @@ def goal_reached( # the body surface. Anything farther reports the cap. MARGIN_CAP_M = 0.3 +# Floor on any length used as a divisor or a direction. +MIN_LENGTH_M = 1e-6 + @dataclass class GateResult: - """Collision check of a path against an obstacle key set.""" + """Collision check of a path against a voxel map key set.""" valid: bool collision_points: NDArray[np.float32] # Indices of the colliding samples in densify(waypoints, voxel_size / 2), # so a viewer can recover the exact body frames the gate tested. collision_indices: NDArray[np.int64] - # Horizontal distance from the body surface to the nearest obstacle in - # the gate's z band, minimized along the path. Negative is penetration - # depth, capped at MARGIN_CAP_M when nothing is near. Gives a smooth - # how-close-to-flipping signal next to the binary verdict. + # Horizontal distance from the body surface to the nearest occupied + # voxel in the gate's band, minimised along the path. Negative is + # penetration depth, capped at MARGIN_CAP_M when nothing is near. min_clearance_m: float def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.float64]: - """Unit direction from a point span/2 behind each sample to a point span/2 - ahead, measured along the path. + """Heading from the rear-foot to the front-foot chord, span metres apart. - This is the rigid body's heading: the chord from the rear feet to the front - feet, not the local tangent between two points under the body center, which - on stepped terrain flips between flat treads and vertical risers. + Steadier than the local tangent, which flips between tread and riser. """ if len(samples) < 2: return np.tile(np.array([1.0, 0.0, 0.0]), (len(samples), 1)) @@ -105,15 +106,18 @@ def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.fl back = np.column_stack([np.interp(back_arc, arc, pts[:, c]) for c in range(3)]) front = np.column_stack([np.interp(front_arc, arc, pts[:, c]) for c in range(3)]) fwd = front - back + norm = np.linalg.norm(fwd, axis=1, keepdims=True) + # A path of zero arc length has no heading, so fall back to a valid frame + # rather than handing a singular basis to the caller. + fwd = np.where(norm > MIN_LENGTH_M, fwd, np.array([1.0, 0.0, 0.0])) return fwd / np.maximum(np.linalg.norm(fwd, axis=1, keepdims=True), 1e-9) def body_frames( samples: NDArray[np.float32], robot_length: float ) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]: - """Per-sample body axes: forward along the robot-length chord, lateral - horizontal, up tilted with the slope, so the box yaws and pitches with the - body rather than the terrain right under its center.""" + """Per-sample body axes: forward along the chord, lateral horizontal, up + tilted with the slope.""" fwd = chord_directions(samples, robot_length) lateral = np.cross(np.array([0.0, 0.0, 1.0]), fwd) ln = np.linalg.norm(lateral, axis=1, keepdims=True) @@ -123,17 +127,12 @@ def body_frames( def check_path( - waypoints: NDArray[np.float32], obstacle_keys: NDArray[np.int64], cfg: EvalConfig + waypoints: NDArray[np.float32], map_keys: NDArray[np.int64], cfg: EvalConfig ) -> GateResult: - """Sweep the robot body box along foot-level waypoints against obstacles. - - At each sample the body is a box of the robot's length and width, centered - over the path point and rotated in place: yawed and pitched along the - robot-length chord, so it stays over the path rather than sliding onto the - chord. Its vertical span is the ground_margin to body_clearance band up the - tilted body axis, so the legs and the ground below never count, only the - elevated body. Candidate voxels come from a padded voxelized cylinder that - covers the box at any orientation and are tested against the exact box. + """Sweep the robot body box along foot-level waypoints against the map. + + The box is the robot's length and width, centred mid-band up the tilted + body axis over each sample, so the legs and the ground never count. """ voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size / 2) @@ -141,17 +140,29 @@ def check_path( half_len = cfg.robot_length / 2.0 half_wid = cfg.robot_width / 2.0 half_band = (cfg.body_clearance - cfg.ground_margin) / 2.0 - mid = np.array([0.0, 0.0, (cfg.ground_margin + cfg.body_clearance) / 2.0]) - circ = float(np.hypot(half_len, half_wid)) + mid_h = (cfg.ground_margin + cfg.body_clearance) / 2.0 + centers = samples + mid_h * up + pad = MARGIN_CAP_M + voxel_size + # The candidate cylinder only has to reach the band, so bound it by the + # path's own steepest pitch instead of assuming any orientation. + sin_p = float(np.abs(fwd[:, 2]).max()) + cos_p = float(np.sqrt(max(1.0 - sin_p * sin_p, 0.0))) + reach_z = (half_len + pad) * sin_p + half_band * cos_p + voxel_size offsets = cylinder_offsets( - circ + MARGIN_CAP_M + voxel_size, - -(half_len + MARGIN_CAP_M + voxel_size), - cfg.body_clearance + half_len + MARGIN_CAP_M + voxel_size, + float(np.hypot(half_len, half_wid)) + pad + (mid_h + half_band) * sin_p, + mid_h * cos_p - reach_z, + mid_h * cos_p + reach_z, voxel_size, ) - keys = offset_keys(samples, offsets, voxel_size) - candidate = keys_contain(obstacle_keys, keys.ravel()).reshape(keys.shape) - s_idx, o_idx = np.nonzero(candidate) + # Samples land two per voxel, so membership runs over the distinct voxels + # and is scattered back rather than being paid for twice. + base = voxel_keys(samples, voxel_size) + deltas = offset_deltas(offsets) + unique_base, inverse = np.unique(base, return_inverse=True) + hit = keys_contain(map_keys, (unique_base[:, None] + deltas[None, :]).ravel()).reshape( + len(unique_base), len(deltas) + ) + s_idx, o_idx = np.nonzero(hit[inverse]) if len(s_idx) == 0: return GateResult( valid=True, @@ -159,8 +170,7 @@ def check_path( collision_indices=np.empty(0, dtype=np.int64), min_clearance_m=MARGIN_CAP_M, ) - # Offset from the box center, which sits mid-band directly over the sample. - delta = key_centers(keys[s_idx, o_idx], voxel_size) - samples[s_idx] - mid + delta = key_centers(base[s_idx] + deltas[o_idx], voxel_size) - centers[s_idx] along = (delta * fwd[s_idx]).sum(1) across = (delta * lateral[s_idx]).sum(1) vertical = (delta * up[s_idx]).sum(1) @@ -193,10 +203,7 @@ def check_support( ) -> SupportResult: """Require occupied voxels beneath every path sample. - A path across a void collides with nothing, so the collision gate alone - cannot catch fabricated bridges. Each densified sample must have at least - one occupied voxel within radius horizontally and from depth below the - foot up to one voxel above it. + The collision gate alone cannot catch a path fabricated across a void. """ voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size) @@ -228,10 +235,7 @@ def _resample(waypoints: NDArray[np.float32], spacing: float) -> NDArray[np.floa def check_kinematics(waypoints: NDArray[np.float32], cfg: EvalConfig) -> KinematicsResult: """Reject paths that climb steeper than the robot can. - The profile is resampled at window_m of arc length so single-cell - quantization in planner waypoints does not read as a cliff. Each - resampled segment may rise at most max_slope times its horizontal run, - with a max_step_m floor so stair risers between close samples pass. + Resampled at window_m of arc so cell quantization does not read as a cliff. """ if len(waypoints) < 2: return KinematicsResult(True, waypoints[:0]) @@ -259,8 +263,7 @@ class Reference: @dataclass class _Visits: - """Every trajectory pose near the start and near the goal, with the walked - length of each start-visit to goal-visit pairing.""" + """Poses near each endpoint, with the walked length of every pairing.""" foot: NDArray[np.float32] near_s: NDArray[np.int64] @@ -275,7 +278,7 @@ def _visits( cfg: EvalConfig, ) -> _Visits | None: """None when either endpoint is farther than snap_max_m from the trajectory.""" - foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + foot = trajectory.foot(cfg.robot_height) ds = np.linalg.norm(foot - np.asarray(start, dtype=np.float32), axis=1) dg = np.linalg.norm(foot - np.asarray(goal, dtype=np.float32), axis=1) if ds.min() > cfg.snap_max_m or dg.min() > cfg.snap_max_m: @@ -297,12 +300,11 @@ def reference_length( goal: tuple[float, float, float], cfg: EvalConfig, ) -> Reference: - """Shortest walked length the trajectory demonstrates between start and goal. + """Shortest walked length demonstrated between start and goal. - Minimizes route length over every combination of start and goal visits, - not the route between single nearest poses. Only causal pairs count when - one exists: the goal visited before the start. Falls back to straight-line - distance when either endpoint is off the trajectory. + Minimised over every start-visit to goal-visit pairing, preferring causal + pairs. Falls back to straight-line distance when an endpoint is off the + trajectory. """ visits = _visits(trajectory, start, goal, cfg) if visits is None: @@ -316,7 +318,7 @@ def reference_length( best = np.unravel_index(totals.argmin(), totals.shape) i = int(visits.near_s[best[0]]) start_ts = float(trajectory.ts[i]) if causal else float("inf") - return Reference(max(float(totals[best]), 1e-6), True, start_ts, causal) + return Reference(max(float(totals[best]), MIN_LENGTH_M), True, start_ts, causal) def ground_truth_route( @@ -325,13 +327,9 @@ def ground_truth_route( goal: tuple[float, float, float], cfg: EvalConfig, ) -> NDArray[np.float32] | None: - """Foot-level polyline of the shortest walk the robot took between start and - goal, or None when either endpoint is off the trajectory. + """Foot-level polyline of the shortest walk between start and goal. - Unlike reference_length this ignores causality: it describes the terrain - between two places, so the nearest-in-time pass is what we want, not the - causal one. Picking the visit pair with the least trajectory between them - keeps the route local instead of a building-spanning detour. + Ignores causality: it describes terrain, not what the robot knew. """ visits = _visits(trajectory, start, goal, cfg) if visits is None: @@ -347,7 +345,7 @@ def ground_truth_route( def spl(success: bool, l_ref: float, p_len: float) -> float: if not success: return 0.0 - return l_ref / max(p_len, l_ref) + return l_ref / max(p_len, l_ref, MIN_LENGTH_M) def soft_progress( diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index 1661a7d700..a11f285d8b 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -28,6 +28,7 @@ import numpy as np +from dimos.core.global_config import global_config from dimos.navigation.nav_3d.evaluator.curation import CurationError from dimos.navigation.nav_3d.evaluator.tagging import elevation_tags @@ -373,7 +374,9 @@ def pick_cases( """Serve the picker until the user exits from the panel or hits ctrl-c.""" import viser - server = viser.ViserServer(label=f"Pair Picker - {dataset}", verbose=False) + server = viser.ViserServer( + host=global_config.listen_host, label=f"Pair Picker - {dataset}", verbose=False + ) server.gui.configure_theme(dark_mode=True) server.scene.set_background_image(np.full((1, 1, 3), 14, dtype=np.uint8)) server.scene.set_up_direction("+z") diff --git a/dimos/navigation/nav_3d/evaluator/pipeline.py b/dimos/navigation/nav_3d/evaluator/pipeline.py index 40b3e20fa5..bafad3d5f6 100644 --- a/dimos/navigation/nav_3d/evaluator/pipeline.py +++ b/dimos/navigation/nav_3d/evaluator/pipeline.py @@ -15,10 +15,7 @@ """The unit under evaluation: lidar and odometry in, paths out. A pipeline owns whatever mapping it needs and the evaluator never looks inside -it. Frames arrive in recording order exactly as a LIO stack produced them, and -a plan is asked for at the point in the stream the case starts. The occupancy -the evaluator grades against is built separately by its own mapper, so what a -pipeline chose to keep does not decide whether it passed. +it. Grading occupancy is built separately, by the evaluator's own mapper. """ from __future__ import annotations @@ -61,9 +58,8 @@ def node_edges(self) -> NDArray[np.float32]: ... class MLSPipeline: """The voxel ray-tracing mapper feeding the MLS planner. - The accumulated map is handed to the planner on the first plan after new - frames rather than on every frame, so a plan following a long stretch of - ingest pays for the rebuild it triggers. + The map reaches the planner on the first plan after new frames, so that + plan pays for the rebuild it triggers. """ def __init__(self, cfg: EvalConfig) -> None: diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py index 1981154081..c493cb93fc 100644 --- a/dimos/navigation/nav_3d/evaluator/recording.py +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -51,8 +51,13 @@ class Trajectory: def arc_lengths(self) -> NDArray[np.float64]: """Cumulative walked distance at each pose, starting at 0.""" - steps = np.linalg.norm(np.diff(self.positions, axis=0), axis=1) - return np.concatenate([[0.0], np.cumsum(steps)]) + from dimos.navigation.nav_3d.evaluator.metrics import arc_lengths + + return arc_lengths(self.positions) + + def foot(self, robot_height: float) -> NDArray[np.float32]: + """Poses dropped to foot level, the frame cases and paths are given in.""" + return self.positions - np.array([0.0, 0.0, robot_height], dtype=np.float32) def iter_world_frames( @@ -62,11 +67,9 @@ def iter_world_frames( align_tol: float = 0.05, end_ts: float | None = None, ) -> Iterator[Frame]: - """Yield lidar frames registered into the world by their aligned odometry pose. + """Yield lidar frames registered into the world by their odometry pose. - Frames at or after end_ts (seconds) are skipped. Clouds must be sensor-frame. - Legacy recordings with pre-registered world-frame clouds are rejected. - Re-record them. + Clouds must be sensor-frame. Frames at or after end_ts are skipped. """ store = SqliteStore(path=str(db_path)) with store: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 1dfa7bd2b3..ff1e2997a9 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -12,14 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run case suites through the ray tracer and MLS planner and score them. - -Auto cases plan twice: online on the incremental map at the case start time, -and final on the map fed the whole recording. Manual and infeasible cases plan -once on the final map. The final path is gated against full final occupancy, -the online path against the incremental map at plan time. Every path must stand -on final-map occupancy and stay within the climb envelope. The headline score -is validity-gated SPL on the incremental map. +"""Replay case suites through a pipeline and score them. + +Generated cases plan twice, online at their start time and again on the whole +recording. Curated and infeasible cases plan once, on the final map. The +headline score is validity-gated SPL on the incremental map. """ from __future__ import annotations @@ -38,7 +35,7 @@ load_or_build_checkpoints, load_or_build_final_map, ) -from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection +from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection, make_pipeline from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers from dimos.utils.logging_config import setup_logger @@ -175,7 +172,7 @@ def _run_plan( pipeline: NavPipeline, case: Case, l_ref: float, - obstacle_keys: NDArray[np.int64], + map_keys: NDArray[np.int64], support_keys: NDArray[np.int64], cfg: EvalConfig, ) -> tuple[PlanOutcome, NDArray[np.float32] | None]: @@ -186,7 +183,7 @@ def _run_plan( return _no_plan(plan_ms), None reached = metrics.goal_reached(waypoints, case.goal, cfg.goal_tolerance) - gate = metrics.check_path(waypoints, obstacle_keys, cfg) + gate = metrics.check_path(waypoints, map_keys, cfg) support = metrics.check_support(waypoints, support_keys, cfg) kinematics = metrics.check_kinematics(waypoints, cfg) length = metrics.path_length(waypoints) @@ -279,18 +276,18 @@ def _snapshot(pipeline: NavPipeline) -> PlannerArtifacts | None: def _final_only(case: Case) -> bool: """Whether a case is scored on the final map only, with no online phase. - Manual and certified-infeasible cases have hand-placed endpoints that are - not tied to the recording timeline, so there is no meaningful incremental - map at plan time to replay against. They are pure final-map tests. + Hand-placed endpoints are not tied to the recording timeline, so there is + no meaningful incremental map to replay against. Generated cases keep their + online phase however their labels are later edited. """ - return case.expect_fail or "manual" in case.tags + return case.expect_fail or "auto" not in case.tags def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> DatasetResult: db_path = suite.db_path() trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) final = load_or_build_final_map(db_path, suite, cfg) - obstacle_keys = final.occupied_keys + map_keys = final.occupied_keys final_only = np.array([_final_only(c) for c in suite.cases], dtype=bool) refs: list[metrics.Reference] = [] @@ -328,7 +325,7 @@ def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> Da case_ckpt = np.searchsorted(checkpoints.times, start_ts) case_ckpt[final_only] = -1 - pipeline = cfg.make_pipeline() + pipeline = make_pipeline(cfg.pipeline, cfg) results: list[CaseResult | None] = [None] * len(suite.cases) online: dict[int, tuple[PlanOutcome, NDArray[np.float32] | None, NDArray[np.int64]]] = {} artifacts: dict[int, PlannerArtifacts | None] = {} @@ -357,7 +354,7 @@ def plan_online(k: int, keys: NDArray[np.int64]) -> None: # Collisions are checked against the incremental map the evaluator # had at plan time, not the final map. Support still uses the final # map, since the ground exists whether or not it was mapped yet. - outcome, waypoints = _run_plan(pipeline, case, ref.length, keys, obstacle_keys, cfg) + outcome, waypoints = _run_plan(pipeline, case, ref.length, keys, map_keys, cfg) online[ci] = (outcome, waypoints, keys) artifacts[ci] = _snapshot(pipeline) if keep_artifacts else None occupied_at_plan[ci] = key_centers(keys, cfg.voxel_size) if keep_artifacts else None @@ -386,11 +383,13 @@ def plan_online(k: int, keys: NDArray[np.int64]) -> None: final_artifacts = _snapshot(pipeline) if keep_artifacts else None for ci, case in enumerate(suite.cases): ref = refs[ci] - final_out, _ = _run_plan(pipeline, case, ref.length, obstacle_keys, obstacle_keys, cfg) + final_out, _ = _run_plan(pipeline, case, ref.length, map_keys, map_keys, cfg) + if case.expect_fail or case.expect_final_fail: + # Both labels certify that the final map holds no route, so the + # planner passes by refusing. Applied before the final-only split + # so a curated case is not scored zero for being right. + final_out = score_negative(final_out) if final_only[ci]: - if case.expect_fail: - # An infeasible case is passed by refusing it. - final_out = score_negative(final_out) results[ci] = _result( case, ref, @@ -402,19 +401,12 @@ def plan_online(k: int, keys: NDArray[np.int64]) -> None: final_only=True, ) continue - if case.expect_final_fail: - # A dynamic obstacle blocked the route by the final map, so the - # planner is right to refuse it there while the online plan, made - # before the closure, is scored normally. - final_out = score_negative(final_out) online_out, online_wp, online_keys = online[ci] end = online_wp[-1] if online_wp is not None and len(online_wp) else None dynamic_candidate, blocking = ( (False, []) if case.expect_final_fail - else _dynamic_candidate( - online_out, final_out, online_wp, online_keys, obstacle_keys, cfg - ) + else _dynamic_candidate(online_out, final_out, online_wp, online_keys, map_keys, cfg) ) results[ci] = _result( case, diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py index e7f5bbfd44..43621f29b8 100644 --- a/dimos/navigation/nav_3d/evaluator/tagging.py +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -12,12 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Geometric tags for a case: elevation, narrow, doorway, corridor. - -Elevation tags come from the endpoints. Shape tags measure corridor width along -the demonstrated route and apply only when it runs roughly straight from start -to goal. Thresholds come from the robot's dimensions in EvalConfig. -""" +"""Geometric tags for a case: elevation from the endpoints, shape from the +corridor width along the demonstrated route.""" from __future__ import annotations @@ -41,36 +37,26 @@ from dimos.navigation.nav_3d.evaluator.config import EvalConfig # A pair of endpoints this far apart in z or beyond is a climb, not a flat -# traverse. Half the body height, the smallest step the elevation tags care to -# call stairs. +# traverse. Half the body height. STAIRS_DZ_M = 0.5 -# A climb earns "long" past either bound: a tall total rise or a long walk. +# A climb earns long past either bound: a tall rise or a long walk. LONG_STAIRS_DZ_M = 1.5 LONG_STAIRS_WALKED_M = 20.0 # A sustained narrow stretch this long is a corridor, not a doorway. CORRIDOR_RUN_M = 2.0 -# A doorway pinch is no longer than this. Beyond it the passage is a corridor. DOORWAY_MAX_RUN_M = 1.2 -# Open space must reappear within this arc on both sides of a pinch for it to be -# a doorway rather than a dead-end narrowing. +# Open space must reappear within this arc either side of a pinch. DOORWAY_FLANK_M = 1.4 -# A door frame makes the width wobble across the threshold, splitting one pinch -# into fragments. Merge narrow runs separated by gaps this small so a sharp -# doorway reads as one passage, not several. +# A door frame splits one pinch into fragments, so merge runs this close. NARROW_MERGE_GAP_M = 0.2 -# A narrow run shorter than this is a single stray voxel, not a passage. +# Shorter than this is a stray voxel, not a passage. NARROW_MIN_RUN_M = 0.15 -# Path-shape tags describe the local terrain between the endpoints, so they only -# apply when the demonstrated route runs roughly straight from start to goal. A -# route far longer than the straight line is a detour through the building, and -# one far shorter is a stub that never spans the endpoints. Neither describes -# the case. Kept strict for precision: a doorway tag the filter can trust is -# worth more than catching every winding-route doorway. +# Shape tags only apply to a route that runs roughly straight between the +# endpoints, so a detour's terrain is not attributed to the case. LOCAL_DETOUR_MAX = 2.0 LOCAL_SPAN_MIN_FRAC = 0.8 -# The tags this module owns and recomputes. Everything else on a case, such as -# auto, manual, negative, or dynamic provenance, is left untouched by a retag. +# The tags a retag recomputes. Provenance tags are left untouched. GEOMETRIC_TAGS = frozenset( {"flat", "up", "down", "stairs", "long", "narrow", "doorway", "corridor"} ) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index 642567ba42..bde13d6743 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -27,9 +27,11 @@ from dimos.navigation.nav_3d.evaluator import metrics, tripwire from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig +from dimos.navigation.nav_3d.evaluator.curation import CaseStore, CurationError, _curated_tags from dimos.navigation.nav_3d.evaluator.final_map import ( FinalMap, MapCheckpoints, + _save_npz, encode_deltas, replay_frames, ) @@ -40,6 +42,7 @@ generate_cases, snap_to_surface, ) +from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection, make_pipeline from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory from dimos.navigation.nav_3d.evaluator.runner import ( @@ -54,7 +57,14 @@ score_negative, ) from dimos.navigation.nav_3d.evaluator.tagging import route_tags -from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys +from dimos.navigation.nav_3d.evaluator.voxel_keys import ( + cylinder_offsets, + key_centers, + keys_contain, + offset_deltas, + offset_keys, + voxel_keys, +) if TYPE_CHECKING: from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline @@ -668,8 +678,6 @@ def test_select_diverse_backfills_to_min_cases() -> None: def test_pick_along_ray() -> None: - from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray - wall = _wall(10.0) origin = np.array([0.0, 0.0, 0.5]) target = np.array([10.0, 0.35, 0.75]) @@ -826,3 +834,110 @@ def test_tripwire_perf_violations() -> None: assert len(violations) == 1 and "plan_ms" in violations[0] # Reports predating the budgets pass. assert tripwire.perf_violations({"datasets": []}) == [] + + +def test_gate_band_follows_the_tilted_body_axis() -> None: + """On a slope the band is measured up the body axis, so an obstacle at the + body centre collides and one down in the leg zone does not.""" + vox = 0.02 + cfg = _cfg(voxel_size=vox) + t = np.arange(-3, 3.01, 0.1) + c = 1 / np.sqrt(2) + path = np.stack([t * c, np.zeros_like(t), t * c], axis=1).astype(np.float32) + samples = metrics.densify(path, vox / 2) + i = len(samples) // 2 + _, _, up = metrics.body_frames(samples, cfg.robot_length) + mid_h = (cfg.ground_margin + cfg.body_clearance) / 2.0 + + def hits(point: np.ndarray) -> bool: + keys = np.unique(voxel_keys(np.asarray([point], dtype=np.float32), vox)) + return not metrics.check_path(path, keys, cfg).valid + + assert hits(samples[i] + mid_h * up[i]) + assert not hits(samples[i] + 0.15 * up[i]) + + +def test_offset_deltas_match_packing_the_summed_indices() -> None: + pts = np.array([[0.05, -3.2, 1.4], [12.0, 0.0, -2.0]], dtype=np.float32) + offs = cylinder_offsets(0.4, -0.2, 0.3, VOXEL) + idx = np.floor(pts.astype(np.float64) / VOXEL).astype(np.int64)[:, None, :] + offs[None, :, :] + packed = voxel_keys((idx.reshape(-1, 3) * VOXEL + VOXEL / 2).astype(np.float32), VOXEL) + assert np.array_equal(offset_keys(pts, offs, VOXEL).ravel(), packed) + assert np.array_equal( + offset_keys(pts, offs, VOXEL)[0], voxel_keys(pts, VOXEL)[0] + offset_deltas(offs) + ) + + +def test_spl_and_body_frames_survive_degenerate_input() -> None: + """A coincident start and goal must score, not divide by zero, and a + zero-length path must still yield a usable body frame.""" + assert metrics.spl(True, 0.0, 0.0) == 0.0 + point = np.array([[1.0, 1.0, 0.0], [1.0, 1.0, 0.0]], dtype=np.float32) + fwd, lateral, up = metrics.body_frames(point, 0.7) + assert np.allclose(np.linalg.norm(fwd, axis=1), 1.0) + assert np.linalg.det(np.stack((fwd, lateral, up), axis=-1)[0]) > 0 + + +def _store(tmp_path: Path, cases: list[Case]) -> CaseStore: + manifest = tmp_path / "demo.yaml" + save_suite(Suite(dataset="demo", cases=cases), manifest) + xs, ys = np.meshgrid(np.arange(0, 8, VOXEL), np.arange(-2, 2, VOXEL)) + surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) + return CaseStore(load_suite(manifest), manifest, surface, _cfg()) + + +def test_editing_a_generated_case_keeps_it_in_the_incremental_score(tmp_path: Path) -> None: + """Relabelling a case must not silently change what it measures.""" + case = Case( + id="auto_00_flat", start=(1.0, 0.0, 0.0), goal=(5.0, 0.0, 0.0), tags=["auto", "flat"] + ) + store = _store(tmp_path, [case]) + store.update("auto_00_flat", "auto_00_flat", ["flat", "doorway"], expect_fail=False) + edited = store.get("auto_00_flat") + assert "manual" not in edited.tags + assert not _final_only(edited) + # A hand-added case is curated, and stays a final-map-only test. + added = store.add((1.0, 0.0, 0.0), (4.0, 0.0, 0.0), ["flat"]) + assert added.tags[0] == "manual" + assert _final_only(added) + + +def test_curation_rejects_bad_requests(tmp_path: Path) -> None: + store = _store(tmp_path, [Case(id="manual_00", start=(1.0, 0.0, 0.0), goal=(5.0, 0.0, 0.0))]) + with pytest.raises(CurationError, match="already exists"): + store.add((1.0, 0.0, 0.0), (5.0, 0.0, 0.0), [], case_id="manual_00") + with pytest.raises(CurationError, match="standable surface"): + store.add((1.0, 0.0, 0.0), (400.0, 0.0, 0.0), []) + # An infeasible goal may sit where nothing is standable. + assert store.add((1.0, 0.0, 0.0), (400.0, 0.0, 0.0), [], expect_fail=True).expect_fail + with pytest.raises(CurationError, match="not found"): + store.get("nope") + assert _curated_tags(["flat", "negative"], True) == ["manual", "negative", "flat"] + + +def test_a_curated_dynamic_case_scores_for_refusing() -> None: + """expect_final_fail inverts the final outcome whatever the provenance.""" + keys, cfg, case = _meta_scene() + for tags in (["auto"], ["manual"]): + marked = replace(case, tags=tags, expect_final_fail=True) + refused, _ = _run_plan(_stub(None), marked, 16.0, keys, keys, cfg) + assert score_negative(refused).spl == 1.0 + assert _final_only(marked) == ("auto" not in tags) + + +def test_cache_writes_are_atomic(tmp_path: Path) -> None: + """An interrupted write must leave the previous cache loadable.""" + cache = tmp_path / "nested" / "x.abc123.npz" + _save_npz(cache, frames=np.array(7)) + assert int(np.load(cache)["frames"]) == 7 + assert not list(cache.parent.glob("*.tmp")) + with pytest.raises(KeyboardInterrupt): + with pytest.MonkeyPatch.context() as mp: + mp.setattr(np, "savez_compressed", _raise_interrupt) + _save_npz(cache, frames=np.array(9)) + assert int(np.load(cache)["frames"]) == 7 + assert not list(cache.parent.glob("*.tmp")) + + +def _raise_interrupt(*args: object, **kwargs: object) -> None: + raise KeyboardInterrupt diff --git a/dimos/navigation/nav_3d/evaluator/tripwire.py b/dimos/navigation/nav_3d/evaluator/tripwire.py index 3a0bcde758..d19180f406 100644 --- a/dimos/navigation/nav_3d/evaluator/tripwire.py +++ b/dimos/navigation/nav_3d/evaluator/tripwire.py @@ -48,7 +48,7 @@ class ReportDiff: def outcomes(report: dict[str, object]) -> Outcomes: - """Pass/fail of both tests for every case in a `run --json` report.""" + """Pass/fail of both tests for every case in a run --json report.""" out: Outcomes = {} for dataset in cast("list[dict[str, object]]", report["datasets"]): cases: dict[str, dict[str, bool]] = {} @@ -134,11 +134,9 @@ def perf_violations(report: dict[str, object]) -> list[str]: def exact_differences(old_report: dict[str, object], new_report: dict[str, object]) -> list[str]: - """Every non-timing field that differs between two reports, at full precision. + """Every non-timing field that differs between two reports. - Two runs of identical code must produce an empty list. This is the - determinism gate: it holds for any algorithm under test, present or - future, because it checks the results rather than the implementation. + The determinism gate: two runs of identical code must return an empty list. """ out: list[str] = [] _walk("report", _strip_timing(old_report), _strip_timing(new_report), out) diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index c872d6143f..acfeb15cf0 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -12,14 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Write an evaluation report into a rerun recording. - -One static scene per dataset: the final voxel map, the walked path, the -planner graph over the aggregated map, and per-case start/goal with the -online and final planned paths colored by verdict and the gate's collision -boxes. Each case also carries a known/ layer holding the incremental map and -planner graph at plan time. -""" +"""Write an evaluation report into a rerun recording, one scene per dataset.""" from __future__ import annotations @@ -31,6 +24,7 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.recording import load_trajectory +from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from pathlib import Path @@ -42,6 +36,8 @@ from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.runner import PlannerArtifacts, PlanOutcome, Report +logger = setup_logger() + WALKED_PATH_COLOR = [255, 255, 255] START_COLOR = [0, 255, 255] GOAL_COLOR = [255, 140, 0] @@ -148,18 +144,15 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) static=True, ) if outcome.collision_indices: - # The gate's body box at each colliding foot sample: the robot length - # and width, centered over the path point and rotated in place (yaw and - # pitch from the chord), elevated over the legs into the ground-margin - # to body-clearance band. Rebuilt from the gate's own sample indices so - # the drawn boxes are the boxes it rejected. Thinned to about a body - # length apart so they read as distinct bodies, not one smear. + # Rebuilt from the gate's own sample indices, thinned to about a body + # length apart so they read as distinct bodies rather than one smear. waypoints = np.asarray(outcome.waypoints, dtype=np.float32) samples = metrics.densify(waypoints, cfg.voxel_size / 2) - axes = np.stack(metrics.body_frames(samples, cfg.robot_length), axis=-1) + frames = metrics.body_frames(samples, cfg.robot_length) + axes = np.stack(frames, axis=-1) idx = np.asarray(outcome.collision_indices, dtype=np.int64) idx = idx[_thin_by_gap(samples[idx], cfg.robot_length)] - mid = np.array([0.0, 0.0, (cfg.ground_margin + cfg.body_clearance) / 2.0]) + mid_h = (cfg.ground_margin + cfg.body_clearance) / 2.0 half = [ cfg.robot_length / 2.0, cfg.robot_width / 2.0, @@ -169,7 +162,7 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) f"{entity}/collisions", rr.Boxes3D( half_sizes=np.tile(half, (len(idx), 1)), - centers=samples[idx] + mid, + centers=samples[idx] + mid_h * frames[2][idx], quaternions=Rotation.from_matrix(axes[idx]).as_quat(), colors=[[*COLLISION_COLOR, COLLISION_FILL_ALPHA]], fill_mode=rr.components.FillMode.Solid, @@ -221,7 +214,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - root = dataset.dataset rr.log( - f"{root}/map/obstacles", + f"{root}/map/voxels", rr.Points3D( final.occupied, colors=turbo_by_height(final.occupied), @@ -229,7 +222,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - ), static=True, ) - foot = trajectory.positions - np.array([0.0, 0.0, cfg.robot_height], dtype=np.float32) + foot = trajectory.foot(cfg.robot_height) rr.log( f"{root}/walked_path", rr.LineStrips3D([foot], colors=[WALKED_PATH_COLOR], radii=0.015), @@ -281,7 +274,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - ) if case.blocking_points: rr.log( - f"{base}/new_obstacle", + f"{base}/new_occupancy", rr.Points3D(case.blocking_points, colors=[DYNAMIC_BLOCK_COLOR], radii=0.06), static=True, ) @@ -291,5 +284,4 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - views = [_dataset_view(d.dataset, [c.id for c in d.cases]) for d in report.datasets] rr.send_blueprint(rrb.Blueprint(rrb.Tabs(*views) if len(views) > 1 else views[0])) - print(f"wrote {out}") - print(f"open with: rerun {out}") + logger.info("wrote %s; open with: rerun %s", out, out) diff --git a/dimos/navigation/nav_3d/evaluator/voxel_keys.py b/dimos/navigation/nav_3d/evaluator/voxel_keys.py index 76dd721400..7db7978bbd 100644 --- a/dimos/navigation/nav_3d/evaluator/voxel_keys.py +++ b/dimos/navigation/nav_3d/evaluator/voxel_keys.py @@ -12,12 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Voxel indices packed into sortable int64 keys. - -Membership tests over a map run as a sorted-array search on these keys rather -than a per-point spatial query, which is what makes the gates cheap enough to -sweep a whole path. -""" +"""Voxel indices packed into sortable int64 keys, so map membership is a +sorted-array search rather than a spatial query.""" from __future__ import annotations @@ -29,18 +25,24 @@ from numpy.typing import NDArray _KEY_OFFSET = 1 << 20 +_FIELD_BITS = 21 +_X_SHIFT = 2 * _FIELD_BITS +_Y_SHIFT = _FIELD_BITS +_FIELD_MASK = (1 << _FIELD_BITS) - 1 def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: """Pack voxel indices into sortable int64 keys, one per point.""" idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - return (idx[:, 0] << 42) | (idx[:, 1] << 21) | idx[:, 2] + return (idx[:, 0] << _X_SHIFT) | (idx[:, 1] << _Y_SHIFT) | idx[:, 2] def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: """Voxel center positions for packed keys, the inverse of voxel_keys.""" - mask = (1 << 21) - 1 - idx = np.stack([keys >> 42, (keys >> 21) & mask, keys & mask], axis=1) - _KEY_OFFSET + idx = ( + np.stack([keys >> _X_SHIFT, (keys >> _Y_SHIFT) & _FIELD_MASK, keys & _FIELD_MASK], axis=1) + - _KEY_OFFSET + ) return ((idx + 0.5) * voxel_size).astype(np.float32) @@ -65,10 +67,18 @@ def cylinder_offsets( return np.asarray(out, dtype=np.int64) +def offset_deltas(offsets: NDArray[np.int64]) -> NDArray[np.int64]: + """Packed key deltas for integer voxel offsets. + + The three index fields occupy disjoint bit ranges and sit far from their + bounds, so adding a packed delta carries no bits between fields and is + identical to packing the summed indices. + """ + return np.asarray((offsets[:, 0] << _X_SHIFT) + (offsets[:, 1] << _Y_SHIFT) + offsets[:, 2]) + + def offset_keys( points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float ) -> NDArray[np.int64]: - """Keys of every (point voxel + offset) pair, shape (P * O,).""" - idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - swept = idx[:, None, :] + offsets[None, :, :] - return np.asarray((swept[..., 0] << 42) | (swept[..., 1] << 21) | swept[..., 2]) + """Keys of every (point voxel + offset) pair, shape (P, O).""" + return voxel_keys(points, voxel_size)[:, None] + offset_deltas(offsets)[None, :] diff --git a/pyproject.toml b/pyproject.toml index ca0afb36c5..6441ea3202 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -208,8 +208,6 @@ visualization = [ # Rerun URDF robot visualization. yourdfpy depends on trimesh[easy], # which pulls embreex; embreex has no Linux aarch64 wheel. "yourdfpy>=0.0.60; sys_platform != 'linux' or platform_machine != 'aarch64'", - # Browser point-picking for nav-3d evaluator case curation. - "viser[urdf]>=1.0.29", ] learning = [ diff --git a/uv.lock b/uv.lock index 203ca6a7f0..799831a286 100644 --- a/uv.lock +++ b/uv.lock @@ -1700,7 +1700,6 @@ base = [ { name = "transformers", extra = ["torch"] }, { name = "ultralytics" }, { name = "uvicorn" }, - { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] cpu = [ @@ -1807,7 +1806,6 @@ unitree = [ { name = "ultralytics" }, { name = "unitree-webrtc-connect" }, { name = "uvicorn" }, - { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] unitree-dds = [ @@ -1843,13 +1841,11 @@ unitree-dds = [ { name = "unitree-sdk2py-dimos" }, { name = "unitree-webrtc-connect" }, { name = "uvicorn" }, - { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] visualization = [ { name = "dimos-viewer" }, { name = "rerun-sdk" }, - { name = "viser", extra = ["urdf"] }, { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] web = [ @@ -2153,7 +2149,6 @@ requires-dist = [ { name = "usd-core", marker = "extra == 'scene'", specifier = ">=23.11" }, { name = "uvicorn", marker = "extra == 'web'", specifier = ">=0.34.0" }, { name = "viser", extras = ["urdf"], marker = "extra == 'manipulation'", specifier = ">=1.0.29" }, - { name = "viser", extras = ["urdf"], marker = "extra == 'visualization'", specifier = ">=1.0.29" }, { name = "websocket-client", specifier = ">=1.8" }, { name = "xacro", marker = "extra == 'manipulation'" }, { name = "xarm-python-sdk", marker = "extra == 'manipulation'", specifier = ">=1.17.0" }, From be6e2267827c54df600a6e6efea03b9820f39333 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 3 Aug 2026 23:32:39 -0700 Subject: [PATCH 26/29] Add a few tests --- dimos/navigation/nav_3d/evaluator/metrics.py | 8 +- .../nav_3d/evaluator/test_evaluator.py | 97 ++++++++++++++++++- 2 files changed, 100 insertions(+), 5 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 625860a32a..597180b707 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -86,13 +86,13 @@ class GateResult: # so a viewer can recover the exact body frames the gate tested. collision_indices: NDArray[np.int64] # Horizontal distance from the body surface to the nearest occupied - # voxel in the gate's band, minimised along the path. Negative is + # voxel in the gate's band, minimized along the path. Negative is # penetration depth, capped at MARGIN_CAP_M when nothing is near. min_clearance_m: float def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.float64]: - """Heading from the rear-foot to the front-foot chord, span metres apart. + """Heading from the rear-foot to the front-foot chord, span meters apart. Steadier than the local tangent, which flips between tread and riser. """ @@ -131,7 +131,7 @@ def check_path( ) -> GateResult: """Sweep the robot body box along foot-level waypoints against the map. - The box is the robot's length and width, centred mid-band up the tilted + The box is the robot's length and width, centered mid-band up the tilted body axis over each sample, so the legs and the ground never count. """ voxel_size = cfg.voxel_size @@ -302,7 +302,7 @@ def reference_length( ) -> Reference: """Shortest walked length demonstrated between start and goal. - Minimised over every start-visit to goal-visit pairing, preferring causal + Minimized over every start-visit to goal-visit pairing, preferring causal pairs. Falls back to straight-line distance when an endpoint is off the trajectory. """ diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index bde13d6743..c605e77de1 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -22,6 +22,7 @@ import numpy as np import pytest +import typer from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.navigation.nav_3d.evaluator import metrics, tripwire @@ -838,7 +839,7 @@ def test_tripwire_perf_violations() -> None: def test_gate_band_follows_the_tilted_body_axis() -> None: """On a slope the band is measured up the body axis, so an obstacle at the - body centre collides and one down in the leg zone does not.""" + body center collides and one down in the leg zone does not.""" vox = 0.02 cfg = _cfg(voxel_size=vox) t = np.arange(-3, 3.01, 0.1) @@ -941,3 +942,97 @@ def test_cache_writes_are_atomic(tmp_path: Path) -> None: def _raise_interrupt(*args: object, **kwargs: object) -> None: raise KeyboardInterrupt + + +def _write_recording( + path: Path, + clouds: list[tuple[float, np.ndarray, str]], + poses: list[tuple[float, tuple[float, float, float]]], +) -> None: + """A minimal mem2 recording: sensor-frame clouds plus odometry.""" + from dimos.memory2.store.sqlite import SqliteStore + from dimos.msgs.geometry_msgs.Pose import Pose + from dimos.msgs.nav_msgs.Odometry import Odometry + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + with SqliteStore(path=str(path)) as store: + lidar = store.stream("lidar", PointCloud2) + for ts, pts, frame_id in clouds: + lidar.append(PointCloud2.from_numpy(pts, frame_id=frame_id, timestamp=ts), ts=ts) + odom = store.stream("odom", Odometry) + for ts, (x, y, z) in poses: + odom.append(Odometry(ts=ts, pose=Pose(x, y, z)), ts=ts) + + +def test_recording_registers_clouds_and_honors_end_ts(tmp_path: Path) -> None: + """Clouds arrive sensor-frame and are placed by their aligned odometry.""" + from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory + + local = np.array([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0]], dtype=np.float32) + db = tmp_path / "rec.db" + _write_recording( + db, + [(1.0, local, "lidar"), (2.0, local, "lidar"), (3.0, local, "lidar")], + [(1.0, (10.0, 0.0, 0.5)), (2.0, (20.0, 0.0, 0.5)), (3.0, (30.0, 0.0, 0.5))], + ) + frames = list(iter_world_frames(db, "lidar", "odom")) + assert [f.ts for f in frames] == [1.0, 2.0, 3.0] + # Translated by the odometry position, and the origin is that position. + assert np.allclose(frames[0].points, local + np.array([10.0, 0.0, 0.5])) + assert frames[1].origin == (20.0, 0.0, 0.5) + # end_ts drops frames at or after it. + assert [f.ts for f in iter_world_frames(db, "lidar", "odom", end_ts=2.5)] == [1.0, 2.0] + + traj = load_trajectory(db, "odom") + assert len(traj.positions) == 3 + assert np.allclose(traj.foot(0.5)[:, 2], 0.0) + assert load_trajectory(db, "odom", end_ts=2.5).positions.shape[0] == 2 + + +def test_recording_rejects_pre_registered_clouds(tmp_path: Path) -> None: + """World-frame clouds would be registered twice, so they are refused.""" + from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory + + db = tmp_path / "legacy.db" + pts = np.array([[1.0, 0.0, 0.0]], dtype=np.float32) + _write_recording(db, [(1.0, pts, "world")], [(1.0, (0.0, 0.0, 0.0))]) + with pytest.raises(ValueError, match="world-frame"): + list(iter_world_frames(db, "lidar", "odom")) + with pytest.raises(ValueError, match="no odometry"): + load_trajectory(db, "missing_stream") + + +def test_apply_overrides_is_the_sweep_interface() -> None: + """--set is how a sweep varies the harness and the pipeline.""" + from dimos.navigation.nav_3d.evaluator.cli import _apply_overrides + + cfg = _apply_overrides( + EvalConfig(), ["goal_tolerance=0.4", "planner.wall_clearance_m=0.0", "pipeline=mls"] + ) + assert cfg.goal_tolerance == pytest.approx(0.4) + assert isinstance(cfg.goal_tolerance, float) + assert cfg.planner == {"wall_clearance_m": 0.0} + assert cfg.pipeline == "mls" + for bad in (["no_equals_sign"], ["not_a_field=1"]): + with pytest.raises(typer.BadParameter): + _apply_overrides(EvalConfig(), bad) + + +def test_diff_exits_nonzero_only_on_a_regression(tmp_path: Path) -> None: + """The tripwire is a CI gate, so its contract is the exit code.""" + from dimos.navigation.nav_3d.evaluator.cli import diff_reports + + clean = tmp_path / "a.json" + broken = tmp_path / "b.json" + clean.write_text(json.dumps(_tripwire_report({"office": {"a": (True, True)}}))) + broken.write_text(json.dumps(_tripwire_report({"office": {"a": (False, True)}}))) + + diff_reports(clean, clean, exact=False) + with pytest.raises(typer.Exit) as regressed: + diff_reports(clean, broken, exact=False) + assert regressed.value.exit_code == 1 + # A fix is not a regression. + diff_reports(broken, clean, exact=False) + # --exact fails on any non-timing difference, in either direction. + with pytest.raises(typer.Exit): + diff_reports(broken, clean, exact=True) From 1993921a6e1d07ed344a2f6ab1cc7b3eac0c4b23 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 4 Aug 2026 01:09:57 -0700 Subject: [PATCH 27/29] Clean up code --- .../china_office.yaml | 0 .../mid360_athens_stairs.yaml | 0 dimos/navigation/nav_3d/evaluator/cases.py | 13 +- .../nav_3d/evaluator/cases/sf_office.yaml | 257 ------------- dimos/navigation/nav_3d/evaluator/cli.py | 115 ++---- dimos/navigation/nav_3d/evaluator/config.py | 8 +- dimos/navigation/nav_3d/evaluator/curation.py | 35 +- .../navigation/nav_3d/evaluator/final_map.py | 42 +-- dimos/navigation/nav_3d/evaluator/generate.py | 97 +++-- dimos/navigation/nav_3d/evaluator/metrics.py | 86 +++-- dimos/navigation/nav_3d/evaluator/picker.py | 144 ++++---- dimos/navigation/nav_3d/evaluator/pipeline.py | 8 +- .../navigation/nav_3d/evaluator/recording.py | 4 +- dimos/navigation/nav_3d/evaluator/runner.py | 272 ++++++++------ dimos/navigation/nav_3d/evaluator/tagging.py | 33 +- .../nav_3d/evaluator/test_evaluator.py | 344 +++++++----------- dimos/navigation/nav_3d/evaluator/tripwire.py | 143 -------- dimos/navigation/nav_3d/evaluator/viz.py | 2 +- .../navigation/nav_3d/evaluator/voxel_keys.py | 8 +- 19 files changed, 563 insertions(+), 1048 deletions(-) rename dimos/navigation/nav_3d/evaluator/{cases => case_manifests}/china_office.yaml (100%) rename dimos/navigation/nav_3d/evaluator/{cases => case_manifests}/mid360_athens_stairs.yaml (100%) delete mode 100644 dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml delete mode 100644 dimos/navigation/nav_3d/evaluator/tripwire.py diff --git a/dimos/navigation/nav_3d/evaluator/cases/china_office.yaml b/dimos/navigation/nav_3d/evaluator/case_manifests/china_office.yaml similarity index 100% rename from dimos/navigation/nav_3d/evaluator/cases/china_office.yaml rename to dimos/navigation/nav_3d/evaluator/case_manifests/china_office.yaml diff --git a/dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/case_manifests/mid360_athens_stairs.yaml similarity index 100% rename from dimos/navigation/nav_3d/evaluator/cases/mid360_athens_stairs.yaml rename to dimos/navigation/nav_3d/evaluator/case_manifests/mid360_athens_stairs.yaml diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index db73cec7aa..0974e1aad6 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -24,7 +24,12 @@ from dimos.utils.data import resolve_named_path -CASES_DIR = Path(__file__).parent / "cases" +MANIFEST_DIR = Path(__file__).parent / "case_manifests" + + +def manifest_path(dataset: str) -> Path: + """Where a dataset's case manifest lives.""" + return MANIFEST_DIR / f"{dataset}.yaml" @dataclass @@ -106,15 +111,15 @@ def load_suite(path: Path) -> Suite: def load_suites(paths: list[Path] | None = None) -> list[Suite]: """Load the given manifests, or every manifest under cases/.""" if paths is None: - paths = sorted(CASES_DIR.glob("*.yaml")) + paths = sorted(MANIFEST_DIR.glob("*.yaml")) if not paths: - raise FileNotFoundError(f"no case manifests found under {CASES_DIR}") + raise FileNotFoundError(f"no case manifests found under {MANIFEST_DIR}") return [load_suite(p) for p in paths] def save_suite(suite: Suite, path: Path | None = None) -> Path: """Write the suite manifest as YAML. Defaults to cases/.yaml.""" - path = path or suite.path or CASES_DIR / f"{suite.dataset}.yaml" + path = path or suite.path or manifest_path(suite.dataset) doc: dict[str, object] = {"dataset": suite.dataset} if suite.db is not None: doc["db"] = suite.db diff --git a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml b/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml deleted file mode 100644 index deb4a761e1..0000000000 --- a/dimos/navigation/nav_3d/evaluator/cases/sf_office.yaml +++ /dev/null @@ -1,257 +0,0 @@ -dataset: sf_office -db: ~/nav_recordings/sf_office.db -cases: -- id: auto_00_flat - start: [-1.64, -2.52, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat, doorway, narrow] -- id: auto_01_flat - start: [-2.84, 8.44, -0.24] - goal: [3.64, 7.64, -0.16] - tags: [auto, flat, doorway] -- id: auto_02_flat - start: [6.44, 0.44, -0.16] - goal: [6.68, -3.0, 0.08] - tags: [auto, flat] -- id: auto_03_flat - start: [8.76, 4.36, -0.16] - goal: [10.84, -0.84, 0.24] - tags: [auto, flat] -- id: auto_04_flat - start: [-2.2, 3.0, -0.16] - goal: [3.56, -1.32, 0.08] - tags: [auto, flat] -- id: auto_05_flat - start: [-0.04, -0.04, -0.16] - goal: [8.92, -0.52, 0.24] - tags: [auto, flat] -- id: auto_06_flat - start: [-0.2, 4.2, -0.08] - goal: [0.04, 8.04, -0.16] - tags: [auto, flat] -- id: auto_07_flat - start: [-2.84, 8.44, -0.24] - goal: [-0.68, 8.2, -0.16] - tags: [auto, flat] -- id: auto_08_flat - start: [3.72, 2.28, -0.16] - goal: [12.6, -0.36, 0.24] - tags: [auto, flat, narrow] -- id: auto_09_flat - start: [11.72, 3.88, -0.08] - goal: [4.92, 4.76, -0.16] - tags: [auto, flat] -- id: auto_10_flat - start: [13.4, 0.2, 0.24] - goal: [2.12, 0.2, -0.16] - tags: [auto, flat] -- id: auto_11_flat - start: [7.0, 3.8, -0.16] - goal: [5.0, -3.56, 0.08] - tags: [auto, flat] -- id: auto_12_flat - start: [-3.56, 5.16, -0.24] - goal: [13.96, 2.28, 0.24] - tags: [auto, flat] -- id: auto_13_flat - start: [13.8, 1.56, 0.32] - goal: [6.04, 2.28, -0.16] - tags: [auto, flat] -- id: auto_14_flat - start: [9.8, -0.68, 0.24] - goal: [5.16, 6.68, -0.16] - tags: [auto, flat] -- id: auto_15_flat - start: [11.64, -0.76, 0.24] - goal: [1.96, 7.72, -0.16] - tags: [auto, flat] -- id: auto_16_flat - start: [7.96, -0.2, 0.08] - goal: [15.64, 1.32, -0.08] - tags: [auto, flat] -- id: auto_17_flat - start: [5.32, 2.84, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_18_flat - start: [10.6, 4.04, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_19_flat - start: [-0.44, 0.76, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_20_flat - start: [14.28, 3.48, -0.08] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_21_flat - start: [7.96, 4.12, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_22_flat - start: [13.96, 2.28, 0.24] - goal: [3.8, 0.36, -0.16] - tags: [auto, flat] -- id: auto_23_flat - start: [1.88, 0.84, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_24_flat - start: [4.36, 1.48, -0.16] - goal: [8.92, -0.52, 0.24] - tags: [auto, flat] -- id: auto_25_flat - start: [12.68, 3.56, -0.08] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_26_flat - start: [3.64, 1.72, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_27_flat - start: [13.88, 1.96, 0.24] - goal: [-3.24, 6.2, -0.24] - tags: [auto, flat] -- id: auto_28_flat - start: [5.16, 2.04, -0.16] - goal: [10.84, -0.84, 0.24] - tags: [auto, flat] -- id: auto_29_flat - start: [12.6, -0.36, 0.24] - goal: [-1.64, 5.0, -0.16] - tags: [auto, flat] -- id: auto_30_flat - start: [1.0, 3.88, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_31_flat - start: [6.36, 3.56, -0.16] - goal: [8.92, -0.52, 0.24] - tags: [auto, flat] -- id: auto_32_flat - start: [-0.44, -0.04, -0.16] - goal: [3.64, -2.36, 0.08] - tags: [auto, flat] -- id: auto_33_flat - start: [-1.8, 2.2, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_34_flat - start: [6.92, 4.36, -0.16] - goal: [10.84, -0.84, 0.24] - tags: [auto, flat] -- id: auto_35_flat - start: [11.64, -0.76, 0.24] - goal: [3.0, -0.68, -0.16] - tags: [auto, flat] -- id: auto_36_flat - start: [7.08, -0.12, 0.0] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_37_flat - start: [13.8, 1.56, 0.32] - goal: [8.92, 4.44, -0.16] - tags: [auto, flat] -- id: auto_38_flat - start: [-2.36, 4.04, -0.16] - goal: [13.8, 1.56, 0.32] - tags: [auto, flat] -- id: auto_39_flat - start: [13.8, 1.56, 0.32] - goal: [6.44, 0.44, -0.16] - tags: [auto, flat] -- id: auto_40_flat - start: [-1.64, -2.52, -0.16] - goal: [11.96, -0.68, 0.24] - tags: [auto, flat, doorway, narrow] -- id: auto_41_flat - start: [13.88, 1.96, 0.24] - goal: [-3.64, 4.36, -0.24] - tags: [auto, flat] -- id: auto_42_flat - start: [-1.64, -2.52, -0.16] - goal: [9.56, -0.76, 0.24] - tags: [auto, flat] -- id: auto_43_flat - start: [13.8, 1.56, 0.32] - goal: [-1.16, 4.68, -0.16] - tags: [auto, flat] -- id: auto_44_flat - start: [13.8, 1.56, 0.32] - goal: [11.32, 3.72, -0.08] - tags: [auto, flat] -- id: auto_45_flat - start: [0.12, 0.04, -0.16] - goal: [10.84, -0.84, 0.24] - tags: [auto, flat, narrow] -- id: auto_46_flat - start: [-0.44, 0.76, -0.16] - goal: [12.76, -0.36, 0.24] - tags: [auto, flat, narrow] -- id: auto_47_flat - start: [13.88, 1.96, 0.24] - goal: [-1.32, 7.64, -0.16] - tags: [auto, flat] -- id: auto_48_flat - start: [13.8, 1.56, 0.32] - goal: [0.36, 4.28, -0.16] - tags: [auto, flat] -- id: auto_49_flat - start: [6.04, 1.32, -0.16] - goal: [7.88, -0.36, 0.08] - tags: [auto, flat] -- id: neg_00 - start: [-1.72, 4.52, -0.16] - goal: [-1.32, 2.76, 0.88] - tags: [manual, negative, stairs, up] - expect_fail: true -- id: neg_01 - start: [-1.0, 8.6, -0.24] - goal: [-0.84, 6.68, 0.48] - tags: [manual, negative, stairs, up] - expect_fail: true -- id: neg_02 - start: [5.08, 1.32, -0.16] - goal: [7.64, 1.4, 0.56] - tags: [manual, negative, stairs, up] - expect_fail: true -- id: manual_00 - start: [9.72, -2.84, 0.24] - goal: [12.6, 0.04, 0.24] - tags: [manual, flat] -- id: neg_03 - start: [2.12, 3.8, -0.16] - goal: [2.12, 4.44, 1.12] - tags: [manual, negative, stairs, up] - expect_fail: true -- id: neg_04 - start: [0.76, 6.44, -0.16] - goal: [0.04, 6.92, -0.24] - tags: [manual, negative, flat] - expect_fail: true -- id: neg_05 - start: [-1.56, -0.2, -0.16] - goal: [-2.36, -0.76, 0.8] - tags: [manual, negative, stairs, up] - expect_fail: true -- id: neg_06 - start: [-1.08, -2.6, 2.24] - goal: [-1.88, 0.52, -0.16] - tags: [manual, negative, stairs, down, long] - expect_fail: true -- id: neg_07 - start: [4.28, -3.48, 0.0] - goal: [4.68, -1.32, 0.08] - tags: [manual, negative, flat] - expect_fail: true -- id: neg_08 - start: [3.48, -2.84, 0.0] - goal: [4.68, -1.8, 1.12] - tags: [manual, negative, stairs, up] - expect_fail: true -- id: neg_09 - start: [4.6, -2.2, 1.12] - goal: [6.52, -4.28, 0.08] - tags: [stairs, down, manual, negative] - expect_fail: true diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 74486c4cac..6dcca968de 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -24,21 +24,23 @@ import sqlite3 from typing import TYPE_CHECKING -import numpy as np import typer -from dimos.navigation.nav_3d.evaluator import tripwire from dimos.navigation.nav_3d.evaluator.cases import ( - CASES_DIR, Suite, load_suite, load_suites, + manifest_path, save_suite, ) from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.curation import CurationError, load_store from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map -from dimos.navigation.nav_3d.evaluator.generate import GenerationParams, generate_cases +from dimos.navigation.nav_3d.evaluator.generate import ( + MIN_CASES, + generate_cases, + resolve_max_cases, +) from dimos.navigation.nav_3d.evaluator.metrics import ground_truth_route from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate @@ -47,7 +49,6 @@ if TYPE_CHECKING: from dimos.navigation.nav_3d.evaluator.curation import CaseStore - from dimos.navigation.nav_3d.evaluator.final_map import FinalMap from dimos.navigation.nav_3d.evaluator.runner import PlanOutcome app = typer.Typer(no_args_is_help=True, add_completion=False) @@ -67,13 +68,13 @@ def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: if name not in fields: raise typer.BadParameter(f"unknown config field {name!r}") current = getattr(cfg, name) + if not isinstance(current, (bool, int, float, str)): + raise typer.BadParameter(f"{name!r} is not a scalar field; set its members instead") setattr(cfg, name, type(current)(value)) return cfg def _score_cell(outcome: PlanOutcome) -> str: - """No path at all shows x. A planned path shows its SPL, which is 0.00 when - the path is invalid and higher when it is valid.""" return "x" if not outcome.planned and not outcome.success else f"{outcome.spl:.2f}" @@ -136,64 +137,10 @@ def _print_report(report: Report) -> None: print(f" not explained by a new obstacle, inspect final map: {', '.join(others)}") -@app.command("diff") -def diff_reports( - old: Path = typer.Argument(..., help="Baseline report JSON from `run --json`"), - new: Path = typer.Argument(..., help="Candidate report JSON from `run --json`"), - exact: bool = typer.Option( - False, - "--exact", - help="Require bit-identical results (ignoring timings); " - "two runs of the same code must pass", - ), -) -> None: - """Name every case whose pass/fail flipped between two runs. - - Exits 1 when any case regressed, so a CI check or before/after comparison - can gate on it. Perf budget breaches always print but only exit 1 with - --exact, which also exits 1 on any non-timing difference. Running the suite - twice and exact-diffing the reports is the determinism check. - """ - old_report = json.loads(old.read_text()) - new_report = json.loads(new.read_text()) - print( - f"score {old_report['score']:.3f} -> {new_report['score']:.3f} | " - f"final {old_report['final_score']:.3f} -> {new_report['final_score']:.3f}" - ) - d = tripwire.diff(old_report, new_report) - print(f"{len(d.fixed)} fixed, {len(d.broke)} broke") - for flip in d.fixed: - print(f" fixed: {flip.key} ({flip.test})") - for flip in d.broke: - print(f" BROKE: {flip.key} ({flip.test}: pass -> fail)") - for key in d.added: - print(f" new case: {key}") - for key in d.removed: - print(f" case gone: {key}") - violations = tripwire.perf_violations(new_report) - for violation in violations: - print(f"PERF BUDGET EXCEEDED: {violation}") - if violations and not exact: - print(" advisory here; the --exact confirmation run is the binding perf gate") - if exact: - differences = tripwire.exact_differences(old_report, new_report) - if differences: - shown = 20 - print(f"{len(differences)} exact difference(s):") - for line in differences[:shown]: - print(f" {line}") - if len(differences) > shown: - print(f" ... and {len(differences) - shown} more") - raise typer.Exit(code=1) - print("exact: reports identical") - if d.broke or (exact and violations): - raise typer.Exit(code=1) - - @app.command() def run( manifests: list[Path] = typer.Argument( - None, help="Suite YAMLs; defaults to every manifest under cases/" + None, help="Suite YAMLs; defaults to every manifest under case_manifests/" ), dataset: str = typer.Option(None, "--dataset", help="Only run suites for this dataset"), tag: list[str] = typer.Option( @@ -231,8 +178,6 @@ def run( cfg = _apply_overrides(EvalConfig(), set_ or []) report = evaluate(suites, cfg, workers=workers, keep_artifacts=rrd_out is not None) _print_report(report) - for violation in tripwire.perf_violations(report.to_dict()): - print(f"PERF BUDGET EXCEEDED: {violation}") if json_out is not None: json_out.parent.mkdir(parents=True, exist_ok=True) json_out.write_text(json.dumps(report.to_dict(), indent=2)) @@ -276,7 +221,7 @@ def ingest( src = source / "mem2.db" if source.is_dir() else source if not src.exists(): raise typer.BadParameter(f"{src} does not exist") - manifest = CASES_DIR / f"{name}.yaml" + manifest = manifest_path(name) if manifest.exists() and not force: raise typer.BadParameter(f"{manifest} already exists; pass --force to regenerate") if external: @@ -305,14 +250,13 @@ def ingest( ) cfg = EvalConfig() final = load_or_build_final_map(dest, suite, cfg) - gen = GenerationParams(max_cases=cases or None) - if cases: - gen.min_cases = cases + max_cases = cases or None + min_cases = cases or MIN_CASES surface = final.standable_surface(cfg.robot_height) - suite.cases = generate_cases(trajectory, final, surface, cfg, gen) + suite.cases = generate_cases(trajectory, final, surface, cfg, max_cases, min_cases) if not suite.cases: raise typer.Exit(code=1) - floor = min(gen.min_cases, gen.resolve_max_cases(float(arcs[-1]))) + floor = min(min_cases, resolve_max_cases(max_cases, float(arcs[-1]))) if len(suite.cases) < floor: print( f"WARNING: only {len(suite.cases)} cases generated; the recording " @@ -325,13 +269,20 @@ def ingest( print(f"\nrun with: dimos nav-eval run --dataset {name}") -def _open(dataset: str) -> tuple[CaseStore, FinalMap]: +def _open(dataset: str) -> CaseStore: try: return load_store(dataset) except CurationError as err: raise typer.BadParameter(str(err)) from err +def _load_manifest(dataset: str) -> tuple[Suite, Path]: + manifest = manifest_path(dataset) + if not manifest.exists(): + raise typer.BadParameter(f"no manifest {manifest}; run ingest first") + return load_suite(manifest), manifest + + @app.command("add-case") def add_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the case"), @@ -344,7 +295,7 @@ def add_case( ), ) -> None: """Append a curated case, with endpoints snapped to the final surface.""" - store, _ = _open(dataset) + store = _open(dataset) try: store.add( start, @@ -373,10 +324,7 @@ def tag_case( for refusing. Use it on a case that shows up as incremental-only because a real obstacle blocked the route by the final map, not a planner bug. """ - manifest = CASES_DIR / f"{dataset}.yaml" - if not manifest.exists(): - raise typer.BadParameter(f"no manifest {manifest}") - suite = load_suite(manifest) + suite, manifest = _load_manifest(dataset) case = next((c for c in suite.cases if c.id == case_id), None) if case is None: raise typer.BadParameter(f"case {case_id!r} not found in {manifest}") @@ -403,10 +351,7 @@ def retag( auto provenance tag survives. Manually curated cases are left untouched: their tags are human intent, not something to recompute. """ - manifest = CASES_DIR / f"{dataset}.yaml" - if not manifest.exists(): - raise typer.BadParameter(f"no manifest {manifest}; run ingest first") - suite = load_suite(manifest) + suite, manifest = _load_manifest(dataset) cfg = EvalConfig() final = load_or_build_final_map(suite.db_path(), suite, cfg) trajectory = load_trajectory(suite.db_path(), suite.odom_stream, suite.end_ts_seconds()) @@ -445,16 +390,16 @@ def pick_case( from dimos.navigation.nav_3d.evaluator.picker import pick_cases from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height - store, final = _open(dataset) + store = _open(dataset) trajectory = load_trajectory( store.suite.db_path(), store.suite.odom_stream, store.suite.end_ts_seconds() ) - foot = trajectory.positions - np.array([0.0, 0.0, store.cfg.robot_height], dtype=np.float32) + foot = trajectory.foot(store.cfg.robot_height) pick_cases( dataset, - final.occupied, - turbo_by_height(final.occupied), - final.voxel_size, + store.final.occupied, + turbo_by_height(store.final.occupied), + store.final.voxel_size, foot, store, ) diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 5d94baa634..5cb9a0bd5c 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -50,13 +50,6 @@ class EvalConfig: # How far an endpoint may sit from a standable surface before it is off the map. snap_max_m: float = 1.0 - # An improvement must not buy score with compute. p95 over the suite. - # A plan is timed end to end, so deferred map work is charged here too. - plan_p95_budget_ms: float = 200.0 - # Per lidar frame, so a pipeline that cannot keep up with the sensor fails - # regardless of how it scores. - map_update_p95_budget_ms: float = 100.0 - # Which pipeline is under test, by registry name. pipeline: str = "mls" # Pipeline constructor overrides, e.g. --set planner.wall_clearance_m=0.0. @@ -64,6 +57,7 @@ class EvalConfig: def make_mapper(self) -> VoxelRayMapper: """The mapper that builds the occupancy every pipeline is graded against.""" + # Lazy: the mapper is a native module, only needed to build a map. from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) diff --git a/dimos/navigation/nav_3d/evaluator/curation.py b/dimos/navigation/nav_3d/evaluator/curation.py index 1558207298..d4f17ca124 100644 --- a/dimos/navigation/nav_3d/evaluator/curation.py +++ b/dimos/navigation/nav_3d/evaluator/curation.py @@ -22,7 +22,12 @@ import numpy as np -from dimos.navigation.nav_3d.evaluator.cases import CASES_DIR, Case, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator.cases import ( + Case, + load_suite, + manifest_path, + save_suite, +) from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map from dimos.navigation.nav_3d.evaluator.generate import snap_to_surface @@ -57,6 +62,7 @@ class CaseStore: manifest: Path surface: NDArray[np.float32] cfg: EvalConfig + final: FinalMap def _snap(self, label: str, point: Point, *, required: bool = True) -> Point: snapped = snap_to_surface( @@ -68,7 +74,9 @@ def _snap(self, label: str, point: Point, *, required: bool = True) -> Point: f"{label} {point} is more than {self.cfg.snap_max_m}m from a standable surface" ) # An infeasible goal may sit on geometry with no standable surface. - logger.warning("%s %s is off any standable surface; kept as picked", label, point) + logger.warning( + "point is off any standable surface, kept as picked", label=label, point=point + ) return point return (float(snapped[0]), float(snapped[1]), float(snapped[2])) @@ -99,11 +107,16 @@ def add( self.save() kind = "negative (must refuse)" if expect_fail else "positive" logger.info( - "added %s %s: %s -> %s to %s", kind, case.id, case.start, case.goal, self.manifest + "added case", + kind=kind, + case=case.id, + start=case.start, + goal=case.goal, + manifest=self.manifest, ) return case - def update(self, case_id: str, new_id: str, tags: list[str], expect_fail: bool) -> Case: + def update(self, case_id: str, new_id: str, tags: list[str], *, expect_fail: bool) -> Case: case = self.get(case_id) if new_id != case_id and any(c.id == new_id for c in self.suite.cases): raise CurationError(f"case id {new_id!r} already exists") @@ -133,22 +146,18 @@ def save(self) -> None: def _curated_tags(tags: list[str], expect_fail: bool, provenance: str = "manual") -> list[str]: - """Rewrite a case's tags around exactly one provenance tag. - - Editing a case must not change what it measures, so an auto case keeps its - generated provenance. The negative tag tracks expect_fail rather than being - editable text, so the two cannot drift. - """ + """Rewrite a case's tags around exactly one provenance tag. An auto case + keeps its generated provenance so editing never changes what it measures.""" keep = [t for t in tags if t not in (*PROVENANCE_TAGS, "negative")] return [provenance, *(["negative"] if expect_fail else []), *keep] -def load_store(dataset: str) -> tuple[CaseStore, FinalMap]: +def load_store(dataset: str) -> CaseStore: """Open a dataset's manifest with the final map and surface it snaps to.""" - manifest = CASES_DIR / f"{dataset}.yaml" + manifest = manifest_path(dataset) if not manifest.exists(): raise CurationError(f"no manifest {manifest}; run ingest first") suite = load_suite(manifest) cfg = EvalConfig() final = load_or_build_final_map(suite.db_path(), suite, cfg) - return CaseStore(suite, manifest, final.standable_surface(cfg.robot_height), cfg), final + return CaseStore(suite, manifest, final.standable_surface(cfg.robot_height), cfg, final) diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index 550015d9c0..97177ea356 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -14,8 +14,7 @@ """The evaluator's own map of a recording, and the checkpoints along it. -Not ground truth, just the most complete occupancy the mapper produces, which -is what returned paths are graded against. +Not ground truth, just the most complete occupancy the mapper produces. """ from __future__ import annotations @@ -25,7 +24,7 @@ import json import os from time import perf_counter -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any import numpy as np @@ -60,9 +59,7 @@ class FinalMap: def standable_surface(self, robot_height: float) -> NDArray[np.float32]: """Occupied cells with robot_height of free space directly above them. - - Decides where an endpoint may sit, not where a path may go. - """ + Decides where an endpoint may sit, not where a path may go.""" keys = self.occupied_keys blocked = np.zeros(len(keys), dtype=bool) for dz in range(1, int(np.ceil(robot_height / self.voxel_size)) + 1): @@ -91,14 +88,14 @@ def iter_snapshots(self) -> Iterator[NDArray[np.int64]]: CHECKPOINT_CACHE_VERSION = 3 -def _save_npz(cache: Path, **arrays: object) -> None: +def _save_npz(cache: Path, **arrays: Any) -> None: """Publish a cache atomically, so an interrupted build cannot poison later runs.""" cache.parent.mkdir(parents=True, exist_ok=True) tmp = cache.with_name(f"{cache.name}.{os.getpid()}.tmp") try: # Written through a handle because savez appends .npz to a bare path. with tmp.open("wb") as fh: - np.savez_compressed(fh, **arrays) # type: ignore[arg-type] + np.savez_compressed(fh, **arrays) os.replace(tmp, cache) finally: tmp.unlink(missing_ok=True) @@ -112,13 +109,19 @@ def _cache_path(db_path: Path, params: dict[str, float | int | str]) -> Path: return CACHE_SUBDIR / f"{db_path.stem}.{digest}.npz" -def _final_params(suite: Suite, cfg: EvalConfig) -> dict[str, float | int | str]: +def _final_params(db_path: Path, suite: Suite, cfg: EvalConfig) -> dict[str, float | int | str]: + # The recording is part of the key. Without it a re-ingested dataset keeps + # its stem, and the stale map silently becomes the grading occupancy. + stat = db_path.stat() params: dict[str, float | int | str] = { **cfg.mapper_fingerprint(), "align_tol": cfg.align_tol, "lidar_stream": suite.lidar_stream, "odom_stream": suite.odom_stream, "cache_version": CACHE_VERSION, + "db": str(db_path.resolve()), + "db_size": stat.st_size, + "db_mtime_ns": stat.st_mtime_ns, } if suite.end_ts is not None: params["end_ts"] = suite.end_ts @@ -132,9 +135,7 @@ def replay_frames( times: NDArray[np.float64], ) -> tuple[FinalMap, list[NDArray[np.int64]]]: """Feed frames through the mapper in order, snapshotting at each requested - time. A snapshot holds exactly the frames with ts <= its time. Returns the - final map and the occupied-key snapshots. - """ + time. A snapshot holds exactly the frames with ts <= its time.""" snapshots: list[NDArray[np.int64]] = [] add_ms: list[float] = [] t0 = perf_counter() @@ -168,11 +169,11 @@ def _save_final(cache: Path, final: FinalMap) -> None: frames=final.frames, **{f"add_{k}": v for k, v in final.add_frame_ms.items()}, ) - logger.info("final map cached: %s (%d voxels)", cache.name, len(final.occupied)) + logger.info("final map cached", cache=cache.name, voxels=len(final.occupied)) def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> FinalMap: - cache = _cache_path(db_path, _final_params(suite, cfg)) + cache = _cache_path(db_path, _final_params(db_path, suite, cfg)) if cache.exists(): data = np.load(cache) return FinalMap( @@ -184,7 +185,7 @@ def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> Fin build_ms=0.0, ) - logger.info("building final map for %s (cache miss)", db_path.name) + logger.info("building final map (cache miss)", recording=db_path.name) final, _ = replay_frames( iter_world_frames( db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() @@ -215,12 +216,11 @@ def load_or_build_checkpoints( ) -> MapCheckpoints: """Occupied key sets at the requested times, deduped and sorted. - A cache miss replays the whole recording once. The replay's final state - also fills the final cache when that is missing. + A cache miss replays the recording once and fills the final cache too. """ times = np.unique(np.asarray(times, dtype=np.float64)) params: dict[str, float | int | str] = { - **_final_params(suite, cfg), + **_final_params(db_path, suite, cfg), "kind": "checkpoints", "times_sha": hashlib.sha1(times.tobytes()).hexdigest()[:10], "checkpoint_version": CHECKPOINT_CACHE_VERSION, @@ -235,7 +235,7 @@ def load_or_build_checkpoints( removed=[data[f"rem_{i}"] for i in range(n)], ) - logger.info("building %d map checkpoints for %s (cache miss)", len(times), db_path.name) + logger.info("building map checkpoints (cache miss)", n=len(times), recording=db_path.name) final, snapshots = replay_frames( iter_world_frames( db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() @@ -244,7 +244,7 @@ def load_or_build_checkpoints( cfg.voxel_size, times, ) - final_cache = _cache_path(db_path, _final_params(suite, cfg)) + final_cache = _cache_path(db_path, _final_params(db_path, suite, cfg)) if not final_cache.exists(): _save_final(final_cache, final) added, removed = encode_deltas(snapshots) @@ -252,5 +252,5 @@ def load_or_build_checkpoints( arrays |= {f"add_{i}": a for i, a in enumerate(added)} arrays |= {f"rem_{i}": r for i, r in enumerate(removed)} _save_npz(cache, **arrays) - logger.info("checkpoints cached: %s", cache.name) + logger.info("checkpoints cached", cache=cache.name) return MapCheckpoints(times=times, added=added, removed=removed) diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 53d10bb06a..55e91b85dd 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -41,34 +41,37 @@ MAX_TRIVIAL_SPAN_M = 30.0 -@dataclass -class GenerationParams: - min_separation_m: float = 3.0 - min_euclid_m: float = 2.0 - detour_ratio_min: float = 1.3 - bin_size_m: float = 2.0 - waypoint_spacing_m: float = 1.0 - # None scales the case count with the walked distance. - max_cases: int | None = None - # Two cases are duplicates when both endpoints land within this radius. - dedupe_radius_m: float = 1.5 - # Share of slots reserved for flat cases when the recording has them. - flat_fraction: float = 0.25 - # Coverage sectors: a case earns a slot first by connecting a sector pair - # no accepted case connects yet. - sector_size_m: float = 8.0 - sector_z_m: float = 1.5 - # A sector may anchor at most this many selected cases, which prevents a - # single high-priority spot from becoming the hub of every case. - endpoint_reuse_max: int = 2 - # Floor on the case count. When strict selection falls short, a relaxed - # pass ignores the sector caps and the flat quota to reach it. - min_cases: int = 10 - - def resolve_max_cases(self, walked_total_m: float) -> int: - if self.max_cases is not None: - return self.max_cases - return int(np.clip(walked_total_m / 25.0, 16, 48)) +# Candidate pairs must be this far apart along the walk and in a straight line. +MIN_SEPARATION_M = 3.0 +MIN_EUCLID_M = 2.0 +# A flat pair is only interesting if the walk detoured this much over the +# straight line. +DETOUR_RATIO_MIN = 1.3 +# Endpoint pairs are binned this coarsely before ranking, so near-identical +# pairs compete for one slot. +BIN_SIZE_M = 2.0 +WAYPOINT_SPACING_M = 1.0 +# Two cases are duplicates when both endpoints land within this radius. +DEDUPE_RADIUS_M = 1.5 +# Share of slots reserved for flat cases when the recording has them. +FLAT_FRACTION = 0.25 +# Coverage sectors: a case earns a slot first by connecting a sector pair +# no accepted case connects yet. +SECTOR_SIZE_M = 8.0 +SECTOR_Z_M = 1.5 +# A sector may anchor at most this many selected cases, which prevents a +# single high-priority spot from becoming the hub of every case. +ENDPOINT_REUSE_MAX = 2 +# Floor on the case count. When strict selection falls short, a relaxed pass +# ignores the sector caps and the flat quota to reach it. +MIN_CASES = 10 + + +def resolve_max_cases(max_cases: int | None, walked_total_m: float) -> int: + """Case count, scaled with the walked distance when not pinned.""" + if max_cases is not None: + return max_cases + return int(np.clip(walked_total_m / 25.0, 16, 48)) @dataclass @@ -94,10 +97,7 @@ def snap_to_surface( snap_max_m: float, ) -> NDArray[np.float32] | None: """Nearest standable surface cell, or None when the point is off the map. - - Horizontal distance dominates so drift in z between passes does not pull - the snap onto another floor. - """ + Horizontal distance dominates so z drift cannot snap onto another floor.""" if len(surface) == 0: return None hd = np.linalg.norm(surface[:, :2] - point[:2], axis=1) @@ -120,14 +120,14 @@ def generate_cases( final: FinalMap, surface: NDArray[np.float32], cfg: EvalConfig, - params: GenerationParams | None = None, + max_cases: int | None = None, + min_cases: int = MIN_CASES, ) -> list[Case]: - params = params or GenerationParams() map_keys = final.occupied_keys arcs = trajectory.arc_lengths() foot = trajectory.foot(cfg.robot_height) - idx = _subsample_indices(trajectory, params.waypoint_spacing_m) + idx = _subsample_indices(trajectory, WAYPOINT_SPACING_M) snaps = np.full((len(idx), 3), np.nan, dtype=np.float32) for n, i in enumerate(idx): hit = snap_to_surface(foot[i], surface, cfg.snap_max_m) @@ -150,7 +150,7 @@ def generate_cases( walked = way_arcs[later] - way_arcs[ai] deltas = snaps[later] - sa euclid = np.linalg.norm(deltas, axis=1) - keep = (walked >= params.min_separation_m) & (euclid >= params.min_euclid_m) + keep = (walked >= MIN_SEPARATION_M) & (euclid >= MIN_EUCLID_M) for bi, w, e in zip(later[keep], walked[keep], euclid[keep], strict=True): sb = snaps[bi] dz = float(sb[2] - sa[2]) @@ -169,7 +169,7 @@ def generate_cases( detour_ratio=detour, dz=d_dz, ), - _bin_key(p_start, p_goal, d_dz, params.bin_size_m), + _bin_key(p_start, p_goal, d_dz, BIN_SIZE_M), ) for p_start, p_goal, d_dz in directed ] @@ -180,7 +180,7 @@ def generate_cases( for cand, key in proposed ): continue - if detour < params.detour_ratio_min and abs(dz) < STAIRS_DZ_M: + if detour < DETOUR_RATIO_MIN and abs(dz) < STAIRS_DZ_M: # A long near-straight flat pair is trivial. Not worth a sweep. if e > MAX_TRIVIAL_SPAN_M: continue @@ -193,7 +193,7 @@ def generate_cases( candidates[key] = cand ranked = sorted(candidates.values(), key=lambda c: (-c.priority, c.start, c.goal)) - selected = _select_diverse(ranked, params, params.resolve_max_cases(float(arcs[-1]))) + selected = _select_diverse(ranked, resolve_max_cases(max_cases, float(arcs[-1])), min_cases) cases = [] for n, cand in enumerate(selected): route = metrics.ground_truth_route(trajectory, cand.start, cand.goal, cfg) @@ -219,26 +219,26 @@ def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> def _select_diverse( - ranked: list[Candidate], params: GenerationParams, max_cases: int + ranked: list[Candidate], max_cases: int, min_cases: int = MIN_CASES ) -> list[Candidate]: """Spread-greedy selection under a sector cap and flat quota, with a relaxed pass to reach min_cases.""" if not ranked: return [] - flat_target = int(max_cases * params.flat_fraction) + flat_target = int(max_cases * FLAT_FRACTION) stairs_cap = max_cases - flat_target starts = np.array([c.start for c in ranked], dtype=np.float32) goals = np.array([c.goal for c in ranked], dtype=np.float32) priorities = np.array([c.priority for c in ranked], dtype=np.float32) is_stairs = np.array([abs(c.dz) >= STAIRS_DZ_M for c in ranked]) - spread_cap = 2.0 * params.sector_size_m + spread_cap = 2.0 * SECTOR_SIZE_M def sector(p: NDArray[np.float32]) -> tuple[int, ...]: return ( - int(np.floor(p[0] / params.sector_size_m)), - int(np.floor(p[1] / params.sector_size_m)), - round(float(p[2]) / params.sector_z_m), + int(np.floor(p[0] / SECTOR_SIZE_M)), + int(np.floor(p[1] / SECTOR_SIZE_M)), + round(float(p[2]) / SECTOR_Z_M), ) usage: dict[tuple[int, ...], int] = {} @@ -268,13 +268,12 @@ def fill(target: int, relax: bool) -> None: cand = ranked[n] sa, sb = sector(starts[n]), sector(goals[n]) if not relax and ( - usage.get(sa, 0) >= params.endpoint_reuse_max - or usage.get(sb, 0) >= params.endpoint_reuse_max + usage.get(sa, 0) >= ENDPOINT_REUSE_MAX or usage.get(sb, 0) >= ENDPOINT_REUSE_MAX ): sector_capped.append(n) continue bucket = stairs if is_stairs[n] else flats - if _is_duplicate(cand, bucket, params.dedupe_radius_m): + if _is_duplicate(cand, bucket, DEDUPE_RADIUS_M): continue usage[sa] = usage.get(sa, 0) + 1 usage[sb] = usage.get(sb, 0) + 1 @@ -283,7 +282,7 @@ def fill(target: int, relax: bool) -> None: bucket.append(cand) fill(max_cases, relax=False) - min_cases = min(params.min_cases, max_cases) + min_cases = min(min_cases, max_cases) if len(stairs) + len(flats) < min_cases: alive[sector_capped] = True fill(min_cases, relax=True) diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 597180b707..0f8ca2eccb 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -17,7 +17,7 @@ from __future__ import annotations from dataclasses import dataclass -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any import numpy as np @@ -26,7 +26,6 @@ key_centers, keys_contain, offset_deltas, - offset_keys, voxel_keys, ) @@ -43,10 +42,10 @@ def path_length(waypoints: NDArray[np.float32]) -> float: return float(np.linalg.norm(np.diff(waypoints, axis=0), axis=1).sum()) -def arc_lengths(points: NDArray[np.float32]) -> NDArray[np.float64]: +def arc_lengths(points: NDArray[np.floating[Any]]) -> NDArray[np.float64]: """Cumulative 3D arc length at each point, starting at zero.""" steps = np.linalg.norm(np.diff(points, axis=0), axis=1) - return np.concatenate([[0.0], np.cumsum(steps)]) + return np.concatenate([[0.0], np.cumsum(steps)]).astype(np.float64) def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: @@ -59,7 +58,8 @@ def densify(points: NDArray[np.float32], step: float) -> NDArray[np.float32]: starts = np.concatenate([[0], np.cumsum(n)[:-1]]) t = ((np.arange(n.sum()) - starts[idx] + 1) / n[idx])[:, None] body = points[idx] * (1 - t) + points[idx + 1] * t - return np.concatenate([points[:1], body]).astype(np.float32) + dense: NDArray[np.float32] = np.concatenate([points[:1], body]).astype(np.float32) + return dense def goal_reached( @@ -85,17 +85,14 @@ class GateResult: # Indices of the colliding samples in densify(waypoints, voxel_size / 2), # so a viewer can recover the exact body frames the gate tested. collision_indices: NDArray[np.int64] - # Horizontal distance from the body surface to the nearest occupied - # voxel in the gate's band, minimized along the path. Negative is - # penetration depth, capped at MARGIN_CAP_M when nothing is near. + # Distance from the body surface to the nearest occupied voxel in the band, + # minimized along the path. Negative is penetration, capped at MARGIN_CAP_M. min_clearance_m: float def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.float64]: """Heading from the rear-foot to the front-foot chord, span meters apart. - - Steadier than the local tangent, which flips between tread and riser. - """ + Steadier than the local tangent, which flips between tread and riser.""" if len(samples) < 2: return np.tile(np.array([1.0, 0.0, 0.0]), (len(samples), 1)) pts = samples.astype(np.float64) @@ -110,7 +107,10 @@ def chord_directions(samples: NDArray[np.float32], span: float) -> NDArray[np.fl # A path of zero arc length has no heading, so fall back to a valid frame # rather than handing a singular basis to the caller. fwd = np.where(norm > MIN_LENGTH_M, fwd, np.array([1.0, 0.0, 0.0])) - return fwd / np.maximum(np.linalg.norm(fwd, axis=1, keepdims=True), 1e-9) + unit: NDArray[np.float64] = fwd / np.maximum( + np.linalg.norm(fwd, axis=1, keepdims=True), MIN_LENGTH_M + ) + return unit def body_frames( @@ -121,7 +121,9 @@ def body_frames( fwd = chord_directions(samples, robot_length) lateral = np.cross(np.array([0.0, 0.0, 1.0]), fwd) ln = np.linalg.norm(lateral, axis=1, keepdims=True) - lateral = np.where(ln > 1e-6, lateral / np.maximum(ln, 1e-9), np.array([0.0, 1.0, 0.0])) + lateral = np.where( + ln > MIN_LENGTH_M, lateral / np.maximum(ln, MIN_LENGTH_M), np.array([0.0, 1.0, 0.0]) + ) up = np.cross(fwd, lateral) return fwd, lateral, up @@ -131,8 +133,7 @@ def check_path( ) -> GateResult: """Sweep the robot body box along foot-level waypoints against the map. - The box is the robot's length and width, centered mid-band up the tilted - body axis over each sample, so the legs and the ground never count. + The box sits mid-band up the tilted body axis, so legs and ground never count. """ voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size / 2) @@ -171,17 +172,26 @@ def check_path( min_clearance_m=MARGIN_CAP_M, ) delta = key_centers(base[s_idx] + deltas[o_idx], voxel_size) - centers[s_idx] + # The band decides membership on its own, so drop the candidates outside it + # before paying for the footprint distance. + vertical = (delta * up[s_idx]).sum(1) + in_band = np.abs(vertical) <= half_band + if not in_band.any(): + return GateResult( + valid=True, + collision_points=samples[:0], + collision_indices=np.empty(0, dtype=np.int64), + min_clearance_m=MARGIN_CAP_M, + ) + delta, s_idx = delta[in_band], s_idx[in_band] along = (delta * fwd[s_idx]).sum(1) across = (delta * lateral[s_idx]).sum(1) - vertical = (delta * up[s_idx]).sum(1) # Signed distance to the oriented footprint rectangle, negative inside. qx = np.abs(along) - half_len qy = np.abs(across) - half_wid sdf = np.hypot(np.maximum(qx, 0.0), np.maximum(qy, 0.0)) + np.minimum(np.maximum(qx, qy), 0.0) - in_band = np.abs(vertical) <= half_band - exact = in_band & (sdf <= 0.0) - clearance = float(sdf[in_band].min()) if in_band.any() else MARGIN_CAP_M - colliding = np.unique(s_idx[exact]) + clearance = float(sdf.min()) + colliding = np.unique(s_idx[sdf <= 0.0]) return GateResult( valid=len(colliding) == 0, collision_points=samples[colliding], @@ -201,15 +211,20 @@ class SupportResult: def check_support( waypoints: NDArray[np.float32], support_keys: NDArray[np.int64], cfg: EvalConfig ) -> SupportResult: - """Require occupied voxels beneath every path sample. - - The collision gate alone cannot catch a path fabricated across a void. - """ + """Require occupied voxels beneath every path sample, which is what + catches a path fabricated across a void.""" voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size) offsets = cylinder_offsets(cfg.support_radius_m, -cfg.support_depth_m, voxel_size, voxel_size) - keys = offset_keys(samples, offsets, voxel_size) - supported = keys_contain(support_keys, keys.ravel()).reshape(keys.shape).any(axis=1) + # Samples repeat voxels, so membership runs over the distinct ones and is + # scattered back, as in check_path. + base = voxel_keys(samples, voxel_size) + deltas = offset_deltas(offsets) + unique_base, inverse = np.unique(base, return_inverse=True) + hit = keys_contain(support_keys, (unique_base[:, None] + deltas[None, :]).ravel()).reshape( + len(unique_base), len(deltas) + ) + supported = np.asarray(hit.any(axis=1))[inverse] return SupportResult(bool(supported.all()), samples[~supported]) @@ -233,10 +248,8 @@ def _resample(waypoints: NDArray[np.float32], spacing: float) -> NDArray[np.floa def check_kinematics(waypoints: NDArray[np.float32], cfg: EvalConfig) -> KinematicsResult: - """Reject paths that climb steeper than the robot can. - - Resampled at window_m of arc so cell quantization does not read as a cliff. - """ + """Reject paths that climb steeper than the robot can. Resampled at + window_m of arc so cell quantization does not read as a cliff.""" if len(waypoints) < 2: return KinematicsResult(True, waypoints[:0]) profile = _resample(waypoints, cfg.kinematic_window_m) @@ -300,11 +313,10 @@ def reference_length( goal: tuple[float, float, float], cfg: EvalConfig, ) -> Reference: - """Shortest walked length demonstrated between start and goal. + """Shortest walked length demonstrated between start and goal, minimized + over every visit pairing and preferring causal ones. - Minimized over every start-visit to goal-visit pairing, preferring causal - pairs. Falls back to straight-line distance when an endpoint is off the - trajectory. + Falls back to straight-line distance when an endpoint is off the trajectory. """ visits = _visits(trajectory, start, goal, cfg) if visits is None: @@ -328,9 +340,7 @@ def ground_truth_route( cfg: EvalConfig, ) -> NDArray[np.float32] | None: """Foot-level polyline of the shortest walk between start and goal. - - Ignores causality: it describes terrain, not what the robot knew. - """ + Ignores causality: it describes terrain, not what the robot knew.""" visits = _visits(trajectory, start, goal, cfg) if visits is None: return None @@ -355,7 +365,7 @@ def soft_progress( ) -> float: """Fraction of the start-goal distance covered by the path endpoint.""" d0 = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) - if end is None or d0 < 1e-6: + if end is None or d0 < MIN_LENGTH_M: return 0.0 d1 = float(np.linalg.norm(np.asarray(goal, dtype=np.float32) - end)) return float(np.clip(1.0 - d1 / d0, 0.0, 1.0)) diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index a11f285d8b..c5b36993dc 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -15,9 +15,7 @@ """Browser point-picking for case curation, served by viser. Serves the final map, the walked path, and every existing case as an editable -panel entry. Shift+click picks new start/goal pairs. Each entry exposes the -coordinates, a name field, tag checkboxes, a negative toggle, and save/delete -buttons, so any case can be renamed, retagged, flipped, or removed. +panel entry. Shift+click picks new start/goal pairs. """ from __future__ import annotations @@ -66,9 +64,8 @@ """ -# three.js ACES filmic tone mapping, the fitted curve and its color -# matrices. Applied by the viewer to mesh materials but not to the point -# and line shaders. +# three.js ACES filmic tone mapping. The viewer applies it to mesh materials +# but not to the point and line shaders. _ACES_INPUT = np.array( [ [0.59719, 0.35458, 0.04823], @@ -83,9 +80,8 @@ [-0.00327, -0.07276, 1.07602], ] ) -# White scene lights, bright enough that inverse-tone-mapped albedos fit -# in [0, 1]. LIGHT_REFERENCE is the ambient plus directional total on a -# typical face. Faces above or below it shade brighter or darker. +# White scene lights, bright enough that inverse-tone-mapped albedos fit in +# [0, 1]. LIGHT_REFERENCE is the ambient plus directional total on a typical face. _AMBIENT_INTENSITY = 3.5 _DIRECTIONAL_INTENSITY = 2.0 _LIGHT_REFERENCE = 4.6 @@ -94,10 +90,8 @@ def _prelit_albedo(srgb: NDArray[np.uint8]) -> NDArray[np.float64]: """Linear albedo that tone-maps back to the wanted sRGB color when lit. - Voxel cubes are lit meshes, so the viewer runs them through ACES tone - mapping and would desaturate the height colormap. Feeding the inverse - curve through the material albedo cancels that out at the reference - light level. + Cancels the viewer's ACES pass, which would otherwise desaturate the + height colormap on lit meshes. """ c = srgb.astype(np.float64) / 255.0 lin = np.where(c <= 0.04045, c / 12.92, ((c + 0.055) / 1.055) ** 2.4) @@ -107,7 +101,8 @@ def _prelit_albedo(srgb: NDArray[np.uint8]) -> NDArray[np.float64]: a0 = -0.000090537 - 0.238081 * t v = (-a1 + np.sqrt(a1 * a1 - 4.0 * a2 * a0)) / (2.0 * a2) x = 0.6 * (v @ np.linalg.inv(_ACES_INPUT).T) - return np.clip(x / _LIGHT_REFERENCE, 0.0, 1.0) + albedo: NDArray[np.float64] = np.clip(x / _LIGHT_REFERENCE, 0.0, 1.0) + return albedo def _cube_colors(srgb: NDArray[np.uint8]) -> NDArray[np.uint8]: @@ -135,9 +130,8 @@ def pick_along_ray( ) -> NDArray[np.float32] | None: """Nearest cloud point inside a tube of the given radius around the click ray. - A physical perpendicular radius, not an angular cone: an angular cone widens - with distance, so it would pick a voxel far down the ray over the near one - the click landed on. With a fixed tube the nearest voxel along the ray wins. + A fixed perpendicular radius, not a cone: a cone widens with distance and + would pick a far voxel over the near one the click landed on. """ rel = points.astype(np.float64) - origin t = rel @ direction @@ -154,6 +148,15 @@ def pick_along_ray( return None +@dataclass +class _PairMarkers: + """The three scene nodes drawn for one start/goal pair.""" + + start: viser.IcosphereHandle + goal: viser.IcosphereHandle + line: viser.LineSegmentsHandle + + @dataclass class _Hooks: """Manifest store and shared state handed to every pair entry.""" @@ -175,7 +178,7 @@ def __init__( start: NDArray[np.float32], goal: NDArray[np.float32], hooks: _Hooks, - markers: list[viser.SceneNodeHandle], + markers: _PairMarkers, case: Case | None = None, ) -> None: self._server = server @@ -200,9 +203,8 @@ def __init__( self._status = "in manifest" self.removed = False self._build(expanded=case is None, order=None) - for marker in markers: - if hasattr(marker, "on_click"): - marker.on_click(self._on_marker_click) + markers.start.on_click(self._on_marker_click) + markers.goal.on_click(self._on_marker_click) def _on_marker_click(self, _event: object) -> None: with self._hooks.lock: @@ -220,36 +222,27 @@ def reveal(self) -> None: def set_highlight(self, on: bool) -> None: if self.removed: return - for marker in self.markers: - if hasattr(marker, "radius"): - marker.radius = HIGHLIGHT_MARKER_RADIUS if on else MARKER_RADIUS - elif hasattr(marker, "line_width"): - marker.line_width = HIGHLIGHT_LINE_WIDTH if on else LINE_WIDTH - marker.colors = np.array(HIGHLIGHT_LINE_COLOR if on else PAIR_COLOR, dtype=np.uint8) + for ball in (self.markers.start, self.markers.goal): + ball.radius = HIGHLIGHT_MARKER_RADIUS if on else MARKER_RADIUS + self.markers.line.line_width = HIGHLIGHT_LINE_WIDTH if on else LINE_WIDTH + self.markers.line.colors = np.array( + HIGHLIGHT_LINE_COLOR if on else PAIR_COLOR, dtype=np.uint8 + ) def _mark_saved(self) -> None: """Repaint the pick markers to the standard saved look, so only pairs not yet in the manifest wear the distinct new-pick colors and line.""" - sphere_colors = [START_COLOR, GOAL_COLOR] - for marker in self.markers: - if hasattr(marker, "radius"): - color = _marker_color(sphere_colors.pop(0)) - if hasattr(marker, "color"): - marker.color = color - elif hasattr(marker, "line_width"): - marker.line_width = LINE_WIDTH - marker.colors = np.array(PAIR_COLOR, dtype=np.uint8) + self.markers.start.color = _marker_color(START_COLOR) + self.markers.goal.color = _marker_color(GOAL_COLOR) + self.markers.line.line_width = LINE_WIDTH + self.markers.line.colors = np.array(PAIR_COLOR, dtype=np.uint8) def _label(self) -> str: return self.saved_id or f"pair {self._n}" def _sync_tags(self, tags: list[str]) -> None: - """Split a manifest tag list into checkbox and custom-text state. - - The negative tag is owned by the checkbox. Everything not in the - suggested set (auto, manual, ...) lands in the custom text so it - stays visible and round-trips verbatim. - """ + """Split a manifest tag list into checkbox and custom-text state, so + unsuggested tags stay visible and round-trip verbatim.""" self._checked = {t for t in tags if t in SUGGESTED_TAGS} self._custom = ", ".join(t for t in tags if t not in SUGGESTED_TAGS and t != "negative") @@ -284,23 +277,23 @@ def _build(self, *, expanded: bool, order: float | None, scroll: bool = False) - self.button = server.gui.add_button("save / update") self.delete_button = server.gui.add_button("delete") - @self.button.on_click - def _(_event: object) -> None: - # save_unsaved calls save_or_update already holding the lock. - # The button path runs on a bare viser callback thread and must - # take it to serialize suite/manifest mutation. + # Button callbacks run on bare viser threads, so they take the lock + # that save_unsaved already holds on its own path. + def _on_save(_event: object) -> None: with self._hooks.lock: self.save_or_update() - @self.delete_button.on_click - def _(_event: object) -> None: + def _on_delete(_event: object) -> None: with self._hooks.lock: self.delete() + self.button.on_click(_on_save) + self.delete_button.on_click(_on_delete) + def remove(self) -> None: self.removed = True self.panel.remove() - for marker in self.markers: + for marker in (self.markers.start, self.markers.goal, self.markers.line): marker.remove() def delete(self) -> None: @@ -341,7 +334,10 @@ def save_or_update(self) -> bool: ) else: case = store.update( - self.saved_id, name or self.saved_id, self.extra_tags(), negative + self.saved_id, + name or self.saved_id, + self.extra_tags(), + expect_fail=negative, ) except CurationError as err: print(err) @@ -372,6 +368,7 @@ def pick_cases( store: CaseStore, ) -> None: """Serve the picker until the user exits from the panel or hits ctrl-c.""" + # Lazy: viser is an optional extra, only needed by this command. import viser server = viser.ViserServer( @@ -432,11 +429,12 @@ def pick_cases( center = map_points.mean(axis=0) span = float(np.ptp(map_points[:, :2])) - @server.on_client_connect - def _(client: viser.ClientHandle) -> None: + def _on_client_connect(client: viser.ClientHandle) -> None: client.camera.position = tuple(center + np.array([0.6 * span, 0.6 * span, 0.45 * span])) client.camera.look_at = tuple(center) + server.on_client_connect(_on_client_connect) + server.gui.add_markdown(INSTRUCTIONS) selected_line = server.gui.add_markdown("selected: —") undo_button = server.gui.add_button("undo last pick") @@ -461,7 +459,7 @@ def highlight(entry: _PairEntry) -> None: hooks = _Hooks(store, lock, lambda entry: pairs.remove(entry), announce, highlight) marker_seq = 0 - def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.SceneNodeHandle: + def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.IcosphereHandle: nonlocal marker_seq marker_seq += 1 return server.scene.add_icosphere( @@ -476,7 +474,7 @@ def pair_line( goal: NDArray[np.float32], color: tuple[int, int, int] = PAIR_COLOR, width: float = LINE_WIDTH, - ) -> viser.SceneNodeHandle: + ) -> viser.LineSegmentsHandle: nonlocal marker_seq marker_seq += 1 return server.scene.add_line_segments( @@ -486,10 +484,10 @@ def pair_line( line_width=width, ) - def pair_markers( - start: NDArray[np.float32], goal: NDArray[np.float32] - ) -> list[viser.SceneNodeHandle]: - return [sphere(start, START_COLOR), sphere(goal, GOAL_COLOR), pair_line(start, goal)] + def pair_markers(start: NDArray[np.float32], goal: NDArray[np.float32]) -> _PairMarkers: + return _PairMarkers( + sphere(start, START_COLOR), sphere(goal, GOAL_COLOR), pair_line(start, goal) + ) for case in store.suite.cases: start = np.asarray(case.start, dtype=np.float32) @@ -498,11 +496,10 @@ def pair_markers( _PairEntry(server, 0, start, goal, hooks, pair_markers(start, goal), case=case) ) - pending: list[tuple[viser.SceneNodeHandle, NDArray[np.float32]]] = [] + pending: list[tuple[viser.IcosphereHandle, NDArray[np.float32]]] = [] pair_count = 0 - @server.scene.on_click(modifier="shift") - def _(event: viser.SceneClickEvent) -> None: + def _on_scene_click(event: viser.SceneClickEvent) -> None: nonlocal pair_count point = pick_along_ray( map_points, np.asarray(event.ray_origin), np.asarray(event.ray_direction), voxel_size @@ -514,23 +511,21 @@ def _(event: viser.SceneClickEvent) -> None: pending.append((sphere(point, NEW_START_COLOR), point)) return start_marker, start = pending.pop() - markers = [ + markers = _PairMarkers( start_marker, sphere(point, NEW_GOAL_COLOR), pair_line(start, point, NEW_PAIR_COLOR, NEW_LINE_WIDTH), - ] + ) pair_count += 1 pairs.append(_PairEntry(server, pair_count, start, point, hooks, markers)) - @undo_button.on_click - def _(_event: object) -> None: + def _on_undo(_event: object) -> None: with lock: if pending: pending.pop()[0].remove() elif pairs and not pairs[-1].preloaded: - # Saved cases stay in the manifest, only the panel entry and - # markers go away. Deleting from the manifest is the per-pair - # delete button. + # Only the panel entry and markers go away. The per-pair delete + # button is what removes it from the manifest. entry = pairs.pop() entry.remove() if entry.saved_id is not None: @@ -540,15 +535,18 @@ def save_unsaved() -> int: with lock: return sum(not pair.save_or_update() for pair in pairs if pair.saved_id is None) - @save_all_button.on_click - def _(_event: object) -> None: + def _on_save_all(_event: object) -> None: save_unsaved() - @exit_button.on_click - def _(_event: object) -> None: + def _on_exit(_event: object) -> None: if save_unsaved() == 0: stop.set() + server.scene.on_click(modifier="shift")(_on_scene_click) + undo_button.on_click(_on_undo) + save_all_button.on_click(_on_save_all) + exit_button.on_click(_on_exit) + print("picker running; ctrl-c to exit (unsaved pairs are discarded)") try: stop.wait() diff --git a/dimos/navigation/nav_3d/evaluator/pipeline.py b/dimos/navigation/nav_3d/evaluator/pipeline.py index bafad3d5f6..5164a1296a 100644 --- a/dimos/navigation/nav_3d/evaluator/pipeline.py +++ b/dimos/navigation/nav_3d/evaluator/pipeline.py @@ -56,13 +56,11 @@ def node_edges(self) -> NDArray[np.float32]: ... class MLSPipeline: - """The voxel ray-tracing mapper feeding the MLS planner. - - The map reaches the planner on the first plan after new frames, so that - plan pays for the rebuild it triggers. - """ + """The voxel ray-tracing mapper feeding the MLS planner. The map reaches + the planner on the first plan after new frames, which pays for the rebuild.""" def __init__(self, cfg: EvalConfig) -> None: + # Lazy: the planner is a native module, only needed by this pipeline. from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner self._mapper = cfg.make_mapper() diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py index c493cb93fc..0a8d95b9d8 100644 --- a/dimos/navigation/nav_3d/evaluator/recording.py +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -68,9 +68,7 @@ def iter_world_frames( end_ts: float | None = None, ) -> Iterator[Frame]: """Yield lidar frames registered into the world by their odometry pose. - - Clouds must be sensor-frame. Frames at or after end_ts are skipped. - """ + Clouds must be sensor-frame. Frames at or after end_ts are skipped.""" store = SqliteStore(path=str(db_path)) with store: lidar = store.stream(lidar_stream, PointCloud2).order_by("ts") diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index ff1e2997a9..6dfc45e7a5 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -15,8 +15,7 @@ """Replay case suites through a pipeline and score them. Generated cases plan twice, online at their start time and again on the whole -recording. Curated and infeasible cases plan once, on the final map. The -headline score is validity-gated SPL on the incremental map. +recording. Curated and infeasible cases plan once, on the final map. """ from __future__ import annotations @@ -41,10 +40,14 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from pathlib import Path + from numpy.typing import NDArray from dimos.navigation.nav_3d.evaluator.cases import Case, Suite + from dimos.navigation.nav_3d.evaluator.final_map import FinalMap, MapCheckpoints from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline + from dimos.navigation.nav_3d.evaluator.recording import Trajectory logger = setup_logger() @@ -225,12 +228,8 @@ def _no_plan(plan_ms: float) -> PlanOutcome: def score_negative(raw: PlanOutcome) -> PlanOutcome: - """Invert an outcome for a human-certified infeasible case. - - The planner succeeds by refusing. Any goal-reaching path it returns is a - false positive scored zero, whether or not the gates would have caught - it, because the planner claimed a route that does not exist. - """ + """Invert an outcome for a human-certified infeasible case: the planner + succeeds by refusing, and any goal-reaching path it returns scores zero.""" refused = not (raw.planned and raw.reached) return replace(raw, success=refused, spl=1.0 if refused else 0.0) @@ -243,13 +242,8 @@ def _dynamic_candidate( final_keys: NDArray[np.int64], cfg: EvalConfig, ) -> tuple[bool, list[list[float]]]: - """Flag a case whose online route is blocked only by new final occupancy. - - An online success with a final failure is either a dynamic obstacle that - appeared after the robot passed or a planner or mapping bug. Gating the - online path against the voxels gained since plan time tells them apart. A - human confirms before labeling the case. - """ + """Flag a case whose online route is blocked only by occupancy gained + since plan time, which separates a dynamic obstacle from a planner bug.""" if online_wp is None or not online.success or final.success: return False, [] # Both come from np.unique, so the sort in setdiff1d is pure waste. @@ -274,22 +268,15 @@ def _snapshot(pipeline: NavPipeline) -> PlannerArtifacts | None: def _final_only(case: Case) -> bool: - """Whether a case is scored on the final map only, with no online phase. - - Hand-placed endpoints are not tied to the recording timeline, so there is - no meaningful incremental map to replay against. Generated cases keep their - online phase however their labels are later edited. - """ + """Whether a case is scored on the final map only. Hand-placed endpoints + are not tied to the recording timeline, so they have no incremental map.""" return case.expect_fail or "auto" not in case.tags -def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> DatasetResult: - db_path = suite.db_path() - trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) - final = load_or_build_final_map(db_path, suite, cfg) - map_keys = final.occupied_keys - - final_only = np.array([_final_only(c) for c in suite.cases], dtype=bool) +def _references( + suite: Suite, trajectory: Trajectory, final_only: NDArray[np.bool_], cfg: EvalConfig +) -> list[metrics.Reference]: + """The demonstrated route length each case is scored against.""" refs: list[metrics.Reference] = [] for i, case in enumerate(suite.cases): if case.expect_fail: @@ -302,137 +289,195 @@ def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> Da # Only online-replayed cases need a causal snap onto the trajectory. if not ref.snapped: logger.warning( - "%s/%s: start or goal is off the walked trajectory; " - "using straight-line reference and the full map", - suite.dataset, - case.id, + "endpoint off the walked trajectory, using a straight-line reference", + dataset=suite.dataset, + case=case.id, ) elif not ref.causal: logger.warning( - "%s/%s: goal is never visited before the start; planning on the full map", - suite.dataset, - case.id, + "goal never visited before the start, planning on the full map", + dataset=suite.dataset, + case=case.id, ) refs.append(ref) + return refs - # Final-only cases never replay online, so they take no checkpoint and their - # plan time drops out of the schedule. - start_ts = np.array( - [float("inf") if final_only[i] else r.start_ts for i, r in enumerate(refs)], - dtype=np.float64, - ) - checkpoints = load_or_build_checkpoints(db_path, suite, cfg, start_ts) - case_ckpt = np.searchsorted(checkpoints.times, start_ts) - case_ckpt[final_only] = -1 - pipeline = make_pipeline(cfg.pipeline, cfg) - results: list[CaseResult | None] = [None] * len(suite.cases) - online: dict[int, tuple[PlanOutcome, NDArray[np.float32] | None, NDArray[np.int64]]] = {} - artifacts: dict[int, PlannerArtifacts | None] = {} - occupied_at_plan: dict[int, NDArray[np.float32] | None] = {} +@dataclass +class _OnlinePass: + """Everything the single replay pass produced, keyed by case index.""" - def _result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: - """Fill the fields every case copies straight from its case and reference.""" - return CaseResult( - id=case.id, - dataset=suite.dataset, - start=case.start, - goal=case.goal, - tags=case.tags, - l_ref=ref.length, - **rest, # type: ignore[arg-type] - ) + outcomes: dict[int, tuple[PlanOutcome, NDArray[np.float32] | None, NDArray[np.int64]]] + artifacts: dict[int, PlannerArtifacts | None] + occupied: dict[int, NDArray[np.float32] | None] + add_ms: list[float] + final_artifacts: PlannerArtifacts | None + + +def _replay_online( + pipeline: NavPipeline, + suite: Suite, + db_path: Path, + cfg: EvalConfig, + checkpoints: MapCheckpoints, + case_ckpt: NDArray[np.int64], + refs: list[metrics.Reference], + map_keys: NDArray[np.int64], + keep_artifacts: bool, +) -> _OnlinePass: + """Feed the recording to the pipeline once, planning each case where its + start time falls, so the pipeline has seen what the robot had seen by then. + + A pipeline's state cannot be snapshotted from outside, hence one pass. + """ + out = _OnlinePass({}, {}, {}, [], None) - def plan_online(k: int, keys: NDArray[np.int64]) -> None: - """Plan every case whose start time this checkpoint covers, against the - pipeline as it stands after the frames seen so far.""" + def plan_at(k: int, keys: NDArray[np.int64]) -> None: for ci in np.flatnonzero(case_ckpt == k): case, ref = suite.cases[ci], refs[ci] if not len(keys): - online[ci] = (_no_plan(0.0), None, keys) + out.outcomes[ci] = (_no_plan(0.0), None, keys) continue - # Collisions are checked against the incremental map the evaluator - # had at plan time, not the final map. Support still uses the final - # map, since the ground exists whether or not it was mapped yet. + # Collisions use the incremental map as of plan time. Support uses + # the final map, since ground exists whether or not it was mapped. outcome, waypoints = _run_plan(pipeline, case, ref.length, keys, map_keys, cfg) - online[ci] = (outcome, waypoints, keys) - artifacts[ci] = _snapshot(pipeline) if keep_artifacts else None - occupied_at_plan[ci] = key_centers(keys, cfg.voxel_size) if keep_artifacts else None - - # One pass. Frames go to the pipeline in recording order and each case is - # planned at the point in the stream its start time falls, so the pipeline - # has seen exactly what the robot had seen by then. A pipeline's state - # cannot be snapshotted from outside, which is why this is sequential. + out.outcomes[ci] = (outcome, waypoints, keys) + out.artifacts[ci] = _snapshot(pipeline) if keep_artifacts else None + out.occupied[ci] = key_centers(keys, cfg.voxel_size) if keep_artifacts else None + snapshots = checkpoints.iter_snapshots() - add_ms: list[float] = [] k = 0 for frame in iter_world_frames( db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() ): while k < len(checkpoints.times) and frame.ts > checkpoints.times[k]: - plan_online(k, next(snapshots)) + plan_at(k, next(snapshots)) k += 1 t0 = perf_counter() pipeline.add_frame(frame.points, frame.origin, frame.ts) - add_ms.append((perf_counter() - t0) * 1000) + out.add_ms.append((perf_counter() - t0) * 1000) while k < len(checkpoints.times): - plan_online(k, next(snapshots)) + plan_at(k, next(snapshots)) k += 1 # The stream is exhausted, so the pipeline now holds the whole recording. - final_artifacts = _snapshot(pipeline) if keep_artifacts else None + out.final_artifacts = _snapshot(pipeline) if keep_artifacts else None + return out + + +def _score_final( + pipeline: NavPipeline, + suite: Suite, + cfg: EvalConfig, + refs: list[metrics.Reference], + final: FinalMap, + final_only: NDArray[np.bool_], + online: _OnlinePass, +) -> list[CaseResult]: + """Plan every case against the completed map and combine both phases.""" + map_keys = final.occupied_keys + + def result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: + return CaseResult( + id=case.id, + dataset=suite.dataset, + start=case.start, + goal=case.goal, + tags=case.tags, + l_ref=ref.length, + **rest, # type: ignore[arg-type] + ) + + results: list[CaseResult] = [] for ci, case in enumerate(suite.cases): ref = refs[ci] final_out, _ = _run_plan(pipeline, case, ref.length, map_keys, map_keys, cfg) if case.expect_fail or case.expect_final_fail: - # Both labels certify that the final map holds no route, so the - # planner passes by refusing. Applied before the final-only split - # so a curated case is not scored zero for being right. + # Both labels certify the final map holds no route, so the planner + # passes by refusing. Before the split, so a curated case scores. final_out = score_negative(final_out) if final_only[ci]: - results[ci] = _result( - case, - ref, - online_voxels=len(final.occupied), - expect_fail=case.expect_fail, - online=final_out, - final=final_out, - soft_progress=final_out.spl, - final_only=True, + results.append( + result( + case, + ref, + online_voxels=len(final.occupied), + expect_fail=case.expect_fail, + online=final_out, + final=final_out, + soft_progress=final_out.spl, + final_only=True, + ) ) continue - online_out, online_wp, online_keys = online[ci] + online_out, online_wp, online_keys = online.outcomes[ci] end = online_wp[-1] if online_wp is not None and len(online_wp) else None dynamic_candidate, blocking = ( (False, []) if case.expect_final_fail else _dynamic_candidate(online_out, final_out, online_wp, online_keys, map_keys, cfg) ) - results[ci] = _result( - case, - ref, - online_voxels=len(online_keys), - expect_fail=False, - online=online_out, - final=final_out, - soft_progress=metrics.soft_progress(end, case.start, case.goal), - dynamic_candidate=dynamic_candidate, - blocking_points=blocking, - online_artifacts=artifacts.get(ci), - online_occupied=occupied_at_plan.get(ci), + results.append( + result( + case, + ref, + online_voxels=len(online_keys), + expect_fail=False, + online=online_out, + final=final_out, + soft_progress=metrics.soft_progress(end, case.start, case.goal), + dynamic_candidate=dynamic_candidate, + blocking_points=blocking, + online_artifacts=online.artifacts.get(ci), + online_occupied=online.occupied.get(ci), + ) ) + return results + - done = [r for r in results if r is not None] - if len(done) != len(suite.cases): - raise RuntimeError(f"{suite.dataset}: {len(suite.cases) - len(done)} cases not planned") +def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> DatasetResult: + db_path = suite.db_path() + if not db_path.exists(): + # sqlite would otherwise create an empty db here and the failure would + # surface as a missing odometry stream. + raise FileNotFoundError(f"{suite.dataset}: recording not found at {db_path}") + trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) + + final_only = np.array([_final_only(c) for c in suite.cases], dtype=bool) + refs = _references(suite, trajectory, final_only, cfg) + # Final-only cases never replay online, so they take no checkpoint and their + # plan time drops out of the schedule. + start_ts = np.array( + [float("inf") if final_only[i] else r.start_ts for i, r in enumerate(refs)], + dtype=np.float64, + ) + # Before the final map, because a cold checkpoint build replays the whole + # recording and fills the final cache on its way through. + checkpoints = load_or_build_checkpoints(db_path, suite, cfg, start_ts) + final = load_or_build_final_map(db_path, suite, cfg) + case_ckpt = np.searchsorted(checkpoints.times, start_ts) + case_ckpt[final_only] = -1 + + pipeline = make_pipeline(cfg.pipeline, cfg) + online = _replay_online( + pipeline, + suite, + db_path, + cfg, + checkpoints, + case_ckpt, + refs, + final.occupied_keys, + keep_artifacts, + ) return DatasetResult( dataset=suite.dataset, - cases=done, + cases=_score_final(pipeline, suite, cfg, refs, final, final_only, online), final_voxels=len(final.occupied), map_build_ms=final.build_ms, - add_frame_ms=metrics.timing_stats(add_ms), - frames=len(add_ms), - final_artifacts=final_artifacts, + add_frame_ms=metrics.timing_stats(online.add_ms), + frames=len(online.add_ms), + final_artifacts=online.final_artifacts, ) @@ -443,8 +488,7 @@ def evaluate( keep_artifacts: bool = False, ) -> Report: """Score every suite. A dataset is one sequential pass over its recording, - so workers only spreads datasets across processes. keep_artifacts snapshots - each pipeline's graph for the rerun recording.""" + so workers only spreads datasets across processes.""" cfg = cfg or EvalConfig() if workers > 1 and len(suites) > 1: with ProcessPoolExecutor(max_workers=min(workers, len(suites))) as pool: diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py index 43621f29b8..3cfc0ab1fb 100644 --- a/dimos/navigation/nav_3d/evaluator/tagging.py +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -65,11 +65,8 @@ def elevation_tags( start: tuple[float, float, float], goal: tuple[float, float, float] ) -> list[str]: - """Elevation from the case endpoints, matching how generation labels them. - - Endpoints, not the recovered route: the route can wander far off the - straight line, and a case's climb is defined by where it starts and ends. - """ + """Elevation from the case endpoints, not the recovered route: a case's + climb is defined by where it starts and ends.""" dz = goal[2] - start[2] euclid = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) if abs(dz) < STAIRS_DZ_M: @@ -85,10 +82,8 @@ def _corridor_width( ) -> NDArray[np.float64]: """Free lateral width at each densified sample, at body height. - Probes outward along both body-lateral directions from a point chest-high - over the path and returns left-plus-right distance to the nearest occupied - voxel, capped when the passage is open. This is the room the body has to - pass, not the room the feet have to stand. + Left-plus-right distance to the nearest occupied voxel, capped when the + passage is open. The room the body has to pass, not the feet to stand. """ _, lateral, _ = body_frames(samples, cfg.robot_length) mid_z = (cfg.ground_margin + cfg.body_clearance) / 2.0 @@ -140,9 +135,8 @@ def _corridor_tags( tight = cfg.robot_width + 2.0 * MARGIN_CAP_M roomy = cfg.robot_width + 2.0 * cfg.robot_length arc = arc_lengths(samples) - # A real passage is at least the robot's own body wide. Anything tighter is - # furniture or map noise the robot could not have walked through, so it does - # not count as a passage. + # A real passage is at least a body wide. Anything tighter is furniture or + # map noise the robot could not have walked through. narrow = (width >= cfg.robot_width) & (width < tight) runs = [ (lo, hi) @@ -172,11 +166,8 @@ def _is_local( goal: tuple[float, float, float], cfg: EvalConfig, ) -> bool: - """True when the route runs roughly straight from start to goal. - - Endpoints closer than a body length have no terrain to describe. Otherwise - the walked route must span the straight line without detouring far past it. - """ + """True when the route runs roughly straight from start to goal, so its + terrain can be attributed to the case.""" eucl = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) if eucl < cfg.robot_length: return False @@ -193,11 +184,9 @@ def route_tags( ) -> list[str]: """Geometric tags for a case, in a stable order. - Elevation comes from the endpoints. Path-shape tags describe the local - terrain between them and need a walked route that runs roughly straight from - start to goal, so they are skipped when the route is off the trajectory or a - long detour. Deterministic and free of provenance: the caller prepends auto - or manual. + Elevation comes from the endpoints. Shape tags describe the terrain between + them and are skipped unless the walked route runs roughly straight. The + caller prepends the provenance tag. """ tags = elevation_tags(start, goal) if route is not None and len(route) >= 2 and _is_local(route, start, goal, cfg): diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_evaluator.py index c605e77de1..ff8ad803bd 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_evaluator.py @@ -16,7 +16,6 @@ from dataclasses import replace import itertools -import json from pathlib import Path from typing import TYPE_CHECKING, cast @@ -24,8 +23,7 @@ import pytest import typer -from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper -from dimos.navigation.nav_3d.evaluator import metrics, tripwire +from dimos.navigation.nav_3d.evaluator import final_map, metrics, runner from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.curation import CaseStore, CurationError, _curated_tags @@ -38,23 +36,17 @@ ) from dimos.navigation.nav_3d.evaluator.generate import ( Candidate, - GenerationParams, _select_diverse, generate_cases, snap_to_surface, ) from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray -from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection, make_pipeline +from dimos.navigation.nav_3d.evaluator.pipeline import PIPELINES, make_pipeline from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory from dimos.navigation.nav_3d.evaluator.runner import ( - CaseResult, - DatasetResult, - Report, _dynamic_candidate, _final_only, - _no_plan, _run_plan, - _snapshot, score_negative, ) from dimos.navigation.nav_3d.evaluator.tagging import route_tags @@ -62,18 +54,20 @@ cylinder_offsets, key_centers, keys_contain, - offset_deltas, offset_keys, voxel_keys, ) if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline VOXEL = 0.1 -def _cfg(**overrides: float) -> EvalConfig: +def _cfg(**overrides: object) -> EvalConfig: return replace(EvalConfig(voxel_size=VOXEL), **overrides) @@ -126,11 +120,6 @@ def test_gate_box_uses_travel_orientation() -> None: assert _gate(path, beside).valid -def test_gate_passes_clear_path() -> None: - path = np.array([[-1, 0, 0], [1, 0, 0]], dtype=np.float32) - assert _gate(path, _wall(5.0)).valid - - def test_gate_ignores_ground() -> None: xs, ys = np.meshgrid(np.arange(-2, 2, VOXEL), np.arange(-2, 2, VOXEL)) floor = np.stack([xs.ravel(), ys.ravel(), np.full(xs.size, -0.05)], axis=1, dtype=np.float32) @@ -217,25 +206,32 @@ def test_checkpoint_deltas_roundtrip() -> None: assert np.array_equal(original, keys) +class _StubMapper: + """Keeps every point it is handed. replay_frames only calls these two.""" + + def __init__(self) -> None: + self._points: list[NDArray[np.float32]] = [] + + def add_frame(self, points: NDArray[np.float32], origin: tuple[float, float, float]) -> None: + self._points.append(points) + + def global_map(self) -> NDArray[np.float32]: + if not self._points: + return np.zeros((0, 3), dtype=np.float32) + return np.concatenate(self._points) + + def test_replay_frames_snapshots_grow_with_time() -> None: """Each checkpoint must contain exactly the frames seen up to its time.""" - mapper = VoxelRayMapper(voxel_size=VOXEL, max_range=30.0, support_min=1) + mapper = cast("VoxelRayMapper", _StubMapper()) def frame_at(ts: float, x: float) -> Frame: return Frame(ts=ts, points=_wall(x), origin=(x - 2.0, 0.0, 0.5)) - # A voxel needs a second observation to persist, so hit each wall twice. - frames = [ - frame_at(0.0, 5.0), - frame_at(0.1, 5.0), - frame_at(1.0, 8.0), - frame_at(1.1, 8.0), - frame_at(2.0, 11.0), - frame_at(2.1, 11.0), - ] + frames = [frame_at(0.0, 5.0), frame_at(1.0, 8.0), frame_at(2.0, 11.0)] times = np.array([0.5, 1.5, np.inf]) final, snapshots = replay_frames(frames, mapper, VOXEL, times) - assert final.frames == 6 + assert final.frames == 3 sizes = [len(s) for s in snapshots] assert 0 < sizes[0] < sizes[1] < sizes[2] assert np.array_equal(snapshots[2], final.occupied_keys) @@ -286,25 +282,13 @@ def _stub(waypoints: np.ndarray | None) -> NavPipeline: return cast("NavPipeline", _StubPipeline(waypoints)) -def test_make_pipeline_rejects_unknown_name() -> None: - assert isinstance(make_pipeline("mls", _cfg()), PipelineIntrospection) +def test_make_pipeline_resolves_by_registry_name() -> None: + cfg = _cfg() + with pytest.MonkeyPatch.context() as mp: + mp.setitem(PIPELINES, "stub", lambda _cfg: _StubPipeline(None)) + assert isinstance(make_pipeline("stub", cfg), _StubPipeline) with pytest.raises(ValueError, match="unknown pipeline"): - make_pipeline("nope", _cfg()) - - -def test_graph_layers_are_optional() -> None: - """A pipeline that keeps its internals to itself still evaluates, it just - contributes no graph layers to the recording.""" - - class _Introspective(_StubPipeline): - def surface_clearance_map(self) -> np.ndarray: - return np.zeros((1, 4), dtype=np.float32) - - def node_edges(self) -> np.ndarray: - return np.zeros((1, 7), dtype=np.float32) - - assert _snapshot(_stub(None)) is None - assert _snapshot(cast("NavPipeline", _Introspective(None))) is not None + make_pipeline("nope", cfg) def _floor(x_lo: float = 0.0, x_hi: float = 20.0) -> np.ndarray: @@ -355,20 +339,6 @@ def test_meta_demonstrated_route_scores_full() -> None: assert out.min_clearance == metrics.MARGIN_CAP_M -def test_meta_everything_occupied_fails_even_good_routes() -> None: - """An all-occupied map must collapse the score, not inflate it.""" - xs, ys, zs = np.meshgrid( - np.arange(0, 20, VOXEL), np.arange(-2, 6, VOXEL), np.arange(0.3, 0.5, VOXEL) - ) - everything = np.stack([xs.ravel(), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) - keys = np.unique(voxel_keys(np.concatenate([_floor(), everything]), VOXEL)) - _, cfg, case = _meta_scene() - out, _ = _run_plan(_stub(_u_route()), case, 24.0, keys, keys, cfg) - assert out.planned and out.reached - assert not out.valid - assert out.spl == 0.0 - - def test_meta_floating_bridge_fails_support() -> None: """A path across a floor gap collides with nothing but must still fail.""" gapped = np.concatenate([_floor(0.0, 6.0), _floor(14.0, 20.0)]) @@ -401,38 +371,6 @@ def test_meta_negative_case_scoring() -> None: assert score_negative(partial).success -def test_expect_final_fail_scores_online_normally_and_refuses_final() -> None: - """A door-closed route: the online plan earns SPL, the final plan must refuse. - - Mirrors the runner, which inverts only the final outcome for an - expect_final_fail case and scores the online outcome as usual. - """ - keys, cfg, case = _meta_scene() - open_keys = np.unique(voxel_keys(_floor(), VOXEL)) - route = _u_route() - l_ref = metrics.path_length(route) - - # Online, before the door closed: the wall is absent and the walked route - # is clean, so it scores in full. - online, _ = _run_plan(_stub(route), case, l_ref, open_keys, open_keys, cfg) - assert online.success - assert online.spl == pytest.approx(1.0) - - # Final, after the door closed: the wall is present and refusing is right. - refused, _ = _run_plan(_stub(None), case, l_ref, keys, keys, cfg) - final = score_negative(refused) - assert final.success - assert final.spl == 1.0 - - # Claiming a route straight through the closed door is a false positive. - line = np.array([case.start, case.goal], dtype=np.float32) - claimed, _ = _run_plan(_stub(line), case, l_ref, keys, keys, cfg) - assert score_negative(claimed).spl == 0.0 - - # Unlike a manual or infeasible case, it still replays online. - assert not _final_only(replace(case, tags=["auto"], expect_final_fail=True)) - - def test_dynamic_candidate_flags_route_blocked_by_new_occupancy() -> None: """Online success with a final failure from newly-appeared occupancy flags.""" _, cfg, case = _meta_scene() @@ -650,9 +588,8 @@ def test_generate_cases_around_wall() -> None: positions = np.column_stack([xy, np.full(len(xy), 0.3)]).astype(np.float32) traj = Trajectory(ts=np.linspace(0, 60, len(positions)), positions=positions) - cases = generate_cases(traj, final, surface, _cfg(), GenerationParams(max_cases=10)) + cases = generate_cases(traj, final, surface, _cfg(), max_cases=10) assert cases - assert len({c.id for c in cases}) == len(cases) assert [c for c in cases if (c.start[0] - 10) * (c.goal[0] - 10) < 0] for c in cases: assert abs(c.start[2]) < 1e-5 and abs(c.goal[2]) < 1e-5 @@ -671,11 +608,10 @@ def test_select_diverse_backfills_to_min_cases() -> None: ) for x in np.arange(0.0, 16.0, 2.0) ] - strict = _select_diverse(candidates, GenerationParams(min_cases=0), max_cases=12) - assert len(strict) == 4 - backfilled = _select_diverse(candidates, GenerationParams(min_cases=10), max_cases=12) - assert len(backfilled) == 8 - assert len({(c.start, c.goal) for c in backfilled}) == 8 + strict = _select_diverse(candidates, max_cases=12, min_cases=0) + backfilled = _select_diverse(candidates, max_cases=12, min_cases=10) + assert len(backfilled) > len(strict) + assert len({(c.start, c.goal) for c in backfilled}) == len(backfilled) def test_pick_along_ray() -> None: @@ -744,99 +680,13 @@ def test_save_suite_roundtrip(tmp_path: Path) -> None: loaded = load_suite(save_suite(suite, tmp_path / "demo.yaml")) assert loaded.dataset == "demo" assert loaded.lidar_stream == "other_lidar" - assert loaded.odom_stream == "pointlio_odometry" assert loaded.db_path() == Path.home() / "recordings/demo.db" assert loaded.cases[0].goal == (1.0, 2.0, 3.0) assert loaded.cases[0].tags == ["x"] - assert not loaded.cases[0].expect_fail and not loaded.cases[0].expect_final_fail assert loaded.cases[1].expect_fail assert loaded.cases[2].expect_final_fail -def _tripwire_report(spec: dict[str, dict[str, tuple[bool, bool]]]) -> dict[str, object]: - """Report JSON from a {dataset: {case_id: (inc, fin)}} pass/fail spec.""" - datasets = [ - DatasetResult( - dataset=dataset, - cases=[ - CaseResult( - id=case_id, - dataset=dataset, - start=(0.0, 0.0, 0.0), - goal=(1.0, 0.0, 0.0), - tags=[], - l_ref=1.0, - online_voxels=0, - expect_fail=False, - online=replace(_no_plan(0.0), success=inc), - final=replace(_no_plan(0.0), success=fin), - soft_progress=0.0, - ) - for case_id, (inc, fin) in cases.items() - ], - final_voxels=0, - map_build_ms=0.0, - add_frame_ms={}, - frames=0, - ) - for dataset, cases in spec.items() - ] - report = Report( - score=0.0, - score_soft=0.0, - final_score=0.0, - n_cases=0, - n_online=0, - n_success=0, - n_success_final=0, - outcome_counts={}, - by_tag={}, - plan_ms={}, - map_update_ms={}, - datasets=datasets, - ) - return json.loads(json.dumps(report.to_dict())) - - -def test_tripwire_diff_names_every_flip() -> None: - old = _tripwire_report( - {"office": {"a": (False, True), "b": (True, True), "gone": (True, True)}} - ) - new = _tripwire_report( - {"office": {"a": (True, True), "b": (False, False), "fresh": (True, True)}} - ) - d = tripwire.diff(old, new) - assert [(f.key, f.test) for f in d.fixed] == [("office/a", "inc")] - assert [(f.key, f.test) for f in d.broke] == [("office/b", "inc"), ("office/b", "fin")] - assert d.added == ["office/fresh"] - assert d.removed == ["office/gone"] - - -def test_tripwire_exact_differences() -> None: - """Wall-clock fields must be ignored while any result field is caught.""" - report = _tripwire_report({"office": {"a": (True, False)}}) - assert tripwire.exact_differences(report, report) == [] - changed = json.loads(json.dumps(report)) - changed["datasets"][0]["cases"][0]["online"]["length"] = 12.34 - changed["datasets"][0]["cases"][0]["online"]["plan_ms"] = 99.0 - diffs = tripwire.exact_differences(report, changed) - assert len(diffs) == 1 - assert "length" in diffs[0] and "12.34" in diffs[0] - - -def test_tripwire_perf_violations() -> None: - report = _tripwire_report({"office": {"a": (True, True)}}) - report["config"] = {"plan_p95_budget_ms": 60.0, "map_update_p95_budget_ms": 3000.0} - report["plan_ms"] = {"p95": 30.0} - report["map_update_ms"] = {"p95": 1500.0} - assert tripwire.perf_violations(report) == [] - report["plan_ms"] = {"p95": 61.0} - violations = tripwire.perf_violations(report) - assert len(violations) == 1 and "plan_ms" in violations[0] - # Reports predating the budgets pass. - assert tripwire.perf_violations({"datasets": []}) == [] - - def test_gate_band_follows_the_tilted_body_axis() -> None: """On a slope the band is measured up the body axis, so an obstacle at the body center collides and one down in the leg zone does not.""" @@ -864,15 +714,10 @@ def test_offset_deltas_match_packing_the_summed_indices() -> None: idx = np.floor(pts.astype(np.float64) / VOXEL).astype(np.int64)[:, None, :] + offs[None, :, :] packed = voxel_keys((idx.reshape(-1, 3) * VOXEL + VOXEL / 2).astype(np.float32), VOXEL) assert np.array_equal(offset_keys(pts, offs, VOXEL).ravel(), packed) - assert np.array_equal( - offset_keys(pts, offs, VOXEL)[0], voxel_keys(pts, VOXEL)[0] + offset_deltas(offs) - ) def test_spl_and_body_frames_survive_degenerate_input() -> None: - """A coincident start and goal must score, not divide by zero, and a - zero-length path must still yield a usable body frame.""" - assert metrics.spl(True, 0.0, 0.0) == 0.0 + """A zero-length path must still yield a usable body frame.""" point = np.array([[1.0, 1.0, 0.0], [1.0, 1.0, 0.0]], dtype=np.float32) fwd, lateral, up = metrics.body_frames(point, 0.7) assert np.allclose(np.linalg.norm(fwd, axis=1), 1.0) @@ -884,7 +729,15 @@ def _store(tmp_path: Path, cases: list[Case]) -> CaseStore: save_suite(Suite(dataset="demo", cases=cases), manifest) xs, ys = np.meshgrid(np.arange(0, 8, VOXEL), np.arange(-2, 2, VOXEL)) surface = np.stack([xs.ravel(), ys.ravel(), np.zeros(xs.size)], axis=1, dtype=np.float32) - return CaseStore(load_suite(manifest), manifest, surface, _cfg()) + final = FinalMap( + voxel_size=VOXEL, + occupied=surface, + occupied_keys=np.unique(voxel_keys(surface, VOXEL)), + frames=0, + add_frame_ms={}, + build_ms=0.0, + ) + return CaseStore(load_suite(manifest), manifest, surface, _cfg(), final) def test_editing_a_generated_case_keeps_it_in_the_incremental_score(tmp_path: Path) -> None: @@ -1018,21 +871,92 @@ def test_apply_overrides_is_the_sweep_interface() -> None: _apply_overrides(EvalConfig(), bad) -def test_diff_exits_nonzero_only_on_a_regression(tmp_path: Path) -> None: - """The tripwire is a CI gate, so its contract is the exit code.""" - from dimos.navigation.nav_3d.evaluator.cli import diff_reports - - clean = tmp_path / "a.json" - broken = tmp_path / "b.json" - clean.write_text(json.dumps(_tripwire_report({"office": {"a": (True, True)}}))) - broken.write_text(json.dumps(_tripwire_report({"office": {"a": (False, True)}}))) - - diff_reports(clean, clean, exact=False) - with pytest.raises(typer.Exit) as regressed: - diff_reports(clean, broken, exact=False) - assert regressed.value.exit_code == 1 - # A fix is not a regression. - diff_reports(broken, clean, exact=False) - # --exact fails on any non-timing difference, in either direction. - with pytest.raises(typer.Exit): - diff_reports(broken, clean, exact=True) +class _RecordingPipeline: + """Returns the fixed path and remembers how many frames it had each time.""" + + def __init__(self, waypoints: np.ndarray | None) -> None: + self._waypoints = waypoints + self.frames = 0 + self.frames_at_plan: list[int] = [] + + def add_frame(self, points: np.ndarray, origin: tuple[float, float, float], ts: float) -> None: + self.frames += 1 + + def plan( + self, start: tuple[float, float, float], goal: tuple[float, float, float] + ) -> np.ndarray | None: + self.frames_at_plan.append(self.frames) + return self._waypoints + + +def _stub_harness(mp: pytest.MonkeyPatch, tmp_path: Path, pipeline: object) -> None: + """Detach run_suite from both native modules: the pipeline under test and + the evaluator's own grading mapper. Caches land under tmp_path.""" + mp.setitem(PIPELINES, "stub", lambda _cfg: pipeline) + mp.setattr(EvalConfig, "make_mapper", lambda _self: _StubMapper()) + mp.setattr(final_map, "CACHE_SUBDIR", tmp_path / "cache") + + +def _corridor_recording(path: Path) -> Suite: + """Ten frames of floor along +x, walked start to finish.""" + slabs = [ + (float(t), _floor(float(t) - 1.0, float(t) + 1.0) - np.array([t, 0, 0], dtype=np.float32)) + for t in range(1, 11) + ] + _write_recording( + path, + [(ts, pts, "lidar") for ts, pts in slabs], + [(float(t), (float(t), 0.0, 0.5)) for t in range(1, 11)], + ) + return Suite( + dataset="corridor", cases=[], db=str(path), lidar_stream="lidar", odom_stream="odom" + ) + + +def test_run_suite_plans_each_case_on_the_map_as_of_its_start_time(tmp_path: Path) -> None: + """The framework's core claim: a case is scored against exactly the frames + the robot had seen by its start time, not the whole recording.""" + suite = _corridor_recording(tmp_path / "corridor.db") + # Both walk backwards, so the goal was visited before the start and the + # reference is causal. The second starts later in the recording. + suite.cases = [ + Case(id="auto_early", start=(4.0, 0.0, 0.0), goal=(2.0, 0.0, 0.0), tags=["auto"]), + Case(id="auto_late", start=(9.0, 0.0, 0.0), goal=(7.0, 0.0, 0.0), tags=["auto"]), + ] + pipeline = _RecordingPipeline(np.array([[4, 0, 0], [2, 0, 0]], dtype=np.float32)) + with pytest.MonkeyPatch.context() as mp: + _stub_harness(mp, tmp_path, pipeline) + result = runner.run_suite(suite, _cfg(pipeline="stub")) + + assert result.frames == 10 + # Two online plans, then one final plan per case on the full recording. + online_early, online_late = pipeline.frames_at_plan[:2] + assert 0 < online_early < online_late < 10 + assert pipeline.frames_at_plan[2:] == [10, 10] + + +def test_evaluate_scores_only_online_cases_in_the_headline(tmp_path: Path) -> None: + """A manual case is final-only, so it must move final_score but not score.""" + suite = _corridor_recording(tmp_path / "corridor.db") + route = np.array([[4, 0, 0], [2, 0, 0]], dtype=np.float32) + suite.cases = [ + Case(id="auto_00", start=(4.0, 0.0, 0.0), goal=(2.0, 0.0, 0.0), tags=["auto"]), + Case(id="manual_00", start=(4.0, 0.0, 0.0), goal=(2.0, 0.0, 0.0), tags=["manual"]), + ] + with pytest.MonkeyPatch.context() as mp: + _stub_harness(mp, tmp_path, _StubPipeline(route)) + report = runner.evaluate([suite], _cfg(pipeline="stub")) + + assert report.n_cases == 2 + assert report.n_online == 1 + assert [c.final_only for d in report.datasets for c in d.cases] == [False, True] + assert set(report.by_tag) == {"auto", "manual"} + assert report.by_tag["manual"].n_online == 0 + + +def test_run_suite_names_a_missing_recording(tmp_path: Path) -> None: + """Without the guard sqlite creates an empty db and the error blames odometry.""" + suite = Suite(dataset="gone", cases=[], db=str(tmp_path / "absent.db")) + with pytest.raises(FileNotFoundError, match="recording not found"): + runner.run_suite(suite, _cfg()) + assert not (tmp_path / "absent.db").exists() diff --git a/dimos/navigation/nav_3d/evaluator/tripwire.py b/dimos/navigation/nav_3d/evaluator/tripwire.py deleted file mode 100644 index d19180f406..0000000000 --- a/dimos/navigation/nav_3d/evaluator/tripwire.py +++ /dev/null @@ -1,143 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Per-case pass/fail diff between two report JSONs. - -The aggregate score can rise while individual cases flip from pass to fail. -Diffing two reports names every flip, so a change is judged case by case -rather than by the average alone. Stateless: which report counts as the -baseline is the caller's decision, typically the last kept run. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from typing import cast - -TESTS = ("inc", "fin") - -Outcomes = dict[str, dict[str, dict[str, bool]]] - - -@dataclass -class Flip: - """One case whose pass/fail state changed on one of the two tests.""" - - key: str - test: str - - -@dataclass -class ReportDiff: - fixed: list[Flip] - broke: list[Flip] - # Case ids present in only one of the two reports. - added: list[str] - removed: list[str] - - -def outcomes(report: dict[str, object]) -> Outcomes: - """Pass/fail of both tests for every case in a run --json report.""" - out: Outcomes = {} - for dataset in cast("list[dict[str, object]]", report["datasets"]): - cases: dict[str, dict[str, bool]] = {} - for case in cast("list[dict[str, object]]", dataset["cases"]): - online = cast("dict[str, object]", case["online"]) - final = cast("dict[str, object]", case["final"]) - cases[cast("str", case["id"])] = { - "inc": bool(online["success"]), - "fin": bool(final["success"]), - } - out[cast("str", dataset["dataset"])] = dict(sorted(cases.items())) - return out - - -def diff(old_report: dict[str, object], new_report: dict[str, object]) -> ReportDiff: - old, new = outcomes(old_report), outcomes(new_report) - fixed: list[Flip] = [] - broke: list[Flip] = [] - added: list[str] = [] - for dataset, cases in new.items(): - old_cases = old.get(dataset, {}) - for case_id, tests in cases.items(): - key = f"{dataset}/{case_id}" - if case_id not in old_cases: - added.append(key) - continue - for test in TESTS: - was, now = old_cases[case_id][test], tests[test] - if was != now: - (fixed if now else broke).append(Flip(key, test)) - removed = [ - f"{dataset}/{case_id}" - for dataset, cases in old.items() - for case_id in cases - if case_id not in new.get(dataset, {}) - ] - return ReportDiff(fixed, broke, sorted(added), sorted(removed)) - - -# Wall-clock fields legitimately differ between runs of identical code. -TIMING_KEYS = frozenset({"plan_ms", "map_update_ms", "map_build_ms", "add_frame_ms"}) - - -def _strip_timing(value: object) -> object: - if isinstance(value, dict): - return {k: _strip_timing(v) for k, v in value.items() if k not in TIMING_KEYS} - if isinstance(value, list): - return [_strip_timing(v) for v in value] - return value - - -def _walk(path: str, old: object, new: object, out: list[str]) -> None: - if isinstance(old, dict) and isinstance(new, dict): - for key in sorted(old.keys() | new.keys()): - if key not in old or key not in new: - out.append(f"{path}.{key}: only in {'old' if key in old else 'new'}") - else: - _walk(f"{path}.{key}", old[key], new[key], out) - elif isinstance(old, list) and isinstance(new, list): - if len(old) != len(new): - out.append(f"{path}: length {len(old)} != {len(new)}") - return - for i, (o, n) in enumerate(zip(old, new, strict=True)): - _walk(f"{path}[{i}]", o, n, out) - elif old != new: - out.append(f"{path}: {old!r} != {new!r}") - - -def perf_violations(report: dict[str, object]) -> list[str]: - """Timing stats that exceed the budgets recorded in the report's config.""" - config = cast("dict[str, float]", report.get("config") or {}) - out: list[str] = [] - for stat_key, budget_key in ( - ("plan_ms", "plan_p95_budget_ms"), - ("map_update_ms", "map_update_p95_budget_ms"), - ): - stats = cast("dict[str, float]", report.get(stat_key) or {}) - budget = config.get(budget_key) - p95 = stats.get("p95") - if budget is not None and p95 is not None and p95 > budget: - out.append(f"{stat_key} p95 {p95:.1f}ms exceeds budget {budget:.0f}ms") - return out - - -def exact_differences(old_report: dict[str, object], new_report: dict[str, object]) -> list[str]: - """Every non-timing field that differs between two reports. - - The determinism gate: two runs of identical code must return an empty list. - """ - out: list[str] = [] - _walk("report", _strip_timing(old_report), _strip_timing(new_report), out) - return out diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index acfeb15cf0..084288f00e 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -284,4 +284,4 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - views = [_dataset_view(d.dataset, [c.id for c in d.cases]) for d in report.datasets] rr.send_blueprint(rrb.Blueprint(rrb.Tabs(*views) if len(views) > 1 else views[0])) - logger.info("wrote %s; open with: rerun %s", out, out) + logger.info("wrote rerun recording", path=out) diff --git a/dimos/navigation/nav_3d/evaluator/voxel_keys.py b/dimos/navigation/nav_3d/evaluator/voxel_keys.py index 7db7978bbd..dd82f16221 100644 --- a/dimos/navigation/nav_3d/evaluator/voxel_keys.py +++ b/dimos/navigation/nav_3d/evaluator/voxel_keys.py @@ -49,6 +49,9 @@ def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float3 def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: if len(sorted_keys) == 0: return np.zeros(len(query), dtype=bool) + # Unsorted keys would report almost nothing occupied, which passes every + # gate and inflates the score instead of failing. + assert np.all(sorted_keys[:-1] <= sorted_keys[1:]), "map keys must be sorted" pos = np.clip(np.searchsorted(sorted_keys, query), 0, len(sorted_keys) - 1) return np.asarray(sorted_keys[pos] == query) @@ -70,9 +73,8 @@ def cylinder_offsets( def offset_deltas(offsets: NDArray[np.int64]) -> NDArray[np.int64]: """Packed key deltas for integer voxel offsets. - The three index fields occupy disjoint bit ranges and sit far from their - bounds, so adding a packed delta carries no bits between fields and is - identical to packing the summed indices. + Valid because the fields sit far from their bounds, so a packed add carries + no bits between them. """ return np.asarray((offsets[:, 0] << _X_SHIFT) + (offsets[:, 1] << _Y_SHIFT) + offsets[:, 2]) From 46eb1e72693e73b84e938e09fa57c68e471ca8de Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 4 Aug 2026 13:07:33 -0700 Subject: [PATCH 28/29] More clean up --- dimos/mapping/voxels/impl/packed.py | 5 +- dimos/mapping/voxels/keys.py | 48 +++ dimos/navigation/nav_3d/evaluator/cases.py | 51 ++- dimos/navigation/nav_3d/evaluator/cli.py | 82 ++--- dimos/navigation/nav_3d/evaluator/config.py | 29 +- dimos/navigation/nav_3d/evaluator/curation.py | 19 +- .../navigation/nav_3d/evaluator/final_map.py | 42 +-- dimos/navigation/nav_3d/evaluator/generate.py | 209 +++++++---- dimos/navigation/nav_3d/evaluator/metrics.py | 22 +- dimos/navigation/nav_3d/evaluator/picker.py | 253 +++++++------ dimos/navigation/nav_3d/evaluator/pipeline.py | 7 +- .../navigation/nav_3d/evaluator/recording.py | 3 +- dimos/navigation/nav_3d/evaluator/runner.py | 98 ++--- dimos/navigation/nav_3d/evaluator/tagging.py | 15 +- .../{test_evaluator.py => test_nav_eval.py} | 342 +++++++++++------- dimos/navigation/nav_3d/evaluator/viz.py | 125 ++++--- .../navigation/nav_3d/evaluator/voxel_keys.py | 43 +-- dimos/navigation/nav_3d/mls_planner/viz.py | 17 +- 18 files changed, 799 insertions(+), 611 deletions(-) create mode 100644 dimos/mapping/voxels/keys.py rename dimos/navigation/nav_3d/evaluator/{test_evaluator.py => test_nav_eval.py} (78%) diff --git a/dimos/mapping/voxels/impl/packed.py b/dimos/mapping/voxels/impl/packed.py index cc0e9bf4bd..e84580da37 100644 --- a/dimos/mapping/voxels/impl/packed.py +++ b/dimos/mapping/voxels/impl/packed.py @@ -16,12 +16,9 @@ import numpy as np +from dimos.mapping.voxels.keys import FIELD_BITS as _BITS, FIELD_MASK as _MASK, KEY_OFFSET as _BIAS from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -_BITS = 21 -_BIAS = 1 << (_BITS - 1) # voxel coords in [-2^20, 2^20): +-52 km at 5 cm voxels -_MASK = (1 << _BITS) - 1 - class PackedVoxels: """CPU voxel store: sorted int64 keys, 21 bits/axis, (x,y) in the high bits. diff --git a/dimos/mapping/voxels/keys.py b/dimos/mapping/voxels/keys.py new file mode 100644 index 0000000000..48cbc0f220 --- /dev/null +++ b/dimos/mapping/voxels/keys.py @@ -0,0 +1,48 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The int64 voxel key layout shared by every packed voxel store. + +Three 21-bit signed-biased indices, x in the high bits, so keys sort in the +same order as (x, y, z) and membership is a sorted-array search. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np + +if TYPE_CHECKING: + from numpy.typing import NDArray + +FIELD_BITS = 21 +# Voxel coords in [-2^20, 2^20): +-52 km at 5 cm voxels. +KEY_OFFSET = 1 << (FIELD_BITS - 1) +X_SHIFT = 2 * FIELD_BITS +Y_SHIFT = FIELD_BITS +FIELD_MASK = (1 << FIELD_BITS) - 1 + + +def pack_indices(idx: NDArray[np.int64]) -> NDArray[np.int64]: + """Pack biased (N, 3) integer voxel indices into sortable int64 keys.""" + return (idx[:, 0] << X_SHIFT) | (idx[:, 1] << Y_SHIFT) | idx[:, 2] + + +def unpack_keys(keys: NDArray[np.int64]) -> NDArray[np.int64]: + """Unbiased (N, 3) integer voxel indices, the inverse of pack_indices.""" + return ( + np.stack([keys >> X_SHIFT, (keys >> Y_SHIFT) & FIELD_MASK, keys & FIELD_MASK], axis=1) + - KEY_OFFSET + ) diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index 0974e1aad6..f87d2ca9a8 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -19,13 +19,21 @@ from dataclasses import dataclass, field from pathlib import Path - -import yaml +from typing import TYPE_CHECKING from dimos.utils.data import resolve_named_path +if TYPE_CHECKING: + from collections.abc import Iterator + + from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory + MANIFEST_DIR = Path(__file__).parent / "case_manifests" +# Default mem2 streams a recording is read from. +LIDAR_STREAM = "pointlio_lidar" +ODOM_STREAM = "pointlio_odometry" + def manifest_path(dataset: str) -> Path: """Where a dataset's case manifest lives.""" @@ -49,8 +57,8 @@ class Case: class Suite: dataset: str cases: list[Case] - lidar_stream: str = "pointlio_lidar" - odom_stream: str = "pointlio_odometry" + lidar_stream: str = LIDAR_STREAM + odom_stream: str = ODOM_STREAM # Recording location override, defaulting to data/.db. # Set this to keep a recording outside data/, in case you don't want it to be tracked. db: str | None = None @@ -67,8 +75,26 @@ def end_ts_seconds(self) -> float | None: """end_ts in the recording's second-based observation timestamps.""" return None if self.end_ts is None else self.end_ts / 1e9 + def trajectory(self) -> Trajectory: + """The walked path this suite's cases are scored against.""" + from dimos.navigation.nav_3d.evaluator.recording import load_trajectory + + return load_trajectory(self.db_path(), self.odom_stream, self.end_ts_seconds()) + + def world_frames(self, align_tol: float) -> Iterator[Frame]: + """Lidar frames registered into the world by their odometry pose.""" + from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames + + return iter_world_frames( + self.db_path(), self.lidar_stream, self.odom_stream, align_tol, self.end_ts_seconds() + ) + def load_suite(path: Path) -> Suite: + # Lazy: pyyaml is not in the base install, and mounting the CLI must not + # require it until a manifest is actually read. + import yaml + raw = yaml.safe_load(path.read_text()) if not isinstance(raw, dict) or "dataset" not in raw or "cases" not in raw: raise ValueError(f"{path}: suite needs 'dataset' and 'cases' keys") @@ -100,8 +126,8 @@ def load_suite(path: Path) -> Suite: return Suite( dataset=str(raw["dataset"]), cases=cases, - lidar_stream=str(raw.get("lidar_stream", "pointlio_lidar")), - odom_stream=str(raw.get("odom_stream", "pointlio_odometry")), + lidar_stream=str(raw.get("lidar_stream", LIDAR_STREAM)), + odom_stream=str(raw.get("odom_stream", ODOM_STREAM)), db=str(raw["db"]) if "db" in raw else None, end_ts=int(raw["end_ts"]) if "end_ts" in raw else None, path=path, @@ -109,7 +135,7 @@ def load_suite(path: Path) -> Suite: def load_suites(paths: list[Path] | None = None) -> list[Suite]: - """Load the given manifests, or every manifest under cases/.""" + """Load the given manifests, or every manifest under case_manifests/.""" if paths is None: paths = sorted(MANIFEST_DIR.glob("*.yaml")) if not paths: @@ -117,15 +143,16 @@ def load_suites(paths: list[Path] | None = None) -> list[Suite]: return [load_suite(p) for p in paths] -def save_suite(suite: Suite, path: Path | None = None) -> Path: - """Write the suite manifest as YAML. Defaults to cases/.yaml.""" - path = path or suite.path or manifest_path(suite.dataset) +def save_suite(suite: Suite, path: Path) -> Path: + """Write the suite manifest as YAML.""" + import yaml + doc: dict[str, object] = {"dataset": suite.dataset} if suite.db is not None: doc["db"] = suite.db - if suite.lidar_stream != "pointlio_lidar": + if suite.lidar_stream != LIDAR_STREAM: doc["lidar_stream"] = suite.lidar_stream - if suite.odom_stream != "pointlio_odometry": + if suite.odom_stream != ODOM_STREAM: doc["odom_stream"] = suite.odom_stream if suite.end_ts is not None: doc["end_ts"] = suite.end_ts diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py index 6dcca968de..9bae452402 100644 --- a/dimos/navigation/nav_3d/evaluator/cli.py +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Nav-3d evaluation CLI, mounted as `dimos nav-eval`.""" +"""Nav-3d evaluation CLI, mounted as the dimos nav-eval sub-command.""" from __future__ import annotations @@ -42,7 +42,6 @@ resolve_max_cases, ) from dimos.navigation.nav_3d.evaluator.metrics import ground_truth_route -from dimos.navigation.nav_3d.evaluator.recording import load_trajectory from dimos.navigation.nav_3d.evaluator.runner import Report, evaluate from dimos.navigation.nav_3d.evaluator.tagging import GEOMETRIC_TAGS, route_tags from dimos.utils.data import get_data_dir @@ -68,9 +67,15 @@ def _apply_overrides(cfg: EvalConfig, overrides: list[str]) -> EvalConfig: if name not in fields: raise typer.BadParameter(f"unknown config field {name!r}") current = getattr(cfg, name) - if not isinstance(current, (bool, int, float, str)): - raise typer.BadParameter(f"{name!r} is not a scalar field; set its members instead") + # bool is left out on purpose: bool("false") is True, so a boolean + # override would silently mean the opposite of what was asked. + if not isinstance(current, (int, float, str)) or isinstance(current, bool): + raise typer.BadParameter(f"{name!r} cannot be set from the command line") setattr(cfg, name, type(current)(value)) + try: + cfg.validate() + except ValueError as err: + raise typer.BadParameter(str(err)) from err return cfg @@ -139,21 +144,23 @@ def _print_report(report: Report) -> None: @app.command() def run( - manifests: list[Path] = typer.Argument( + manifests: list[Path] | None = typer.Argument( None, help="Suite YAMLs; defaults to every manifest under case_manifests/" ), - dataset: str = typer.Option(None, "--dataset", help="Only run suites for this dataset"), - tag: list[str] = typer.Option( + dataset: str | None = typer.Option(None, "--dataset", help="Only run suites for this dataset"), + tag: list[str] | None = typer.Option( None, "--tag", help="Only run cases carrying every given tag, e.g. --tag stairs --tag up" ), - json_out: Path = typer.Option(None, "--json", help="Write the full report as JSON"), - rrd_out: Path = typer.Option(None, "--rrd", help="Write a rerun recording of every case"), + json_out: Path | None = typer.Option(None, "--json", help="Write the full report as JSON"), + rrd_out: Path | None = typer.Option( + None, "--rrd", help="Write a rerun recording of every case" + ), workers: int = typer.Option( os.cpu_count() or 1, "--workers", help="Datasets evaluated in parallel processes", ), - set_: list[str] = typer.Option( + set_: list[str] | None = typer.Option( None, "--set", help="Repeatable EvalConfig override, e.g. goal_tolerance=0.4" ), ) -> None: @@ -241,7 +248,7 @@ def ingest( odom_stream=odom_stream, db=str(dest) if external else None, ) - trajectory = load_trajectory(dest, odom_stream) + trajectory = suite.trajectory() arcs = trajectory.arc_lengths() print( f"trajectory: {len(trajectory.positions)} poses, " @@ -249,7 +256,7 @@ def ingest( f"z [{trajectory.positions[:, 2].min():.2f}, {trajectory.positions[:, 2].max():.2f}]" ) cfg = EvalConfig() - final = load_or_build_final_map(dest, suite, cfg) + final = load_or_build_final_map(suite, cfg) max_cases = cases or None min_cases = cases or MIN_CASES surface = final.standable_surface(cfg.robot_height) @@ -269,7 +276,7 @@ def ingest( print(f"\nrun with: dimos nav-eval run --dataset {name}") -def _open(dataset: str) -> CaseStore: +def _open_store(dataset: str) -> CaseStore: try: return load_store(dataset) except CurationError as err: @@ -288,14 +295,14 @@ def add_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the case"), start: tuple[float, float, float] = typer.Option(..., "--start", help="Foot-level xyz"), goal: tuple[float, float, float] = typer.Option(..., "--goal", help="Foot-level xyz"), - case_id: str = typer.Option(None, "--id", help="Case id; default manual_ or neg_"), - tags: str = typer.Option(None, "--tags", help="Comma-separated tags"), + case_id: str | None = typer.Option(None, "--id", help="Case id; default manual_ or neg_"), + tags: str | None = typer.Option(None, "--tags", help="Comma-separated tags"), expect_fail: bool = typer.Option( False, "--expect-fail", help="Certified-infeasible pair; the planner must refuse" ), ) -> None: """Append a curated case, with endpoints snapped to the final surface.""" - store = _open(dataset) + store = _open_store(dataset) try: store.add( start, @@ -318,12 +325,7 @@ def tag_case( help="Mark a dynamic-obstacle route: online path expected, final path expected refused", ), ) -> None: - """Flag an auto case as a dynamic-obstacle route, e.g. a door that closed. - - The online plan is still scored normally, the final plan is scored 1.0 - for refusing. Use it on a case that shows up as incremental-only because a - real obstacle blocked the route by the final map, not a planner bug. - """ + """Flag an auto case as a dynamic-obstacle route, e.g. a door that closed.""" suite, manifest = _load_manifest(dataset) case = next((c for c in suite.cases if c.id == case_id), None) if case is None: @@ -344,17 +346,11 @@ def tag_case( def retag( dataset: str = typer.Argument(..., help="Dataset whose manifest gets retagged"), ) -> None: - """Recompute geometric tags for auto-generated cases from the final map. - - Only the geometric tags (flat, stairs, narrow, switchback, and the rest) - are replaced, so improving the tagger never churns start/goal pairs. The - auto provenance tag survives. Manually curated cases are left untouched: - their tags are human intent, not something to recompute. - """ + """Recompute geometric tags for auto cases. Curated cases keep their tags.""" suite, manifest = _load_manifest(dataset) cfg = EvalConfig() - final = load_or_build_final_map(suite.db_path(), suite, cfg) - trajectory = load_trajectory(suite.db_path(), suite.odom_stream, suite.end_ts_seconds()) + final = load_or_build_final_map(suite, cfg) + trajectory = suite.trajectory() changed = 0 for case in suite.cases: if "auto" not in case.tags: @@ -379,30 +375,14 @@ def retag( def pick_case( dataset: str = typer.Argument(..., help="Dataset whose manifest gets the cases"), ) -> None: - """Pick and edit cases by shift+clicking the map in a browser viewer. - - Serves the final map, the walked path, and every case already in the - manifest as an editable panel entry. Shift+click picks new start/goal - pairs. Any case can be renamed, retagged, flipped negative, or deleted. - New pairs save to the manifest snapped like add-case. - """ + """Pick and edit cases by shift+clicking the map in a browser viewer.""" # Lazy: picker/viz pull in viser and matplotlib, only needed for pick-case. from dimos.navigation.nav_3d.evaluator.picker import pick_cases - from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height - store = _open(dataset) - trajectory = load_trajectory( - store.suite.db_path(), store.suite.odom_stream, store.suite.end_ts_seconds() - ) + store = _open_store(dataset) + trajectory = store.suite.trajectory() foot = trajectory.foot(store.cfg.robot_height) - pick_cases( - dataset, - store.final.occupied, - turbo_by_height(store.final.occupied), - store.final.voxel_size, - foot, - store, - ) + pick_cases(store, foot) print(f"\nrun with: dimos nav-eval run --dataset {dataset}") diff --git a/dimos/navigation/nav_3d/evaluator/config.py b/dimos/navigation/nav_3d/evaluator/config.py index 5cb9a0bd5c..e38b8c9cca 100644 --- a/dimos/navigation/nav_3d/evaluator/config.py +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -23,10 +23,7 @@ @dataclass class EvalConfig: - """Harness and gate parameters, sized for the Unitree Go2. - - Algorithm tuning lives in the algorithm packages, not here. - """ + """Harness and gate parameters, sized for the Unitree Go2.""" voxel_size: float = 0.08 max_range: float = 30.0 @@ -55,6 +52,23 @@ class EvalConfig: # Pipeline constructor overrides, e.g. --set planner.wall_clearance_m=0.0. planner: dict[str, float] = field(default_factory=dict) + def __post_init__(self) -> None: + self.validate() + + def validate(self) -> None: + """Check the invariants the gates rely on. Called again after --set, + which mutates a config that was already constructed.""" + # An inverted band makes check_path admit nothing and pass every path, + # which reads as a perfect score rather than a failure. + if self.body_clearance <= self.ground_margin: + raise ValueError( + f"body_clearance ({self.body_clearance}) must exceed " + f"ground_margin ({self.ground_margin})" + ) + for name in ("voxel_size", "robot_length", "robot_width", "max_range"): + if getattr(self, name) <= 0: + raise ValueError(f"{name} must be positive, got {getattr(self, name)}") + def make_mapper(self) -> VoxelRayMapper: """The mapper that builds the occupancy every pipeline is graded against.""" # Lazy: the mapper is a native module, only needed to build a map. @@ -63,9 +77,6 @@ def make_mapper(self) -> VoxelRayMapper: return VoxelRayMapper(voxel_size=self.voxel_size, max_range=self.max_range) def mapper_fingerprint(self) -> dict[str, float | int]: - """Cache key parameters for the final map. - - Mapper internals are not fingerprinted, so a mapper change needs - dimos cache clean rather than a new key here. - """ + """Cache key parameters for the final map. Mapper internals are not + fingerprinted, so a mapper change needs dimos cache clean.""" return {"voxel_size": self.voxel_size, "max_range": self.max_range} diff --git a/dimos/navigation/nav_3d/evaluator/curation.py b/dimos/navigation/nav_3d/evaluator/curation.py index d4f17ca124..ba4d2ffb90 100644 --- a/dimos/navigation/nav_3d/evaluator/curation.py +++ b/dimos/navigation/nav_3d/evaluator/curation.py @@ -59,11 +59,16 @@ class CaseStore: """Mutable view of one dataset's manifest, saved after every change.""" suite: Suite - manifest: Path surface: NDArray[np.float32] cfg: EvalConfig final: FinalMap + @property + def path(self) -> Path: + """The manifest this store saves to, set when the suite was loaded.""" + assert self.suite.path is not None, "a curated suite is always loaded from a file" + return self.suite.path + def _snap(self, label: str, point: Point, *, required: bool = True) -> Point: snapped = snap_to_surface( np.asarray(point, dtype=np.float32), self.surface, self.cfg.snap_max_m @@ -102,7 +107,7 @@ def add( expect_fail=expect_fail, ) if any(c.id == case.id for c in self.suite.cases): - raise CurationError(f"case id {case.id!r} already exists in {self.manifest}") + raise CurationError(f"case id {case.id!r} already exists in {self.path}") self.suite.cases.append(case) self.save() kind = "negative (must refuse)" if expect_fail else "positive" @@ -112,7 +117,7 @@ def add( case=case.id, start=case.start, goal=case.goal, - manifest=self.manifest, + manifest=self.path, ) return case @@ -135,11 +140,11 @@ def delete(self, case_id: str) -> None: def get(self, case_id: str) -> Case: case = next((c for c in self.suite.cases if c.id == case_id), None) if case is None: - raise CurationError(f"case {case_id!r} not found in {self.manifest}") + raise CurationError(f"case {case_id!r} not found in {self.path}") return case def save(self) -> None: - save_suite(self.suite, self.manifest) + save_suite(self.suite, self.path) PROVENANCE_TAGS = ("auto", "manual") @@ -159,5 +164,5 @@ def load_store(dataset: str) -> CaseStore: raise CurationError(f"no manifest {manifest}; run ingest first") suite = load_suite(manifest) cfg = EvalConfig() - final = load_or_build_final_map(suite.db_path(), suite, cfg) - return CaseStore(suite, manifest, final.standable_surface(cfg.robot_height), cfg, final) + final = load_or_build_final_map(suite, cfg) + return CaseStore(suite, final.standable_surface(cfg.robot_height), cfg, final) diff --git a/dimos/navigation/nav_3d/evaluator/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py index 97177ea356..094a27194b 100644 --- a/dimos/navigation/nav_3d/evaluator/final_map.py +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -12,10 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""The evaluator's own map of a recording, and the checkpoints along it. - -Not ground truth, just the most complete occupancy the mapper produces. -""" +"""The evaluator's own map of a recording, and the checkpoints along it.""" from __future__ import annotations @@ -29,8 +26,6 @@ import numpy as np from dimos.constants import CACHE_DIR -from dimos.navigation.nav_3d.evaluator.metrics import timing_stats -from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers, keys_contain, voxel_keys from dimos.utils.logging_config import setup_logger @@ -53,8 +48,6 @@ class FinalMap: voxel_size: float occupied: NDArray[np.float32] occupied_keys: NDArray[np.int64] - frames: int - add_frame_ms: dict[str, float] build_ms: float def standable_surface(self, robot_height: float) -> NDArray[np.float32]: @@ -84,8 +77,8 @@ def iter_snapshots(self) -> Iterator[NDArray[np.int64]]: yield keys -CACHE_VERSION = 3 -CHECKPOINT_CACHE_VERSION = 3 +CACHE_VERSION = 4 +CHECKPOINT_CACHE_VERSION = 4 def _save_npz(cache: Path, **arrays: Any) -> None: @@ -137,14 +130,11 @@ def replay_frames( """Feed frames through the mapper in order, snapshotting at each requested time. A snapshot holds exactly the frames with ts <= its time.""" snapshots: list[NDArray[np.int64]] = [] - add_ms: list[float] = [] t0 = perf_counter() for frame in frames: while len(snapshots) < len(times) and frame.ts > times[len(snapshots)]: snapshots.append(np.unique(voxel_keys(mapper.global_map(), voxel_size))) - t1 = perf_counter() mapper.add_frame(frame.points, frame.origin) - add_ms.append((perf_counter() - t1) * 1000) build_ms = (perf_counter() - t0) * 1000 occupied = mapper.global_map() occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) @@ -154,8 +144,6 @@ def replay_frames( voxel_size=voxel_size, occupied=occupied, occupied_keys=occupied_keys, - frames=len(add_ms), - add_frame_ms=timing_stats(add_ms), build_ms=build_ms, ) return final, snapshots @@ -166,13 +154,12 @@ def _save_final(cache: Path, final: FinalMap) -> None: cache, occupied=final.occupied, occupied_keys=final.occupied_keys, - frames=final.frames, - **{f"add_{k}": v for k, v in final.add_frame_ms.items()}, ) logger.info("final map cached", cache=cache.name, voxels=len(final.occupied)) -def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> FinalMap: +def load_or_build_final_map(suite: Suite, cfg: EvalConfig) -> FinalMap: + db_path = suite.db_path() cache = _cache_path(db_path, _final_params(db_path, suite, cfg)) if cache.exists(): data = np.load(cache) @@ -180,16 +167,12 @@ def load_or_build_final_map(db_path: Path, suite: Suite, cfg: EvalConfig) -> Fin voxel_size=cfg.voxel_size, occupied=data["occupied"], occupied_keys=data["occupied_keys"], - frames=int(data["frames"]), - add_frame_ms={k: float(data[f"add_{k}"]) for k in ("p50", "p95", "max")}, build_ms=0.0, ) logger.info("building final map (cache miss)", recording=db_path.name) final, _ = replay_frames( - iter_world_frames( - db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() - ), + suite.world_frames(cfg.align_tol), cfg.make_mapper(), cfg.voxel_size, np.array([], dtype=np.float64), @@ -212,12 +195,11 @@ def encode_deltas( def load_or_build_checkpoints( - db_path: Path, suite: Suite, cfg: EvalConfig, times: NDArray[np.float64] + suite: Suite, cfg: EvalConfig, times: NDArray[np.float64] ) -> MapCheckpoints: - """Occupied key sets at the requested times, deduped and sorted. - - A cache miss replays the recording once and fills the final cache too. - """ + """Occupied key sets at the requested times, deduped and sorted. A cache + miss replays the recording once and fills the final cache too.""" + db_path = suite.db_path() times = np.unique(np.asarray(times, dtype=np.float64)) params: dict[str, float | int | str] = { **_final_params(db_path, suite, cfg), @@ -237,9 +219,7 @@ def load_or_build_checkpoints( logger.info("building map checkpoints (cache miss)", n=len(times), recording=db_path.name) final, snapshots = replay_frames( - iter_world_frames( - db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() - ), + suite.world_frames(cfg.align_tol), cfg.make_mapper(), cfg.voxel_size, times, diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 55e91b85dd..11cedaf4d6 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -12,11 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Generate evaluation cases from a recorded trajectory. - -Endpoint pairs come off the walked path, so both are proven reachable, and are -kept only when non-trivial and causal. Deterministic. -""" +"""Generate evaluation cases from a recorded trajectory. Endpoint pairs come off +the walked path, so both are proven reachable.""" from __future__ import annotations @@ -62,16 +59,28 @@ # A sector may anchor at most this many selected cases, which prevents a # single high-priority spot from becoming the hub of every case. ENDPOINT_REUSE_MAX = 2 +# Endpoint spread stops earning score past this distance, and is worth this +# much next to a candidate's own priority. +SPREAD_CAP_M = 2.0 * SECTOR_SIZE_M +SPREAD_WEIGHT = 0.4 +# Snapping prefers a horizontally near cell. A cell further in z is still +# eligible up to MAX_SNAP_DZ_M, at this much penalty per meter. +MAX_SNAP_DZ_M = 1.0 +SNAP_Z_PENALTY = 0.5 # Floor on the case count. When strict selection falls short, a relaxed pass # ignores the sector caps and the flat quota to reach it. MIN_CASES = 10 +# An unpinned case count scales with the walk, between these bounds. +METERS_PER_CASE = 25.0 +MIN_AUTO_CASES = 16 +MAX_AUTO_CASES = 48 def resolve_max_cases(max_cases: int | None, walked_total_m: float) -> int: """Case count, scaled with the walked distance when not pinned.""" if max_cases is not None: return max_cases - return int(np.clip(walked_total_m / 25.0, 16, 48)) + return int(np.clip(walked_total_m / METERS_PER_CASE, MIN_AUTO_CASES, MAX_AUTO_CASES)) @dataclass @@ -102,9 +111,11 @@ def snap_to_surface( return None hd = np.linalg.norm(surface[:, :2] - point[:2], axis=1) zd = np.abs(surface[:, 2] - point[2]) - score = hd + np.where(zd < 1.0, zd * 0.5, np.inf) + # Reachability filters the candidates. Scoring first would let an unreachable + # cell with no z offset beat a reachable one that carries the z penalty. + score = np.where((hd <= snap_max_m) & (zd < MAX_SNAP_DZ_M), hd + zd * SNAP_Z_PENALTY, np.inf) best = int(score.argmin()) - if not np.isfinite(score[best]) or hd[best] > snap_max_m: + if not np.isfinite(score[best]): return None return np.asarray(surface[best], dtype=np.float32) @@ -115,27 +126,34 @@ def _subsample_indices(trajectory: Trajectory, spacing_m: float) -> NDArray[np.i return np.unique(np.searchsorted(arcs, targets)) -def generate_cases( - trajectory: Trajectory, - final: FinalMap, - surface: NDArray[np.float32], - cfg: EvalConfig, - max_cases: int | None = None, - min_cases: int = MIN_CASES, -) -> list[Case]: - map_keys = final.occupied_keys - arcs = trajectory.arc_lengths() +def _waypoint_snaps( + trajectory: Trajectory, surface: NDArray[np.float32], cfg: EvalConfig +) -> tuple[NDArray[np.int64], NDArray[np.float32], NDArray[np.bool_]]: + """Trajectory waypoints snapped onto the standable surface, with a mask of + the ones that landed.""" foot = trajectory.foot(cfg.robot_height) - idx = _subsample_indices(trajectory, WAYPOINT_SPACING_M) snaps = np.full((len(idx), 3), np.nan, dtype=np.float32) for n, i in enumerate(idx): hit = snap_to_surface(foot[i], surface, cfg.snap_max_m) if hit is not None: snaps[n] = hit - ok = np.isfinite(snaps[:, 0]) - way_arcs = arcs[idx] + return idx, snaps, np.isfinite(snaps[:, 0]) + +def _candidates( + trajectory: Trajectory, + idx: NDArray[np.int64], + snaps: NDArray[np.float32], + ok: NDArray[np.bool_], + arcs: NDArray[np.float64], + map_keys: NDArray[np.int64], + cfg: EvalConfig, +) -> dict[tuple[int, ...], Candidate]: + """The best candidate pair per spatial bin, over every ordered waypoint pair + the walk demonstrates.""" + foot = trajectory.foot(cfg.robot_height) + way_arcs = arcs[idx] candidates: dict[tuple[int, ...], Candidate] = {} for ai in range(len(idx)): if not ok[ai]: @@ -191,7 +209,23 @@ def generate_cases( best = candidates.get(key) if best is None or cand.priority > best.priority: candidates[key] = cand + return candidates + +def generate_cases( + trajectory: Trajectory, + final: FinalMap, + surface: NDArray[np.float32], + cfg: EvalConfig, + max_cases: int | None = None, + min_cases: int = MIN_CASES, +) -> list[Case]: + """Snap the walk onto the surface, pair up waypoints, and keep a diverse + spread of the demonstrated routes.""" + map_keys = final.occupied_keys + arcs = trajectory.arc_lengths() + idx, snaps, ok = _waypoint_snaps(trajectory, surface, cfg) + candidates = _candidates(trajectory, idx, snaps, ok, arcs, map_keys, cfg) ranked = sorted(candidates.values(), key=lambda c: (-c.priority, c.start, c.goal)) selected = _select_diverse(ranked, resolve_max_cases(max_cases, float(arcs[-1])), min_cases) cases = [] @@ -218,76 +252,99 @@ def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> return False -def _select_diverse( - ranked: list[Candidate], max_cases: int, min_cases: int = MIN_CASES -) -> list[Candidate]: - """Spread-greedy selection under a sector cap and flat quota, with a - relaxed pass to reach min_cases.""" - if not ranked: - return [] - flat_target = int(max_cases * FLAT_FRACTION) - stairs_cap = max_cases - flat_target +class _DiverseSelector: + """Spread-greedy selection state: availability, sector usage, and distance + to the nearest already-selected endpoint.""" + + def __init__(self, ranked: list[Candidate], max_cases: int) -> None: + self.ranked = ranked + self.flat_target = int(max_cases * FLAT_FRACTION) + self.stairs_cap = max_cases - self.flat_target + self.starts = np.array([c.start for c in ranked], dtype=np.float32) + self.goals = np.array([c.goal for c in ranked], dtype=np.float32) + self.priorities = np.array([c.priority for c in ranked], dtype=np.float32) + self.is_stairs = np.array([abs(c.dz) >= STAIRS_DZ_M for c in ranked]) + self.alive = np.ones(len(ranked), dtype=bool) + self.usage: dict[tuple[int, ...], int] = {} + self.sector_capped: list[int] = [] + self.stairs: list[Candidate] = [] + self.flats: list[Candidate] = [] + # Running minima over the selected endpoints. Seeded to infinity so the + # first pick sees the uniform spread an empty selection gives. + self.d_start = np.full(len(ranked), np.inf, dtype=np.float32) + self.d_goal = np.full(len(ranked), np.inf, dtype=np.float32) - starts = np.array([c.start for c in ranked], dtype=np.float32) - goals = np.array([c.goal for c in ranked], dtype=np.float32) - priorities = np.array([c.priority for c in ranked], dtype=np.float32) - is_stairs = np.array([abs(c.dz) >= STAIRS_DZ_M for c in ranked]) - spread_cap = 2.0 * SECTOR_SIZE_M + @property + def selected(self) -> list[Candidate]: + return self.stairs + self.flats - def sector(p: NDArray[np.float32]) -> tuple[int, ...]: + def _sector(self, p: NDArray[np.float32]) -> tuple[int, ...]: return ( int(np.floor(p[0] / SECTOR_SIZE_M)), int(np.floor(p[1] / SECTOR_SIZE_M)), round(float(p[2]) / SECTOR_Z_M), ) - usage: dict[tuple[int, ...], int] = {} - used_points: list[NDArray[np.float32]] = [] - alive = np.ones(len(ranked), dtype=bool) - sector_capped: list[int] = [] - stairs: list[Candidate] = [] - flats: list[Candidate] = [] - - def fill(target: int, relax: bool) -> None: - while alive.any() and len(stairs) + len(flats) < target: - if used_points: - used = np.stack(used_points) - d_start = np.linalg.norm(starts[:, None] - used[None], axis=2).min(axis=1) - d_goal = np.linalg.norm(goals[:, None] - used[None], axis=2).min(axis=1) - spread = np.minimum(d_start, spread_cap) + np.minimum(d_goal, spread_cap) - else: - spread = np.full(len(ranked), 2.0 * spread_cap, dtype=np.float32) - score = priorities + 0.4 * spread - score[~alive] = -np.inf - if not relax and len(stairs) >= stairs_cap: - score[is_stairs] = -np.inf + def _accept(self, n: int) -> None: + """Record a selection and fold its endpoints into the running minima.""" + for point in (self.starts[n], self.goals[n]): + sector = self._sector(point) + self.usage[sector] = self.usage.get(sector, 0) + 1 + np.minimum(self.d_start, _distance_to(self.starts, point), out=self.d_start) + np.minimum(self.d_goal, _distance_to(self.goals, point), out=self.d_goal) + bucket = self.stairs if self.is_stairs[n] else self.flats + bucket.append(self.ranked[n]) + + def _scores(self, relax: bool) -> NDArray[np.float32]: + spread = np.minimum(self.d_start, SPREAD_CAP_M) + np.minimum(self.d_goal, SPREAD_CAP_M) + score = self.priorities + SPREAD_WEIGHT * spread + score[~self.alive] = -np.inf + if not relax and len(self.stairs) >= self.stairs_cap: + score[self.is_stairs] = -np.inf + return np.asarray(score, dtype=np.float32) + + def fill(self, target: int, relax: bool) -> None: + """Take the highest-scoring live candidate until target is reached.""" + while self.alive.any() and len(self.selected) < target: + score = self._scores(relax) if not np.isfinite(score).any(): break n = int(score.argmax()) - alive[n] = False - cand = ranked[n] - sa, sb = sector(starts[n]), sector(goals[n]) + self.alive[n] = False + sa, sb = self._sector(self.starts[n]), self._sector(self.goals[n]) if not relax and ( - usage.get(sa, 0) >= ENDPOINT_REUSE_MAX or usage.get(sb, 0) >= ENDPOINT_REUSE_MAX + self.usage.get(sa, 0) >= ENDPOINT_REUSE_MAX + or self.usage.get(sb, 0) >= ENDPOINT_REUSE_MAX ): - sector_capped.append(n) + self.sector_capped.append(n) continue - bucket = stairs if is_stairs[n] else flats - if _is_duplicate(cand, bucket, DEDUPE_RADIUS_M): + bucket = self.stairs if self.is_stairs[n] else self.flats + if _is_duplicate(self.ranked[n], bucket, DEDUPE_RADIUS_M): continue - usage[sa] = usage.get(sa, 0) + 1 - usage[sb] = usage.get(sb, 0) + 1 - used_points.append(starts[n]) - used_points.append(goals[n]) - bucket.append(cand) - - fill(max_cases, relax=False) - min_cases = min(min_cases, max_cases) - if len(stairs) + len(flats) < min_cases: - alive[sector_capped] = True - fill(min_cases, relax=True) - - return (stairs + flats)[:max_cases] + self._accept(n) + + def revive_sector_capped(self) -> None: + self.alive[self.sector_capped] = True + + +def _distance_to(points: NDArray[np.float32], target: NDArray[np.float32]) -> NDArray[np.float32]: + dist: NDArray[np.float32] = np.linalg.norm(points - target, axis=1) + return dist + + +def _select_diverse( + ranked: list[Candidate], max_cases: int, min_cases: int = MIN_CASES +) -> list[Candidate]: + """Spread-greedy selection under a sector cap and flat quota, with a + relaxed pass to reach min_cases.""" + if not ranked: + return [] + selector = _DiverseSelector(ranked, max_cases) + selector.fill(max_cases, relax=False) + if len(selector.selected) < min(min_cases, max_cases): + selector.revive_sector_capped() + selector.fill(min(min_cases, max_cases), relax=True) + return selector.selected[:max_cases] def _to_case(cand: Candidate, n: int, tags: list[str]) -> Case: diff --git a/dimos/navigation/nav_3d/evaluator/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py index 0f8ca2eccb..64f7f22222 100644 --- a/dimos/navigation/nav_3d/evaluator/metrics.py +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -82,8 +82,8 @@ class GateResult: valid: bool collision_points: NDArray[np.float32] - # Indices of the colliding samples in densify(waypoints, voxel_size / 2), - # so a viewer can recover the exact body frames the gate tested. + # Indices of the colliding samples along the densified path, so a viewer + # can recover the exact body frames the gate tested. collision_indices: NDArray[np.int64] # Distance from the body surface to the nearest occupied voxel in the band, # minimized along the path. Negative is penetration, capped at MARGIN_CAP_M. @@ -131,10 +131,8 @@ def body_frames( def check_path( waypoints: NDArray[np.float32], map_keys: NDArray[np.int64], cfg: EvalConfig ) -> GateResult: - """Sweep the robot body box along foot-level waypoints against the map. - - The box sits mid-band up the tilted body axis, so legs and ground never count. - """ + """Sweep the robot body box along foot-level waypoints against the map. The + box sits mid-band up the tilted body axis, so legs and ground never count.""" voxel_size = cfg.voxel_size samples = densify(waypoints, voxel_size / 2) fwd, lateral, up = body_frames(samples, cfg.robot_length) @@ -149,10 +147,12 @@ def check_path( sin_p = float(np.abs(fwd[:, 2]).max()) cos_p = float(np.sqrt(max(1.0 - sin_p * sin_p, 0.0))) reach_z = (half_len + pad) * sin_p + half_band * cos_p + voxel_size + # Union with the level window: a steep segment lowers the pitched window, + # which would otherwise let a flat stretch of the same path hide a collision. offsets = cylinder_offsets( float(np.hypot(half_len, half_wid)) + pad + (mid_h + half_band) * sin_p, - mid_h * cos_p - reach_z, - mid_h * cos_p + reach_z, + min(mid_h * cos_p - reach_z, mid_h - half_band - voxel_size), + max(mid_h * cos_p + reach_z, mid_h + half_band + voxel_size), voxel_size, ) # Samples land two per voxel, so membership runs over the distinct voxels @@ -314,10 +314,8 @@ def reference_length( cfg: EvalConfig, ) -> Reference: """Shortest walked length demonstrated between start and goal, minimized - over every visit pairing and preferring causal ones. - - Falls back to straight-line distance when an endpoint is off the trajectory. - """ + over every visit pairing and preferring causal ones. Falls back to + straight-line distance when an endpoint is off the trajectory.""" visits = _visits(trajectory, start, goal, cfg) if visits is None: straight = float(np.linalg.norm(np.asarray(goal) - np.asarray(start))) diff --git a/dimos/navigation/nav_3d/evaluator/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py index c5b36993dc..dd9c9d0310 100644 --- a/dimos/navigation/nav_3d/evaluator/picker.py +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -12,11 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Browser point-picking for case curation, served by viser. - -Serves the final map, the walked path, and every existing case as an editable -panel entry. Shift+click picks new start/goal pairs. -""" +"""Browser point-picking for case curation, served by viser.""" from __future__ import annotations @@ -29,6 +25,7 @@ from dimos.core.global_config import global_config from dimos.navigation.nav_3d.evaluator.curation import CurationError from dimos.navigation.nav_3d.evaluator.tagging import elevation_tags +from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height if TYPE_CHECKING: from collections.abc import Callable @@ -56,6 +53,11 @@ NEW_GOAL_COLOR = (255, 0, 200) NEW_PAIR_COLOR = (0, 255, 128) NEW_LINE_WIDTH = 5.0 +# Cube meshes are drawn slightly inside their voxel so neighbors show a seam. +CUBE_SHRINK = 0.42 +BACKGROUND_LEVEL = 14 +# Opening camera placement, as a multiple of the map's horizontal span. +CAMERA_OFFSET = (0.6, 0.6, 0.45) SUGGESTED_TAGS = ("stairs", "flat", "up", "down", "long", "doorway") INSTRUCTIONS = """**shift+click** picks START then GOAL, repeated per case. @@ -88,11 +90,8 @@ def _prelit_albedo(srgb: NDArray[np.uint8]) -> NDArray[np.float64]: - """Linear albedo that tone-maps back to the wanted sRGB color when lit. - - Cancels the viewer's ACES pass, which would otherwise desaturate the - height colormap on lit meshes. - """ + """Linear albedo that tone-maps back to the wanted sRGB color when lit, + cancelling the viewer's ACES pass.""" c = srgb.astype(np.float64) / 255.0 lin = np.where(c <= 0.04045, c / 12.92, ((c + 0.055) / 1.055) ** 2.4) t = np.clip(lin @ np.linalg.inv(_ACES_OUTPUT).T, 0.0, 0.99) @@ -128,11 +127,8 @@ def pick_along_ray( direction: NDArray[np.float64], radius: float, ) -> NDArray[np.float32] | None: - """Nearest cloud point inside a tube of the given radius around the click ray. - - A fixed perpendicular radius, not a cone: a cone widens with distance and - would pick a far voxel over the near one the click landed on. - """ + """Nearest cloud point inside a tube of the given radius around the click + ray. A cone would widen with distance and pick a far voxel over the near one.""" rel = points.astype(np.float64) - origin t = rel @ direction ahead = t > 0.05 @@ -359,23 +355,16 @@ def save_or_update(self) -> bool: return True -def pick_cases( - dataset: str, +def _add_map_scene( + server: viser.ViserServer, map_points: NDArray[np.float32], map_colors: NDArray[np.uint8], voxel_size: float, walked: NDArray[np.float32], - store: CaseStore, ) -> None: - """Serve the picker until the user exits from the panel or hits ctrl-c.""" - # Lazy: viser is an optional extra, only needed by this command. - import viser - - server = viser.ViserServer( - host=global_config.listen_host, label=f"Pair Picker - {dataset}", verbose=False - ) + """Draw the voxel map, the walked path, and the lighting they are lit by.""" server.gui.configure_theme(dark_mode=True) - server.scene.set_background_image(np.full((1, 1, 3), 14, dtype=np.uint8)) + server.scene.set_background_image(np.full((1, 1, 3), BACKGROUND_LEVEL, dtype=np.uint8)) server.scene.set_up_direction("+z") # Neutral white lighting instead of the default HDRI environment map, # which tints the height colormap. @@ -387,7 +376,7 @@ def pick_cases( ) # Cubes sit slightly under the voxel size so neighbors show a seam # instead of z-fighting, keeping individual voxels distinguishable. - half = 0.42 * voxel_size + half = CUBE_SHRINK * voxel_size corners = half * np.array( [[x, y, z] for x in (-1, 1) for y in (-1, 1) for z in (-1, 1)], dtype=np.float32 ) @@ -430,129 +419,159 @@ def pick_cases( span = float(np.ptp(map_points[:, :2])) def _on_client_connect(client: viser.ClientHandle) -> None: - client.camera.position = tuple(center + np.array([0.6 * span, 0.6 * span, 0.45 * span])) + client.camera.position = tuple(center + span * np.array(CAMERA_OFFSET)) client.camera.look_at = tuple(center) server.on_client_connect(_on_client_connect) - server.gui.add_markdown(INSTRUCTIONS) - selected_line = server.gui.add_markdown("selected: —") - undo_button = server.gui.add_button("undo last pick") - save_all_button = server.gui.add_button("save all unsaved") - exit_button = server.gui.add_button("save all & exit") - - lock = threading.Lock() - stop = threading.Event() - pairs: list[_PairEntry] = [] - - def announce(label: str) -> None: - selected_line.content = f"selected: **{label}**" - highlighted: list[_PairEntry] = [] - - def highlight(entry: _PairEntry) -> None: - while highlighted: - highlighted.pop().set_highlight(False) +class _PickerSession: + """Owns the scene markers and the live pair list for one picker run.""" + + def __init__(self, server: viser.ViserServer, store: CaseStore) -> None: + self.server = server + self.store = store + self.lock = threading.Lock() + self.stop = threading.Event() + self.pairs: list[_PairEntry] = [] + self.pending: list[tuple[viser.IcosphereHandle, NDArray[np.float32]]] = [] + self.highlighted: list[_PairEntry] = [] + self.pair_count = 0 + self._marker_seq = 0 + self.selected_line = server.gui.add_markdown("selected: none") + self.hooks = _Hooks(store, self.lock, self.pairs.remove, self.announce, self.highlight) + + def announce(self, label: str) -> None: + self.selected_line.content = f"selected: **{label}**" + + def highlight(self, entry: _PairEntry) -> None: + while self.highlighted: + self.highlighted.pop().set_highlight(False) entry.set_highlight(True) - highlighted.append(entry) + self.highlighted.append(entry) - hooks = _Hooks(store, lock, lambda entry: pairs.remove(entry), announce, highlight) - marker_seq = 0 + def _next_path(self) -> str: + self._marker_seq += 1 + return f"/picks/m{self._marker_seq}" - def sphere(point: NDArray[np.float32], color: tuple[int, int, int]) -> viser.IcosphereHandle: - nonlocal marker_seq - marker_seq += 1 - return server.scene.add_icosphere( - f"/picks/m{marker_seq}", + def sphere( + self, point: NDArray[np.float32], color: tuple[int, int, int] + ) -> viser.IcosphereHandle: + return self.server.scene.add_icosphere( + self._next_path(), radius=MARKER_RADIUS, color=_marker_color(color), position=(float(point[0]), float(point[1]), float(point[2]) + MARKER_LIFT), ) def pair_line( + self, start: NDArray[np.float32], goal: NDArray[np.float32], color: tuple[int, int, int] = PAIR_COLOR, width: float = LINE_WIDTH, ) -> viser.LineSegmentsHandle: - nonlocal marker_seq - marker_seq += 1 - return server.scene.add_line_segments( - f"/picks/m{marker_seq}", - np.stack([start, goal])[None], - colors=color, - line_width=width, - ) - - def pair_markers(start: NDArray[np.float32], goal: NDArray[np.float32]) -> _PairMarkers: - return _PairMarkers( - sphere(start, START_COLOR), sphere(goal, GOAL_COLOR), pair_line(start, goal) - ) - - for case in store.suite.cases: - start = np.asarray(case.start, dtype=np.float32) - goal = np.asarray(case.goal, dtype=np.float32) - pairs.append( - _PairEntry(server, 0, start, goal, hooks, pair_markers(start, goal), case=case) + return self.server.scene.add_line_segments( + self._next_path(), np.stack([start, goal])[None], colors=color, line_width=width ) - pending: list[tuple[viser.IcosphereHandle, NDArray[np.float32]]] = [] - pair_count = 0 + def load_manifest_pairs(self) -> None: + """Draw every case already in the manifest as an editable entry.""" + for case in self.store.suite.cases: + start = np.asarray(case.start, dtype=np.float32) + goal = np.asarray(case.goal, dtype=np.float32) + markers = _PairMarkers( + self.sphere(start, START_COLOR), + self.sphere(goal, GOAL_COLOR), + self.pair_line(start, goal), + ) + self.pairs.append( + _PairEntry(self.server, 0, start, goal, self.hooks, markers, case=case) + ) - def _on_scene_click(event: viser.SceneClickEvent) -> None: - nonlocal pair_count + def on_click(self, event: viser.SceneClickEvent) -> None: + """Shift+click picks a start, then a goal, then opens the new pair.""" + final = self.store.final point = pick_along_ray( - map_points, np.asarray(event.ray_origin), np.asarray(event.ray_direction), voxel_size + final.occupied, + np.asarray(event.ray_origin), + np.asarray(event.ray_direction), + final.voxel_size, ) if point is None: return - with lock: - if not pending: - pending.append((sphere(point, NEW_START_COLOR), point)) + with self.lock: + if not self.pending: + self.pending.append((self.sphere(point, NEW_START_COLOR), point)) return - start_marker, start = pending.pop() + start_marker, start = self.pending.pop() markers = _PairMarkers( start_marker, - sphere(point, NEW_GOAL_COLOR), - pair_line(start, point, NEW_PAIR_COLOR, NEW_LINE_WIDTH), + self.sphere(point, NEW_GOAL_COLOR), + self.pair_line(start, point, NEW_PAIR_COLOR, NEW_LINE_WIDTH), ) - pair_count += 1 - pairs.append(_PairEntry(server, pair_count, start, point, hooks, markers)) - - def _on_undo(_event: object) -> None: - with lock: - if pending: - pending.pop()[0].remove() - elif pairs and not pairs[-1].preloaded: + self.pair_count += 1 + self.pairs.append( + _PairEntry(self.server, self.pair_count, start, point, self.hooks, markers) + ) + + def on_undo(self, _event: object) -> None: + with self.lock: + if self.pending: + self.pending.pop()[0].remove() + elif self.pairs and not self.pairs[-1].preloaded: # Only the panel entry and markers go away. The per-pair delete # button is what removes it from the manifest. - entry = pairs.pop() + entry = self.pairs.pop() entry.remove() if entry.saved_id is not None: print(f"{entry.saved_id} stays in the manifest; use delete to remove it") - def save_unsaved() -> int: - with lock: - return sum(not pair.save_or_update() for pair in pairs if pair.saved_id is None) - - def _on_save_all(_event: object) -> None: - save_unsaved() - - def _on_exit(_event: object) -> None: - if save_unsaved() == 0: - stop.set() - - server.scene.on_click(modifier="shift")(_on_scene_click) - undo_button.on_click(_on_undo) - save_all_button.on_click(_on_save_all) - exit_button.on_click(_on_exit) - - print("picker running; ctrl-c to exit (unsaved pairs are discarded)") - try: - stop.wait() - except KeyboardInterrupt: - unsaved = sum(1 for p in pairs if p.saved_id is None) - if unsaved: - print(f"discarded {unsaved} unsaved pair(s)") - finally: - server.stop() + def save_unsaved(self) -> int: + """Save every pair not yet in the manifest, returning the failure count.""" + with self.lock: + return sum(not pair.save_or_update() for pair in self.pairs if pair.saved_id is None) + + def on_save_all(self, _event: object) -> None: + self.save_unsaved() + + def on_exit(self, _event: object) -> None: + if self.save_unsaved() == 0: + self.stop.set() + + def serve(self) -> None: + """Block until the panel exits or the terminal interrupts.""" + print("picker running; ctrl-c to exit (unsaved pairs are discarded)") + try: + self.stop.wait() + except KeyboardInterrupt: + unsaved = sum(1 for p in self.pairs if p.saved_id is None) + if unsaved: + print(f"discarded {unsaved} unsaved pair(s)") + finally: + self.server.stop() + + +def pick_cases(store: CaseStore, walked: NDArray[np.float32]) -> None: + """Serve the picker until the user exits from the panel or hits ctrl-c.""" + # Lazy: viser is an optional extra, only needed by this command. + import viser + + final = store.final + server = viser.ViserServer( + host=global_config.listen_host, + label=f"Pair Picker - {store.suite.dataset}", + verbose=False, + ) + _add_map_scene( + server, final.occupied, turbo_by_height(final.occupied), final.voxel_size, walked + ) + server.gui.add_markdown(INSTRUCTIONS) + session = _PickerSession(server, store) + session.load_manifest_pairs() + + server.scene.on_click(modifier="shift")(session.on_click) + server.gui.add_button("undo last pick").on_click(session.on_undo) + server.gui.add_button("save all unsaved").on_click(session.on_save_all) + server.gui.add_button("save all & exit").on_click(session.on_exit) + session.serve() diff --git a/dimos/navigation/nav_3d/evaluator/pipeline.py b/dimos/navigation/nav_3d/evaluator/pipeline.py index 5164a1296a..3f1300dc4f 100644 --- a/dimos/navigation/nav_3d/evaluator/pipeline.py +++ b/dimos/navigation/nav_3d/evaluator/pipeline.py @@ -12,11 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""The unit under evaluation: lidar and odometry in, paths out. - -A pipeline owns whatever mapping it needs and the evaluator never looks inside -it. Grading occupancy is built separately, by the evaluator's own mapper. -""" +"""The unit under evaluation: lidar and odometry in, paths out. A pipeline owns +whatever mapping it needs, and grading occupancy is built separately.""" from __future__ import annotations diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py index 0a8d95b9d8..be4623e1f4 100644 --- a/dimos/navigation/nav_3d/evaluator/recording.py +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -27,6 +27,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.nav_3d.evaluator.metrics import arc_lengths if TYPE_CHECKING: from collections.abc import Iterator @@ -51,8 +52,6 @@ class Trajectory: def arc_lengths(self) -> NDArray[np.float64]: """Cumulative walked distance at each pose, starting at 0.""" - from dimos.navigation.nav_3d.evaluator.metrics import arc_lengths - return arc_lengths(self.positions) def foot(self, robot_height: float) -> NDArray[np.float32]: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index 6dfc45e7a5..c5d259a3db 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -12,17 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Replay case suites through a pipeline and score them. - -Generated cases plan twice, online at their start time and again on the whole -recording. Curated and infeasible cases plan once, on the final map. -""" +"""Replay case suites through a pipeline and score them. Generated cases plan +twice, online at their start time and again on the whole recording.""" from __future__ import annotations from concurrent.futures import ProcessPoolExecutor from dataclasses import asdict, dataclass, field, replace import itertools +import multiprocessing from time import perf_counter from typing import TYPE_CHECKING @@ -35,13 +33,10 @@ load_or_build_final_map, ) from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection, make_pipeline -from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from pathlib import Path - from numpy.typing import NDArray from dimos.navigation.nav_3d.evaluator.cases import Case, Suite @@ -61,14 +56,12 @@ class PlanOutcome: valid: bool # Every sample stands on final-map occupancy. Fabricated bridges fail. supported: bool - # For an ordinary case: all of the above. For an expect_fail case: the - # planner correctly refused the infeasible goal. + # All of the above, or for an infeasible case, that the planner refused. success: bool length: float plan_ms: float spl: float - # Gate margin along the path (see GateResult.min_clearance_m). None when - # no path was planned. + # Gate margin along the path. None when no path was planned. min_clearance: float | None waypoints: list[list[float]] # Indices of the colliding samples along the densified path, so a viewer @@ -303,13 +296,22 @@ def _references( return refs +@dataclass +class _OnlineCase: + """One case's online plan and the map it was planned against.""" + + outcome: PlanOutcome + waypoints: NDArray[np.float32] | None + map_keys: NDArray[np.int64] + artifacts: PlannerArtifacts | None = None + occupied: NDArray[np.float32] | None = None + + @dataclass class _OnlinePass: """Everything the single replay pass produced, keyed by case index.""" - outcomes: dict[int, tuple[PlanOutcome, NDArray[np.float32] | None, NDArray[np.int64]]] - artifacts: dict[int, PlannerArtifacts | None] - occupied: dict[int, NDArray[np.float32] | None] + cases: dict[int, _OnlineCase] add_ms: list[float] final_artifacts: PlannerArtifacts | None @@ -317,7 +319,6 @@ class _OnlinePass: def _replay_online( pipeline: NavPipeline, suite: Suite, - db_path: Path, cfg: EvalConfig, checkpoints: MapCheckpoints, case_ckpt: NDArray[np.int64], @@ -326,30 +327,29 @@ def _replay_online( keep_artifacts: bool, ) -> _OnlinePass: """Feed the recording to the pipeline once, planning each case where its - start time falls, so the pipeline has seen what the robot had seen by then. - - A pipeline's state cannot be snapshotted from outside, hence one pass. - """ - out = _OnlinePass({}, {}, {}, [], None) + start time falls, so the pipeline has seen what the robot had seen by then.""" + out = _OnlinePass({}, [], None) def plan_at(k: int, keys: NDArray[np.int64]) -> None: for ci in np.flatnonzero(case_ckpt == k): case, ref = suite.cases[ci], refs[ci] if not len(keys): - out.outcomes[ci] = (_no_plan(0.0), None, keys) + out.cases[ci] = _OnlineCase(_no_plan(0.0), None, keys) continue # Collisions use the incremental map as of plan time. Support uses # the final map, since ground exists whether or not it was mapped. outcome, waypoints = _run_plan(pipeline, case, ref.length, keys, map_keys, cfg) - out.outcomes[ci] = (outcome, waypoints, keys) - out.artifacts[ci] = _snapshot(pipeline) if keep_artifacts else None - out.occupied[ci] = key_centers(keys, cfg.voxel_size) if keep_artifacts else None + out.cases[ci] = _OnlineCase( + outcome, + waypoints, + keys, + artifacts=_snapshot(pipeline) if keep_artifacts else None, + occupied=key_centers(keys, cfg.voxel_size) if keep_artifacts else None, + ) snapshots = checkpoints.iter_snapshots() k = 0 - for frame in iter_world_frames( - db_path, suite.lidar_stream, suite.odom_stream, cfg.align_tol, suite.end_ts_seconds() - ): + for frame in suite.world_frames(cfg.align_tol): while k < len(checkpoints.times) and frame.ts > checkpoints.times[k]: plan_at(k, next(snapshots)) k += 1 @@ -410,26 +410,28 @@ def result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: ) ) continue - online_out, online_wp, online_keys = online.outcomes[ci] - end = online_wp[-1] if online_wp is not None and len(online_wp) else None + run = online.cases[ci] + end = run.waypoints[-1] if run.waypoints is not None and len(run.waypoints) else None dynamic_candidate, blocking = ( (False, []) if case.expect_final_fail - else _dynamic_candidate(online_out, final_out, online_wp, online_keys, map_keys, cfg) + else _dynamic_candidate( + run.outcome, final_out, run.waypoints, run.map_keys, map_keys, cfg + ) ) results.append( result( case, ref, - online_voxels=len(online_keys), + online_voxels=len(run.map_keys), expect_fail=False, - online=online_out, + online=run.outcome, final=final_out, soft_progress=metrics.soft_progress(end, case.start, case.goal), dynamic_candidate=dynamic_candidate, blocking_points=blocking, - online_artifacts=online.artifacts.get(ci), - online_occupied=online.occupied.get(ci), + online_artifacts=run.artifacts, + online_occupied=run.occupied, ) ) return results @@ -441,7 +443,7 @@ def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> Da # sqlite would otherwise create an empty db here and the failure would # surface as a missing odometry stream. raise FileNotFoundError(f"{suite.dataset}: recording not found at {db_path}") - trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) + trajectory = suite.trajectory() final_only = np.array([_final_only(c) for c in suite.cases], dtype=bool) refs = _references(suite, trajectory, final_only, cfg) @@ -453,8 +455,8 @@ def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> Da ) # Before the final map, because a cold checkpoint build replays the whole # recording and fills the final cache on its way through. - checkpoints = load_or_build_checkpoints(db_path, suite, cfg, start_ts) - final = load_or_build_final_map(db_path, suite, cfg) + checkpoints = load_or_build_checkpoints(suite, cfg, start_ts) + final = load_or_build_final_map(suite, cfg) case_ckpt = np.searchsorted(checkpoints.times, start_ts) case_ckpt[final_only] = -1 @@ -462,7 +464,6 @@ def run_suite(suite: Suite, cfg: EvalConfig, keep_artifacts: bool = False) -> Da online = _replay_online( pipeline, suite, - db_path, cfg, checkpoints, case_ckpt, @@ -491,7 +492,12 @@ def evaluate( so workers only spreads datasets across processes.""" cfg = cfg or EvalConfig() if workers > 1 and len(suites) > 1: - with ProcessPoolExecutor(max_workers=min(workers, len(suites))) as pool: + # Spawn, not fork: the mapper and planner carry a native thread pool, and + # a forked child inherits its mutexes locked if the parent ever built one. + with ProcessPoolExecutor( + max_workers=min(workers, len(suites)), + mp_context=multiprocessing.get_context("spawn"), + ) as pool: datasets = list( pool.map(run_suite, suites, itertools.repeat(cfg), itertools.repeat(keep_artifacts)) ) @@ -520,13 +526,13 @@ def mean(values: list[float]) -> float: by_tag: dict[str, TagStats] = {} for tag in sorted({t for c in cases for t in c.tags}): - tc = [c for c in cases if tag in c.tags] - oc = [c for c in tc if not c.final_only] + tagged = [c for c in cases if tag in c.tags] + online_tagged = [c for c in tagged if not c.final_only] by_tag[tag] = TagStats( - n=len(tc), - n_online=len(oc), - inc_score=mean([c.online.spl for c in oc]), - fin_score=mean([c.final.spl for c in tc]), + n=len(tagged), + n_online=len(online_tagged), + inc_score=mean([c.online.spl for c in online_tagged]), + fin_score=mean([c.final.spl for c in tagged]), ) return Report( diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py index 3cfc0ab1fb..386c68646e 100644 --- a/dimos/navigation/nav_3d/evaluator/tagging.py +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -80,11 +80,8 @@ def elevation_tags( def _corridor_width( samples: NDArray[np.float32], occupied_keys: NDArray[np.int64], cfg: EvalConfig ) -> NDArray[np.float64]: - """Free lateral width at each densified sample, at body height. - - Left-plus-right distance to the nearest occupied voxel, capped when the - passage is open. The room the body has to pass, not the feet to stand. - """ + """Free lateral width at each densified sample, at body height: the room + the body has to pass, not the room the feet have to stand.""" _, lateral, _ = body_frames(samples, cfg.robot_length) mid_z = (cfg.ground_margin + cfg.body_clearance) / 2.0 origin = samples.astype(np.float64) + np.array([0.0, 0.0, mid_z]) @@ -182,12 +179,8 @@ def route_tags( occupied_keys: NDArray[np.int64], cfg: EvalConfig, ) -> list[str]: - """Geometric tags for a case, in a stable order. - - Elevation comes from the endpoints. Shape tags describe the terrain between - them and are skipped unless the walked route runs roughly straight. The - caller prepends the provenance tag. - """ + """Geometric tags for a case, in a stable order. Shape tags are skipped + unless the walked route runs roughly straight from start to goal.""" tags = elevation_tags(start, goal) if route is not None and len(route) >= 2 and _is_local(route, start, goal, cfg): tags += _corridor_tags(route, occupied_keys, cfg) diff --git a/dimos/navigation/nav_3d/evaluator/test_evaluator.py b/dimos/navigation/nav_3d/evaluator/test_nav_eval.py similarity index 78% rename from dimos/navigation/nav_3d/evaluator/test_evaluator.py rename to dimos/navigation/nav_3d/evaluator/test_nav_eval.py index ff8ad803bd..dd697b3327 100644 --- a/dimos/navigation/nav_3d/evaluator/test_evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/test_nav_eval.py @@ -20,11 +20,22 @@ from typing import TYPE_CHECKING, cast import numpy as np +from numpy.typing import NDArray import pytest import typer +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_3d.evaluator import final_map, metrics, runner -from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite +from dimos.navigation.nav_3d.evaluator.cases import ( + Case, + Suite, + load_suite, + save_suite, +) +from dimos.navigation.nav_3d.evaluator.cli import _apply_overrides from dimos.navigation.nav_3d.evaluator.config import EvalConfig from dimos.navigation.nav_3d.evaluator.curation import CaseStore, CurationError, _curated_tags from dimos.navigation.nav_3d.evaluator.final_map import ( @@ -41,8 +52,13 @@ snap_to_surface, ) from dimos.navigation.nav_3d.evaluator.picker import pick_along_ray -from dimos.navigation.nav_3d.evaluator.pipeline import PIPELINES, make_pipeline -from dimos.navigation.nav_3d.evaluator.recording import Frame, Trajectory +from dimos.navigation.nav_3d.evaluator.pipeline import PIPELINES +from dimos.navigation.nav_3d.evaluator.recording import ( + Frame, + Trajectory, + iter_world_frames, + load_trajectory, +) from dimos.navigation.nav_3d.evaluator.runner import ( _dynamic_candidate, _final_only, @@ -52,50 +68,35 @@ from dimos.navigation.nav_3d.evaluator.tagging import route_tags from dimos.navigation.nav_3d.evaluator.voxel_keys import ( cylinder_offsets, - key_centers, keys_contain, - offset_keys, + offset_deltas, voxel_keys, ) if TYPE_CHECKING: - from numpy.typing import NDArray - from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.navigation.nav_3d.evaluator.pipeline import NavPipeline VOXEL = 0.1 +Point = tuple[float, float, float] def _cfg(**overrides: object) -> EvalConfig: return replace(EvalConfig(voxel_size=VOXEL), **overrides) -def _wall(x: float) -> np.ndarray: +def _wall(x: float) -> NDArray[np.float32]: ys, zs = np.meshgrid(np.arange(-1, 1, VOXEL), np.arange(0.05, 1.5, VOXEL)) return np.stack([np.full(ys.size, x), ys.ravel(), zs.ravel()], axis=1, dtype=np.float32) -def _ywall(y: float, x_lo: float, x_hi: float) -> np.ndarray: +def _ywall(y: float, x_lo: float, x_hi: float) -> NDArray[np.float32]: """A wall parallel to +x travel, at constant y, spanning body height.""" xs, zs = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(0.05, 1.5, VOXEL)) return np.stack([xs.ravel(), np.full(xs.size, y), zs.ravel()], axis=1, dtype=np.float32) -def test_voxel_key_roundtrip() -> None: - pts = np.array([[0.05, 0.05, 0.05], [-3.21, 4.7, -0.09], [80.0, -80.0, 12.3]], dtype=np.float32) - centers = key_centers(voxel_keys(pts, VOXEL), VOXEL) - assert np.all(np.abs(centers - pts) <= VOXEL / 2 + 1e-5) - - -def test_keys_contain() -> None: - keys = np.sort(voxel_keys(np.array([[0, 0, 0], [1, 1, 1]], dtype=np.float32), VOXEL)) - query = voxel_keys(np.array([[0, 0, 0], [5, 5, 5]], dtype=np.float32), VOXEL) - assert keys_contain(keys, query).tolist() == [True, False] - assert keys_contain(np.array([], dtype=np.int64), query).tolist() == [False, False] - - -def _gate(waypoints: np.ndarray, obstacles: np.ndarray) -> metrics.GateResult: +def _gate(waypoints: NDArray[np.float32], obstacles: NDArray[np.float32]) -> metrics.GateResult: keys = np.unique(voxel_keys(obstacles, VOXEL)) return metrics.check_path(waypoints, keys, _cfg()) @@ -231,7 +232,6 @@ def frame_at(ts: float, x: float) -> Frame: frames = [frame_at(0.0, 5.0), frame_at(1.0, 8.0), frame_at(2.0, 11.0)] times = np.array([0.5, 1.5, np.inf]) final, snapshots = replay_frames(frames, mapper, VOXEL, times) - assert final.frames == 3 sizes = [len(s) for s in snapshots] assert 0 < sizes[0] < sizes[1] < sizes[2] assert np.array_equal(snapshots[2], final.occupied_keys) @@ -265,33 +265,26 @@ def test_check_kinematics_rejects_cliff_jumps() -> None: class _StubPipeline: """Returns a fixed path regardless of what it was fed, for gaming the scorer.""" - def __init__(self, waypoints: np.ndarray | None) -> None: + def __init__(self, waypoints: NDArray[np.float32] | None) -> None: self._waypoints = waypoints self.frames = 0 - def add_frame(self, points: np.ndarray, origin: tuple[float, float, float], ts: float) -> None: + def add_frame( + self, points: NDArray[np.float32], origin: tuple[float, float, float], ts: float + ) -> None: self.frames += 1 def plan( self, start: tuple[float, float, float], goal: tuple[float, float, float] - ) -> np.ndarray | None: + ) -> NDArray[np.float32] | None: return self._waypoints -def _stub(waypoints: np.ndarray | None) -> NavPipeline: +def _stub(waypoints: NDArray[np.float32] | None) -> NavPipeline: return cast("NavPipeline", _StubPipeline(waypoints)) -def test_make_pipeline_resolves_by_registry_name() -> None: - cfg = _cfg() - with pytest.MonkeyPatch.context() as mp: - mp.setitem(PIPELINES, "stub", lambda _cfg: _StubPipeline(None)) - assert isinstance(make_pipeline("stub", cfg), _StubPipeline) - with pytest.raises(ValueError, match="unknown pipeline"): - make_pipeline("nope", cfg) - - -def _floor(x_lo: float = 0.0, x_hi: float = 20.0) -> np.ndarray: +def _floor(x_lo: float = 0.0, x_hi: float = 20.0) -> NDArray[np.float32]: xs, ys = np.meshgrid(np.arange(x_lo, x_hi, VOXEL), np.arange(-2, 6, VOXEL)) return np.stack([xs.ravel(), ys.ravel(), np.full(xs.size, -0.05)], axis=1, dtype=np.float32) @@ -305,7 +298,7 @@ def _meta_scene() -> tuple[np.ndarray, EvalConfig, Case]: return keys, cfg, case -def _u_route() -> np.ndarray: +def _u_route() -> NDArray[np.float32]: return np.array([[2, 0, 0], [2, 4, 0], [18, 4, 0], [18, 0, 0]], dtype=np.float32) @@ -436,23 +429,13 @@ def test_ground_truth_route_orients_start_to_goal() -> None: assert route[-1, 2] < route[0, 2] -def _tags(route: np.ndarray, keys: np.ndarray) -> list[str]: +def _tags(route: NDArray[np.float32], keys: np.ndarray) -> list[str]: """Tag a synthetic route, taking its endpoints for elevation.""" start = (float(route[0, 0]), float(route[0, 1]), float(route[0, 2])) goal = (float(route[-1, 0]), float(route[-1, 1]), float(route[-1, 2])) return route_tags(start, goal, route, keys, _cfg()) -def test_route_tags_flat_wide_has_no_shape_tags() -> None: - """A wide flat traverse is just flat: no narrow, doorway, or corridor.""" - route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) - walls = np.concatenate([_ywall(-2.0, 0, 4), _ywall(2.0, 0, 4)]) - keys = np.unique(voxel_keys(walls, VOXEL)) - tags = _tags(route, keys) - assert "flat" in tags - assert "narrow" not in tags and "doorway" not in tags and "corridor" not in tags - - def test_route_tags_narrow_passage() -> None: """Walls under a body-plus-clearance apart the whole way make a corridor.""" route = np.array([[0, 0, 0], [4, 0, 0]], dtype=np.float32) @@ -464,23 +447,15 @@ def test_route_tags_narrow_passage() -> None: def test_route_tags_doorway_is_a_short_pinch() -> None: - """An open corridor that pinches briefly and reopens is a doorway.""" + """An open corridor that pinches and reopens is a doorway, down to a pinch + only a couple of voxels long.""" route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) - pinch = np.concatenate([_ywall(-0.35, 2.0, 2.6), _ywall(0.35, 2.0, 2.6)]) - keys = np.unique(voxel_keys(np.concatenate([far, pinch]), VOXEL)) - tags = _tags(route, keys) - assert "doorway" in tags and "narrow" in tags - - -def test_route_tags_sharp_doorway() -> None: - """A door frame is a sharp pinch: the narrow stretch is only a couple of - voxels, but flanked by open space it is still a doorway.""" - route = np.array([[0, 0, 0], [5, 0, 0]], dtype=np.float32) - far = np.concatenate([_ywall(-2.0, 0, 5), _ywall(2.0, 0, 5)]) - frame = np.concatenate([_ywall(-0.35, 2.4, 2.6), _ywall(0.35, 2.4, 2.6)]) - keys = np.unique(voxel_keys(np.concatenate([far, frame]), VOXEL)) - assert "doorway" in _tags(route, keys) + for x_lo, x_hi in ((2.0, 2.6), (2.4, 2.6)): + pinch = np.concatenate([_ywall(-0.35, x_lo, x_hi), _ywall(0.35, x_lo, x_hi)]) + keys = np.unique(voxel_keys(np.concatenate([far, pinch]), VOXEL)) + tags = _tags(route, keys) + assert "doorway" in tags and "narrow" in tags def test_route_tags_stairs_from_endpoints() -> None: @@ -513,13 +488,11 @@ def _final_map(points: np.ndarray) -> FinalMap: voxel_size=VOXEL, occupied=points, occupied_keys=np.unique(voxel_keys(points, VOXEL)), - frames=1, - add_frame_ms={"p50": 0.0, "p95": 0.0, "max": 0.0}, build_ms=0.0, ) -def _surface_keys(points: np.ndarray, robot_height: float = 0.3) -> np.ndarray: +def _surface_keys(points: NDArray[np.float32], robot_height: float = 0.3) -> NDArray[np.float32]: return np.unique(voxel_keys(_final_map(points).standable_surface(robot_height), VOXEL)) @@ -570,8 +543,6 @@ def test_generate_cases_around_wall() -> None: voxel_size=VOXEL, occupied=wall_pts, occupied_keys=np.unique(voxel_keys(wall_pts, VOXEL)), - frames=1, - add_frame_ms={"p50": 0.0, "p95": 0.0, "max": 0.0}, build_ms=0.0, ) xs, ys = np.meshgrid(np.arange(0, 20, VOXEL), np.arange(-3, 6, VOXEL)) @@ -633,39 +604,6 @@ def test_pick_along_ray() -> None: assert pick_along_ray(wall, origin, up, VOXEL) is None -def test_load_suite_rejects_malformed_manifests(tmp_path: Path) -> None: - """Manifests are hand-edited, so a wrong one must fail loudly, not silently.""" - manifest = tmp_path / "demo.yaml" - manifest.write_text( - "dataset: demo\n" - "cases:\n" - " - id: a\n" - " start: [0, 0, 0]\n" - " goal: [1, 2, 3]\n" - " tags: [stairs]\n" - ) - suite = load_suite(manifest) - assert suite.dataset == "demo" - assert suite.cases[0].goal == (1.0, 2.0, 3.0) - assert suite.cases[0].tags == ["stairs"] - - manifest.write_text( - "dataset: demo\ncases:\n" - " - {id: a, start: [0, 0, 0], goal: [1, 2, 3]}\n" - " - {id: a, start: [0, 0, 0], goal: [4, 5, 6]}\n" - ) - with pytest.raises(ValueError, match="duplicate"): - load_suite(manifest) - - manifest.write_text( - "dataset: demo\ncases:\n" - " - {id: bad, start: [0, 0, 0], goal: [1, 0, 0], " - "expect_fail: true, expect_final_fail: true}\n" - ) - with pytest.raises(ValueError, match="exclusive"): - load_suite(manifest) - - def test_save_suite_roundtrip(tmp_path: Path) -> None: suite = Suite( dataset="demo", @@ -709,11 +647,14 @@ def hits(point: np.ndarray) -> bool: def test_offset_deltas_match_packing_the_summed_indices() -> None: + """Adding a packed delta must equal packing the summed indices, which is + what lets the gate probe a neighborhood without unpacking.""" pts = np.array([[0.05, -3.2, 1.4], [12.0, 0.0, -2.0]], dtype=np.float32) offs = cylinder_offsets(0.4, -0.2, 0.3, VOXEL) idx = np.floor(pts.astype(np.float64) / VOXEL).astype(np.int64)[:, None, :] + offs[None, :, :] packed = voxel_keys((idx.reshape(-1, 3) * VOXEL + VOXEL / 2).astype(np.float32), VOXEL) - assert np.array_equal(offset_keys(pts, offs, VOXEL).ravel(), packed) + shifted = voxel_keys(pts, VOXEL)[:, None] + offset_deltas(offs)[None, :] + assert np.array_equal(shifted.ravel(), packed) def test_spl_and_body_frames_survive_degenerate_input() -> None: @@ -733,11 +674,9 @@ def _store(tmp_path: Path, cases: list[Case]) -> CaseStore: voxel_size=VOXEL, occupied=surface, occupied_keys=np.unique(voxel_keys(surface, VOXEL)), - frames=0, - add_frame_ms={}, build_ms=0.0, ) - return CaseStore(load_suite(manifest), manifest, surface, _cfg(), final) + return CaseStore(load_suite(manifest), surface, _cfg(), final) def test_editing_a_generated_case_keeps_it_in_the_incremental_score(tmp_path: Path) -> None: @@ -803,11 +742,6 @@ def _write_recording( poses: list[tuple[float, tuple[float, float, float]]], ) -> None: """A minimal mem2 recording: sensor-frame clouds plus odometry.""" - from dimos.memory2.store.sqlite import SqliteStore - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.nav_msgs.Odometry import Odometry - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - with SqliteStore(path=str(path)) as store: lidar = store.stream("lidar", PointCloud2) for ts, pts, frame_id in clouds: @@ -819,8 +753,6 @@ def _write_recording( def test_recording_registers_clouds_and_honors_end_ts(tmp_path: Path) -> None: """Clouds arrive sensor-frame and are placed by their aligned odometry.""" - from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory - local = np.array([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0]], dtype=np.float32) db = tmp_path / "rec.db" _write_recording( @@ -844,8 +776,6 @@ def test_recording_registers_clouds_and_honors_end_ts(tmp_path: Path) -> None: def test_recording_rejects_pre_registered_clouds(tmp_path: Path) -> None: """World-frame clouds would be registered twice, so they are refused.""" - from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory - db = tmp_path / "legacy.db" pts = np.array([[1.0, 0.0, 0.0]], dtype=np.float32) _write_recording(db, [(1.0, pts, "world")], [(1.0, (0.0, 0.0, 0.0))]) @@ -857,8 +787,6 @@ def test_recording_rejects_pre_registered_clouds(tmp_path: Path) -> None: def test_apply_overrides_is_the_sweep_interface() -> None: """--set is how a sweep varies the harness and the pipeline.""" - from dimos.navigation.nav_3d.evaluator.cli import _apply_overrides - cfg = _apply_overrides( EvalConfig(), ["goal_tolerance=0.4", "planner.wall_clearance_m=0.0", "pipeline=mls"] ) @@ -866,25 +794,41 @@ def test_apply_overrides_is_the_sweep_interface() -> None: assert isinstance(cfg.goal_tolerance, float) assert cfg.planner == {"wall_clearance_m": 0.0} assert cfg.pipeline == "mls" - for bad in (["no_equals_sign"], ["not_a_field=1"]): + for bad in (["no_equals_sign"], ["not_a_field=1"], ["planner=0.5"]): with pytest.raises(typer.BadParameter): _apply_overrides(EvalConfig(), bad) + # An override that inverts the gate band is caught after it is applied, + # since __post_init__ already ran on the config being mutated. + with pytest.raises(typer.BadParameter, match="body_clearance"): + _apply_overrides(EvalConfig(), ["ground_margin=0.46"]) + + +def test_an_inverted_body_band_is_rejected_not_silently_passed() -> None: + """With ground_margin above body_clearance the gate admits nothing, so every + path would pass and the score would read as perfect.""" + with pytest.raises(ValueError, match="body_clearance"): + EvalConfig(ground_margin=0.5, body_clearance=0.45) + for name in ("voxel_size", "robot_length", "robot_width", "max_range"): + with pytest.raises(ValueError, match=name): + EvalConfig(**{name: 0.0}) class _RecordingPipeline: """Returns the fixed path and remembers how many frames it had each time.""" - def __init__(self, waypoints: np.ndarray | None) -> None: + def __init__(self, waypoints: NDArray[np.float32] | None) -> None: self._waypoints = waypoints self.frames = 0 self.frames_at_plan: list[int] = [] - def add_frame(self, points: np.ndarray, origin: tuple[float, float, float], ts: float) -> None: + def add_frame( + self, points: NDArray[np.float32], origin: tuple[float, float, float], ts: float + ) -> None: self.frames += 1 def plan( self, start: tuple[float, float, float], goal: tuple[float, float, float] - ) -> np.ndarray | None: + ) -> NDArray[np.float32] | None: self.frames_at_plan.append(self.frames) return self._waypoints @@ -897,16 +841,27 @@ def _stub_harness(mp: pytest.MonkeyPatch, tmp_path: Path, pipeline: object) -> N mp.setattr(final_map, "CACHE_SUBDIR", tmp_path / "cache") +WALK_HEIGHT = 0.5 + + def _corridor_recording(path: Path) -> Suite: - """Ten frames of floor along +x, walked start to finish.""" + """Ten frames of floor along +x, walked start to finish. + + Clouds are sensor-frame, so each slab is written relative to the pose that + registers it and lands back at z = -0.05 in the world. + """ slabs = [ - (float(t), _floor(float(t) - 1.0, float(t) + 1.0) - np.array([t, 0, 0], dtype=np.float32)) + ( + float(t), + _floor(float(t) - 1.0, float(t) + 1.0) + - np.array([t, 0, WALK_HEIGHT], dtype=np.float32), + ) for t in range(1, 11) ] _write_recording( path, [(ts, pts, "lidar") for ts, pts in slabs], - [(float(t), (float(t), 0.0, 0.5)) for t in range(1, 11)], + [(float(t), (float(t), 0.0, WALK_HEIGHT)) for t in range(1, 11)], ) return Suite( dataset="corridor", cases=[], db=str(path), lidar_stream="lidar", odom_stream="odom" @@ -935,28 +890,141 @@ def test_run_suite_plans_each_case_on_the_map_as_of_its_start_time(tmp_path: Pat assert pipeline.frames_at_plan[2:] == [10, 10] +class _PerCasePipeline: + """Plans only between the endpoints it was given, refusing anything else.""" + + def __init__(self, routes: dict[tuple[Point, Point], np.ndarray]) -> None: + self._routes = routes + + def add_frame(self, points: NDArray[np.float32], origin: Point, ts: float) -> None: + pass + + def plan(self, start: Point, goal: Point) -> NDArray[np.float32] | None: + return self._routes.get((start, goal)) + + def test_evaluate_scores_only_online_cases_in_the_headline(tmp_path: Path) -> None: - """A manual case is final-only, so it must move final_score but not score.""" + """The headline score covers online cases only, so a final-only case that + fails must drop final_score while leaving score untouched.""" suite = _corridor_recording(tmp_path / "corridor.db") - route = np.array([[4, 0, 0], [2, 0, 0]], dtype=np.float32) + walked: Point = (4.0, 0.0, 0.0) + goal: Point = (2.0, 0.0, 0.0) suite.cases = [ - Case(id="auto_00", start=(4.0, 0.0, 0.0), goal=(2.0, 0.0, 0.0), tags=["auto"]), - Case(id="manual_00", start=(4.0, 0.0, 0.0), goal=(2.0, 0.0, 0.0), tags=["manual"]), + Case(id="auto_00", start=walked, goal=goal, tags=["auto"]), + # Same endpoints, but a manual case never replays online. + Case(id="manual_00", start=walked, goal=goal, tags=["manual"]), ] + route = np.array([walked, (3.0, 0.0, 0.0), goal], dtype=np.float32) with pytest.MonkeyPatch.context() as mp: - _stub_harness(mp, tmp_path, _StubPipeline(route)) + # Only the auto case's plan call is answered; the manual one is refused. + _stub_harness(mp, tmp_path, _PerCasePipeline({(walked, goal): route})) report = runner.evaluate([suite], _cfg(pipeline="stub")) assert report.n_cases == 2 assert report.n_online == 1 assert [c.final_only for d in report.datasets for c in d.cases] == [False, True] - assert set(report.by_tag) == {"auto", "manual"} + # The online case succeeds on the route the robot demonstrated. + assert report.score == pytest.approx(1.0) + assert report.n_success == 1 + # Both cases plan against the final map, and both succeed there, so the + # final score stays at 1.0 while covering twice as many cases. + assert report.n_success_final == 2 + assert report.final_score == pytest.approx(1.0) assert report.by_tag["manual"].n_online == 0 -def test_run_suite_names_a_missing_recording(tmp_path: Path) -> None: - """Without the guard sqlite creates an empty db and the error blames odometry.""" - suite = Suite(dataset="gone", cases=[], db=str(tmp_path / "absent.db")) - with pytest.raises(FileNotFoundError, match="recording not found"): - runner.run_suite(suite, _cfg()) - assert not (tmp_path / "absent.db").exists() +def test_spl_scales_with_how_far_the_path_overshoots_the_reference() -> None: + """Only 0.0 and 1.0 are asserted elsewhere, so the ratio itself is unpinned.""" + assert metrics.spl(True, 10.0, 20.0) == pytest.approx(0.5) + assert metrics.spl(True, 10.0, 12.5) == pytest.approx(0.8) + # A path shorter than the demonstrated route earns 1.0, never more. + assert metrics.spl(True, 10.0, 4.0) == pytest.approx(1.0) + # A failed case scores zero however short the path. + assert metrics.spl(False, 10.0, 10.0) == 0.0 + + +def test_soft_progress_is_the_share_of_the_gap_closed() -> None: + """soft_progress drives the headline soft score and had no test.""" + start: Point = (0.0, 0.0, 0.0) + goal: Point = (10.0, 0.0, 0.0) + assert metrics.soft_progress(np.array([5.0, 0.0, 0.0], dtype=np.float32), start, goal) == ( + pytest.approx(0.5) + ) + assert metrics.soft_progress(np.array([10.0, 0.0, 0.0], dtype=np.float32), start, goal) == ( + pytest.approx(1.0) + ) + # No path at all, and moving away from the goal, both floor at zero. + assert metrics.soft_progress(None, start, goal) == 0.0 + assert metrics.soft_progress(np.array([-5.0, 0.0, 0.0], dtype=np.float32), start, goal) == 0.0 + # A coincident start and goal has no gap to close. + assert metrics.soft_progress(np.array([0.0, 0.0, 0.0], dtype=np.float32), start, start) == 0.0 + + +def test_recording_applies_the_odometry_rotation(tmp_path: Path) -> None: + """Clouds are sensor-frame, so a yawed pose must rotate them, not just shift.""" + db = tmp_path / "yaw.db" + ahead = np.array([[1.0, 0.0, 0.0]], dtype=np.float32) + # 90 degrees about +z, so the sensor's +x points along world +y. + quarter = np.sqrt(0.5) + with SqliteStore(path=str(db)) as store: + store.stream("lidar", PointCloud2).append( + PointCloud2.from_numpy(ahead, frame_id="lidar", timestamp=1.0), ts=1.0 + ) + store.stream("odom", Odometry).append( + Odometry(ts=1.0, pose=Pose(5.0, 0.0, 0.0, 0.0, 0.0, quarter, quarter)), ts=1.0 + ) + + frames = list(iter_world_frames(db, "lidar", "odom")) + assert np.allclose(frames[0].points, [[5.0, 1.0, 0.0]], atol=1e-5) + + +def test_final_map_cache_key_tracks_the_recording(tmp_path: Path) -> None: + """Without this a re-ingested dataset is graded against the previous map.""" + db = tmp_path / "rec.db" + _write_recording(db, [(1.0, _floor(0.0, 1.0), "lidar")], [(1.0, (0.0, 0.0, 0.5))]) + suite = Suite(dataset="rec", cases=[], db=str(db), lidar_stream="lidar", odom_stream="odom") + before = final_map._cache_path(db, final_map._final_params(db, suite, _cfg())) + + # Same path, same stem, same config: only the bytes change. + _write_recording(db, [(1.0, _floor(0.0, 5.0), "lidar")], [(1.0, (0.0, 0.0, 0.5))]) + after = final_map._cache_path(db, final_map._final_params(db, suite, _cfg())) + assert before != after + + +def test_checkpoints_reload_from_cache(tmp_path: Path) -> None: + """The npz delta format is only exercised on a cache hit, which no other + test reaches because each one gets a fresh tmp_path.""" + db = tmp_path / "rec.db" + _write_recording( + db, + [(float(t), _floor(float(t), float(t) + 1.0), "lidar") for t in range(1, 5)], + [(float(t), (float(t), 0.0, 0.0)) for t in range(1, 5)], + ) + suite = Suite(dataset="rec", cases=[], db=str(db), lidar_stream="lidar", odom_stream="odom") + times = np.array([2.5, np.inf]) + with pytest.MonkeyPatch.context() as mp: + mp.setattr(EvalConfig, "make_mapper", lambda _self: _StubMapper()) + mp.setattr(final_map, "CACHE_SUBDIR", tmp_path / "cache") + cold = final_map.load_or_build_checkpoints(suite, _cfg(), times) + warm = final_map.load_or_build_checkpoints(suite, _cfg(), times) + + assert [s.tolist() for s in cold.iter_snapshots()] == [ + s.tolist() for s in warm.iter_snapshots() + ] + assert len(list(warm.iter_snapshots())) == 2 + + +def test_a_pipeline_without_graph_layers_still_records(tmp_path: Path) -> None: + """Recording a run must not require a pipeline to expose its internals, + which is the whole point of the optional introspection protocol.""" + suite = _corridor_recording(tmp_path / "corridor.db") + suite.cases = [Case(id="auto_00", start=(4.0, 0.0, 0.0), goal=(2.0, 0.0, 0.0), tags=["auto"])] + route = np.array([[4, 0, 0], [3, 0, 0], [2, 0, 0]], dtype=np.float32) + with pytest.MonkeyPatch.context() as mp: + _stub_harness(mp, tmp_path, _StubPipeline(route)) + result = runner.run_suite(suite, _cfg(pipeline="stub"), keep_artifacts=True) + + assert result.final_artifacts is None + assert result.cases[0].online_artifacts is None + # The occupied cloud is the evaluator's own, so it is kept either way. + assert result.cases[0].online_occupied is not None diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 084288f00e..7e0bb09cac 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -23,7 +23,7 @@ from dimos.navigation.nav_3d.evaluator import metrics from dimos.navigation.nav_3d.evaluator.final_map import load_or_build_final_map -from dimos.navigation.nav_3d.evaluator.recording import load_trajectory +from dimos.navigation.nav_3d.mls_planner.viz import clearance_colors from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -34,10 +34,18 @@ from dimos.navigation.nav_3d.evaluator.cases import Suite from dimos.navigation.nav_3d.evaluator.config import EvalConfig - from dimos.navigation.nav_3d.evaluator.runner import PlannerArtifacts, PlanOutcome, Report + from dimos.navigation.nav_3d.evaluator.runner import ( + CaseResult, + PlannerArtifacts, + PlanOutcome, + Report, + ) logger = setup_logger() +# Voxels are drawn well inside their cell so the surface reads as a grid. +VOXEL_RADIUS_SCALE = 0.25 +ENDPOINT_RADIUS = 0.05 WALKED_PATH_COLOR = [255, 255, 255] START_COLOR = [0, 255, 255] GOAL_COLOR = [255, 140, 0] @@ -71,10 +79,8 @@ def turbo_by_height(points: NDArray[np.float32]) -> NDArray[np.uint8]: def _clearance_colors(clearance: NDArray[np.float32], hard_clearance: float) -> NDArray[np.uint8]: - norm = np.clip(np.nan_to_num(clearance / CLEARANCE_CLAMP_M, nan=1.0, posinf=1.0), 0.0, 1.0) - blocked = np.array([4.0, 8.0, 48.0]) - clear = np.array([150.0, 200.0, 255.0]) - out = np.asarray(blocked + norm[:, None] * (clear - blocked), dtype=np.uint8) + """The planner's clearance ramp, with cells below the hard limit called out.""" + out: NDArray[np.uint8] = clearance_colors(clearance, CLEARANCE_CLAMP_M) out[clearance < hard_clearance] = NEAR_WALL_COLOR return out @@ -99,7 +105,7 @@ def _log_planner(entity: str, artifacts: PlannerArtifacts | None, cfg: EvalConfi rr.Points3D( surface[:, :3], colors=_clearance_colors(surface[:, 3], CLEARANCE_NEAR_WALL_M), - radii=cfg.voxel_size / 4, + radii=cfg.voxel_size * VOXEL_RADIUS_SCALE, ), static=True, ) @@ -197,6 +203,55 @@ def _dataset_view(root: str, case_ids: list[str]) -> rrb.Spatial3DView: ) +def _log_case(base: str, case: CaseResult, cfg: EvalConfig) -> None: + """Endpoints, intent line, the map known at plan time, and both paths.""" + import rerun as rr + + rr.log( + f"{base}/start", + rr.Points3D([case.start], colors=[START_COLOR], radii=ENDPOINT_RADIUS), + static=True, + ) + rr.log( + f"{base}/goal", + rr.Points3D([case.goal], colors=[GOAL_COLOR], radii=ENDPOINT_RADIUS), + static=True, + ) + if case.expect_fail: + # Always visible, so a correct refusal is reviewable too. + rr.log( + f"{base}/intent", + rr.LineStrips3D([[case.start, case.goal]], colors=[NEGATIVE_INTENT_COLOR], radii=0.006), + static=True, + ) + elif not case.online.success: + rr.log( + f"{base}/intent", + rr.LineStrips3D([[case.start, case.goal]], colors=[INVALID_PATH_COLOR], radii=0.003), + static=True, + ) + # The incremental map at plan time, saved for every case. + _log_planner(f"{base}/known", case.online_artifacts, cfg) + if case.online_occupied is not None and len(case.online_occupied): + rr.log( + f"{base}/known/voxels", + rr.Points3D( + case.online_occupied, + colors=turbo_by_height(case.online_occupied), + radii=cfg.voxel_size * VOXEL_RADIUS_SCALE, + ), + static=True, + ) + if case.blocking_points: + rr.log( + f"{base}/new_occupancy", + rr.Points3D(case.blocking_points, colors=[DYNAMIC_BLOCK_COLOR], radii=0.06), + static=True, + ) + _log_path(f"{base}/online", case.online, radius=0.04, cfg=cfg) + _log_path(f"{base}/final", case.final, radius=0.02, cfg=cfg) + + def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) -> None: import rerun as rr import rerun.blueprint as rrb @@ -208,9 +263,8 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - suites_by_dataset = {suite.dataset: suite for suite in suites} for dataset in report.datasets: suite = suites_by_dataset[dataset.dataset] - db_path = suite.db_path() - final = load_or_build_final_map(db_path, suite, cfg) - trajectory = load_trajectory(db_path, suite.odom_stream, suite.end_ts_seconds()) + final = load_or_build_final_map(suite, cfg) + trajectory = suite.trajectory() root = dataset.dataset rr.log( @@ -218,7 +272,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - rr.Points3D( final.occupied, colors=turbo_by_height(final.occupied), - radii=cfg.voxel_size / 4, + radii=cfg.voxel_size * VOXEL_RADIUS_SCALE, ), static=True, ) @@ -232,54 +286,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - _log_planner(f"{root}/planner_final", dataset.final_artifacts, cfg) for case in dataset.cases: - base = f"{root}/cases/{case.id}" - rr.log( - f"{base}/start", - rr.Points3D([case.start], colors=[START_COLOR], radii=0.05), - static=True, - ) - rr.log( - f"{base}/goal", - rr.Points3D([case.goal], colors=[GOAL_COLOR], radii=0.05), - static=True, - ) - if case.expect_fail: - # Always visible, so a correct refusal is reviewable too. - rr.log( - f"{base}/intent", - rr.LineStrips3D( - [[case.start, case.goal]], colors=[NEGATIVE_INTENT_COLOR], radii=0.006 - ), - static=True, - ) - elif not case.online.success: - rr.log( - f"{base}/intent", - rr.LineStrips3D( - [[case.start, case.goal]], colors=[INVALID_PATH_COLOR], radii=0.003 - ), - static=True, - ) - # The incremental map at plan time, saved for every case. - _log_planner(f"{base}/known", case.online_artifacts, cfg) - if case.online_occupied is not None and len(case.online_occupied): - rr.log( - f"{base}/known/voxels", - rr.Points3D( - case.online_occupied, - colors=turbo_by_height(case.online_occupied), - radii=cfg.voxel_size / 4, - ), - static=True, - ) - if case.blocking_points: - rr.log( - f"{base}/new_occupancy", - rr.Points3D(case.blocking_points, colors=[DYNAMIC_BLOCK_COLOR], radii=0.06), - static=True, - ) - _log_path(f"{base}/online", case.online, radius=0.04, cfg=cfg) - _log_path(f"{base}/final", case.final, radius=0.02, cfg=cfg) + _log_case(f"{root}/cases/{case.id}", case, cfg) views = [_dataset_view(d.dataset, [c.id for c in d.cases]) for d in report.datasets] rr.send_blueprint(rrb.Blueprint(rrb.Tabs(*views) if len(views) > 1 else views[0])) diff --git a/dimos/navigation/nav_3d/evaluator/voxel_keys.py b/dimos/navigation/nav_3d/evaluator/voxel_keys.py index dd82f16221..d5d9b380ad 100644 --- a/dimos/navigation/nav_3d/evaluator/voxel_keys.py +++ b/dimos/navigation/nav_3d/evaluator/voxel_keys.py @@ -21,29 +21,28 @@ import numpy as np +from dimos.mapping.voxels.keys import ( + KEY_OFFSET, + X_SHIFT, + Y_SHIFT, + pack_indices, + unpack_keys, +) + if TYPE_CHECKING: from numpy.typing import NDArray -_KEY_OFFSET = 1 << 20 -_FIELD_BITS = 21 -_X_SHIFT = 2 * _FIELD_BITS -_Y_SHIFT = _FIELD_BITS -_FIELD_MASK = (1 << _FIELD_BITS) - 1 - def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: - """Pack voxel indices into sortable int64 keys, one per point.""" - idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + _KEY_OFFSET - return (idx[:, 0] << _X_SHIFT) | (idx[:, 1] << _Y_SHIFT) | idx[:, 2] + """Pack voxel indices into sortable int64 keys, one per point. Quantizes in + float64 so no point lands in a neighboring voxel to float32 rounding.""" + idx = np.floor(points.astype(np.float64) / voxel_size).astype(np.int64) + KEY_OFFSET + return pack_indices(idx) def key_centers(keys: NDArray[np.int64], voxel_size: float) -> NDArray[np.float32]: """Voxel center positions for packed keys, the inverse of voxel_keys.""" - idx = ( - np.stack([keys >> _X_SHIFT, (keys >> _Y_SHIFT) & _FIELD_MASK, keys & _FIELD_MASK], axis=1) - - _KEY_OFFSET - ) - return ((idx + 0.5) * voxel_size).astype(np.float32) + return ((unpack_keys(keys) + 0.5) * voxel_size).astype(np.float32) def keys_contain(sorted_keys: NDArray[np.int64], query: NDArray[np.int64]) -> NDArray[np.bool_]: @@ -71,16 +70,6 @@ def cylinder_offsets( def offset_deltas(offsets: NDArray[np.int64]) -> NDArray[np.int64]: - """Packed key deltas for integer voxel offsets. - - Valid because the fields sit far from their bounds, so a packed add carries - no bits between them. - """ - return np.asarray((offsets[:, 0] << _X_SHIFT) + (offsets[:, 1] << _Y_SHIFT) + offsets[:, 2]) - - -def offset_keys( - points: NDArray[np.float32], offsets: NDArray[np.int64], voxel_size: float -) -> NDArray[np.int64]: - """Keys of every (point voxel + offset) pair, shape (P, O).""" - return voxel_keys(points, voxel_size)[:, None] + offset_deltas(offsets)[None, :] + """Packed key deltas for integer voxel offsets. Valid because the fields sit + far from their bounds, so a packed add carries no bits between them.""" + return np.asarray((offsets[:, 0] << X_SHIFT) + (offsets[:, 1] << Y_SHIFT) + offsets[:, 2]) diff --git a/dimos/navigation/nav_3d/mls_planner/viz.py b/dimos/navigation/nav_3d/mls_planner/viz.py index 2964eb6dda..41e4f4ed07 100644 --- a/dimos/navigation/nav_3d/mls_planner/viz.py +++ b/dimos/navigation/nav_3d/mls_planner/viz.py @@ -33,11 +33,22 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 if TYPE_CHECKING: + from numpy.typing import NDArray from rerun._baseclasses import Archetype # Small lift so graph artifacts render visibly above the surface points instead of z-fighting. _GRAPH_Z_LIFT = 0.05 +TIGHT_COLOR = (4.0, 8.0, 48.0) +OPEN_COLOR = (150.0, 200.0, 255.0) + + +def clearance_colors(clearance: NDArray[np.float32], clamp_m: float) -> NDArray[np.uint8]: + """Blue ramp from tight to open, saturating at clamp_m of clearance.""" + norm = np.clip(np.nan_to_num(clearance / clamp_m, nan=1.0, posinf=1.0), 0.0, 1.0) + tight, open_ = np.array(TIGHT_COLOR), np.array(OPEN_COLOR) + return np.asarray(tight + norm[:, None] * (open_ - tight), dtype=np.uint8) + def render_surface_map( msg: PointCloud2, @@ -58,13 +69,9 @@ def render_surface_map( return msg.to_rerun(voxel_size=voxel_size, colors=[40, 75, 130]) passable = clearance >= wall_clearance_m pts, clearance = pts[passable], clearance[passable] - norm = np.clip(np.nan_to_num(clearance / clearance_clamp_m, nan=1.0, posinf=1.0), 0.0, 1.0) - tight = np.array([4.0, 8.0, 48.0]) - open_ = np.array([150.0, 200.0, 255.0]) - colors = (tight + norm[:, None] * (open_ - tight)).astype(np.uint8) return rr.Points3D( positions=pts, - colors=colors, + colors=clearance_colors(clearance, clearance_clamp_m), radii=voxel_size * 0.5, ) From 452f875ee3bd6a391a1bf6a6a504ca98c134fbe6 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 4 Aug 2026 15:56:14 -0700 Subject: [PATCH 29/29] Match some colors --- dimos/navigation/nav_3d/evaluator/cases.py | 7 +++ dimos/navigation/nav_3d/evaluator/generate.py | 11 +++-- dimos/navigation/nav_3d/evaluator/runner.py | 32 ++++++-------- dimos/navigation/nav_3d/evaluator/viz.py | 43 +++++++++++++++---- 4 files changed, 63 insertions(+), 30 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index f87d2ca9a8..33483bead4 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -21,6 +21,7 @@ from pathlib import Path from typing import TYPE_CHECKING +from dimos.constants import DIMOS_PROJECT_ROOT from dimos.utils.data import resolve_named_path if TYPE_CHECKING: @@ -147,6 +148,12 @@ def save_suite(suite: Suite, path: Path) -> Path: """Write the suite manifest as YAML.""" import yaml + if path.is_relative_to(MANIFEST_DIR) and not (DIMOS_PROJECT_ROOT / ".git").exists(): + raise RuntimeError( + f"case manifests live in the dimos source tree ({MANIFEST_DIR}); " + "run ingest and curation from a git checkout" + ) + doc: dict[str, object] = {"dataset": suite.dataset} if suite.db is not None: doc["db"] = suite.db diff --git a/dimos/navigation/nav_3d/evaluator/generate.py b/dimos/navigation/nav_3d/evaluator/generate.py index 11cedaf4d6..9f0957c45c 100644 --- a/dimos/navigation/nav_3d/evaluator/generate.py +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -238,9 +238,14 @@ def generate_cases( def _bin_key( start: NDArray[np.float32], goal: NDArray[np.float32], dz: float, bin_size_m: float -) -> tuple[int, ...]: - bins = np.floor(np.array([*start[:2], *goal[:2]]) / bin_size_m).astype(int) - return (*bins, int(np.sign(dz)) if abs(dz) >= STAIRS_DZ_M else 0) +) -> tuple[int, int, int, int, int]: + return ( + int(start[0] // bin_size_m), + int(start[1] // bin_size_m), + int(goal[0] // bin_size_m), + int(goal[1] // bin_size_m), + (1 if dz > 0 else -1) if abs(dz) >= STAIRS_DZ_M else 0, + ) def _is_duplicate(cand: Candidate, accepted: list[Candidate], radius: float) -> bool: diff --git a/dimos/navigation/nav_3d/evaluator/runner.py b/dimos/navigation/nav_3d/evaluator/runner.py index c5d259a3db..eddb0fb89f 100644 --- a/dimos/navigation/nav_3d/evaluator/runner.py +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -376,18 +376,6 @@ def _score_final( ) -> list[CaseResult]: """Plan every case against the completed map and combine both phases.""" map_keys = final.occupied_keys - - def result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: - return CaseResult( - id=case.id, - dataset=suite.dataset, - start=case.start, - goal=case.goal, - tags=case.tags, - l_ref=ref.length, - **rest, # type: ignore[arg-type] - ) - results: list[CaseResult] = [] for ci, case in enumerate(suite.cases): ref = refs[ci] @@ -398,9 +386,13 @@ def result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: final_out = score_negative(final_out) if final_only[ci]: results.append( - result( - case, - ref, + CaseResult( + id=case.id, + dataset=suite.dataset, + start=case.start, + goal=case.goal, + tags=case.tags, + l_ref=ref.length, online_voxels=len(final.occupied), expect_fail=case.expect_fail, online=final_out, @@ -420,9 +412,13 @@ def result(case: Case, ref: metrics.Reference, **rest: object) -> CaseResult: ) ) results.append( - result( - case, - ref, + CaseResult( + id=case.id, + dataset=suite.dataset, + start=case.start, + goal=case.goal, + tags=case.tags, + l_ref=ref.length, online_voxels=len(run.map_keys), expect_fail=False, online=run.outcome, diff --git a/dimos/navigation/nav_3d/evaluator/viz.py b/dimos/navigation/nav_3d/evaluator/viz.py index 7e0bb09cac..e132547b59 100644 --- a/dimos/navigation/nav_3d/evaluator/viz.py +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -46,6 +46,17 @@ # Voxels are drawn well inside their cell so the surface reads as a grid. VOXEL_RADIUS_SCALE = 0.25 ENDPOINT_RADIUS = 0.05 +EDGE_RADIUS = 0.008 +WALKED_RADIUS = 0.015 +BLOCKING_RADIUS = 0.06 +# The online path is drawn over the final one, so it is the thicker of the two. +ONLINE_PATH_RADIUS = 0.04 +FINAL_PATH_RADIUS = 0.02 +# A refusal is reviewed on its own, an unreachable goal only against its path. +NEGATIVE_INTENT_RADIUS = 0.006 +FAILED_INTENT_RADIUS = 0.003 +# Gate violations are drawn as points on their path, wide enough to spot. +VIOLATION_RADIUS_SCALE = 3.0 WALKED_PATH_COLOR = [255, 255, 255] START_COLOR = [0, 255, 255] GOAL_COLOR = [255, 140, 0] @@ -116,7 +127,7 @@ def _log_planner(entity: str, artifacts: PlannerArtifacts | None, cfg: EvalConfi rr.LineStrips3D( edges[:, :6].reshape(-1, 2, 3), colors=_edge_cost_colors(edges[:, 6]), - radii=0.008, + radii=EDGE_RADIUS, ), static=True, ) @@ -178,13 +189,21 @@ def _log_path(entity: str, outcome: PlanOutcome, radius: float, cfg: EvalConfig) if outcome.unsupported: rr.log( f"{entity}/unsupported", - rr.Points3D(outcome.unsupported, colors=[UNSUPPORTED_COLOR], radii=radius * 3), + rr.Points3D( + outcome.unsupported, + colors=[UNSUPPORTED_COLOR], + radii=radius * VIOLATION_RADIUS_SCALE, + ), static=True, ) if outcome.steep: rr.log( f"{entity}/steep", - rr.Points3D(outcome.steep, colors=[STEEP_COLOR], radii=radius * 3), + rr.Points3D( + outcome.steep, + colors=[STEEP_COLOR], + radii=radius * VIOLATION_RADIUS_SCALE, + ), static=True, ) @@ -221,13 +240,19 @@ def _log_case(base: str, case: CaseResult, cfg: EvalConfig) -> None: # Always visible, so a correct refusal is reviewable too. rr.log( f"{base}/intent", - rr.LineStrips3D([[case.start, case.goal]], colors=[NEGATIVE_INTENT_COLOR], radii=0.006), + rr.LineStrips3D( + [[case.start, case.goal]], + colors=[NEGATIVE_INTENT_COLOR], + radii=NEGATIVE_INTENT_RADIUS, + ), static=True, ) elif not case.online.success: rr.log( f"{base}/intent", - rr.LineStrips3D([[case.start, case.goal]], colors=[INVALID_PATH_COLOR], radii=0.003), + rr.LineStrips3D( + [[case.start, case.goal]], colors=[INVALID_PATH_COLOR], radii=FAILED_INTENT_RADIUS + ), static=True, ) # The incremental map at plan time, saved for every case. @@ -245,11 +270,11 @@ def _log_case(base: str, case: CaseResult, cfg: EvalConfig) -> None: if case.blocking_points: rr.log( f"{base}/new_occupancy", - rr.Points3D(case.blocking_points, colors=[DYNAMIC_BLOCK_COLOR], radii=0.06), + rr.Points3D(case.blocking_points, colors=[DYNAMIC_BLOCK_COLOR], radii=BLOCKING_RADIUS), static=True, ) - _log_path(f"{base}/online", case.online, radius=0.04, cfg=cfg) - _log_path(f"{base}/final", case.final, radius=0.02, cfg=cfg) + _log_path(f"{base}/online", case.online, radius=ONLINE_PATH_RADIUS, cfg=cfg) + _log_path(f"{base}/final", case.final, radius=FINAL_PATH_RADIUS, cfg=cfg) def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) -> None: @@ -279,7 +304,7 @@ def write_rrd(report: Report, suites: list[Suite], cfg: EvalConfig, out: Path) - foot = trajectory.foot(cfg.robot_height) rr.log( f"{root}/walked_path", - rr.LineStrips3D([foot], colors=[WALKED_PATH_COLOR], radii=0.015), + rr.LineStrips3D([foot], colors=[WALKED_PATH_COLOR], radii=WALKED_RADIUS), static=True, )