navigation: fix live frame consistency + calibratable carve focal

The live mapper was building the voxel map in LingBot-Map's own monocular
world frame while the robot pose came from odometry — two different
frames, so goto would drive to wrong coordinates. Fix: use the odometry
frame as the single world frame and project the model's camera-frame
geometry (local_points) through the odometry pose (new
pipeline.local_points_to_world). The model is now a depth source, not the
world-frame authority — matching the plan.

Also: carve focal length is derived from a --camera-hfov-deg knob instead
of a placeholder; conservative default speed caps (0.4 m/s / 0.8 rad/s)
for first bring-up.

New: tests/navigation/test_live_mapper.py exercises the real perceive→
project→integrate loop with a fake robot + FakeGeometryRunner, asserting
voxels land in (and track) the odometry world frame. 155 tests total.

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

src/lerobot/navigation/dog_cli.py

@@ -125,15 +125,22 @@ 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.
+    updates the base pose from odometry, runs the geometry model + feature
+    extractor on the frame, and integrates the keyframe.
+
+    Frame convention (important): the **odometry frame is the one world
+    frame**. The geometry model supplies only relative camera-frame
+    geometry (``local_points``/depth); those points are projected through
+    the base's odometry pose, so the voxel map and the robot pose live in
+    the same coordinates and ``goto`` drives to the right place. The
+    model's own ``camera_poses`` (its internal monocular frame) are not
+    used as the world frame. Constructed lazily — no SDK/model 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.base = base  # RobotBaseController (unwrapped) for feed_observation/pose
         self.safe = None  # optional SafeBaseController for the watchdog
         self.geometry = geometry
         self.siglip = siglip
@@ -148,11 +155,13 @@ class LiveMapper:
             KeyframeContext,
             PipelineConfig,
             integrate_keyframe,
+            local_points_to_world,
             upsample_features_to_view,
         )
 
         obs = self.robot.get_observation()
-        self.base.feed_observation(obs)
+        self.base.feed_observation(obs)  # updates the odometry world pose
+        pose = self.base.pose()  # camera-to-world in the odometry frame
 
         frame = obs.get("front")
         if frame is None:
@@ -166,14 +175,17 @@ class LiveMapper:
             patches = self.siglip.encode_views(views)[0]  # (Hp, Wp, D)
             feat_map = upsample_features_to_view(patches, h, w)
 
+        # World points come from the model's camera-frame geometry projected
+        # through the odometry pose — NOT the model's own world frame.
+        points_world = local_points_to_world(geo.local_points[0], pose)
         ctx = KeyframeContext(
             frame_idx=self._frame,
             t_sec=t_sec,
             rgb_uint8=views[0],
-            points_world=geo.points[0],
+            points_world=points_world,
             local_points=geo.local_points[0],
             conf=geo.conf[0],
-            pose=geo.camera_poses[0],
+            pose=pose,
             feat_map=feat_map,
         )
         integrate_keyframe(self.voxel_map, ctx, self.pcfg or PipelineConfig())
@@ -185,30 +197,59 @@ class LiveMapper:
 def _build_live(
     network_interface: str = "eth0",
     device: str = "cuda",
+    camera_hfov_deg: float = 90.0,
+    max_lin_speed: float = 0.4,
+    max_yaw_rate: float = 0.8,
 ) -> 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.
