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>

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