From 4a2ff75c9838d951d314bd5eb6d32a4f868ed178 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Wed, 1 Jul 2026 07:20:02 -0700
Subject: [PATCH 01/19] china_office.db: pointlio-only (mid360_link), fastlio +
 tf_graph removed, in-cloud ts aligned to recording time

---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index 641302e1c9..985afb1a69 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:5c7d9b8d83b7eccff302d3881068f2064657b5903340ae54995e9cce80e41ffe
-size 4058639695
+oid sha256:243a19ac6aef7f7feb9841e131be7d657781f3a1f5ed8a3be25b80e9d453aa4b
+size 4507943536

From b0a900ebba8b5c010a2d956e39c68a3e888b2b32 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Wed, 1 Jul 2026 10:06:43 -0500
Subject: [PATCH 02/19] fix: register map/replay clouds via frame_id + TF tree

Drive PointCloud/Odometry world-registration off each message's frame_id
instead of a baked-in per-observation pose. World-frame messages render
directly; non-world frames are looked up in the recording's `tf` stream via
a read-only DbTf (no loop closure) and skipped with a warning if unplaceable.

DbTf uses an unbounded time tolerance so once-published static transforms
(mounts, world<-map) latch across the whole recording, while densely sampled
dynamic transforms still resolve to their nearest sample.
---
 dimos/mapping/utils/cli/map.py                | 68 +++++--------
 dimos/mapping/utils/cli/replay.py             | 90 +++++++++++++----
 .../utils/cli/test_world_registration.py      | 73 ++++++++++++++
 dimos/mapping/utils/cli/world_registration.py | 96 +++++++++++++++++++
 dimos/protocol/tf/db_tf.py                    | 73 ++++++++++++++
 dimos/protocol/tf/test_db_tf.py               | 93 ++++++++++++++++++
 6 files changed, 431 insertions(+), 62 deletions(-)
 create mode 100644 dimos/mapping/utils/cli/test_world_registration.py
 create mode 100644 dimos/mapping/utils/cli/world_registration.py
 create mode 100644 dimos/protocol/tf/db_tf.py
 create mode 100644 dimos/protocol/tf/test_db_tf.py

diff --git a/dimos/mapping/utils/cli/map.py b/dimos/mapping/utils/cli/map.py
index cae3e04114..1adb018470 100644
--- a/dimos/mapping/utils/cli/map.py
+++ b/dimos/mapping/utils/cli/map.py
@@ -29,6 +29,7 @@
 # module just to register the `map` subcommand — stays fast. See test_cli_startup.py.
 if TYPE_CHECKING:
     from dimos.mapping.loop_closure.pgo import PoseGraph
+    from dimos.mapping.utils.cli.world_registration import WorldRegistrar
     from dimos.memory2.stream import Stream
     from dimos.memory2.type.observation import Observation
     from dimos.msgs.sensor_msgs.Image import Image
@@ -97,34 +98,21 @@ def _accumulate(
     block_count: int,
     device: str,
     graph: PoseGraph | None = None,
-    world_frame: bool = True,
+    registrar: WorldRegistrar | None = None,
     carve_columns: bool = False,
     progress_cb: Callable[[Observation[Any]], None] | None = None,
 ) -> PointCloud2 | None:
     """Accumulate a voxel map from `obs_iter`, optionally PGO-correcting each frame.
 
-    By default the clouds are assumed already world-registered (the go2/fastlio
-    path) — only the PGO correction is applied, if any. Set ``world_frame=False``
-    (the ``--use-tf`` path) when each frame's cloud is in the sensor/body frame
-    and must be registered into the world via its per-frame pose.
+    Each cloud is world-registered from its ``frame_id`` via ``registrar``:
+    clouds already in ``world`` pass through untouched, others are looked up in
+    the recording's ``tf`` stream, and frames with no resolvable transform are
+    dropped. The PGO correction, if any, is applied on top of the world cloud.
 
     Returns the final ``PointCloud2`` (or ``None`` if the input was empty).
     Disposal of the underlying ``VoxelGrid`` is handled by ``VoxelMapTransformer``.
     """
     from dimos.mapping.voxels import VoxelMapTransformer
-    from dimos.msgs.geometry_msgs.Quaternion import Quaternion
-    from dimos.msgs.geometry_msgs.Transform import Transform
-    from dimos.msgs.geometry_msgs.Vector3 import Vector3
-
-    def _pose_tf(obs: Observation[Any]) -> Transform:
-        pose = obs.pose
-        assert pose is not None
-        return Transform(
-            translation=Vector3(pose.position.x, pose.position.y, pose.position.z),
-            rotation=Quaternion(
-                pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w
-            ),
-        )
 
     def prepared() -> Iterable[Observation[PointCloud2]]:
         for obs in obs_iter:
@@ -132,20 +120,14 @@ def prepared() -> Iterable[Observation[PointCloud2]]:
                 progress_cb(obs)
             if len(obs.data) == 0:
                 continue
-            # body->world via the per-frame pose, unless the clouds are already
-            # world-registered (go2 default). graph adds the PGO correction on top
-            # (correction ∘ pose), applied after the pose.
-            tf: Transform | None = None
-            if not world_frame:
-                if obs.pose is None:
-                    continue
-                tf = _pose_tf(obs)
+            cloud = obs.data if registrar is None else registrar.register_cloud(obs.data, obs.ts)
+            if cloud is None:
+                continue
             if graph is not None:
                 if obs.pose_tuple is None:
                     continue