+
+    ``camera_hfov_deg`` sets the pinhole focal length used for free-space
+    carving (``focal = W / (2·tan(HFOV/2))``). Calibrate it to the Go2
+    front camera for correct carving; a wrong value only degrades dynamic
+    removal, not the additive map. Speed caps are deliberately low for
+    first bring-up.
     """
-    from lerobot.navigation.base_controller import RobotBaseController, SafeBaseController
+    import math
+
+    from lerobot.navigation.base_controller import (
+        RobotBaseController,
+        RobotBaseControllerConfig,
+        SafeBaseController,
+    )
     from lerobot.navigation.features import SiglipFeatureExtractor
     from lerobot.navigation.geometry import LingBotMapRunner
+    from lerobot.navigation.pipeline import PipelineConfig
     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)
+    robot_cfg = UnitreeGo2Config(network_interface=network_interface)
+    robot = UnitreeGo2(robot_cfg)
+    inner = RobotBaseController(
+        robot, RobotBaseControllerConfig(max_lin_speed=max_lin_speed, max_yaw_rate=max_yaw_rate)
+    )
+    safe = SafeBaseController(inner=inner, max_lin_speed=max_lin_speed, max_yaw_rate=max_yaw_rate)
     voxel_map = VoxelMap(voxel_size=0.05)
     siglip = SiglipFeatureExtractor(device=device)
     geometry = LingBotMapRunner(device=device)
 
+    w = robot_cfg.front_camera_width
+    focal_px = w / (2.0 * math.tan(math.radians(camera_hfov_deg) / 2.0))
+    pcfg = PipelineConfig(focal_px=focal_px)
+
     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 = LiveMapper(robot, inner, geometry, siglip, voxel_map, pcfg=pcfg)
     mapper.safe = safe
-    LOG.info("live stack wired (iface=%s, device=%s) — connect the dog and run", network_interface, device)
+    LOG.info(
+        "live stack wired (iface=%s, device=%s, focal=%.1fpx, vmax=%.2f m/s) — connect the dog and run",
+        network_interface,
+        device,
+        focal_px,
+        max_lin_speed,
+    )
     return controller, mapper
 
 
@@ -306,6 +347,14 @@ def main(argv: list[str] | None = None) -> int:
     )
     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(
+        "--camera-hfov-deg",
+        type=float,
+        default=90.0,
+        help="Go2 front-camera horizontal FOV, for the carve focal length. Calibrate to your camera.",
+    )
+    ap.add_argument("--max-lin-speed", type=float, default=0.4, help="Body linear speed cap (m/s).")
+    ap.add_argument("--max-yaw-rate", type=float, default=0.8, help="Yaw-rate cap (rad/s).")
     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)
@@ -315,7 +364,13 @@ def main(argv: list[str] | None = None) -> int:
     )
 
     if args.live:
-        controller, mapper = _build_live(args.network_interface, args.device)
+        controller, mapper = _build_live(
+            args.network_interface,
+            args.device,
+            camera_hfov_deg=args.camera_hfov_deg,
+            max_lin_speed=args.max_lin_speed,
+            max_yaw_rate=args.max_yaw_rate,
+        )
         return run_live_repl(controller, mapper)
 
     if not args.dry_run:

src/lerobot/navigation/pipeline.py

@@ -98,6 +98,27 @@ def integrate_keyframe(
     return carve, stats
 
 
+def local_points_to_world(local_points: np.ndarray, pose: np.ndarray) -> np.ndarray:
+    """Transform camera-frame points ``(H, W, 3)`` into the world frame using
+    a 4×4 camera-to-world ``pose``.
+
+    On the robot the world frame is the odometry frame (from the base
+    controller), and the geometry model supplies only relative
+    camera-frame geometry — so projecting through the odometry pose keeps
+    the voxel map and the robot pose in ONE consistent frame. Returns
+    ``(H, W, 3)`` float32.
+    """
+    if local_points.ndim != 3 or local_points.shape[-1] != 3:
+        raise ValueError(f"expected (H, W, 3), got {local_points.shape}")
+    if pose.shape != (4, 4):
+        raise ValueError(f"pose must be (4, 4); got {pose.shape}")
+    r = pose[:3, :3].astype(np.float64)
+    t = pose[:3, 3].astype(np.float64)
+    flat = local_points.reshape(-1, 3).astype(np.float64)
+    world = flat @ r.T + t
+    return world.reshape(local_points.shape).astype(np.float32)
+
+
 def upsample_features_to_view(
     patch_feats_one_view: np.ndarray,
     view_h: int,

tests/navigation/test_live_mapper.py

@@ -0,0 +1,115 @@
+#!/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.
+
+"""Exercise the live mapping loop (LiveMapper.tick) with no hardware.
+
+A fake robot serves canned front-camera frames + odometry; FakeGeometryRunner
+supplies planar depth. This validates the perceive → project-through-odometry
+→ integrate path — the logic that runs on the real dog — including the
+frame-consistency fix (voxels land in the odometry world frame).
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+from lerobot.navigation.base_controller import RobotBaseController
+from lerobot.navigation.dog_cli import LiveMapper
+from lerobot.navigation.geometry import FakeGeometryRunner
+from lerobot.navigation.pipeline import PipelineConfig
+from lerobot.navigation.voxel_map import VoxelMap
+
+
+class FakeGo2:
+    """Robot stand-in: canned front frame + programmable odometry."""
+
+    def __init__(self, h=14, w=14):
+        self.h, self.w = h, w
+        self.odom = {"x.pos": 0.0, "y.pos": 0.0, "theta.pos": 0.0}
+        self.actions = []
+
+    def get_observation(self):
+        return {
+            "front": np.full((self.h, self.w, 3), 120, dtype=np.uint8),
+            **self.odom,
+        }
+
+    def send_action(self, action):
+        self.actions.append(action)
+        return action
+
+
+def _mapper(**pcfg_kwargs):
+    robot = FakeGo2()
+    base = RobotBaseController(robot)
+    vm = VoxelMap(voxel_size=0.05)
+    geom = FakeGeometryRunner(depth=2.0, focal_px=100.0)
+    pcfg = PipelineConfig(focal_px=100.0, **pcfg_kwargs)
+    mapper = LiveMapper(robot, base, geom, siglip=None, voxel_map=vm, pcfg=pcfg)
+    return mapper, robot, base, vm
+
+
+def test_tick_populates_voxel_map():
+    mapper, _, _, vm = _mapper()
+    mapper.tick(0.0)
+    assert len(vm) > 0
+
+
+def test_tick_places_voxels_in_odometry_frame():
+    """With the dog at origin facing +z, the planar floor at depth 2 lands
+    around world z≈2 — i.e. in the odometry world frame, not the model's."""
+    mapper, robot, base, vm = _mapper()
+    mapper.tick(0.0)
+    snap = vm.snapshot()
+    # Median z of the observed floor should be near the camera depth (2 m).
+    assert 1.0 < float(np.median(snap.xyz[:, 2])) < 3.0
+
+
+def test_tick_follows_odometry_translation():
+    """Move the dog forward 5 m in odometry; the new floor voxels shift with
+    it — proof the map tracks the odometry world frame."""
+    mapper, robot, base, vm = _mapper()
+    mapper.tick(0.0)
+    z0 = float(np.median(vm.snapshot().xyz[:, 2]))
+
+    robot.odom = {"x.pos": 5.0, "y.pos": 0.0, "theta.pos": 0.0}  # forward 5 m
+    vm2 = VoxelMap(voxel_size=0.05)
+    mapper.voxel_map = vm2
+    mapper.tick(1.0)
+    z1 = float(np.median(vm2.snapshot().xyz[:, 2]))
+    # Forward odometry (+x_odom → +z_world) shifts the floor ~5 m in world z.
+    assert z1 - z0 > 4.0
+
+
+def test_tick_without_front_frame_is_safe():
+    mapper, robot, _, vm = _mapper()
+    robot.get_observation = lambda: {"x.pos": 0.0, "y.pos": 0.0, "theta.pos": 0.0}
+    mapper.tick(0.0)  # no 'front' → no-op, must not raise
+    assert len(vm) == 0
+
+
+def test_tick_feeds_watchdog_when_present():
+    from lerobot.navigation.base_controller import SafeBaseController
+
+    mapper, _, base, _ = _mapper()
+    safe = SafeBaseController(inner=base)
+    safe.e_stop_latched = True  # will clear on a healthy feed via reset elsewhere
+    mapper.safe = safe
+    # feed_watchdog just refreshes the timer; assert tick calls it (no raise,
+    # and the timestamp advances).
+    before = safe._last_keyframe_walltime
+    mapper.tick(0.0)
+    assert safe._last_keyframe_walltime >= before