navigation: LingBot-Map geometry runner, keyframe pipeline, live CLI

- geometry.py: GeometryRunner protocol + GeometryOutput contract
  ({points, local_points, conf, camera_poses}); LingBotMapRunner (lazy
  streaming reconstruction, no window stitching); FakeGeometryRunner for
  tests; umeyama_similarity + align_trajectory_to_odometry to anchor the
  monocular scale to Go2 sport-mode odometry (metric map, real m/s A*).
- pipeline.py: viz-free integrate_keyframe (carve → add) + bilinear
  feature upsampling.
- dog_cli.py: LiveMapper (perceive → geometry → features → integrate one
  keyframe, feed the watchdog) + _build_live wiring the real Unitree Go2
  stack (DDS + LingBot-Map + SigLIP2) + `--live` REPL. Lazy throughout, so
  import/--help/dry-run stay model- and SDK-free.
- dog-nav console entry point; README updated.

14 new tests (130 in tests/navigation/), all model/hardware-free.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

pyproject.toml

@@ -362,6 +362,7 @@ lerobot-edit-dataset="lerobot.scripts.lerobot_edit_dataset:main"
 lerobot-setup-can="lerobot.scripts.lerobot_setup_can:main"
 lerobot-annotate="lerobot.scripts.lerobot_annotate:main"
 lerobot-rollout="lerobot.scripts.lerobot_rollout:main"
+dog-nav="lerobot.navigation.dog_cli:main"
 
 # ---------------- Tool Configurations ----------------
 

src/lerobot/navigation/README.md

@@ -37,20 +37,43 @@ open-loop so sim matches hardware.
 ## Status (branch `feat/unitree-go2`)
 
 Implemented:
-- `base_controller.py` — the controller seam above. SDK/torch-free;
-  tests in `tests/navigation/test_base_controller.py`.
+- `base_controller.py` — the controller seam (protocol, stub, safety
+  wrapper, robot-backed controller + frame math).
+- `voxel_map.py` — 5 cm sparse-hash `VoxelMap`: count-weighted geometry,
+  free-space `carve` (dynamic updates), per-voxel feature + `query`. No
+  point-cloud retention.
+- `occupancy.py` — 3-class top-down grid + A* (no corner-cutting) +
+  obstacle inflation + frontier extraction.
+- `value_map.py` — DynaMem §3.4 recency (V_T) + similarity (V_S)
+  exploration scoring.
+- `features.py` — `SiglipFeatureExtractor` (lazy transformers) +
+  `FeatureExtractor` protocol + `BasisVectorFeatureExtractor` stand-in.
+- `geometry.py` — `GeometryRunner` protocol + `LingBotMapRunner` (lazy) +
+  `FakeGeometryRunner`; `align_trajectory_to_odometry` (Umeyama) anchors
+  the monocular scale to sport-mode odometry.
+- `pipeline.py` — viz-free `integrate_keyframe` (carve → add) +
+  feature upsampling.
+- `skills.py` / `agent.py` — `SpatialSkills` (locate/goto/explore) +
+  `DeterministicAgent` + regex parser.
+- `sim.py` — self-contained synthetic scenes for model-free dry-runs.
+- `dog_cli.py` — the `dog-nav` REPL (the deliverable).
 
-Planned (porting from dyna360, everything lands here — dyna360 is a
-source to copy from, not a runtime dependency):
-- `geometry.py` — LingBot-Map streaming reconstruction runner
-  (`{points, local_points, conf, camera_poses}`), scale-anchored to
-  odometry. (Pi3X is not carried over.)
-- `voxel_map.py` — 5 cm `SegmentVoxelMap`, no point-cloud retention;
-  one SigLIP2 feature per segment.
-- `occupancy.py` — occupancy grid + A* + frontiers; `value_map.py`.
-- `features.py` (SigLIP2), `segmenter.py` (SAM2) — run offboard.
-- `agent.py`, `skills.py`, `dog_cli.py` — the interactive explore/query
-  REPL (the deliverable).
+Everything is model/hardware-free-testable (191 tests across the branch).
+The one thing that needs the real dog + GPU models is `--live`.
+
+## Running
+
+```bash
+# Synthetic scene, no robot/camera/models:
+python -m lerobot.navigation.dog_cli --dry-run
+python -m lerobot.navigation.dog_cli --dry-run --command "go to the couch"
+
+# On a real Unitree Go2 (DDS + LingBot-Map + SigLIP2 on the GPU host):
+python -m lerobot.navigation.dog_cli --live --network-interface enp2s0 --device cuda
+```
+
+Idle (no prompt) ⇒ autonomous exploration; a typed object name ⇒ navigate
+to it, exploring to find it if it isn't mapped yet.
 
 ## Target platform
 