-                correction = graph.correction_at(obs.ts)
-                tf = correction if tf is None else correction + tf
-            yield obs if tf is None else obs.derive(data=obs.data.transform(tf))
+                cloud = cloud.transform(graph.correction_at(obs.ts))
+            yield obs if cloud is obs.data else obs.derive(data=cloud)
 
     vmt = VoxelMapTransformer(
         emit_every=0,  # batch mode: emit once on exhaustion
@@ -328,12 +310,6 @@ def main(
         None, "--out", help="Output .rrd path (default: ./<dataset>.rrd)"
     ),
     no_gui: bool = typer.Option(False, "--no-gui", help="Write the .rrd but don't launch rerun"),
-    use_tf: bool = typer.Option(
-        False,
-        "--use-tf",
-        help="Clouds are in the sensor/body frame; register each by its per-frame pose. "
-        "By default clouds are assumed already world-registered (e.g. go2/fastlio).",
-    ),
     carve: bool = typer.Option(
         False,
         "--carve/--no-carve",
@@ -388,6 +364,7 @@ def main(
 ) -> None:
     """Rebuild a voxel map from a recorded SQLite dataset, write a .rrd, and open it in rerun."""
     from dimos.mapping.loop_closure.pgo import PGO
+    from dimos.mapping.utils.cli.world_registration import WorldRegistrar
     from dimos.memory2.store.sqlite import SqliteStore
     from dimos.memory2.transform import QualityWindow, SpeedLimit
     from dimos.memory2.utils.progress import progress
@@ -406,6 +383,7 @@ def main(
         pgo = True
 
     store = SqliteStore(path=db_path)
+    registrar = WorldRegistrar(store)
     lidar = store.stream(lidar_stream, PointCloud2).from_time(seek or None).to_time(duration)
 
     print(lidar.summary())
@@ -415,17 +393,17 @@ def main(
     # Spatial dedup: bucket frames by 3D cell using the raw pose, keep the
     # latest per cell. Shared by raw and PGO rebuilds. Doesn't touch obs.data
     # so it stays cheap (no pointcloud loading). With pgo_tol<=0 the bucketing
-    # is disabled and every posed frame is kept (keyed by index).
+    # is disabled and every posed frame is kept (keyed by index). Frames with no
+    # baked pose (e.g. world-frame clouds that carry no per-frame pose) can't be
+    # spatially bucketed, so they're all kept by index and placed by the registrar.
     seen: dict[Any, Observation[Any]] = {}
     for i, obs in enumerate(lidar):
         pose = obs.pose
-        if pose is None:
-            continue
         # Reject placeholder poses: zero translation OR uninitialized rotation.
         # Same condition as pgo_keyframes so dedup and PGO see the same frames.
-        if pose.position.is_zero() or pose.orientation.is_zero():
+        if pose is not None and (pose.position.is_zero() or pose.orientation.is_zero()):
             continue
-        if pgo_tol > 0:
+        if pose is not None and pgo_tol > 0:
             t = pose.position
             # math.floor so negative coords bucket consistently; int() truncates
             # toward zero and silently folds -0.5 and 0.5 into the same cell.
@@ -445,8 +423,8 @@ def main(
     else:
         print(f"dedup: disabled, kept all [{n_kept}/{total}] posed frames")
 
-    # Dict insertion order = lidar iteration order = chronological.
-    # `seen` only contains entries with non-None poses (filtered above).
+    # Dict insertion order = lidar iteration order = chronological. Frames
+    # without a baked pose contribute no path point.
     path: list[tuple[float, float, float]] = [
         (p[0], p[1], p[2]) for obs in seen.values() if (p := obs.pose_tuple) is not None
     ]
@@ -470,7 +448,7 @@ def main(
             block_count=block_count,
             device=device,
             graph=graph,
-            world_frame=not use_tf,
+            registrar=registrar,
             carve_columns=carve,
             progress_cb=progress(n_kept, "pgo pass 2 (rebuilding)"),
         )
@@ -484,7 +462,7 @@ def main(
             block_count=block_count,
             device=device,
             graph=graph,
-            world_frame=not use_tf,
+            registrar=registrar,
             carve_columns=carve,
             progress_cb=progress(total, "full pgo (rebuilding)"),
         )
@@ -495,7 +473,7 @@ def main(
         voxel=voxel,
         block_count=block_count,
         device=device,
-        world_frame=not use_tf,
+        registrar=registrar,
         carve_columns=carve,
         progress_cb=progress(n_kept, "reconstructing global map"),
     )
diff --git a/dimos/mapping/utils/cli/replay.py b/dimos/mapping/utils/cli/replay.py
index c78704f725..76953730a2 100644
--- a/dimos/mapping/utils/cli/replay.py
+++ b/dimos/mapping/utils/cli/replay.py
@@ -44,8 +44,11 @@
 # main() so that `dimos map --help` stays fast. See test_cli_startup.py and the
 # same pattern in dimos/mapping/utils/cli/map.py.
 if TYPE_CHECKING:
+    from dimos.mapping.utils.cli.world_registration import WorldRegistrar
     from dimos.memory2.stream import Stream
     from dimos.memory2.type.observation import Observation
+    from dimos.msgs.geometry_msgs.Transform import Transform
+    from dimos.msgs.nav_msgs.Odometry import Odometry
     from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 
 TIMELINE = "ts"
@@ -89,6 +92,7 @@ def _log_clouds(
     voxel: float,
     point_mode: str,
     *,
+    registrar: WorldRegistrar | None = None,
     total: int | None = None,
     bottom_cutoff: float | None = None,
 ) -> None:
@@ -96,16 +100,20 @@ def _log_clouds(
 
     ``total`` overrides the progress denominator — useful for transform
     pipelines where calling :py:meth:`Stream.count` would materialize the
-    whole pipeline.
+    whole pipeline. With ``registrar``, non-world clouds are registered into
+    world via the recording's tf tree (and dropped if that lookup fails).
     """
     n = total if total is not None else stream.count()
     cb = _progress(n, label)
     for obs in stream:
         cb(obs)
+        cloud = obs.data if registrar is None else registrar.register_cloud(obs.data, obs.ts)
+        if cloud is None:
+            continue
         rr.set_time(TIMELINE, timestamp=obs.ts)
         rr.log(
             entity,
-            obs.data.to_rerun(voxel_size=voxel, mode=point_mode, bottom_cutoff=bottom_cutoff),
+            cloud.to_rerun(voxel_size=voxel, mode=point_mode, bottom_cutoff=bottom_cutoff),
         )
 
 
@@ -147,6 +155,29 @@ def _log_path(
         rr.log(entity, rr.LineStrips3D([points], colors=[color]))
 
 
+def _odom_world_pose(registrar: WorldRegistrar, obs: Observation[Odometry]) -> Transform | None:
+    """World pose of an odometry observation, or ``None`` if it can't be placed.
+
+    A world-frame (or frame-less) odometry pose is returned as-is; otherwise the
+    ``world <- frame_id`` transform from the recording's tf tree is composed onto
+    the payload pose. Returns ``None`` when the tf lookup fails.
+    """
+    from dimos.msgs.geometry_msgs.Transform import Transform
+
+    odom = obs.data
+    keep, world_from_frame = registrar.world_transform(getattr(odom, "frame_id", "") or "", obs.ts)
+    if not keep:
+        return None
+    pose = Transform(
+        translation=odom.position,
+        rotation=odom.orientation,
+        frame_id=odom.frame_id,
+        child_frame_id=odom.child_frame_id,
+        ts=obs.ts,
+    )
+    return pose if world_from_frame is None else world_from_frame + pose
+
+
 def main(
     dataset: str = typer.Argument(..., help="Dataset .db: bare name (cwd or data/) or path"),
     out: Path | None = typer.Option(
@@ -207,6 +238,7 @@ def main(
 ) -> None:
     """Dump a recording to .rrd (lidar clouds + camera frames) and open it in rerun."""
     from dimos.mapping.utils.cli.summary import _stream_payload_types
+    from dimos.mapping.utils.cli.world_registration import WorldRegistrar
     from dimos.mapping.voxels import VoxelMapTransformer
     from dimos.memory2.store.sqlite import SqliteStore
     from dimos.memory2.transform import throttle
@@ -252,6 +284,11 @@ def main(
     with store:
         print(store.summary())
 
+        # world-frame clouds/odometry render directly; anything in another frame is
+        # registered into world via the recording's tf stream (missing lookups warn
+        # and skip). See WorldRegistrar / DbTf.
+        registrar = WorldRegistrar(store)
+
         def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
             return store.stream(name, ptype).from_time(seek or None).to_time(duration)
 
@@ -261,9 +298,16 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
         livox = clipped("fastlio_lidar", PointCloud2) if has_livox else None
 
         # Per-frame raw clouds.
-        _log_clouds("       lidar", lidar, "world/lidar", voxel, point_mode)
+        _log_clouds("       lidar", lidar, "world/lidar", voxel, point_mode, registrar=registrar)
         if livox is not None:
-            _log_clouds("fastlio_lidar", livox, "world/fastlio_lidar", voxel, point_mode)
+            _log_clouds(
+                "fastlio_lidar",
+                livox,
+                "world/fastlio_lidar",
+                voxel,
+                point_mode,
+                registrar=registrar,
+            )
 
         # Accumulated voxel maps over the selected PointCloud2 streams.
         # --map logs a growing map per stream; --map-final logs one static map
@@ -275,10 +319,13 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
                 src = clipped(name, PointCloud2)
                 if not src.exists():
                     continue
+                # Register into world before voxelizing so the accumulated grid is
+                # built in world frame regardless of the source cloud's frame_id.
+                registered = registrar.register_clouds(src)
                 if map:
                     _log_clouds(
                         f"{name}_voxels",
-                        src.transform(
+                        registered.transform(
                             VoxelMapTransformer(
                                 emit_every=map_emit_every,
                                 carve_columns=map_carve_columns,
@@ -293,7 +340,7 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
                     )
                 if map_final:
                     # emit_every=0 → one accumulated obs at exhaustion
-                    final = src.transform(
+                    final = registered.transform(
                         VoxelMapTransformer(
                             emit_every=0, carve_columns=map_carve_columns, **grid_kwargs
                         )
@@ -306,29 +353,38 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
                         static=True,
                     )
 
-        # fastlio pose axis + path from fastlio_odometry stream.
+        # fastlio pose axis + path from fastlio_odometry stream. World-frame odometry
+        # renders directly; other frames are composed through the tf tree and frames
+        # with no tf chain are skipped (see WorldRegistrar).
         if "fastlio_odometry" in store.streams:
             odometry = clipped("fastlio_odometry", Odometry)
             cb = _progress(odometry.count(), "fastlio_odometry")
+            fastlio_path: list[tuple[float, float, float]] = []
+            last_ts: float | None = None
             for obs in odometry:
                 cb(obs)
-                if obs.pose_tuple is None:
+                world_pose = _odom_world_pose(registrar, obs)
+                if world_pose is None:
                     continue
+                translation, rotation = world_pose.translation, world_pose.rotation
                 rr.set_time(TIMELINE, timestamp=obs.ts)
-                x, y, z, qx, qy, qz, qw = obs.pose_tuple
                 rr.log(
                     "world/fastlio",
                     rr.Transform3D(
-                        translation=[x, y, z],
-                        quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw]),
+                        translation=[translation.x, translation.y, translation.z],
+                        quaternion=rr.Quaternion(
+                            xyzw=[rotation.x, rotation.y, rotation.z, rotation.w]
+                        ),
                     ),
                 )
-            _log_path(
-                "  fastlio_path",
-                clipped("fastlio_odometry", Odometry),
-                "world/fastlio_path",
-                color=(255, 165, 0),  # orange
-            )
+                fastlio_path.append((translation.x, translation.y, translation.z))
+                last_ts = obs.ts
+            if last_ts is not None and len(fastlio_path) >= 2:
+                rr.set_time(TIMELINE, timestamp=last_ts)
+                rr.log(
+                    "world/fastlio_path",
+                    rr.LineStrips3D([fastlio_path], colors=[(255, 165, 0)]),  # orange
+                )
 
         # Go2 native odom pose axis + path.
         if "odom" in store.streams:
diff --git a/dimos/mapping/utils/cli/test_world_registration.py b/dimos/mapping/utils/cli/test_world_registration.py
new file mode 100644
index 0000000000..2ae5e868c5
--- /dev/null
+++ b/dimos/mapping/utils/cli/test_world_registration.py
@@ -0,0 +1,73 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+import numpy as np
+
+from dimos.mapping.utils.cli.world_registration import WorldRegistrar
+from dimos.memory2.store.memory import MemoryStore
+from dimos.msgs.geometry_msgs.Transform import Transform
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+
+
+def _cloud(frame_id: str) -> PointCloud2:
+    """Single point at the origin in ``frame_id``."""
+    return PointCloud2.from_numpy(np.zeros((1, 3), dtype=np.float32), frame_id=frame_id)
+
+
+def _world_from_lidar(dx: float, ts: float) -> TFMessage:
+    return TFMessage(
+        Transform(
+            translation=Vector3(dx, 0.0, 0.0), frame_id="world", child_frame_id="lidar", ts=ts
+        )
+    )
+
+
+def test_world_frame_cloud_passes_through_untouched() -> None:
+    with MemoryStore() as store:
+        registrar = WorldRegistrar(store)
+        cloud = _cloud("world")
+        assert registrar.register_cloud(cloud, ts=1.0) is cloud
+        assert registrar.skipped == 0
+
+
+def test_non_world_cloud_is_registered_via_tf() -> None:
+    with MemoryStore() as store:
+        store.stream("tf", TFMessage).append(_world_from_lidar(1.0, ts=5.0), ts=5.0)
+        registrar = WorldRegistrar(store)
+
+        registered = registrar.register_cloud(_cloud("lidar"), ts=5.0)
+        assert registered is not None
+        points = registered.pointcloud_tensor.point["positions"].numpy()
+        # world <- lidar shifts the origin point +1.0 along x.
+        assert points[0][0] == 1.0
+        assert registrar.skipped == 0
+
+
+def test_non_world_cloud_skipped_without_tf_stream() -> None:
+    with MemoryStore() as store:
+        registrar = WorldRegistrar(store)
+        assert registrar.register_cloud(_cloud("lidar"), ts=1.0) is None
+        assert registrar.skipped == 1
+
+
+def test_non_world_cloud_skipped_for_unknown_frame() -> None:
+    with MemoryStore() as store:
+        store.stream("tf", TFMessage).append(_world_from_lidar(1.0, ts=5.0), ts=5.0)
+        registrar = WorldRegistrar(store)
+        assert registrar.register_cloud(_cloud("camera"), ts=5.0) is None
+        assert registrar.skipped == 1
diff --git a/dimos/mapping/utils/cli/world_registration.py b/dimos/mapping/utils/cli/world_registration.py
new file mode 100644
index 0000000000..2341059263
--- /dev/null
+++ b/dimos/mapping/utils/cli/world_registration.py
@@ -0,0 +1,96 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+from collections.abc import Iterator
+from typing import TYPE_CHECKING
+
+from dimos.protocol.tf.db_tf import DbTf
+from dimos.utils.logging_config import setup_logger
+
+if TYPE_CHECKING:
+    from dimos.memory2.store.base import Store
+    from dimos.memory2.stream import Stream
+    from dimos.memory2.type.observation import Observation
+    from dimos.msgs.geometry_msgs.Transform import Transform
+    from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+
+logger = setup_logger()
+
+WORLD_FRAME = "world"
+
+
+class _Unset:
+    pass
+
+
+_UNSET = _Unset()
+
+
+class WorldRegistrar:
+    """Bring frame-tagged observations into ``world`` using a recording's tf stream.
+
+    Messages already in ``world`` (or with no frame) pass through untouched.
+    Otherwise the ``world <- frame_id`` transform is looked up in the recording's
+    ``tf`` stream (loaded lazily on first non-world frame). A lookup that fails —
+    no tf stream, or no chain to that frame — is warned about, counted in
+    :attr:`skipped`, and the message is dropped.
+    """
+
+    def __init__(self, store: Store) -> None:
+        self._store = store
+        self._db_tf: DbTf | None | _Unset = _UNSET
+        self.skipped = 0
+
+    def _tf(self) -> DbTf | None:
+        if isinstance(self._db_tf, _Unset):
+            self._db_tf = DbTf.from_store(self._store)
+        return self._db_tf
+
+    def world_transform(self, frame_id: str, ts: float) -> tuple[bool, Transform | None]:
+        """Resolve how to place ``frame_id`` data into world at ``ts``.
+
+        Returns ``(keep, transform)``: ``keep=False`` means skip the message;
+        ``transform=None`` means it is already world (render/accumulate directly).
+        """
+        if not frame_id or frame_id == WORLD_FRAME:
+            return True, None
+        db_tf = self._tf()
+        transform = db_tf.get(WORLD_FRAME, frame_id, time_point=ts) if db_tf is not None else None
+        if transform is None:
+            self.skipped += 1
+            logger.warning(f"tf: no '{WORLD_FRAME} <- {frame_id}' at ts={ts:.3f} — skipping frame")
+            return False, None
+        return True, transform
+
+    def register_cloud(self, cloud: PointCloud2, ts: float) -> PointCloud2 | None:
+        """World-registered copy of ``cloud`` (itself if already world), or ``None`` to skip."""
+        keep, transform = self.world_transform(cloud.frame_id, ts)
+        if not keep:
+            return None
+        return cloud if transform is None else cloud.transform(transform)
+
+    def register_clouds(self, stream: Stream[PointCloud2]) -> Stream[PointCloud2]:
+        """World-register a PointCloud2 stream, dropping frames that can't be placed."""
+
+        def _register(
+            upstream: Iterator[Observation[PointCloud2]],
+        ) -> Iterator[Observation[PointCloud2]]:
+            for observation in upstream:
+                cloud = self.register_cloud(observation.data, observation.ts)
+                if cloud is not None:
+                    yield observation.derive(data=cloud)
+
+        return stream.transform(_register)
diff --git a/dimos/protocol/tf/db_tf.py b/dimos/protocol/tf/db_tf.py
new file mode 100644
index 0000000000..2ebb72ebd3
--- /dev/null
+++ b/dimos/protocol/tf/db_tf.py
@@ -0,0 +1,73 @@
+# 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 typing import TYPE_CHECKING
+
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+from dimos.protocol.tf.tf import MultiTBuffer
+
+if TYPE_CHECKING:
+    from dimos.memory2.store.base import Store
+    from dimos.msgs.geometry_msgs.Transform import Transform
+
+WORLD_FRAME = "world"
+TF_STREAM = "tf"
+
+
+class DbTf:
+    """Read-only TF tree backed by a recording's ``tf`` stream.
+
+    Loads every recorded :class:`TFMessage` into a :class:`MultiTBuffer` (with
+    an unbounded time window so lookups work at any point across the whole
+    recording) and exposes :meth:`get` for ``parent <- child`` lookups. Use it
+    to register frame-tagged observations into ``world`` at replay time instead
+    of trusting a baked-in per-observation pose.
+
+    Lookups use an unbounded time tolerance by default: static transforms
+    (mounts, ``world <- map``) are published once at the start of a recording,
+    so a fixed tolerance would drop them for every later observation. Densely
+    sampled dynamic transforms still resolve to their nearest sample.
+    """
+
+    def __init__(self, buffer: MultiTBuffer) -> None:
+        self._buffer = buffer
+
+    @classmethod
+    def from_store(cls, store: Store, stream_name: str = TF_STREAM) -> DbTf | None:
+        """Build a :class:`DbTf` from ``store``'s tf stream, or ``None`` if absent."""
+        if stream_name not in store.list_streams():
+            return None
+        buffer = MultiTBuffer(buffer_size=float("inf"))
+        for observation in store.stream(stream_name, TFMessage):
+            buffer.receive_transform(*observation.data.transforms)
+        return cls(buffer)
+
+    def get(
+        self,
+        target_frame: str,
+        source_frame: str,
+        time_point: float | None = None,
+        time_tolerance: float | None = None,
+    ) -> Transform | None:
+        """Transform mapping a point in ``source_frame`` into ``target_frame`` at
+        ``time_point`` (``None`` if no chain connects them)."""
+        return self._buffer.get(
+            target_frame, source_frame, time_point=time_point, time_tolerance=time_tolerance
+        )
+
+    @property
+    def frames(self) -> set[str]:
+        return self._buffer.get_frames()
diff --git a/dimos/protocol/tf/test_db_tf.py b/dimos/protocol/tf/test_db_tf.py
new file mode 100644
index 0000000000..98a1369a3f
--- /dev/null
+++ b/dimos/protocol/tf/test_db_tf.py
@@ -0,0 +1,93 @@
+# 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 dimos.memory2.store.memory import MemoryStore
+from dimos.msgs.geometry_msgs.Transform import Transform
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+from dimos.protocol.tf.db_tf import DbTf
+
+
+def _shift(parent: str, child: str, dx: float, ts: float) -> Transform:
+    """Identity-rotation transform translated by ``dx`` along x."""
+    return Transform(
+        translation=Vector3(dx, 0.0, 0.0),
+        frame_id=parent,
+        child_frame_id=child,
+        ts=ts,
+    )
+
+
+def test_from_store_returns_none_without_tf_stream() -> None:
+    store = MemoryStore()
+    with store:
+        store.stream("lidar", str).append("frame", ts=1.0)
+        assert DbTf.from_store(store) is None
+
+
+def test_get_composes_chain_across_frames() -> None:
+    store = MemoryStore()
+    with store:
+        tf_stream = store.stream("tf", TFMessage)
+        tf_stream.append(TFMessage(_shift("world", "base_link", 1.0, 5.0)), ts=5.0)
+        tf_stream.append(TFMessage(_shift("base_link", "lidar", 0.25, 5.0)), ts=5.0)
+
+        db_tf = DbTf.from_store(store)
+        assert db_tf is not None
+
+        composed = db_tf.get("world", "lidar", time_point=5.0)
+        assert composed is not None
+        # world<-base_link (+1.0) composed with base_link<-lidar (+0.25).
+        assert composed.translation.x == 1.25
+
+
+def test_get_returns_none_for_unknown_frame() -> None:
+    store = MemoryStore()
+    with store:
+        tf_stream = store.stream("tf", TFMessage)
+        tf_stream.append(TFMessage(_shift("world", "base_link", 1.0, 5.0)), ts=5.0)
+
+        db_tf = DbTf.from_store(store)
+        assert db_tf is not None
+        assert db_tf.get("world", "camera_optical", time_point=5.0) is None
+
+
+def test_get_respects_time_tolerance() -> None:
+    store = MemoryStore()
+    with store:
+        tf_stream = store.stream("tf", TFMessage)
+        tf_stream.append(TFMessage(_shift("world", "base_link", 1.0, 5.0)), ts=5.0)
+
+        db_tf = DbTf.from_store(store)
+        assert db_tf is not None
+        # A lookup far outside an explicit tolerance window finds nothing.
+        assert db_tf.get("world", "base_link", time_point=100.0, time_tolerance=0.5) is None
+
+
+def test_static_transform_latches_across_recording() -> None:
+    """A once-published static transform resolves at any later timestamp."""
+    store = MemoryStore()
+    with store:
+        tf_stream = store.stream("tf", TFMessage)
+        # A single static mount published only at the start of the recording.
+        tf_stream.append(TFMessage(_shift("world", "base_link", 1.0, 5.0)), ts=5.0)
+
+        db_tf = DbTf.from_store(store)
+        assert db_tf is not None
+        # Default (unbounded) tolerance latches the static transform long after.
+        far = db_tf.get("world", "base_link", time_point=1000.0)
+        assert far is not None
+        assert far.translation.x == 1.0

From 6d4d055e1125ba255241060fa90d94170d336af4 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Wed, 1 Jul 2026 20:15:40 -0500
Subject: [PATCH 03/19] china_office.db: regenerate pointlio at 2x replay
 (denser odom, tighter loop)

Regenerated pointlio_lidar + pointlio_odometry via pcap_to_db --rate 2.0 on
china_office.pcap, timestamps re-anchored to the recording epoch and tf tree
rebuilt (static mounts + odom->base_link). Reproduces the known-good __rate2
trajectory; 2x packs denser scans per Point-LIO wall-clock accumulation window
than 1x, yielding a tighter loop closure.
---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index 985afb1a69..9e2050237e 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:243a19ac6aef7f7feb9841e131be7d657781f3a1f5ed8a3be25b80e9d453aa4b
-size 4507943536
+oid sha256:9aaabe46ae0186cb2415cc05e4d726d0280f5e31eb94984889564aa8d177b5c5
+size 4804596832

From adf431817951a01ae211dbc8e392146d399d9b8d Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Wed, 1 Jul 2026 20:53:57 -0500
Subject: [PATCH 04/19] map cli: skip streams with unresolvable payload types

Recordings can reference Python classes not present in the current
checkout. summary/replay now warn and skip those streams instead of
crashing; requesting an unresolvable stream by name still raises.
---
 dimos/mapping/utils/cli/summary.py | 21 ++++++++++++++++++--
 dimos/memory2/store/sqlite.py      | 21 ++++++++++++++++++++
 dimos/memory2/test_store.py        | 32 ++++++++++++++++++++++++++++++
 3 files changed, 72 insertions(+), 2 deletions(-)

diff --git a/dimos/mapping/utils/cli/summary.py b/dimos/mapping/utils/cli/summary.py
index 694565b6e8..3d99b4d874 100644
--- a/dimos/mapping/utils/cli/summary.py
+++ b/dimos/mapping/utils/cli/summary.py
@@ -29,16 +29,33 @@
 from dimos.memory2.codecs.base import _resolve_payload_type
 from dimos.memory2.store.sqlite import SqliteStore
 from dimos.utils.data import resolve_named_path
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger()
 
 
 def _stream_payload_types(db_path: Path) -> dict[str, type]:
-    """Read each stream's registered payload type from the _streams table."""
+    """Read each stream's resolvable payload type from the _streams table.
+
+    Streams whose payload class can't be imported (recordings that reference
+    Python types not present in this checkout) are skipped with a warning
+    rather than crashing the whole command.
+    """
     conn = sqlite3.connect(str(db_path))
     try:
         rows = conn.execute("SELECT name, config FROM _streams").fetchall()
     finally:
         conn.close()
-    return {name: _resolve_payload_type(json.loads(cfg)["payload_module"]) for name, cfg in rows}
+    resolved: dict[str, type] = {}
+    for name, cfg in rows:
+        payload_module = json.loads(cfg)["payload_module"]
+        try:
+            resolved[name] = _resolve_payload_type(payload_module)
+        except (ImportError, AttributeError, ValueError) as error:
+            logger.warning(
+                f"skipping stream {name!r}: cannot load type {payload_module!r} ({error})"
+            )
+    return resolved
 
 
 def main(
diff --git a/dimos/memory2/store/sqlite.py b/dimos/memory2/store/sqlite.py
index 229961a126..a395e1343e 100644
--- a/dimos/memory2/store/sqlite.py
+++ b/dimos/memory2/store/sqlite.py
@@ -27,10 +27,14 @@
 from dimos.memory2.observationstore.sqlite import SqliteObservationStore
 from dimos.memory2.registry import RegistryStore, deserialize_component, qual
 from dimos.memory2.store.base import Store, StoreConfig
+from dimos.memory2.stream import Stream
 from dimos.memory2.utils.sqlite import open_disposable_sqlite_connection
 from dimos.memory2.utils.validation import validate_identifier
 from dimos.memory2.vectorstore.base import VectorStore
 from dimos.memory2.vectorstore.sqlite import SqliteVectorStore
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger()
 
 
 class SqliteStoreConfig(StoreConfig):
@@ -210,6 +214,23 @@ def list_streams(self) -> list[str]:
         db_names = set(self._registry.list_streams())
         return sorted(db_names | set(self._streams.keys()))
 
+    def summary(self) -> str:
+        """One line per stream, skipping any whose payload type can't be loaded.
+
+        Recordings may reference Python types not present in this checkout;
+        those streams are warned about and omitted rather than crashing the
+        whole summary. Requesting such a stream by name still raises.
+        """
+        lines: list[str] = []
+        for name in self.list_streams():
+            try:
+                stream: Stream[Any] = self.stream(name)
+            except (ImportError, AttributeError, ValueError) as error:
+                logger.warning(f"skipping stream {name!r}: unresolved payload type ({error})")
+                continue
+            lines.append(stream.summary())
+        return "\n".join(lines)
+
     def delete_stream(self, name: str) -> None:
         super().delete_stream(name)
         self._registry_conn.execute(f'DROP TABLE IF EXISTS "{name}"')
diff --git a/dimos/memory2/test_store.py b/dimos/memory2/test_store.py
index 984740d55f..647607cb82 100644
--- a/dimos/memory2/test_store.py
+++ b/dimos/memory2/test_store.py
@@ -274,6 +274,38 @@ def test_memory_lazy_with_blobstore(self, tmp_path) -> None:
             assert obs.data == "data1"
         bs.stop()
 
+    def test_sqlite_summary_skips_unresolvable_types(self, tmp_path) -> None:
+        """summary() skips streams whose payload class can't be imported.
+
+        Recordings may reference Python types not present in this checkout;
+        those must not crash a summary, but explicitly requesting one still errors.
+        """
+        import json
+        import sqlite3
+
+        from dimos.memory2.store.sqlite import SqliteStore
+        from dimos.msgs.std_msgs.Int32 import Int32
+
+        db = str(tmp_path / "unknown_type.db")
+        with SqliteStore(path=db) as store:
+            store.stream("good", Int32).append(Int32(data=1))
+            store.stream("ghost", Int32).append(Int32(data=2))
+
+        conn = sqlite3.connect(db)
+        row = conn.execute("SELECT config FROM _streams WHERE name='ghost'").fetchone()
+        cfg = json.loads(row[0])
+        cfg["payload_module"] = "dimos.msgs.does_not_exist.GhostType.GhostType"
+        conn.execute("UPDATE _streams SET config=? WHERE name='ghost'", (json.dumps(cfg),))
+        conn.commit()
+        conn.close()
+
+        with SqliteStore(path=db, must_exist=True) as store:
+            summary = store.summary()
+            assert "good" in summary
+            assert "ghost" not in summary
+            with pytest.raises((ImportError, AttributeError, ValueError)):
+                store.stream("ghost")
+
 
 class SpyBlobStore(BlobStore):
     """BlobStore that records all calls for verification."""

From 9bdc6ae493228970370dc7d369ed2393ee43b70e Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Wed, 1 Jul 2026 22:31:24 -0500
Subject: [PATCH 05/19] china_office.db: drop gt streams, add grid-config odom,
 align timestamps

Remove the 4 ground-truth pointlio streams (all worse than the 1.5x
replay), copy the six surf*/map* config runs' odometry from the grid
sweep, and align every pointlio_odometry* stream (including the __rate*
variants) to the canonical recording anchor so they overlay the tf tree
and lidar.
---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index 9e2050237e..10e50d9e0b 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9aaabe46ae0186cb2415cc05e4d726d0280f5e31eb94984889564aa8d177b5c5
-size 4804596832
+oid sha256:9e2afe6a42f4936dd272a704d19eea857ea158dec1331e5582f3f0b4d302d1f1
+size 3965600660

From 0fbc8b9a1e4a023791d1d36a94297ea854fb1f6b Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 00:09:17 -0500
Subject: [PATCH 06/19] china_office.db: replace frozen odom/tf/lidar with
 clean 1x regen

pointlio_odometry, tf, and pointlio_lidar are pulled from the 1x
regen (unfrozen odom, correctly-recorded 5-edge tf tree) and shifted
onto the June-12 data clock so they align with the kept color_image /
gtsam_odom / raw_april_tags streams. Intermediate checkpoint before PGO.
---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index 10e50d9e0b..f9430b045a 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9e2afe6a42f4936dd272a704d19eea857ea158dec1331e5582f3f0b4d302d1f1
-size 3965600660
+oid sha256:688e4951227b35031f23523f67e789763d109430b284b323dda7c0899382d2e7
+size 3624710348

From edd2036ae471d7ff33052b8e0f04b9bdcdbbeba0 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 03:45:00 -0500
Subject: [PATCH 07/19] china_office.db: add PGO gt_* streams + fix world<-map
 tf for map CLI

jnav_pgo added gt_pointlio_odometry, gt_pointlio_lidar, and the exotic-payload
streams (gt_tf_deformation_nodes, pose_graph, tf_graph). The map/replay CLI
registers clouds into the `world` frame, but the regen tf tree was rooted at
`map`, so every lidar frame was dropped (empty global map). Inject a static
`world <- map` identity transform so the full chain resolves. Validated:
map summary/global/replay all succeed, global reconstructs 94/94 frames.
---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index f9430b045a..a8a5d20a7c 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:688e4951227b35031f23523f67e789763d109430b284b323dda7c0899382d2e7
-size 3624710348
+oid sha256:622c20fe6e981e9c7d3bbf5e2fa74d7e537211d05802561dcb11e250c1ad122c
+size 4349302080

From 4c274e8e35f54391828cde237fa08ae3075b6b5d Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 04:37:47 -0500
Subject: [PATCH 08/19] test: skip sqlite summary test on macos/aarch64
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

test_sqlite_summary_skips_unresolvable_types opens a SqliteStore, which loads
the sqlite_vec extension. sqlite_vec has no working wheel on macOS or Linux ARM
(the aarch64 runner loads a wrong-arch vec0.so → ELFCLASS32), so mark it with
the same skipif_macos/skipif_aarch64 markers the other sqlite_vec tests use.
---
 dimos/memory2/test_store.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/dimos/memory2/test_store.py b/dimos/memory2/test_store.py
index 647607cb82..268d4845b4 100644
--- a/dimos/memory2/test_store.py
+++ b/dimos/memory2/test_store.py
@@ -274,6 +274,8 @@ def test_memory_lazy_with_blobstore(self, tmp_path) -> None:
             assert obs.data == "data1"
         bs.stop()
 
+    @pytest.mark.skipif_macos
+    @pytest.mark.skipif_aarch64
     def test_sqlite_summary_skips_unresolvable_types(self, tmp_path) -> None:
         """summary() skips streams whose payload class can't be imported.
 

From 4e07566f9472cfb73435d988183ce0a168991af0 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 12:48:32 -0500
Subject: [PATCH 09/19] china_office.db: compensate mount pitch in
 body->base_link tf

Point-LIO runs with extrinsic_r=identity, so its odom->body pose (and the
mid360_link-stamped clouds) are in the sensor's own frame. The recorded
body->base_link identity edge made the tf chain apply the 44 degree mount
pitch a second time, smearing `dimos map global` whenever the robot rotated.
Rewrote body->base_link to (base_link->mid360_link)^-1 so the chain cancels:
world<-mid360_link resolves to the sensor pose and base_link becomes the
true trunk pose.
---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index a8a5d20a7c..95317af6e0 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:622c20fe6e981e9c7d3bbf5e2fa74d7e537211d05802561dcb11e250c1ad122c
-size 4349302080
+oid sha256:c2ca2400b4f958ea11e2ebc361236b319495e5183dfd503bcc3cd6c632d8c9c5
+size 4349564285

From 72c008e77151dc3a2f896bf5bdd2ba4ba665c83d Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 14:19:58 -0500
Subject: [PATCH 10/19] fix: root the go2-mid360 tf tree at pointlio's sensor
 frame

Point-LIO runs with identity extrinsics, so its odometry child IS the
mid360 sensor frame. Name it honestly (odom->mid360_link) and hang the
robot off it via a derived inverse-mount static
(mid360_link->base_link), giving world->map->odom->mid360_link->
base_link->front_camera->camera_optical. Relabel china_office.db to
match (pure renames, no pose changes).
---
 data/.lfs/china_office.db.tar.gz              |  4 +-
 .../hardware/sensors/lidar/pointlio/module.py | 14 ++++---
 dimos/navigation/cmu_nav/frames.py            |  2 +-
 .../go2/go2_mid360_static_transforms.py       | 42 ++++++++++++++++---
 4 files changed, 48 insertions(+), 14 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index 95317af6e0..93f1c05af1 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:c2ca2400b4f958ea11e2ebc361236b319495e5183dfd503bcc3cd6c632d8c9c5
-size 4349564285
+oid sha256:05ad6a7f57ef1b4a6498bc749ba0c1ebbd379e3dd94a580203d246d078ee8e96
+size 4349565195
diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py
index 9db92e9f6d..4544af5bda 100644
--- a/dimos/hardware/sensors/lidar/pointlio/module.py
+++ b/dimos/hardware/sensors/lidar/pointlio/module.py
@@ -58,7 +58,7 @@
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-from dimos.navigation.cmu_nav.frames import FRAME_BODY, FRAME_ODOM
+from dimos.navigation.cmu_nav.frames import FRAME_ODOM
 from dimos.spec import perception
 
 # Human-readable enums; the C++ binary (main.cpp) maps these strings to
@@ -81,12 +81,14 @@ class PointLioConfig(NativeModuleConfig):
     lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP"))
     frequency: float = 10.0
 
-    # Odometry is published as frame_id (fixed) -> child_frame_id (moving body),
-    # and also broadcast on TF. The point cloud is stamped with sensor_frame_id
-    # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet
-    # transformed into the body frame).
+    # Odometry is published as frame_id (fixed) -> child_frame_id (moving), and
+    # also broadcast on TF. Point-LIO runs with identity extrinsics, so its
+    # tracked pose IS the sensor frame — child_frame_id defaults to the sensor
+    # frame name, and the rest of the robot tree (base_link, cameras) hangs off
+    # it via static transforms (see Go2Mid360StaticTf). The point cloud is
+    # stamped with sensor_frame_id (undistorted scan in the sensor frame).
     frame_id: str = FRAME_ODOM
-    child_frame_id: str = FRAME_BODY
+    child_frame_id: str = "mid360_link"
     sensor_frame_id: str = "mid360_link"
 
     # Point-LIO internal processing rates (Hz)
diff --git a/dimos/navigation/cmu_nav/frames.py b/dimos/navigation/cmu_nav/frames.py
index b8c13f4fb9..ed1c1af889 100644
--- a/dimos/navigation/cmu_nav/frames.py
+++ b/dimos/navigation/cmu_nav/frames.py
@@ -16,5 +16,5 @@
 
 FRAME_MAP = "map"
 FRAME_ODOM = "odom"
-FRAME_BODY = "body"
+FRAME_BODY = "base_link"
 FRAME_SENSOR = "sensor"
diff --git a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
index b430a11429..73037c7f50 100644
--- a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
+++ b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
@@ -15,8 +15,13 @@
 """Static mount frames for the Go2 + Mid-360 + front-camera rig.
 
 Published continuously onto tf while recording (see :class:`Go2Mid360StaticTf`) so the
-mount geometry lands in the recording's tf stream and companion streams (camera, go2
-lidar) can be anchored to ``base_link``.
+mount geometry lands in the recording's tf stream.
+
+The tree is rooted at ``mid360_link`` because Point-LIO runs with identity extrinsics:
+its odometry child frame IS the sensor frame, so odometry supplies the moving
+``odom -> mid360_link`` edge and the rest of the robot hangs off it:
+
+    odom -> mid360_link -> base_link -> front_camera -> camera_optical
 
 Mount geometry (measured on the physical rig)
 ---------------------------------------------
@@ -24,13 +29,18 @@
 - front_camera -> mid360_link: lidar is 3.2cm back, 12cm up, pitched 44 deg down.
 - front_camera -> camera_optical: the standard ROS optical rotation (x-right, y-down,
   z-forward).
+
+``mid360_link -> base_link`` is derived as the inverse of the composed mount
+(base_link -> front_camera -> mid360_link) rather than measured directly.
 """
 
 from __future__ import annotations
 
 import math
 
+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.protocol.tf.static_tf_publisher import (
     FrameSpec,
     StaticTfPublisher,
@@ -42,10 +52,32 @@
 # rpy that maps a sensor frame to its optical frame (z-forward, x-right, y-down)
 OPTICAL_RPY = (-math.pi / 2, 0.0, -math.pi / 2)
 
+BASE_TO_CAMERA_XYZ = (0.32715, -0.00003, 0.04297)
+CAMERA_TO_MID360_XYZ = (-0.032, 0.0, 0.12)
+CAMERA_TO_MID360_RPY = (0.0, MID360_PITCH_DOWN, 0.0)
+
+
+def _mid360_to_base() -> tuple[tuple[float, float, float], tuple[float, float, float]]:
+    base_to_camera = Transform(
+        translation=Vector3(*BASE_TO_CAMERA_XYZ),
+        rotation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)),
+    )
+    camera_to_mid360 = Transform(
+        translation=Vector3(*CAMERA_TO_MID360_XYZ),
+        rotation=Quaternion.from_euler(Vector3(*CAMERA_TO_MID360_RPY)),
+    )
+    mid360_to_base = (base_to_camera + camera_to_mid360).inverse()
+    translation = mid360_to_base.translation
+    rpy = mid360_to_base.rotation.to_euler()
+    return (translation.x, translation.y, translation.z), (rpy.x, rpy.y, rpy.z)
+
+
+MID360_TO_BASE_XYZ, MID360_TO_BASE_RPY = _mid360_to_base()
+
 FRAMES: list[FrameSpec] = [
-    ("base_link", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)),
-    ("front_camera", "base_link", (0.32715, -0.00003, 0.04297), (0.0, 0.0, 0.0)),
-    ("mid360_link", "front_camera", (-0.032, 0.0, 0.12), (0.0, MID360_PITCH_DOWN, 0.0)),
+    ("mid360_link", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)),
+    ("base_link", "mid360_link", MID360_TO_BASE_XYZ, MID360_TO_BASE_RPY),
+    ("front_camera", "base_link", BASE_TO_CAMERA_XYZ, (0.0, 0.0, 0.0)),
     ("camera_optical", "front_camera", (0.0, 0.0, 0.0), OPTICAL_RPY),
 ]
 

From 7ebe554f601a763b7261ad8a63cd278a82da9d7d Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 14:43:09 -0500
Subject: [PATCH 11/19] fix: plan_rrd errors on missing or empty streams

SqliteStore.stream() silently registers a typo'd name as a new empty
stream, so a wrong --lidar-stream/--odom-stream replayed zero frames
with no error (only the static goal point rendered).
---
 .../nav_3d/mls_planner/utils/plan_rrd.py           | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py
index b0cc2dec59..3997ee53bd 100644
--- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py
+++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py
@@ -314,12 +314,24 @@ def main(
 
     store = SqliteStore(path=str(db_path))
     with store:
+        # Guard against typo'd names: store.stream() silently registers a new
+        # empty stream, which would replay as zero frames with no error.
+        available = store.list_streams()
+        for stream_name in (lidar_stream, odom_stream):
+            if stream_name not in available:
+                raise typer.BadParameter(
+                    f"stream {stream_name!r} not found in {db_path}; "
+                    f"available: {', '.join(available)}"
+                )
         lidar = store.stream(lidar_stream, PointCloud2).order_by("ts")
+        odom = store.stream(odom_stream, Odometry).order_by("ts")
+        for stream_name, stream in ((lidar_stream, lidar), (odom_stream, odom)):
+            if next(iter(stream), None) is None:
+                raise typer.BadParameter(f"stream {stream_name!r} in {db_path} is empty")
         if from_time is not None:
             lidar = lidar.from_time(from_time)
         if to_time is not None:
             lidar = lidar.to_time(to_time)
-        odom = store.stream(odom_stream, Odometry).order_by("ts")
 
         pose_tagged = lidar.align(odom, tolerance=align_tol).transform(
             FnTransformer(_attach_pose_from_odom)

From 8885d80f6da312238373806f9f9e3e4bf029222c Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 14:44:37 -0500
Subject: [PATCH 12/19] docs: clarify plan_rrd --from-time/--to-time are
 relative offsets

---
 dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py
index 3997ee53bd..804cba9151 100644
--- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py
+++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py
@@ -303,10 +303,10 @@ def main(
         1.0, "--clearance-clamp", help="Max clearance (m) for the surface color scale"
     ),
     from_time: float | None = typer.Option(
-        None, "--from-time", help="Start timestamp (s); default is the stream start"
+        None, "--from-time", help="Start offset into the recording (s); default is the start"
     ),
     to_time: float | None = typer.Option(
-        None, "--to-time", help="End timestamp (s); default is the stream end"
+        None, "--to-time", help="Duration to replay after --from-time (s); default is the end"
     ),
 ) -> None:
     db_path = resolve_named_path(dataset, ".db")

From 43d526f4767fa9aefbf7b051c51cfa98bc1f0991 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 14:55:55 -0500
Subject: [PATCH 13/19] fix: record pointlio odometry against odom, not world

Drop the frame_id="world" override so recordings carry the same moving
edge (odom -> mid360_link) as existing datasets; world/map identity
edges are a localization concern, not the recorder's.
---
 .../unitree/go2/blueprints/basic/unitree_go2_mid360_record.py   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py
index be041b4788..a88f46d7a6 100644
--- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py
+++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py
@@ -77,7 +77,7 @@ def _default_recording_dir() -> Path:
             (Mid360, "imu", "livox_imu"),
         ]
     ),
-    PointLio.blueprint(frame_id="world").remappings(
+    PointLio.blueprint().remappings(
         [
             (PointLio, "lidar", "pointlio_lidar"),
             (PointLio, "odometry", "pointlio_odometry"),

From ab2144e2d3539c49581df5be914ab62a2e414113 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 14:58:07 -0500
Subject: [PATCH 14/19] feat: root the go2-mid360 recording tf tree at world

Publish identity world -> map -> odom edges from the static tf so a
fresh recording matches existing datasets (world-rooted tree with
pointlio's odom -> mid360_link as the moving edge).
---
 .../unitree/go2/go2_mid360_static_transforms.py    | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
index 73037c7f50..3c31127ee6 100644
--- a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
+++ b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
@@ -17,11 +17,12 @@
 Published continuously onto tf while recording (see :class:`Go2Mid360StaticTf`) so the
 mount geometry lands in the recording's tf stream.
 
-The tree is rooted at ``mid360_link`` because Point-LIO runs with identity extrinsics:
-its odometry child frame IS the sensor frame, so odometry supplies the moving
-``odom -> mid360_link`` edge and the rest of the robot hangs off it:
+The tree is rooted at ``world``, with identity ``world -> map -> odom`` edges.
+Point-LIO runs with identity extrinsics: its odometry child frame IS the sensor
+frame, so odometry supplies the moving ``odom -> mid360_link`` edge and the rest
+of the robot hangs off it:
 
-    odom -> mid360_link -> base_link -> front_camera -> camera_optical
+    world -> map -> odom -> mid360_link -> base_link -> front_camera -> camera_optical
 
 Mount geometry (measured on the physical rig)
 ---------------------------------------------
@@ -75,7 +76,10 @@ def _mid360_to_base() -> tuple[tuple[float, float, float], tuple[float, float, f
 MID360_TO_BASE_XYZ, MID360_TO_BASE_RPY = _mid360_to_base()
 
 FRAMES: list[FrameSpec] = [
-    ("mid360_link", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)),
+    ("world", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)),
+    ("map", "world", (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)),
+    ("odom", "map", (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)),
+    # odom -> mid360_link is the moving edge, published by Point-LIO.
     ("base_link", "mid360_link", MID360_TO_BASE_XYZ, MID360_TO_BASE_RPY),
     ("front_camera", "base_link", BASE_TO_CAMERA_XYZ, (0.0, 0.0, 0.0)),
     ("camera_optical", "front_camera", (0.0, 0.0, 0.0), OPTICAL_RPY),

From ae67b5553e92fcad3db071ca5711995d7b9edbf0 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 15:09:20 -0500
Subject: [PATCH 15/19] fix: mid360 mount is also yawed -90 deg about its own z

The puck is mounted sideways: pitched down 44 deg AND rotated -90 deg
about its own z. With pitch-only compensation base_link sat 61.7 deg
off level (measured against gravity in the china_office recording);
with the composed rotation it is level to ~4 deg.
---
 dimos/robot/unitree/go2/go2_mid360_static_transforms.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
index 3c31127ee6..97a2b44aa8 100644
--- a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
+++ b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py
@@ -27,7 +27,8 @@
 Mount geometry (measured on the physical rig)
 ---------------------------------------------
 - base_link -> front_camera: 32.7cm forward, ~4.3cm up (URDF front_camera mount).
-- front_camera -> mid360_link: lidar is 3.2cm back, 12cm up, pitched 44 deg down.
+- front_camera -> mid360_link: lidar is 3.2cm back, 12cm up, pitched 44 deg down
+  and yawed -90 deg about its own z (the puck is mounted sideways).
 - front_camera -> camera_optical: the standard ROS optical rotation (x-right, y-down,
   z-forward).
 
@@ -55,7 +56,10 @@
 
 BASE_TO_CAMERA_XYZ = (0.32715, -0.00003, 0.04297)
 CAMERA_TO_MID360_XYZ = (-0.032, 0.0, 0.12)
-CAMERA_TO_MID360_RPY = (0.0, MID360_PITCH_DOWN, 0.0)
+# Pitched down 44 deg, then yawed -90 deg about the sensor's own z (the puck is
+# mounted sideways). Ry(44) @ Rz(-90) expressed as extrinsic-xyz rpy — verified
+# against gravity in the china_office recording (base_link level to ~4 deg).
+CAMERA_TO_MID360_RPY = (-MID360_PITCH_DOWN, 0.0, -math.pi / 2)
 
 
 def _mid360_to_base() -> tuple[tuple[float, float, float], tuple[float, float, float]]:

From 062058e6bf7cf9127b3a142d52012faacede9ad9 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 15:45:46 -0500
Subject: [PATCH 16/19] feat: consolidate china_office.db streams and stamp
 april tags

- delete pose_graph, gtsam_odom, and the empty lidar stream
- rename gt_tf_deformation_nodes -> tf_deformation_nodes
- rename raw_april_tags -> april_tags_raw
- replace april_tags with the denser april_tags_dense detections
- stamp april_tags / april_tags_raw with frame_id=camera_optical so
  they resolve through the tf tree into world
---
 data/.lfs/china_office.db.tar.gz | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/data/.lfs/china_office.db.tar.gz b/data/.lfs/china_office.db.tar.gz
index 93f1c05af1..aa407f9f36 100644
--- a/data/.lfs/china_office.db.tar.gz
+++ b/data/.lfs/china_office.db.tar.gz
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:05ad6a7f57ef1b4a6498bc749ba0c1ebbd379e3dd94a580203d246d078ee8e96
-size 4349565195
+oid sha256:190ee898807baa069e40a4d6f12eda55f2d109a989f9228a813743fbb0ee8fef
+size 4349496315

From c5377c4472923931f637bb8eb2d6039ef4834e73 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Thu, 2 Jul 2026 15:45:54 -0500
Subject: [PATCH 17/19] feat: render april_tags markers in dimos map global

Tag poses are placed into world via the recording's tf tree (frame_id
lookup through WorldRegistrar), colored per marker_id, and drawn with a
short pin and visible label.
---
 dimos/mapping/utils/cli/map.py | 78 ++++++++++++++++++++++++++++++++--
 1 file changed, 75 insertions(+), 3 deletions(-)

diff --git a/dimos/mapping/utils/cli/map.py b/dimos/mapping/utils/cli/map.py
index 1adb018470..1bc770a3ab 100644
--- a/dimos/mapping/utils/cli/map.py
+++ b/dimos/mapping/utils/cli/map.py
@@ -30,6 +30,7 @@
 if TYPE_CHECKING:
     from dimos.mapping.loop_closure.pgo import PoseGraph
     from dimos.mapping.utils.cli.world_registration import WorldRegistrar
+    from dimos.memory2.store.base import Store
     from dimos.memory2.stream import Stream
     from dimos.memory2.type.observation import Observation
     from dimos.msgs.sensor_msgs.Image import Image
@@ -40,6 +41,8 @@
 # from each marker with the label floating at the top so multi-marker
 # labels never overlap the boxes.
 MARKER_STEM = 1.0
+# Tags are dense along the trajectory, so a tall pin just adds clutter.
+APRIL_TAG_STEM = 0.3
 
 
 def _log_markers(
@@ -51,11 +54,12 @@ def _log_markers(
     outline_half: list[tuple[float, float, float]],
     colors: list[tuple[int, int, int]],
     labels: list[str],
+    stem: float = MARKER_STEM,
 ) -> None:
     """Render per-marker fill + outline + pin-stem + label as four static entities."""
     n = len(centers)
-    pin_strips = [[(cx, cy, cz), (cx, cy, cz + MARKER_STEM)] for (cx, cy, cz) in centers]
-    label_positions = [(cx, cy, cz + MARKER_STEM + 0.01) for (cx, cy, cz) in centers]
+    pin_strips = [[(cx, cy, cz), (cx, cy, cz + stem)] for (cx, cy, cz) in centers]
+    label_positions = [(cx, cy, cz + stem + 0.01) for (cx, cy, cz) in centers]
     rr.log(
         f"{prefix}/fill",
         rr.Boxes3D(
@@ -86,7 +90,13 @@ def _log_markers(
     )
     rr.log(
         f"{prefix}/label",
-        rr.Points3D(positions=label_positions, labels=labels, colors=colors, radii=[0.001] * n),
+        rr.Points3D(
+            positions=label_positions,
+            labels=labels,
+            show_labels=True,
+            colors=colors,
+            radii=[0.001] * n,
+        ),
         static=True,
     )
 
@@ -270,6 +280,67 @@ def _log_reconstruction(
             )
 
 
+def _log_april_tags(
+    store: Store,
+    registrar: WorldRegistrar,
+    *,
+    marker_size: float,
+    seek: float,
+    duration: float | None,
+) -> None:
+    """Render the ``april_tags`` stream (if any) as static world markers.
+
+    Each tag pose is placed into ``world`` via the registrar: world/empty
+    frame_ids pass through, others are looked up in the recording's tf tree,
+    and poses whose frame can't be resolved are skipped with a warning.
+    """
+    from dimos.memory2.vis.color import Color
+    from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
+    from dimos.msgs.geometry_msgs.Transform import Transform
+
+    if "april_tags" not in store.list_streams():
+        return
+    tag_stream = store.stream("april_tags", PoseStamped).from_time(seek or None).to_time(duration)
+
+    centers: list[tuple[float, float, float]] = []
+    quats: list[tuple[float, float, float, float]] = []
+    colors: list[tuple[int, int, int]] = []
+    labels: list[str] = []
+    for obs in tag_stream:
+        keep, world_transform = registrar.world_transform(obs.data.frame_id, obs.ts)
+        if not keep:
+            continue
+        pose = Transform.from_pose(obs.data.frame_id, obs.data)
+        if world_transform is not None:
+            pose = world_transform + pose
+        marker_id = (obs.tags or {}).get("marker_id")
+        centers.append((pose.translation.x, pose.translation.y, pose.translation.z))
+        quats.append((pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w))
+        colors.append(
+            Color.from_cmap("tab10", (int(marker_id) % 10) / 10.0).rgb_u8()
+            if marker_id is not None
+            else (255, 255, 255)
+        )
+        labels.append(f"tag={marker_id}")
+    if not centers:
+        return
+
+    n = len(centers)
+    half = marker_size / 2.0
+    outline_bump = marker_size * 0.05
+    _log_markers(
+        "world/april_tags",
+        centers,
+        quats,
+        fill_half=[(half, half, 0.005)] * n,
+        outline_half=[(half + outline_bump, half + outline_bump, 0.006)] * n,
+        colors=colors,
+        labels=labels,
+        stem=APRIL_TAG_STEM,
+    )
+    print(f"april_tags: rendered {n} tag poses")
+
+
 def main(
     dataset: str = typer.Argument(..., help="Dataset .db: bare name (cwd or data/) or path"),
     lidar_stream: str = typer.Option(
@@ -544,6 +615,7 @@ def main(
         marker_size=marker_size,
         bottom_cutoff=bottom_cutoff,
     )
+    _log_april_tags(store, registrar, marker_size=marker_size, seek=seek, duration=duration)
     print(f"wrote {out}")
     if no_gui:
         print(f"open with: rerun {out}")

From a1ab537cfb01c378f95f5726aee3aba6f674305f Mon Sep 17 00:00:00 2001
From: Ivan Nikolic <lesh@sysphere.org>
Date: Fri, 3 Jul 2026 00:32:11 +0300
Subject: [PATCH 18/19] *lio publishes pointclouds in IMU frame, no mention of
 body

---
 dimos/control/blueprints/mobile.py            |  2 ++
 .../sensors/lidar/fastlio2/cpp/flake.lock     |  8 ++++----
 .../sensors/lidar/fastlio2/cpp/flake.nix      |  5 +++--
 .../sensors/lidar/fastlio2/cpp/main.cpp       |  4 +---
 .../hardware/sensors/lidar/fastlio2/module.py |  9 +++------
 .../sensors/lidar/pointlio/cpp/flake.lock     | 20 +++++++++----------
 .../sensors/lidar/pointlio/cpp/flake.nix      |  4 +++-
 .../sensors/lidar/pointlio/cpp/main.cpp       |  4 +---
 .../hardware/sensors/lidar/pointlio/module.py |  9 +++------
 .../robot/diy/alfred/blueprints/alfred_nav.py |  1 +
 .../navigation/unitree_go2_nav_3d.py          | 10 +++++-----
 11 files changed, 36 insertions(+), 40 deletions(-)

diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py
index 710526b926..703fe5d6a8 100644
--- a/dimos/control/blueprints/mobile.py
+++ b/dimos/control/blueprints/mobile.py
@@ -140,6 +140,8 @@ def _flowbase_twist_base(
                 "publish_free_paths": False,
             },
             simple_planner={
+                # FastLio2 publishes odom -> mid360_link (no separate body frame).
+                "body_frame": "mid360_link",
                 "cell_size": 0.2,
                 "obstacle_height_threshold": 0.15,
                 "inflation_radius": 0.3,  # FlowBase footprint smaller than G1's 0.5
diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock
index 34bf2f67bf..9435c81456 100644
--- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock
+++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock
@@ -37,16 +37,16 @@
     "fast-lio": {
       "flake": false,
       "locked": {
-        "lastModified": 1781776629,
-        "narHash": "sha256-Ik3OwjSUZza/C545iPC4G/fzfJfFsdIo2GvplgN45hA=",
+        "lastModified": 1783023362,
+        "narHash": "sha256-h+cK9PIA8dQKEaWendfxHz1kqfEc5sqaaPRG0ma1SRc=",
         "owner": "dimensionalOS",
         "repo": "dimos-module-fastlio2",
-        "rev": "a32c9f599940a94595aa72868e2e4ab436a44b75",
+        "rev": "7841812385d4ee79ed981b90e9a411933b293754",
         "type": "github"
       },
       "original": {
         "owner": "dimensionalOS",
-        "ref": "jeff/feat/fastlio-body-cloud",
+        "ref": "ivan/fix/body-cloud-imu-extrinsic",
         "repo": "dimos-module-fastlio2",
         "type": "github"
       }
diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix
index c7c4319440..15312f3f9f 100644
--- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix
+++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix
@@ -13,8 +13,9 @@
       flake = false;
     };
     fast-lio = {
-      # v0.3.0-quiet-logs + get_body_cloud() (sensor-frame cloud).
-      url = "github:dimensionalOS/dimos-module-fastlio2?ref=jeff/feat/fastlio-body-cloud";
+      # get_body_cloud()/get_body_cloud_down() with the IMU<-lidar extrinsic
+      # applied (PR #1); retarget to jeff/feat/fastlio-body-cloud once merged.
+      url = "github:dimensionalOS/dimos-module-fastlio2?ref=ivan/fix/body-cloud-imu-extrinsic";
       flake = false;
     };
     lcm-extended = {
diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp
index 94294ec312..0c2207dbc1 100644
--- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp
+++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp
@@ -59,7 +59,6 @@ static FastLio* g_fastlio = nullptr;
 static std::string g_lidar_topic;
 static std::string g_odometry_topic;
 static std::string g_frame_id;  // required via --frame_id
-static std::string g_child_frame_id;  // required via --child_frame_id
 static std::string g_sensor_frame_id;  // required via --sensor_frame_id
 static float g_frequency = 10.0f;
 
@@ -154,7 +153,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times
 
     nav_msgs::Odometry msg;
     msg.header = make_header(g_frame_id, timestamp);
-    msg.child_frame_id = g_child_frame_id;
+    msg.child_frame_id = g_sensor_frame_id;
 
     msg.pose.pose.position.x = odom.pose.pose.position.x;
     msg.pose.pose.position.y = odom.pose.pose.position.y;
@@ -399,7 +398,6 @@ int main(int argc, char** argv) {
     std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155");
     g_frequency = mod.arg_float("frequency", 10.0f);
     g_frame_id = mod.arg_required("frame_id");
-    g_child_frame_id = mod.arg_required("child_frame_id");
     g_sensor_frame_id = mod.arg_required("sensor_frame_id");
     float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f);
     float odom_freq = mod.arg_float("odom_freq", 50.0f);
diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py
index 6f0694ad4a..21c8363023 100644
--- a/dimos/hardware/sensors/lidar/fastlio2/module.py
+++ b/dimos/hardware/sensors/lidar/fastlio2/module.py
@@ -52,7 +52,7 @@
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-from dimos.navigation.cmu_nav.frames import FRAME_BODY, FRAME_ODOM
+from dimos.navigation.cmu_nav.frames import FRAME_ODOM
 from dimos.spec import perception
 
 # Human-readable enums; the C++ binary maps these strings to FAST-LIO's int codes.
@@ -71,12 +71,9 @@ class FastLio2Config(NativeModuleConfig):
     lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_FASTLIO_LIDAR_IP"))
     frequency: float = 10.0
 
-    # Odometry is published as frame_id (fixed) -> child_frame_id (moving body),
+    # Odometry is published as frame_id (fixed) -> sensor_frame_id (moving sensor),
     # and also broadcast on TF. The point cloud is stamped with sensor_frame_id
-    # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet
-    # transformed into the body frame).
     frame_id: str = FRAME_ODOM
-    child_frame_id: str = FRAME_BODY
     sensor_frame_id: str = "mid360_link"
 
     # FAST-LIO internal processing rates
@@ -150,7 +147,7 @@ def _on_odom_for_tf(self, msg: Odometry) -> None:
         self.tf.publish(
             Transform(
                 frame_id=self.frame_id,
-                child_frame_id=self.config.child_frame_id,
+                child_frame_id=self.config.sensor_frame_id,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,
diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock
index 3cb06c2284..6588a595e7 100644
--- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock
+++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock
@@ -37,18 +37,18 @@
     "fast-lio": {
       "flake": false,
       "locked": {
-        "lastModified": 1781782101,
-        "narHash": "sha256-2phOAdagFal8BTBEKxEbl3LDSx/7SNGVTFu0zYEXB1g=",
-        "owner": "dimensionalOS",
-        "repo": "dimos-module-fastlio2",
-        "rev": "288e357e5457723c1cce4d4060f76ed7f85b10d4",
-        "type": "github"
+        "lastModified": 1783023293,
+        "narHash": "sha256-isV6Jn3ACmpRFzUvm65cknT4B8bvswox93BBzEUHgUw=",
+        "ref": "main",
+        "rev": "82ef3a327347e2866e981bd95c8bece8b72903cf",
+        "revCount": 76,
+        "type": "git",
+        "url": "ssh://git@github.com/dimensionalOS/dimos-module-pointlio"
       },
       "original": {
-        "owner": "dimensionalOS",
-        "ref": "pointlio",
-        "repo": "dimos-module-fastlio2",
-        "type": "github"
+        "ref": "main",
+        "type": "git",
+        "url": "ssh://git@github.com/dimensionalOS/dimos-module-pointlio"
       }
     },
     "flake-utils": {
diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix
index 0ef30ba768..04b26b88fb 100644
--- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix
+++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix
@@ -13,7 +13,9 @@
       flake = false;
     };
     fast-lio = {
-      url = "github:dimensionalOS/dimos-module-fastlio2/pointlio";
+      # Point-LIO fork (split out of dimos-module-fastlio2's pointlio branch).
+      # Repo is org-internal for now, hence git+ssh instead of github:.
+      url = "git+ssh://git@github.com/dimensionalOS/dimos-module-pointlio?ref=main";
       flake = false;
     };
     lcm-extended = {
diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp
index 9c7bcb3714..91c5c53e0c 100644
--- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp
+++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp
@@ -77,7 +77,6 @@ static std::vector<double> parse_doubles(const std::string& csv) {
 static std::string g_lidar_topic;
 static std::string g_odometry_topic;
 static std::string g_frame_id;          // required via --frame_id
-static std::string g_child_frame_id;     // required via --child_frame_id
 static std::string g_sensor_frame_id;    // required via --sensor_frame_id
 static float g_frequency = 10.0f;
 
@@ -158,7 +157,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times
 
     nav_msgs::Odometry msg;
     msg.header = make_header(g_frame_id, timestamp);
-    msg.child_frame_id = g_child_frame_id;
+    msg.child_frame_id = g_sensor_frame_id;
 
     // Pose in the SLAM/sensor frame.
     msg.pose.pose.position.x = odom.pose.pose.position.x;
@@ -382,7 +381,6 @@ int main(int argc, char** argv) {
     std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155");
     g_frequency = mod.arg_float("frequency", 10.0f);
     g_frame_id = mod.arg_required("frame_id");
-    g_child_frame_id = mod.arg_required("child_frame_id");
     g_sensor_frame_id = mod.arg_required("sensor_frame_id");
     float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f);
     float odom_freq = mod.arg_float("odom_freq", 50.0f);
diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py
index 9db92e9f6d..352768254b 100644
--- a/dimos/hardware/sensors/lidar/pointlio/module.py
+++ b/dimos/hardware/sensors/lidar/pointlio/module.py
@@ -58,7 +58,7 @@
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-from dimos.navigation.cmu_nav.frames import FRAME_BODY, FRAME_ODOM
+from dimos.navigation.cmu_nav.frames import FRAME_ODOM
 from dimos.spec import perception
 
 # Human-readable enums; the C++ binary (main.cpp) maps these strings to
@@ -81,12 +81,9 @@ class PointLioConfig(NativeModuleConfig):
     lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP"))
     frequency: float = 10.0
 
-    # Odometry is published as frame_id (fixed) -> child_frame_id (moving body),
+    # Odometry is published as frame_id (fixed) -> sensor_frame_id (moving sensor),
     # and also broadcast on TF. The point cloud is stamped with sensor_frame_id
-    # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet
-    # transformed into the body frame).
     frame_id: str = FRAME_ODOM
-    child_frame_id: str = FRAME_BODY
     sensor_frame_id: str = "mid360_link"
 
     # Point-LIO internal processing rates (Hz)
@@ -186,7 +183,7 @@ def _on_odom_for_tf(self, msg: Odometry) -> None:
         self.tf.publish(
             Transform(
                 frame_id=self.frame_id,
-                child_frame_id=self.config.child_frame_id,
+                child_frame_id=self.config.sensor_frame_id,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,
diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py
index 16530ffadf..f7ec7f5485 100644
--- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py
+++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py
@@ -40,6 +40,7 @@
         "publish_free_paths": False,
     },
     simple_planner={
+        "body_frame": "mid360_link",
         "cell_size": 0.2,
         "obstacle_height_threshold": 0.15,
         "inflation_radius": 0.3,
diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py
index b18782043c..ef71c385ba 100644
--- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py
+++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py
@@ -47,11 +47,11 @@ def _render_path(msg: Any) -> Any:
 
 
 def _static_robot_body(rr: Any) -> list[Any]:
-    """Go2-shaped box on pointlio's body frame, counter-rotated for the lidar pitch."""
+    """Go2-shaped box on pointlio's sensor frame, counter-rotated for the lidar pitch."""
     return [
         rr.Boxes3D(half_sizes=[0.35, 0.155, 0.2], colors=[(0, 255, 127)]),
         rr.Transform3D(
-            parent_frame="tf#/body",
+            parent_frame="tf#/mid360_link",
             rotation=rr.RotationAxisAngle(axis=(0, 1, 0), degrees=-45.0),
         ),
     ]
@@ -66,9 +66,9 @@ def _static_robot_body(rr: Any) -> list[Any]:
     },
     "memory_limit": "256MB",
     # base_link tf comes from the go2 internal odometry, which is not the map
-    # frame. Anchor the robot box to pointlio's body frame instead and hide the
+    # frame. Anchor the robot box to pointlio's sensor frame instead and hide the
     # camera frustum that rides base_link.
-    "static": {"world/tf/body": _static_robot_body},
+    "static": {"world/tf/mid360_link": _static_robot_body},
     "visual_override": {
         **rerun_config["visual_override"],
         "world/global_map": _render_global_map,
@@ -91,7 +91,7 @@ def _static_robot_body(rr: Any) -> list[Any]:
             (GO2Connection, "odom", "odom_go2"),
         ]
     ),
-    PointLio.blueprint(child_frame_id="body"),
+    PointLio.blueprint(),
     RayTracingVoxelMap.blueprint(
         voxel_size=voxel_size,
         emit_every=1,

From b4c00ec112fa92aac58d0da1a07be31fff1b462c Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sun, 5 Jul 2026 09:52:32 -0700
Subject: [PATCH 19/19] feat: add go2 tf tree fix script for navigation-dev
 recordings

---
 dimos/experimental/go2_tf_tree_fix.py | 125 ++++++++++++++++++++++++++
 1 file changed, 125 insertions(+)
 create mode 100644 dimos/experimental/go2_tf_tree_fix.py

diff --git a/dimos/experimental/go2_tf_tree_fix.py b/dimos/experimental/go2_tf_tree_fix.py
new file mode 100644
index 0000000000..6fe56b0289
--- /dev/null
+++ b/dimos/experimental/go2_tf_tree_fix.py
@@ -0,0 +1,125 @@
+# 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.
+
+"""Fix the tf tree of go2 recordings made on andrew/feat/navigation-dev.
+
+Those recordings contain two competing pose chains:
+
+    world->base_link (from go2 odom)
+    world->body (from pointlio)
+        base_link->front_camera->{mid360_link, camera_optical}
+        base_link->camera_link->camera_optical (go2 driver)
+
+This rewrites them into a single tree rooted at the pointlio estimate:
+
+    world->go2_base_link->go2_camera_link->go2_camera_optical
+    world->mid360_link (from pointlio, was world->body)
+        mid360_link->base_link->front_camera->camera_optical
+
+The pointlio_odometry child frame is renamed body->mid360_link to match.
+
+Usage: uv run python -m dimos.experimental.go2_tf_tree_fix <recording.db>
+(edits the db in place)
+"""
+
+import argparse
+import sqlite3
+
+from dimos.msgs.geometry_msgs.Transform import Transform
+from dimos.msgs.nav_msgs.Odometry import Odometry
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+
+GO2_CHAIN_RENAMES = {
+    "base_link": "go2_base_link",
+    "camera_link": "go2_camera_link",
+    "camera_optical": "go2_camera_optical",
+}
+
+
+def rewrite_tf_message(message: TFMessage, latest_base_to_front: list[Transform | None]) -> bool:
+    changed = False
+    for transform in message.transforms:
+        if transform.frame_id == "base_link" and transform.child_frame_id == "front_camera":
+            latest_base_to_front[0] = transform
+
+    new_transforms = []
+    for transform in message.transforms:
+        edge = (transform.frame_id, transform.child_frame_id)
+        if edge == ("world", "base_link"):
+            transform.child_frame_id = "go2_base_link"
+            changed = True
+        elif edge == ("world", "body"):
+            transform.child_frame_id = "mid360_link"
+            changed = True
+        elif edge in (("base_link", "camera_link"), ("camera_link", "camera_optical")):
+            transform.frame_id = GO2_CHAIN_RENAMES[transform.frame_id]
+            transform.child_frame_id = GO2_CHAIN_RENAMES[transform.child_frame_id]
+            changed = True
+        elif edge == ("front_camera", "mid360_link"):
+            base_to_front = latest_base_to_front[0]
+            if base_to_front is None:
+                raise RuntimeError("saw front_camera->mid360_link before base_link->front_camera")
+            base_to_mid = base_to_front.to_matrix() @ transform.to_matrix()
+            transform = Transform.from_matrix(base_to_mid).inverse()
+            transform.frame_id = "mid360_link"
+            transform.child_frame_id = "base_link"
+            transform.ts = base_to_front.ts
+            changed = True
+        new_transforms.append(transform)
+    message.transforms = new_transforms
+    return changed
+
+
+def fix_database(db_path: str) -> None:
+    connection = sqlite3.connect(db_path)
+
+    latest_base_to_front: list[Transform | None] = [None]
+    tf_updates = 0
+    for row_id, data in connection.execute("SELECT id, data FROM tf_blob ORDER BY id"):
+        message = TFMessage.lcm_decode(bytes(data))
+        if rewrite_tf_message(message, latest_base_to_front):
+            connection.execute(
+                "UPDATE tf_blob SET data = ? WHERE id = ?", (message.lcm_encode(), row_id)
+            )
+            tf_updates += 1
+    print(f"rewrote {tf_updates} tf messages")
+
+    odom_updates = 0
+    for row_id, data in connection.execute("SELECT id, data FROM pointlio_odometry_blob"):
+        odometry = Odometry.lcm_decode(bytes(data))
+        if odometry.child_frame_id == "body":
+            odometry.child_frame_id = "mid360_link"
+            connection.execute(
+                "UPDATE pointlio_odometry_blob SET data = ? WHERE id = ?",
+                (odometry.lcm_encode(), row_id),
+            )
+            odom_updates += 1
+    print(f"renamed child frame on {odom_updates} pointlio_odometry messages")
+
+    connection.commit()
+    integrity = connection.execute("PRAGMA integrity_check").fetchone()[0]
+    connection.execute("PRAGMA wal_checkpoint(TRUNCATE)")
+    connection.close()
+    print(f"integrity_check: {integrity}")
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("db_path", help="recording .db to fix in place")
+    arguments = parser.parse_args()
+    fix_database(arguments.db_path)
+
+
+if __name__ == "__main__":
+    main()