diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index ee6a92f457..2d7380a4e8 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -755,6 +755,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/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/blueprints.py b/dimos/navigation/nav_3d/evaluator/blueprints.py deleted file mode 100644 index 348b498e10..0000000000 --- a/dimos/navigation/nav_3d/evaluator/blueprints.py +++ /dev/null @@ -1,89 +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 - -from typing import TYPE_CHECKING - -from dimos.core.coordination.blueprints import autoconnect -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.navigation.nav_3d.mls_planner.viz import ( - render_node_edges, - render_nodes, - render_surface_map, -) -from dimos.visualization.rerun.bridge import RerunBridgeModule -from dimos.visualization.rerun.websocket_server import RerunWebSocketServer - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -_POSE_MARKER_RADIUS = 0.4 - - -def _render_start_pose(msg: PoseStamped) -> Archetype: - import rerun as rr - - 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: - import rerun as rr - - 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]) - - -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, - # The evaluator exists to inspect the planner, so these are always on. - "world/surface_map": render_surface_map, - "world/nodes": render_nodes, - "world/node_edges": render_node_edges, - } - ), -) diff --git a/dimos/navigation/nav_3d/evaluator/case_manifests/china_office.yaml b/dimos/navigation/nav_3d/evaluator/case_manifests/china_office.yaml new file mode 100644 index 0000000000..f22f67d7de --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/case_manifests/china_office.yaml @@ -0,0 +1,143 @@ +dataset: china_office +cases: +- id: auto_00_up + start: [-10.2, 15.8, -0.48] + goal: [-8.12, 14.6, 2.56] + tags: [auto, stairs, up, long] +- id: auto_01_down + start: [-3.72, -4.6, 2.96] + goal: [0.04, 0.04, -0.4] + tags: [auto, stairs, down, long] +- id: auto_02_up + start: [4.12, -21.72, -0.48] + goal: [9.56, -14.28, 4.24] + tags: [auto, stairs, up, long] +- id: auto_03_down + start: [2.52, 10.84, 3.12] + goal: [5.64, 23.64, -0.24] + tags: [auto, stairs, down, long] +- id: auto_04_down + start: [10.44, -4.12, 4.16] + goal: [16.52, -32.28, -0.96] + tags: [auto, stairs, down, long] +- id: auto_05_up + start: [18.2, -20.6, -1.36] + goal: [-1.4, -14.2, 2.96] + tags: [auto, stairs, up, long] +- id: auto_06_down + start: [-4.6, 4.52, 6.32] + goal: [11.72, 6.36, -0.96] + tags: [auto, stairs, down, long] +- id: auto_07_down + start: [6.36, 2.6, 3.2] + goal: [15.4, -9.88, -1.28] + tags: [auto, stairs, down, long] +- id: auto_08_up + start: [7.72, -33.8, -0.72] + goal: [-3.32, 11.32, 2.88] + tags: [auto, stairs, up, long] +- id: auto_09_down + start: [12.2, -9.24, 4.16] + goal: [3.56, -7.72, -0.4] + tags: [auto, stairs, down, long] +- id: auto_10_down + start: [-7.4, 7.32, 2.8] + goal: [9.8, 14.04, -0.56] + tags: [auto, stairs, down, long] +- id: auto_11_down + start: [-2.52, 0.68, 4.0] + goal: [13.4, -1.16, -1.2] + tags: [auto, stairs, down, long] +- id: auto_12_down + start: [6.28, -2.04, 4.16] + goal: [3.32, 16.12, -0.24] + tags: [auto, stairs, down, long] +- id: auto_13_down + start: [-1.72, -9.72, 2.96] + goal: [5.64, -13.56, -0.4] + tags: [auto, stairs, down, long] +- id: auto_14_up + start: [7.48, -27.48, -0.48] + goal: [-6.2, 3.48, 2.88] + tags: [auto, stairs, up, long] +- id: auto_15_up + start: [19.8, -27.8, -1.12] + goal: [2.6, -16.2, 2.08] + tags: [auto, stairs, up, long] +- id: auto_16_up + start: [0.6, 6.6, -0.4] + goal: [2.04, -0.28, 3.12] + tags: [auto, stairs, up, long] +- id: auto_17_down + start: [4.28, 5.96, 3.12] + goal: [17.0, -15.48, -1.28] + tags: [auto, stairs, down, long] +- id: auto_18_down + start: [-4.6, 8.2, 6.56] + goal: [8.84, 18.84, -0.4] + tags: [auto, stairs, down, long] +- id: auto_19_up + start: [-8.84, 10.76, -0.48] + goal: [-1.4, -2.04, 3.12] + tags: [auto, stairs, up, long] +- id: auto_20_up + start: [14.6, -27.88, -0.24] + goal: [1.0, 13.56, 2.96] + tags: [auto, stairs, up, long] +- id: auto_21_down + start: [-7.4, 11.96, 3.04] + goal: [8.2, -17.32, -0.4] + 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] + tags: [auto, stairs, up, long] +- id: auto_23_up + start: [-0.28, 10.92, -0.4] + goal: [0.6, -12.52, 3.12] + tags: [auto, stairs, up, long] +- id: auto_24_flat + start: [12.52, -33.4, -0.8] + goal: [14.52, -5.96, -1.28] + tags: [auto, flat] +- id: auto_25_flat + start: [10.68, 9.88, -0.72] + goal: [5.24, 12.52, -0.24] + tags: [auto, flat] +- id: auto_26_flat + start: [19.16, -24.44, -1.36] + goal: [12.6, 2.68, -1.04] + tags: [auto, flat] +- id: auto_27_flat + start: [3.56, -18.36, 0.16] + goal: [8.92, -13.32, -0.32] + tags: [auto, flat] +- id: auto_28_flat + start: [-4.92, 11.0, -0.4] + goal: [10.6, -34.92, -0.8] + tags: [auto, flat] +- id: auto_29_flat + start: [-0.12, -17.08, 2.08] + goal: [2.76, 7.4, 1.6] + tags: [auto, flat] +- id: auto_30_flat + start: [6.04, -24.2, -0.48] + goal: [7.0, 14.12, -0.08] + tags: [auto, flat] +- id: auto_31_flat + start: [8.36, -30.44, -0.48] + goal: [18.28, -32.28, -0.96] + tags: [auto, flat] +- id: auto_32_flat + start: [11.32, 8.12, -0.88] + goal: [5.72, -9.72, -0.4] + tags: [auto, flat] +- id: neg_00 + start: [5.0, 2.44, 3.2] + goal: [9.32, 5.24, 6.4] + tags: [manual, negative] + expect_fail: true +- id: manual_00 + start: [13.88, 1.48, -1.04] + goal: [12.28, 8.6, -0.8] + tags: [manual, flat] diff --git a/dimos/navigation/nav_3d/evaluator/case_manifests/mid360_athens_stairs.yaml b/dimos/navigation/nav_3d/evaluator/case_manifests/mid360_athens_stairs.yaml new file mode 100644 index 0000000000..ca53f2a91a --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/case_manifests/mid360_athens_stairs.yaml @@ -0,0 +1,203 @@ +dataset: mid360_athens_stairs +end_ts: 1783527459453386006 +cases: +- 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_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.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: [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: [-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] +- id: auto_08_down + start: [-0.04, -1.96, 2.56] + goal: [7.16, -3.8, -3.12] + tags: [auto, stairs, down, long] +- 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] diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py new file mode 100644 index 0000000000..33483bead4 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -0,0 +1,181 @@ +# 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, one YAML file per dataset. Endpoints are foot-level world +coordinates.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +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: + 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.""" + return MANIFEST_DIR / f"{dataset}.yaml" + + +@dataclass +class Case: + id: str + start: tuple[float, float, float] + goal: tuple[float, float, float] + tags: list[str] = field(default_factory=list) + # If the pair should not have a valid path, this is assigned by humans + expect_fail: bool = False + # 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 + + +@dataclass +class Suite: + dataset: str + cases: list[Case] + 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 + # Discard frames after this timestamp + end_ts: int | 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 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") + 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"]) + 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), + goal=(gx, gy, gz), + tags=[str(t) for t in entry.get("tags", [])], + expect_fail=expect_fail, + expect_final_fail=expect_final_fail, + ) + 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", 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, + ) + + +def load_suites(paths: list[Path] | None = None) -> list[Suite]: + """Load the given manifests, or every manifest under case_manifests/.""" + if paths is None: + paths = sorted(MANIFEST_DIR.glob("*.yaml")) + if not paths: + 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) -> 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 + if suite.lidar_stream != LIDAR_STREAM: + doc["lidar_stream"] = suite.lidar_stream + 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 + 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], + "tags": case.tags, + } + 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)) + return path diff --git a/dimos/navigation/nav_3d/evaluator/cli.py b/dimos/navigation/nav_3d/evaluator/cli.py new file mode 100644 index 0000000000..9bae452402 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/cli.py @@ -0,0 +1,403 @@ +# 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, mounted as the dimos nav-eval sub-command.""" + +from __future__ import annotations + +import contextlib +import dataclasses +import json +import os +from pathlib import Path +import sqlite3 +from typing import TYPE_CHECKING + +import typer + +from dimos.navigation.nav_3d.evaluator.cases import ( + 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 ( + MIN_CASES, + generate_cases, + resolve_max_cases, +) +from dimos.navigation.nav_3d.evaluator.metrics import ground_truth_route +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: + from dimos.navigation.nav_3d.evaluator.curation import CaseStore + 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 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) + # 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 + + +def _score_cell(outcome: PlanOutcome) -> str: + 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} {'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"{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)) + for d in report.datasets: + print( + f"{d.dataset}: {d.frames} frames, " + 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(): + 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_online} " + f"fin {report.n_success_final}/{report.n_cases} | " + f"outcomes {report.outcome_counts} | " + f"plan p95 {report.plan_ms['p95']:.1f}ms | " + f"ingest p95 {report.map_update_ms['p95']:.1f}ms/frame" + ) + 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: dimos nav-eval tag --final-fail") + if others: + print(f" not explained by a new obstacle, inspect final map: {', '.join(others)}") + + +@app.command() +def run( + manifests: list[Path] | None = typer.Argument( + None, help="Suite YAMLs; defaults to every manifest under case_manifests/" + ), + 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 | 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] | None = typer.Option( + 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.""" + suites = load_suites(manifests or None) + if dataset is not None: + 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}") + 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, keep_artifacts=rrd_out is not None) + _print_report(report) + 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: + # 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) + + +def _copy_recording(src: Path, dest: Path) -> None: + """Copy via the sqlite backup API so WAL sidecar content is never lost.""" + with ( + contextlib.closing(sqlite3.connect(src)) as source, + contextlib.closing(sqlite3.connect(dest)) as target, + ): + source.backup(target) + + +@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"), + 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.""" + src = source / "mem2.db" if source.is_dir() else source + if not src.exists(): + raise typer.BadParameter(f"{src} does not exist") + manifest = manifest_path(name) + if manifest.exists() and not force: + raise typer.BadParameter(f"{manifest} already exists; pass --force to regenerate") + 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 = suite.trajectory() + 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}]" + ) + cfg = EvalConfig() + 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) + suite.cases = generate_cases(trajectory, final, surface, cfg, max_cases, min_cases) + if not suite.cases: + raise typer.Exit(code=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 " + "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}: [{', '.join(case.tags)}]") + print(f"\nrun with: dimos nav-eval run --dataset {name}") + + +def _open_store(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"), + 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 | 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_store(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") +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.""" + 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}") + 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("retag") +def retag( + dataset: str = typer.Argument(..., help="Dataset whose manifest gets retagged"), +) -> None: + """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, cfg) + trajectory = suite.trajectory() + 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) + 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"), +) -> None: + """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 + + store = _open_store(dataset) + trajectory = store.suite.trajectory() + foot = trajectory.foot(store.cfg.robot_height) + pick_cases(store, foot) + print(f"\nrun with: dimos nav-eval run --dataset {dataset}") + + +@app.command("list") +def list_cases() -> None: + """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)}{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..e38b8c9cca --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/config.py @@ -0,0 +1,82 @@ +# 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, field +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper + + +@dataclass +class EvalConfig: + """Harness and gate parameters, sized for the Unitree Go2.""" + + voxel_size: float = 0.08 + max_range: float = 30.0 + robot_height: float = 0.3 + + # 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 + # Ground-support reach. The radius models straddling small scan holes. + support_radius_m: float = 0.35 + support_depth_m: float = 0.35 + # 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 is off the map. + snap_max_m: float = 1.0 + + # Which pipeline is under test, by registry name. + pipeline: str = "mls" + # 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. + from dimos.mapping.ray_tracing.voxel_map import 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.""" + 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 new file mode 100644 index 0000000000..ba4d2ffb90 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/curation.py @@ -0,0 +1,168 @@ +# 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, shared by the CLI and the browser picker.""" + +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 ( + 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 +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.final_map import FinalMap + +logger = setup_logger() + +Point = tuple[float, float, float] + + +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.""" + + suite: Suite + 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 + ) + 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. + 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])) + + 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.path}") + self.suite.cases.append(case) + self.save() + kind = "negative (must refuse)" if expect_fail else "positive" + logger.info( + "added case", + kind=kind, + case=case.id, + start=case.start, + goal=case.goal, + manifest=self.path, + ) + 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, _provenance(case.tags)) + 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.path}") + return case + + def save(self) -> None: + save_suite(self.suite, self.path) + + +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. 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) -> CaseStore: + """Open a dataset's manifest with the final map and surface it snaps to.""" + 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, cfg) + return CaseStore(suite, final.standable_surface(cfg.robot_height), cfg, final) 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/final_map.py b/dimos/navigation/nav_3d/evaluator/final_map.py new file mode 100644 index 0000000000..094a27194b --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/final_map.py @@ -0,0 +1,236 @@ +# 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 evaluator's own map of a recording, and the checkpoints along it.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import json +import os +from time import perf_counter +from typing import TYPE_CHECKING, Any + +import numpy as np + +from dimos.constants import CACHE_DIR +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: + 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() + + +@dataclass +class FinalMap: + voxel_size: float + occupied: NDArray[np.float32] + occupied_keys: NDArray[np.int64] + build_ms: float + + 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.""" + 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: + """The mapper's occupied set at increasing times, delta-encoded.""" + + times: NDArray[np.float64] + added: list[NDArray[np.int64]] + removed: list[NDArray[np.int64]] + + 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 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 + + +CACHE_VERSION = 4 +CHECKPOINT_CACHE_VERSION = 4 + + +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) + 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 CACHE_SUBDIR / f"{db_path.stem}.{digest}.npz" + + +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 + return params + + +def replay_frames( + frames: Iterable[Frame], + mapper: VoxelRayMapper, + voxel_size: float, + 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.""" + snapshots: list[NDArray[np.int64]] = [] + 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))) + mapper.add_frame(frame.points, frame.origin) + build_ms = (perf_counter() - t0) * 1000 + occupied = mapper.global_map() + occupied_keys = np.unique(voxel_keys(occupied, voxel_size)) + while len(snapshots) < len(times): + snapshots.append(occupied_keys) + final = FinalMap( + voxel_size=voxel_size, + occupied=occupied, + occupied_keys=occupied_keys, + build_ms=build_ms, + ) + return final, snapshots + + +def _save_final(cache: Path, final: FinalMap) -> None: + _save_npz( + cache, + occupied=final.occupied, + occupied_keys=final.occupied_keys, + ) + logger.info("final map cached", cache=cache.name, voxels=len(final.occupied)) + + +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) + return FinalMap( + voxel_size=cfg.voxel_size, + occupied=data["occupied"], + occupied_keys=data["occupied_keys"], + build_ms=0.0, + ) + + logger.info("building final map (cache miss)", recording=db_path.name) + final, _ = replay_frames( + suite.world_frames(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( + 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.""" + 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), + "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)], + ) + + logger.info("building map checkpoints (cache miss)", n=len(times), recording=db_path.name) + final, snapshots = replay_frames( + suite.world_frames(cfg.align_tol), + cfg.make_mapper(), + cfg.voxel_size, + times, + ) + 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) + 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)} + _save_npz(cache, **arrays) + 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 new file mode 100644 index 0000000000..9f0957c45c --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/generate.py @@ -0,0 +1,357 @@ +# 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. Endpoint pairs come off +the walked path, so both are proven reachable.""" + +from __future__ import annotations + +from dataclasses import dataclass +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 +from dimos.navigation.nav_3d.evaluator.tagging import STAIRS_DZ_M, route_tags + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.navigation.nav_3d.evaluator.config import EvalConfig + from dimos.navigation.nav_3d.evaluator.final_map import FinalMap + 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 + + +# 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 +# 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 / METERS_PER_CASE, MIN_AUTO_CASES, MAX_AUTO_CASES)) + + +@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 z drift cannot 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]) + # 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]): + 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 _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 + 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]: + continue + sa = snaps[ai] + 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]] + 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 >= 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]) + detour = float(w / e) + # 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)) + 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, BIN_SIZE_M), + ) + 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 < 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 + 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 = [] + 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, 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, 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: + 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 + + +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) + + @property + def selected(self) -> list[Candidate]: + return self.stairs + self.flats + + 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), + ) + + 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()) + self.alive[n] = False + sa, sb = self._sector(self.starts[n]), self._sector(self.goals[n]) + if not relax and ( + self.usage.get(sa, 0) >= ENDPOINT_REUSE_MAX + or self.usage.get(sb, 0) >= ENDPOINT_REUSE_MAX + ): + self.sector_capped.append(n) + continue + bucket = self.stairs if self.is_stairs[n] else self.flats + if _is_duplicate(self.ranked[n], bucket, DEDUPE_RADIUS_M): + continue + 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: + 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/mesh_loader.py b/dimos/navigation/nav_3d/evaluator/mesh_loader.py deleted file mode 100644 index c6c371ca24..0000000000 --- a/dimos/navigation/nav_3d/evaluator/mesh_loader.py +++ /dev/null @@ -1,78 +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 - -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. - """ - import open3d as o3d # type: ignore[import-untyped] - - 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/metrics.py b/dimos/navigation/nav_3d/evaluator/metrics.py new file mode 100644 index 0000000000..64f7f22222 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/metrics.py @@ -0,0 +1,380 @@ +# 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, Any + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.voxel_keys import ( + cylinder_offsets, + key_centers, + keys_contain, + offset_deltas, + voxel_keys, +) + +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 + + +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 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)]).astype(np.float64) + + +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 + dense: NDArray[np.float32] = np.concatenate([points[:1], body]).astype(np.float32) + return dense + + +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) + + +# Clearance margins are only measured out to this horizontal distance from +# 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 a voxel map key set.""" + + valid: bool + collision_points: NDArray[np.float32] + # 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. + 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.""" + if len(samples) < 2: + return np.tile(np.array([1.0, 0.0, 0.0]), (len(samples), 1)) + pts = samples.astype(np.float64) + 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]) + 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])) + unit: NDArray[np.float64] = fwd / np.maximum( + np.linalg.norm(fwd, axis=1, keepdims=True), MIN_LENGTH_M + ) + return unit + + +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 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) + 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 + + +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.""" + voxel_size = cfg.voxel_size + samples = densify(waypoints, voxel_size / 2) + 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_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 + # 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, + 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 + # 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, + collision_points=samples[:0], + collision_indices=np.empty(0, dtype=np.int64), + 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) + # 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) + clearance = float(sdf.min()) + colliding = np.unique(s_idx[sdf <= 0.0]) + return GateResult( + valid=len(colliding) == 0, + collision_points=samples[colliding], + collision_indices=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], cfg: EvalConfig +) -> SupportResult: + """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) + # 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]) + + +@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.""" + arc = arc_lengths(waypoints) + 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], 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.""" + if len(waypoints) < 2: + return KinematicsResult(True, waypoints[:0]) + 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 * cfg.max_slope, cfg.max_step_m) + return KinematicsResult(not bad.any(), profile[1:][bad]) + + +@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 + + +@dataclass +class _Visits: + """Poses near each endpoint, with the walked length of every 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.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: + 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], + 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.""" + 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(visits.near_s[best[0]]) + start_ts = float(trajectory.ts[i]) if causal else float("inf") + return Reference(max(float(totals[best]), MIN_LENGTH_M), True, start_ts, causal) + + +def ground_truth_route( + trajectory: Trajectory, + start: tuple[float, float, float], + goal: tuple[float, float, float], + 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.""" + visits = _visits(trajectory, start, goal, cfg) + if visits is None: + return None + 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 = visits.foot[i : j + 1] if i <= j else visits.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 + return l_ref / max(p_len, l_ref, MIN_LENGTH_M) + + +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 < 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)) + + +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/picker.py b/dimos/navigation/nav_3d/evaluator/picker.py new file mode 100644 index 0000000000..dd9c9d0310 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/picker.py @@ -0,0 +1,577 @@ +# 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.""" + +from __future__ import annotations + +from dataclasses import dataclass +import threading +from typing import TYPE_CHECKING + +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 +from dimos.navigation.nav_3d.evaluator.viz import turbo_by_height + +if TYPE_CHECKING: + from collections.abc import Callable + + from numpy.typing import NDArray + import viser + + from dimos.navigation.nav_3d.evaluator.cases import Case + from dimos.navigation.nav_3d.evaluator.curation import CaseStore + +START_COLOR = (0, 255, 255) +GOAL_COLOR = (255, 140, 0) +PAIR_COLOR = (255, 255, 0) +HIGHLIGHT_LINE_COLOR = (255, 255, 255) +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 +# 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. +**click** an endpoint sphere to highlight and open its case. +Plain drag orbits, scroll zooms, right-drag pans. +""" + + +# 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], + [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. +_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, + 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) + 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) + 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]: + """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 _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], + 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 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 + if not ahead.any(): + return None + t = t[ahead] + perp = np.linalg.norm(rel[ahead] - t[:, None] * direction, axis=1) + 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])]]) + 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.""" + + store: CaseStore + lock: threading.Lock + unregister: Callable[[_PairEntry], None] + announce: Callable[[str], None] + highlight: Callable[[_PairEntry], None] + + +class _PairEntry: + """One start/goal pair and its editable panel widgets and scene markers.""" + + def __init__( + self, + server: viser.ViserServer, + n: int, + start: NDArray[np.float32], + goal: NDArray[np.float32], + hooks: _Hooks, + markers: _PairMarkers, + case: Case | None = None, + ) -> None: + self._server = server + self._n = n + self.start = start + self.goal = goal + 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 = set(elevation_tags(_point(start), _point(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) + 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: + 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 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.""" + 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, 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") + + def _build(self, *, expanded: bool, order: float | None, scroll: bool = False) -> None: + server = self._server + start, goal = self.start, self.goal + 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})" + ) + 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.delete_button = server.gui.add_button("delete") + + # 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() + + 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.start, self.markers.goal, self.markers.line): + marker.remove() + + def delete(self) -> None: + if self.saved_id is not None: + 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() + + 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) -> bool: + name = self.id_text.value.strip() + 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(), + expect_fail=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) + 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 + self._snapshot() + self._name = case.id + self._sync_tags(case.tags) + self._status = msg + order = self.panel.order + self.panel.remove() + self._build(expanded=False, order=order) + return True + + +def _add_map_scene( + server: viser.ViserServer, + map_points: NDArray[np.float32], + map_colors: NDArray[np.uint8], + voxel_size: float, + walked: NDArray[np.float32], +) -> None: + """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), 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. + 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 = 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 + ) + 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", + 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) + 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])) + + def _on_client_connect(client: viser.ClientHandle) -> None: + client.camera.position = tuple(center + span * np.array(CAMERA_OFFSET)) + client.camera.look_at = tuple(center) + + server.on_client_connect(_on_client_connect) + + +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) + self.highlighted.append(entry) + + def _next_path(self) -> str: + self._marker_seq += 1 + return f"/picks/m{self._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: + return self.server.scene.add_line_segments( + self._next_path(), np.stack([start, goal])[None], colors=color, line_width=width + ) + + 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_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( + final.occupied, + np.asarray(event.ray_origin), + np.asarray(event.ray_direction), + final.voxel_size, + ) + if point is None: + return + with self.lock: + if not self.pending: + self.pending.append((self.sphere(point, NEW_START_COLOR), point)) + return + start_marker, start = self.pending.pop() + markers = _PairMarkers( + start_marker, + self.sphere(point, NEW_GOAL_COLOR), + self.pair_line(start, point, NEW_PAIR_COLOR, NEW_LINE_WIDTH), + ) + 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 = 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(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 new file mode 100644 index 0000000000..3f1300dc4f --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/pipeline.py @@ -0,0 +1,98 @@ +# 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 grading occupancy is built separately.""" + +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 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() + 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/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py new file mode 100644 index 0000000000..be4623e1f4 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -0,0 +1,117 @@ +# 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 +from dimos.navigation.nav_3d.evaluator.metrics import arc_lengths + +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.""" + 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( + db_path: Path, + 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 odometry pose. + 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") + 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 " + "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, 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))) + 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..eddb0fb89f --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/runner.py @@ -0,0 +1,555 @@ +# 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. + +"""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 + +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.final_map import ( + load_or_build_checkpoints, + load_or_build_final_map, +) +from dimos.navigation.nav_3d.evaluator.pipeline import PipelineIntrospection, make_pipeline +from dimos.navigation.nav_3d.evaluator.voxel_keys import key_centers +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.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() + +MAX_COLLISIONS_KEPT = 50 + + +@dataclass +class PlanOutcome: + planned: bool + reached: bool + valid: bool + # Every sample stands on final-map occupancy. Fabricated bridges fail. + supported: bool + # 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. 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 + # can redraw the exact body boxes the gate rejected. + collision_indices: list[int] + unsupported: list[list[float]] + steep: list[list[float]] + + +@dataclass +class PlannerArtifacts: + """Graph state a pipeline chose to expose. Not serialized to JSON.""" + + surface_clearance: NDArray[np.float32] + edges: NDArray[np.float32] + + +@dataclass +class CaseResult: + id: str + dataset: str + start: tuple[float, float, float] + goal: tuple[float, float, float] + tags: list[str] + l_ref: float + online_voxels: int + expect_fail: bool + 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 + # 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 +class DatasetResult: + dataset: str + 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 + + +@dataclass +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 + + +@dataclass +class Report: + score: float + 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 + # 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] + # 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, object] = field(default_factory=dict) + + def to_dict(self) -> dict[str, object]: + out = asdict(self) + for dataset in out["datasets"]: + dataset.pop("final_artifacts") + for case in dataset["cases"]: + case.pop("online_artifacts") + case.pop("online_occupied") + return out + + +def _run_plan( + pipeline: NavPipeline, + case: Case, + l_ref: float, + map_keys: NDArray[np.int64], + support_keys: NDArray[np.int64], + cfg: EvalConfig, +) -> tuple[PlanOutcome, NDArray[np.float32] | None]: + t0 = perf_counter() + 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 + + reached = metrics.goal_reached(waypoints, case.goal, cfg.goal_tolerance) + 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) + success = reached and gate.valid and support.valid and kinematics.valid + outcome = PlanOutcome( + planned=True, + reached=reached, + valid=gate.valid, + supported=support.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(), + 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(), + ) + return outcome, waypoints + + +def _no_plan(plan_ms: float) -> PlanOutcome: + return PlanOutcome( + planned=False, + reached=False, + valid=False, + supported=True, + success=False, + length=0.0, + plan_ms=plan_ms, + spl=0.0, + min_clearance=None, + waypoints=[], + collision_indices=[], + unsupported=[], + steep=[], + ) + + +def score_negative(raw: PlanOutcome) -> PlanOutcome: + """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) + + +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 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. + 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) + if gate.valid: + return False, [] + return True, gate.collision_points[:MAX_COLLISIONS_KEPT].tolist() + + +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=pipeline.surface_clearance_map(), + edges=pipeline.node_edges(), + ) + + +def _final_only(case: Case) -> bool: + """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 _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: + # 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) + if not final_only[i]: + # Only online-replayed cases need a causal snap onto the trajectory. + if not ref.snapped: + logger.warning( + "endpoint off the walked trajectory, using a straight-line reference", + dataset=suite.dataset, + case=case.id, + ) + elif not ref.causal: + logger.warning( + "goal never visited before the start, planning on the full map", + dataset=suite.dataset, + case=case.id, + ) + refs.append(ref) + 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.""" + + cases: dict[int, _OnlineCase] + add_ms: list[float] + final_artifacts: PlannerArtifacts | None + + +def _replay_online( + pipeline: NavPipeline, + suite: Suite, + 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.""" + 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.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.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 suite.world_frames(cfg.align_tol): + while k < len(checkpoints.times) and frame.ts > checkpoints.times[k]: + plan_at(k, next(snapshots)) + k += 1 + t0 = perf_counter() + pipeline.add_frame(frame.points, frame.origin, frame.ts) + out.add_ms.append((perf_counter() - t0) * 1000) + while k < len(checkpoints.times): + plan_at(k, next(snapshots)) + k += 1 + + # The stream is exhausted, so the pipeline now holds the whole recording. + 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 + 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 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.append( + 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, + final=final_out, + soft_progress=final_out.spl, + final_only=True, + ) + ) + continue + 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( + run.outcome, final_out, run.waypoints, run.map_keys, map_keys, cfg + ) + ) + results.append( + 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, + final=final_out, + soft_progress=metrics.soft_progress(end, case.start, case.goal), + dynamic_candidate=dynamic_candidate, + blocking_points=blocking, + online_artifacts=run.artifacts, + online_occupied=run.occupied, + ) + ) + return results + + +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 = suite.trajectory() + + 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(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 + + pipeline = make_pipeline(cfg.pipeline, cfg) + online = _replay_online( + pipeline, + suite, + cfg, + checkpoints, + case_ckpt, + refs, + final.occupied_keys, + keep_artifacts, + ) + return DatasetResult( + dataset=suite.dataset, + 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(online.add_ms), + frames=len(online.add_ms), + final_artifacts=online.final_artifacts, + ) + + +def evaluate( + suites: list[Suite], + cfg: EvalConfig | None = None, + workers: int = 1, + keep_artifacts: bool = False, +) -> Report: + """Score every suite. A dataset is one sequential pass over its recording, + so workers only spreads datasets across processes.""" + cfg = cfg or EvalConfig() + if workers > 1 and len(suites) > 1: + # 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)) + ) + else: + 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") + + # 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 mean(values: list[float]) -> float: + return float(np.mean(values)) if values else 0.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 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}): + 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(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( + 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=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), + 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 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/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()] diff --git a/dimos/navigation/nav_3d/evaluator/tagging.py b/dimos/navigation/nav_3d/evaluator/tagging.py new file mode 100644 index 0000000000..386c68646e --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/tagging.py @@ -0,0 +1,187 @@ +# 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. + +"""Geometric tags for a case: elevation from the endpoints, shape from the +corridor width along the demonstrated route.""" + +from __future__ import annotations + +import itertools +from typing import TYPE_CHECKING + +import numpy as np + +from dimos.navigation.nav_3d.evaluator.metrics import ( + MARGIN_CAP_M, + arc_lengths, + body_frames, + densify, + path_length, +) +from dimos.navigation.nav_3d.evaluator.voxel_keys import keys_contain, voxel_keys + +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. +STAIRS_DZ_M = 0.5 +# 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 +DOORWAY_MAX_RUN_M = 1.2 +# Open space must reappear within this arc either side of a pinch. +DOORWAY_FLANK_M = 1.4 +# A door frame splits one pinch into fragments, so merge runs this close. +NARROW_MERGE_GAP_M = 0.2 +# Shorter than this is a stray voxel, not a passage. +NARROW_MIN_RUN_M = 0.15 +# 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 a retag recomputes. Provenance tags are left untouched. +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, 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: + 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( + samples: NDArray[np.float32], occupied_keys: NDArray[np.int64], cfg: EvalConfig +) -> NDArray[np.float64]: + """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]) + 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 = 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 + + +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 [] + 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 + arc = arc_lengths(samples) + # 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) + 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, 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 + arc = path_length(route) + 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. 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) + return tags diff --git a/dimos/navigation/nav_3d/evaluator/test_nav_eval.py b/dimos/navigation/nav_3d/evaluator/test_nav_eval.py new file mode 100644 index 0000000000..dd697b3327 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/test_nav_eval.py @@ -0,0 +1,1030 @@ +# 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 replace +import itertools +from pathlib import Path +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.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 ( + FinalMap, + MapCheckpoints, + _save_npz, + encode_deltas, + replay_frames, +) +from dimos.navigation.nav_3d.evaluator.generate import ( + Candidate, + _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 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, + _run_plan, + score_negative, +) +from dimos.navigation.nav_3d.evaluator.tagging import route_tags +from dimos.navigation.nav_3d.evaluator.voxel_keys import ( + cylinder_offsets, + keys_contain, + offset_deltas, + voxel_keys, +) + +if TYPE_CHECKING: + 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) -> 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) -> 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 _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()) + + +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 + # 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_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_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 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( + [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_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 + # 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 + 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_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) + # Walking toward a never-yet-visited goal is not causal. + 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), _cfg()) + assert ref.causal + assert 9.0 <= ref.start_ts <= 10.0 + ref = metrics.reference_length(traj, (0, 5, 0), (10, 0, 0), _cfg()) + assert not ref.snapped + assert ref.start_ts == float("inf") + + +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) + 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) + + +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), + ] + added, removed = encode_deltas(snapshots) + 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) + + +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 = 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)) + + 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) + 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() + # 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(snapshots[0], wall1).all() + assert not keys_contain(snapshots[0], wall3).any() + 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 _StubPipeline: + """Returns a fixed path regardless of what it was fed, for gaming the scorer.""" + + def __init__(self, waypoints: NDArray[np.float32] | None) -> None: + self._waypoints = waypoints + self.frames = 0 + + 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] + ) -> NDArray[np.float32] | None: + return self._waypoints + + +def _stub(waypoints: NDArray[np.float32] | None) -> NavPipeline: + return cast("NavPipeline", _StubPipeline(waypoints)) + + +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) + + +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() -> NDArray[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: + """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(_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 + assert out.min_clearance is not None and out.min_clearance < 0 + + +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.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(_stub(route), case, l_ref, keys, keys, cfg) + assert out.success + assert out.spl == pytest.approx(1.0) + assert out.min_clearance == metrics.MARGIN_CAP_M + + +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(_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 + 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_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(_stub(None), case, 16.0, keys, keys, cfg) + out = score_negative(refused) + assert out.success + assert out.spl == 1.0 + 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(_stub(wander), case, 16.0, keys, keys, cfg) + assert score_negative(partial).success + + +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: 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_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 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)]) + 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: + """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 _final_map(points: np.ndarray) -> FinalMap: + return FinalMap( + voxel_size=VOXEL, + occupied=points, + occupied_keys=np.unique(voxel_keys(points, VOXEL)), + build_ms=0.0, + ) + + +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)) + + +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) + 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)), + 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(), max_cases=10) + assert 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, 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: + 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_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.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 loaded.cases[1].expect_fail + assert loaded.cases[2].expect_final_fail + + +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.""" + 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: + """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) + 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: + """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) + 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) + final = FinalMap( + voxel_size=VOXEL, + occupied=surface, + occupied_keys=np.unique(voxel_keys(surface, VOXEL)), + build_ms=0.0, + ) + return CaseStore(load_suite(manifest), surface, _cfg(), final) + + +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 + + +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.""" + 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.""" + 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.""" + 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.""" + 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"], ["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: NDArray[np.float32] | None) -> None: + self._waypoints = waypoints + self.frames = 0 + self.frames_at_plan: list[int] = [] + + 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] + ) -> NDArray[np.float32] | 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") + + +WALK_HEIGHT = 0.5 + + +def _corridor_recording(path: Path) -> Suite: + """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, 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, WALK_HEIGHT)) 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] + + +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: + """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") + walked: Point = (4.0, 0.0, 0.0) + goal: Point = (2.0, 0.0, 0.0) + suite.cases = [ + 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: + # 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] + # 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_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 new file mode 100644 index 0000000000..e132547b59 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/viz.py @@ -0,0 +1,319 @@ +# 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 scene per dataset.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +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.mls_planner.viz import clearance_colors +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from pathlib import Path + + from numpy.typing import NDArray + import rerun.blueprint as rrb + + 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 ( + 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 +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] +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 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]: + # 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()) + 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]: + """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 + + +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: + import rerun as rr + + 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], CLEARANCE_NEAR_WALL_M), + radii=cfg.voxel_size * VOXEL_RADIUS_SCALE, + ), + 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=EDGE_RADIUS, + ), + 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 _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): + 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: + import rerun as rr + + if not outcome.waypoints: + return + rr.log( + entity, + rr.LineStrips3D([outcome.waypoints], colors=[_outcome_color(outcome)], radii=radius), + static=True, + ) + if outcome.collision_indices: + # 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) + 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_h = (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.Boxes3D( + half_sizes=np.tile(half, (len(idx), 1)), + 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, + ), + static=True, + ) + if outcome.unsupported: + rr.log( + f"{entity}/unsupported", + 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 * VIOLATION_RADIUS_SCALE, + ), + static=True, + ) + + +def _dataset_view(root: str, case_ids: list[str]) -> rrb.Spatial3DView: + """One view per dataset. Every case is hidden until toggled on, and the + final planner graph edges start off.""" + import rerun.blueprint as rrb + + hidden = [f"{root}/planner_final/edges"] + hidden += [f"{root}/cases/{cid}" for cid in case_ids] + return rrb.Spatial3DView( + origin=f"/{root}", + name=root, + overrides={path: rrb.EntityBehavior(visible=False) for path in hidden}, + ) + + +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=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=FAILED_INTENT_RADIUS + ), + 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=BLOCKING_RADIUS), + static=True, + ) + _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: + import rerun as rr + import rerun.blueprint as rrb + + 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} + for dataset in report.datasets: + suite = suites_by_dataset[dataset.dataset] + final = load_or_build_final_map(suite, cfg) + trajectory = suite.trajectory() + root = dataset.dataset + + rr.log( + f"{root}/map/voxels", + rr.Points3D( + final.occupied, + colors=turbo_by_height(final.occupied), + radii=cfg.voxel_size * VOXEL_RADIUS_SCALE, + ), + static=True, + ) + foot = trajectory.foot(cfg.robot_height) + rr.log( + f"{root}/walked_path", + rr.LineStrips3D([foot], colors=[WALKED_PATH_COLOR], radii=WALKED_RADIUS), + static=True, + ) + + _log_planner(f"{root}/planner_final", dataset.final_artifacts, cfg) + + for case in dataset.cases: + _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])) + + 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 new file mode 100644 index 0000000000..d5d9b380ad --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/voxel_keys.py @@ -0,0 +1,75 @@ +# 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, so map membership is a +sorted-array search rather than a spatial query.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +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 + + +def voxel_keys(points: NDArray[np.float32], voxel_size: float) -> NDArray[np.int64]: + """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.""" + 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_]: + 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) + + +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_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]) 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, ) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ecf75ac543..1e0dea683f 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -89,7 +89,6 @@ "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", "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator", - "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "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", "teleop-hosted-xarm6": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_xarm6", @@ -189,7 +188,6 @@ "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", "episode-monitor-module": "dimos.imitation.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",