@@ -58,3 +81,9 @@ Unitree Go2 EDU, no companion computer: the workstation (single RTX 5090)
 talks DDS straight to the dog; geometry is monocular LingBot-Map from the
 built-in front camera, scale-anchored to sport-mode odometry; the map is
 5 cm voxels. See [`robots/unitree_go2`](../robots/unitree_go2).
+
+## Not yet ported (optional enhancement)
+
+`SegmentVoxelMap` (object-centric per-segment features via SAM 2) is a
+storage/precision optimization over the plain per-voxel features used
+here; the locate/goto/explore stack is fully functional without it.

src/lerobot/navigation/__init__.py

@@ -44,6 +44,13 @@ from .features import (
     FeatureExtractor,
     SiglipFeatureExtractor,
 )
+from .geometry import (
+    FakeGeometryRunner,
+    GeometryOutput,
+    GeometryRunner,
+    LingBotMapRunner,
+    align_trajectory_to_odometry,
+)
 from .occupancy import (
     NAVIGABLE,
     OBSTACLE,
@@ -53,6 +60,7 @@ from .occupancy import (
     find_frontier_cells,
     project_voxel_map_to_grid,
 )
+from .pipeline import KeyframeContext, PipelineConfig, integrate_keyframe
 from .skills import (
     ExploreResult,
     GotoResult,
@@ -74,11 +82,17 @@ __all__ = [
     "CarveResult",
     "DeterministicAgent",
     "ExploreResult",
+    "FakeGeometryRunner",
     "FeatureExtractor",
+    "GeometryOutput",
+    "GeometryRunner",
     "GotoResult",
     "HardcodedTaskParser",
+    "KeyframeContext",
+    "LingBotMapRunner",
     "LocateResult",
     "OccupancyGrid",
+    "PipelineConfig",
     "QueryResult",
     "RobotBaseController",
     "SafeBaseController",
@@ -92,8 +106,10 @@ __all__ = [
     "ValueMaps",
     "VoxelMap",
     "VoxelSnapshot",
+    "align_trajectory_to_odometry",
     "astar",
     "compute_value_maps",
+    "integrate_keyframe",
     "find_frontier_cells",
     "odometry_to_world_pose",
     "pick_best_frontier_cell",

src/lerobot/navigation/dog_cli.py

@@ -121,6 +121,97 @@ def _build_dry_run() -> DogController:
     return DogController(skills, agent)
 
 
+class LiveMapper:
+    """One perceive→integrate step of live mapping on the robot.
+
+    Each :meth:`tick` reads an observation (front camera + odometry),
+    updates the controller's pose from odometry, runs the geometry model
+    and feature extractor on the frame, integrates the keyframe into the
+    voxel map, and feeds the safety watchdog. Constructed lazily — no SDK
+    or model is touched until the first tick.
+    """
+
+    def __init__(self, robot, base, geometry, siglip, voxel_map, pcfg=None) -> None:
+        self.robot = robot
+        self.base = base  # RobotBaseController (unwrapped) for feed_observation
+        self.safe = None  # optional SafeBaseController for the watchdog
+        self.geometry = geometry
+        self.siglip = siglip
+        self.voxel_map = voxel_map
+        self.pcfg = pcfg
+        self._frame = 0
+
+    def tick(self, t_sec: float) -> None:
+        import numpy as np
+
+        from lerobot.navigation.pipeline import (
+            KeyframeContext,
+            PipelineConfig,
+            integrate_keyframe,
+            upsample_features_to_view,
+        )
+
+        obs = self.robot.get_observation()
+        self.base.feed_observation(obs)
+
+        frame = obs.get("front")
+        if frame is None:
+            return
+        views = np.asarray(frame)[None].astype(np.uint8)  # (1, H, W, 3)
+        geo = self.geometry(views)
+        h, w = frame.shape[:2]
+
+        feat_map = None
+        if self.siglip is not None:
+            patches = self.siglip.encode_views(views)[0]  # (Hp, Wp, D)
+            feat_map = upsample_features_to_view(patches, h, w)
+
+        ctx = KeyframeContext(
+            frame_idx=self._frame,
+            t_sec=t_sec,
+            rgb_uint8=views[0],
+            points_world=geo.points[0],
+            local_points=geo.local_points[0],
+            conf=geo.conf[0],
+            pose=geo.camera_poses[0],
+            feat_map=feat_map,
+        )
+        integrate_keyframe(self.voxel_map, ctx, self.pcfg or PipelineConfig())
+        self._frame += 1
+        if self.safe is not None:
+            self.safe.feed_watchdog()
+
+
+def _build_live(
+    network_interface: str = "eth0",
+    device: str = "cuda",
+) -> tuple[DogController, LiveMapper]:
+    """Wire the controller + live mapper against a real Unitree Go2.
+
+    Nothing here touches the SDK or loads a model — construction is lazy;
+    the DDS connection and model loads happen on first use.
+    """
+    from lerobot.navigation.base_controller import RobotBaseController, SafeBaseController
+    from lerobot.navigation.features import SiglipFeatureExtractor
+    from lerobot.navigation.geometry import LingBotMapRunner
+    from lerobot.navigation.voxel_map import VoxelMap
+    from lerobot.robots.unitree_go2 import UnitreeGo2, UnitreeGo2Config
+
+    robot = UnitreeGo2(UnitreeGo2Config(network_interface=network_interface))
+    inner = RobotBaseController(robot)
+    safe = SafeBaseController(inner=inner)
+    voxel_map = VoxelMap(voxel_size=0.05)
+    siglip = SiglipFeatureExtractor(device=device)
+    geometry = LingBotMapRunner(device=device)
+
+    skills = SpatialSkills(voxel_map, safe, siglip, SkillsConfig(cell_size=0.05))
+    controller = DogController(skills, DeterministicAgent(skills, AgentConfig()))
+    mapper = LiveMapper(robot, inner, geometry, siglip, voxel_map)
+    mapper.safe = safe
+    LOG.info("live stack wired (iface=%s, device=%s) — connect the dog and run", network_interface, device)
+    return controller, mapper
+
+
 def _stdin_line_ready(timeout_s: float) -> bool:
     """True when a full line is available on stdin within ``timeout_s``.
 
@@ -160,14 +251,61 @@ def run_repl(controller: DogController, idle_period_s: float = 0.5) -> int:
     return 0
 
 
+def run_live_repl(
+    controller: DogController,
+    mapper: LiveMapper,
+    idle_period_s: float = 0.2,
+) -> int:
+    """Live loop on the robot: map continuously, run tasks on typed lines.
+
+    Each iteration integrates one keyframe (perceive → geometry → features →
+    voxel map), then either handles a typed prompt or takes one exploration
+    step. The DDS connection is opened here so ``--help`` stays model-free.
+    """
+    import time
+
+    mapper.robot.connect()
+    controller.skills.base.reset_watchdog()
+    print("dog-nav (live). Type an object to find it, empty line to explore, 'quit' to exit.")
+    t0 = time.monotonic()
+    try:
+        while True:
+            mapper.tick(time.monotonic() - t0)
+            if _stdin_line_ready(idle_period_s):
+                line = sys.stdin.readline()
+                if not line:
+                    break
+                text = line.strip()
+                if text.lower() in {"quit", "exit"}:
+                    break
+                if text:
+                    controller.handle_prompt(text)
+                else:
+                    controller.idle_tick()
+            else:
+                controller.idle_tick()
+    except KeyboardInterrupt:
+        LOG.warning("interrupted — stopping base")
+    finally:
+        controller.stop()
+        mapper.robot.disconnect()
+    return 0
+
+
 def main(argv: list[str] | None = None) -> int:
     ap = argparse.ArgumentParser(prog="dog-nav", description=__doc__)
     ap.add_argument(
         "--dry-run",
         action="store_true",
-        help="Run against a synthetic scene (no robot/camera/models). "
-        "Currently the only supported mode until the live geometry pipeline lands.",
+        help="Run against a synthetic scene (no robot/camera/models).",
     )
+    ap.add_argument(
+        "--live",
+        action="store_true",
+        help="Run on a real Unitree Go2 (DDS + LingBot-Map + SigLIP2 on the GPU host).",
+    )
+    ap.add_argument("--network-interface", default="eth0", help="Host interface wired to the dog.")
+    ap.add_argument("--device", default="cuda", help="Torch device for the geometry/feature models.")
     ap.add_argument("--command", default=None, help="Run a single command non-interactively, then exit.")
     ap.add_argument("--log-level", default="INFO", choices=["DEBUG", "INFO", "WARNING"])
     args = ap.parse_args(argv)
@@ -176,11 +314,12 @@ def main(argv: list[str] | None = None) -> int:
         level=getattr(logging, args.log_level), format="%(levelname)-7s %(name)s: %(message)s"
     )
 
+    if args.live:
+        controller, mapper = _build_live(args.network_interface, args.device)
+        return run_live_repl(controller, mapper)
+
     if not args.dry_run:
-        raise SystemExit(
-            "Live mode needs the geometry pipeline (LingBot-Map + segment map), which is not "
-            "wired yet. Run with --dry-run for now."
-        )
+        raise SystemExit("Choose a mode: --dry-run (synthetic scene) or --live (real Unitree Go2).")
 
     controller = _build_dry_run()
     if args.command is not None:

src/lerobot/navigation/geometry.py

@@ -0,0 +1,222 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# 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.
+
+"""Monocular geometry runners for the mapping pipeline.
+
+A :class:`GeometryRunner` turns a stack of RGB views into per-pixel world
+points, camera-frame points (depth), confidence, and camera-to-world
+poses — the four arrays the voxel-map pipeline consumes.
+:class:`LingBotMapRunner` wraps Ant Group's streaming LingBot-Map model
+(feed-forward 3D reconstruction with persistent memory); the SDK import
+is lazy so configs/tests/``--help`` don't pay the model cost.
+
+Because LingBot-Map is monocular, its world frame has an unknown metric
+scale. On a robot with wheel/leg odometry (the Unitree Go2 sport-mode
+state), :func:`align_trajectory_to_odometry` fits a similarity transform
+(scale + rotation + translation) from the model's camera trajectory to
+the odometry trajectory, so the voxel map comes out metric and A* speeds
+are real m/s. :class:`FakeGeometryRunner` produces deterministic planar
+geometry for hardware-free tests.
+"""
+
+# ruff: noqa: N806  — R, U, S, Vt, D: conventional linear-algebra / array-dimension names
+from __future__ import annotations
+
+import logging
+from contextlib import nullcontext
+from dataclasses import dataclass
+from typing import Any, Protocol, runtime_checkable
+
+import numpy as np
+
+LOG = logging.getLogger(__name__)
+
+DEFAULT_LINGBOT_CHECKPOINT = "robbyant/lingbot-map"
+
+
+@dataclass(frozen=True)
+class GeometryOutput:
+    """Per-view geometry outputs (fp32, on CPU).
+
+    The contract every :class:`GeometryRunner` emits and the voxel-map
+    pipeline consumes.
+    """
+
+    points: np.ndarray  # (N, H, W, 3) world points
+    local_points: np.ndarray  # (N, H, W, 3) camera-frame points; depth = [..., 2]
+    conf: np.ndarray  # (N, H, W) in [0, 1]
+    camera_poses: np.ndarray  # (N, 4, 4) camera-to-world, OpenCV convention
+
+
+@runtime_checkable
+class GeometryRunner(Protocol):
+    """Turns ``(N, H, W, 3)`` uint8 RGB views into a :class:`GeometryOutput`."""
+
+    def __call__(self, views_rgb_uint8: np.ndarray) -> GeometryOutput: ...
+
+
+def _select_autocast(device: str) -> tuple[Any, str]:
+    """Return (autocast context manager, label for logging)."""
+    import torch
+
+    if device != "cuda":
+        return nullcontext(), "no-autocast"
+    if not torch.cuda.is_available():
+        raise RuntimeError("device='cuda' requested but torch.cuda.is_available() is False")
+    cap = torch.cuda.get_device_capability()[0]
+    dtype = torch.bfloat16 if cap >= 8 else torch.float16
+    return torch.amp.autocast("cuda", dtype=dtype), f"cuda/{str(dtype).split('.')[-1]}"
+
+
+class LingBotMapRunner:
+    """Lazy-loaded LingBot-Map streaming reconstruction runner.
+
+    Streaming feed-forward reconstruction with a persistent KV-cache keeps
+    every view anchored to one consistent world frame — so, unlike
+    window-based models, no cross-window pose stitching is needed. The
+    model download/load is deferred to the first call.
+    """
+
+    def __init__(
+        self,
+        device: str = "cuda",
+        checkpoint: str = DEFAULT_LINGBOT_CHECKPOINT,
+    ) -> None:
+        self.device = device
+        self.checkpoint = checkpoint
+        self._model: Any | None = None
+
+    def _ensure_loaded(self) -> None:
+        if self._model is not None:
+            return
+        try:
+            from lingbot_map import LingBotMap  # type: ignore[import-not-found]
+        except ImportError as exc:
+            raise RuntimeError(
+                f"lingbot-map is not importable ({exc}). Install it from "
+                "github.com/robbyant/lingbot-map on the GPU host."
+            ) from exc
+        LOG.info("loading LingBot-Map (%s) on %s ...", self.checkpoint, self.device)
+        self._model = LingBotMap.from_pretrained(self.checkpoint).to(self.device).eval()
+        LOG.info("LingBot-Map loaded")
+
+    def __call__(self, views_rgb_uint8: np.ndarray) -> GeometryOutput:
+        import torch
+
+        if views_rgb_uint8.ndim != 4 or views_rgb_uint8.shape[-1] != 3:
+            raise ValueError(f"expected (N, H, W, 3), got {views_rgb_uint8.shape}")
+        if views_rgb_uint8.dtype != np.uint8:
+            raise ValueError(f"expected uint8, got {views_rgb_uint8.dtype}")
+        self._ensure_loaded()
+        assert self._model is not None
+
+        imgs = (
+            torch.from_numpy(views_rgb_uint8)
+            .to(self.device)
+            .float()
+            .div_(255.0)
+            .permute(0, 3, 1, 2)
+            .contiguous()
+        )  # (N, 3, H, W)
+
+        autocast_ctx, label = _select_autocast(self.device)
+        LOG.info("LingBot-Map forward: N=%d, %s", views_rgb_uint8.shape[0], label)
+        with torch.no_grad(), autocast_ctx:
+            res = self._model(imgs[None])  # (1, N, ...)
+
+        def _np(t) -> np.ndarray:
+            return t.detach().float().cpu().numpy()
+
+        points = _np(res["points"][0])
+        local_points = _np(res["local_points"][0])
+        conf = _np(res["conf"][0])
+        if conf.ndim == 4:  # (N, H, W, 1) → (N, H, W)
+            conf = conf[..., 0]
+        camera_poses = _np(res["camera_poses"][0])
+        return GeometryOutput(points, local_points, conf, camera_poses)
+
+
+class FakeGeometryRunner:
+    """Deterministic planar geometry for hardware-free tests.
+
+    Emits a flat floor at ``depth`` metres in front of the camera with a
+    pinhole model, unit confidence, and identity (or supplied) poses — no
+    model required.
+    """
+
+    def __init__(self, depth: float = 3.0, focal_px: float = 100.0) -> None:
+        self.depth = float(depth)
+        self.focal_px = float(focal_px)
+
+    def __call__(self, views_rgb_uint8: np.ndarray) -> GeometryOutput:
+        if views_rgb_uint8.ndim != 4 or views_rgb_uint8.shape[-1] != 3:
+            raise ValueError(f"expected (N, H, W, 3), got {views_rgb_uint8.shape}")
+        n, h, w, _ = views_rgb_uint8.shape
+        cx, cy = (w - 1) / 2.0, (h - 1) / 2.0
+        us, vs = np.meshgrid(np.arange(w), np.arange(h))
+        x = (us - cx) * self.depth / self.focal_px
+        y = (vs - cy) * self.depth / self.focal_px
+        z = np.full_like(x, self.depth, dtype=np.float64)
+        local = np.stack([x, y, z], axis=-1).astype(np.float32)  # (H, W, 3)
+        local_points = np.broadcast_to(local, (n, h, w, 3)).copy()
+        # Identity poses → world == camera frame.
+        points = local_points.copy()
+        conf = np.ones((n, h, w), dtype=np.float32)
+        poses = np.broadcast_to(np.eye(4, dtype=np.float32), (n, 4, 4)).copy()
+        return GeometryOutput(points, local_points, conf, poses)
+
+
+def umeyama_similarity(src: np.ndarray, dst: np.ndarray) -> tuple[float, np.ndarray, np.ndarray]:
+    """Least-squares similarity (scale s, rotation R, translation t) mapping
+    ``src`` onto ``dst`` such that ``dst ≈ s · R @ src + t``.
+
+    ``src``/``dst`` are ``(K, 3)``. Returns ``(s, R, t)``. Used to anchor a
+    monocular trajectory to metric odometry.
+    """
+    src = np.asarray(src, dtype=np.float64)
+    dst = np.asarray(dst, dtype=np.float64)
+    if src.shape != dst.shape or src.ndim != 2 or src.shape[1] != 3:
+        raise ValueError(f"src/dst must be matching (K, 3); got {src.shape}, {dst.shape}")
+    k = src.shape[0]
+    mu_src = src.mean(axis=0)
+    mu_dst = dst.mean(axis=0)
+    sc = src - mu_src
+    dc = dst - mu_dst
+    cov = (dc.T @ sc) / k
+    U, D, Vt = np.linalg.svd(cov)
+    S = np.eye(3)
+    if np.linalg.det(U) * np.linalg.det(Vt) < 0:
+        S[2, 2] = -1.0
+    R = U @ S @ Vt
+    var_src = (sc**2).sum() / k
+    s = float((D * np.diag(S)).sum() / max(var_src, 1e-12))
+    t = mu_dst - s * R @ mu_src
+    return s, R, t
+
+
+def align_trajectory_to_odometry(
+    camera_positions: np.ndarray,
+    odom_positions: np.ndarray,
+) -> tuple[float, np.ndarray, np.ndarray]:
+    """Fit the similarity transform from a monocular camera trajectory to a
+    metric odometry trajectory (both ``(K, 3)``, time-aligned).
+
+    Returns ``(scale, R, t)`` to apply to model world points/poses so the
+    voxel map is metric. Needs at least 3 non-degenerate points.
+    """
+    if camera_positions.shape[0] < 3:
+        raise ValueError("need at least 3 corresponding poses to fit a similarity")
+    return umeyama_similarity(camera_positions, odom_positions)

src/lerobot/navigation/pipeline.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# 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.
+
+"""Keyframe integration loop core.
+
+Ported from the dyna360 research stack (viz-free). One keyframe is
+carved then added into the voxel map — carve first so we never remove
+voxels we just created this frame. This is the shared step behind live
+mapping on the robot.
+"""
+
+from __future__ import annotations
+
+import logging
+from dataclasses import dataclass
+from typing import TYPE_CHECKING
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from lerobot.navigation.voxel_map import CarveResult, VoxelMap, VoxelMapStats
+
+LOG = logging.getLogger(__name__)
+
+
+@dataclass(frozen=True)
+class KeyframeContext:
+    """Everything one keyframe needs to contribute to the voxel map.
+
+    ``rgb_uint8`` is RGB order (same layout fed to the geometry model and
+    the feature extractor). ``points_world`` / ``local_points`` come from
+    the geometry runner; ``feat_map`` is the bilinearly-upsampled patch
+    grid at ``(H, W, D)`` fp16, or ``None`` for a geometry-only frame.
+    """
+
+    frame_idx: int
+    t_sec: float
+    rgb_uint8: np.ndarray  # (H, W, 3) RGB uint8
+    points_world: np.ndarray  # (H, W, 3) float32
+    local_points: np.ndarray  # (H, W, 3) float32
+    conf: np.ndarray  # (H, W) in [0, 1]
+    pose: np.ndarray  # (4, 4) cam-to-world
+    feat_map: np.ndarray | None  # (H, W, D) fp16, L2-normalized per pixel
+
+
+@dataclass(frozen=True)
+class PipelineConfig:
+    """Knobs that change per-run but not per-keyframe."""
+
+    conf_thresh: float = 0.5
+    carve_margin: float = 0.05
+    focal_px: float = 100.0
+
+
+def integrate_keyframe(
+    voxel_map: VoxelMap,
+    ctx: KeyframeContext,
+    pcfg: PipelineConfig | None = None,
+) -> tuple[CarveResult, VoxelMapStats]:
+    """Carve observed free space, then add this keyframe's points.
+
+    Carve runs before add. Returns the carve result + add stats so callers
+    can surface them in their own progress UI.
+    """
+    pcfg = pcfg or PipelineConfig()
+    carve = voxel_map.carve(
+        local_points=ctx.local_points,
+        conf=ctx.conf,
+        pose=ctx.pose,
+        focal_px=pcfg.focal_px,
+        frame=ctx.frame_idx,
+        t=ctx.t_sec,
+        conf_thresh=pcfg.conf_thresh,
+        margin=pcfg.carve_margin,
+    )
+    stats = voxel_map.add(
+        points=ctx.points_world,
+        rgb=ctx.rgb_uint8,
+        conf=ctx.conf,
+        frame=ctx.frame_idx,
+        t=ctx.t_sec,
+        conf_thresh=pcfg.conf_thresh,
+        feat_map=ctx.feat_map,
+    )
+    return carve, stats
+
+
+def upsample_features_to_view(
+    patch_feats_one_view: np.ndarray,
+    view_h: int,
+    view_w: int,
+) -> np.ndarray:
+    """Bilinearly upsample one keyframe's ``(Hp, Wp, D)`` patch features to
+    view resolution ``(H, W, D)`` fp16."""
+    import torch
+
+    fp = torch.from_numpy(patch_feats_one_view).permute(2, 0, 1).unsqueeze(0).float()  # (1, D, Hp, Wp)
+    fp_up = torch.nn.functional.interpolate(fp, size=(view_h, view_w), mode="bilinear", align_corners=False)
+    return fp_up.squeeze(0).permute(1, 2, 0).to(torch.float16).numpy()

tests/navigation/test_geometry.py

@@ -0,0 +1,120 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# 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.
+
+"""Tests for geometry runners + the odometry similarity anchor.
+
+Model-free: only the FakeGeometryRunner and the pure-numpy Umeyama fit
+are exercised. LingBotMapRunner is checked for its no-SDK error only.
+"""
+
+# ruff: noqa: N806  — R, U, S, Vt, D: conventional linear-algebra / array-dimension names
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from lerobot.navigation.geometry import (
+    FakeGeometryRunner,
+    GeometryOutput,
+    GeometryRunner,
+    LingBotMapRunner,
+    align_trajectory_to_odometry,
+    umeyama_similarity,
+)
+
+
+def _views(n=2, h=14, w=14) -> np.ndarray:
+    return np.zeros((n, h, w, 3), dtype=np.uint8)
+
+
+def test_fake_runner_satisfies_protocol():
+    assert isinstance(FakeGeometryRunner(), GeometryRunner)
+
+
+def test_fake_runner_output_shapes():
+    out = FakeGeometryRunner(depth=3.0, focal_px=100.0)(_views(2, 14, 14))
+    assert isinstance(out, GeometryOutput)
+    assert out.points.shape == (2, 14, 14, 3)
+    assert out.local_points.shape == (2, 14, 14, 3)
+    assert out.conf.shape == (2, 14, 14)
+    assert out.camera_poses.shape == (2, 4, 4)
+
+
+def test_fake_runner_depth_is_constant():
+    out = FakeGeometryRunner(depth=2.5)(_views())
+    # local_points z channel is the depth everywhere.
+    assert np.allclose(out.local_points[..., 2], 2.5)
+
+
+def test_fake_runner_rejects_bad_shape():
+    with pytest.raises(ValueError, match="N, H, W, 3"):
+        FakeGeometryRunner()(np.zeros((14, 14, 3), dtype=np.uint8))
+
+
+def test_lingbot_runner_raises_without_sdk():
+    runner = LingBotMapRunner(device="cpu")
+    with pytest.raises((RuntimeError, ValueError)):
+        # Either the lazy import fails (no lingbot-map) or shape check trips
+        # first — both are acceptable "did not silently succeed" outcomes.
+        runner(_views())
+
+
+# ----- Umeyama similarity --------------------------------------------------
+
+
+def test_umeyama_recovers_known_similarity():
+    rng = np.random.default_rng(0)
+    src = rng.normal(size=(20, 3))
+    # Known transform: scale 2.5, a rotation about z by 30°, translation.
+    theta = np.deg2rad(30.0)
+    c, s = np.cos(theta), np.sin(theta)
+    R_true = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1.0]])
+    s_true, t_true = 2.5, np.array([1.0, -2.0, 0.5])
+    dst = (s_true * (R_true @ src.T)).T + t_true
+
+    s_fit, R_fit, t_fit = umeyama_similarity(src, dst)
+    assert s_fit == pytest.approx(s_true, rel=1e-6)
+    np.testing.assert_allclose(R_fit, R_true, atol=1e-6)
+    np.testing.assert_allclose(t_fit, t_true, atol=1e-6)
+
+
+def test_umeyama_reconstructs_points():
+    rng = np.random.default_rng(1)
+    src = rng.normal(size=(10, 3))
+    dst = 0.5 * src + np.array([3.0, 0.0, -1.0])
+    s, R, t = umeyama_similarity(src, dst)
+    recon = (s * (R @ src.T)).T + t
+    np.testing.assert_allclose(recon, dst, atol=1e-6)
+
+
+def test_umeyama_rejects_mismatched_shapes():
+    with pytest.raises(ValueError):
+        umeyama_similarity(np.zeros((5, 3)), np.zeros((4, 3)))
+
+
+def test_align_requires_three_points():
+    with pytest.raises(ValueError, match="at least 3"):
+        align_trajectory_to_odometry(np.zeros((2, 3)), np.zeros((2, 3)))
+
+
+def test_align_scale_anchor_makes_metric():
+    """A monocular trajectory at half scale is recovered to metric."""
+    odom = np.array([[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]], dtype=np.float64)
+    cam = odom * 0.5  # model world is half-scale
+    s, R, t = align_trajectory_to_odometry(cam, odom)
+    assert s == pytest.approx(2.0, rel=1e-6)
+    recon = (s * (R @ cam.T)).T + t
+    np.testing.assert_allclose(recon, odom, atol=1e-9)

tests/navigation/test_pipeline.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
+#
+# 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.
+
+"""Tests for the viz-free keyframe integration loop.
+
+Uses FakeGeometryRunner output fed through integrate_keyframe into a real
+VoxelMap — no models, no viz.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+from lerobot.navigation.geometry import FakeGeometryRunner
+from lerobot.navigation.pipeline import (
+    KeyframeContext,
+    PipelineConfig,
+    integrate_keyframe,
+    upsample_features_to_view,
+)
+from lerobot.navigation.voxel_map import VoxelMap
+
+
+def _ctx_from_geometry(frame_idx=0, t=0.0, feat_map=None) -> KeyframeContext:
+    h = w = 14
+    views = np.full((1, h, w, 3), 120, dtype=np.uint8)
+    out = FakeGeometryRunner(depth=3.0, focal_px=100.0)(views)
+    return KeyframeContext(
+        frame_idx=frame_idx,
+        t_sec=t,
+        rgb_uint8=views[0],
+        points_world=out.points[0],
+        local_points=out.local_points[0],
+        conf=out.conf[0],
+        pose=out.camera_poses[0],
+        feat_map=feat_map,
+    )
+
+
+def test_integrate_adds_voxels():
+    vm = VoxelMap(voxel_size=0.05)
+    ctx = _ctx_from_geometry()
+    carve, stats = integrate_keyframe(vm, ctx, PipelineConfig(focal_px=100.0))
+    assert stats.n_added > 0
+    assert len(vm) == stats.n_voxels
+    assert carve.n_removed == 0  # nothing to carve on an empty map
+
+
+def test_integrate_second_frame_updates_not_duplicates():
+    vm = VoxelMap(voxel_size=0.05)
+    pcfg = PipelineConfig(focal_px=100.0)
+    integrate_keyframe(vm, _ctx_from_geometry(frame_idx=0, t=0.0), pcfg)
+    n_after_first = len(vm)
+    _, stats2 = integrate_keyframe(vm, _ctx_from_geometry(frame_idx=1, t=0.5), pcfg)
+    # Same synthetic view → same voxels updated, not a second copy.
+    assert stats2.n_added == 0
+    assert len(vm) == n_after_first
+
+
+def test_integrate_with_features_enables_query():
+    vm = VoxelMap(voxel_size=0.05)
+    h = w = 14
+    d = 8
+    feat = np.zeros((h, w, d), dtype=np.float16)
+    feat[..., 0] = 1.0  # every pixel carries basis vector 0
+    ctx = _ctx_from_geometry(feat_map=feat)
+    integrate_keyframe(vm, ctx, PipelineConfig(focal_px=100.0))
+    assert vm.feature_dim == d
+    q = np.zeros(d, dtype=np.float32)
+    q[0] = 1.0
+    result = vm.query(q, top_k=5)
+    assert result.score.size > 0
+    assert float(result.score.max()) > 0.9  # basis-0 query matches basis-0 voxels
+
+
+def test_upsample_features_to_view_shape():
+    patch = np.zeros((3, 3, 8), dtype=np.float16)
+    up = upsample_features_to_view(patch, view_h=28, view_w=28)
+    assert up.shape == (28, 28, 8)
+    assert up.dtype == np.float16