From 199d41673ba14056a356cb2ec18f39241c26ca03 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Fri, 16 Oct 2026 08:50:45 -0700
Subject: [PATCH 01/21] r1pro-nav: dimSLAM owns odom -> base_link, fusing
 Point-LIO and the wheels

Point-LIO's own tf goes off the tree and the lidar mount hangs under base_link; the
planners read dimSLAM's pose. No cameras or IMU. Bumps dimSLAM to v0.4.2, whose
no-IMU blend no longer counts a returning source's gap twice.
---
 dimos/mapping/dim_slam/dim_slam.py            | 10 +--
 dimos/mapping/dim_slam/rust/Cargo.lock        |  2 +-
 dimos/mapping/dim_slam/rust/Cargo.toml        |  2 +-
 .../blueprints/basic/r1pro_coordinator.py     | 60 ++++++++++++++--
 .../r1pro/blueprints/navigation/r1pro_nav.py  | 17 +++--
 dimos/robot/galaxea/r1pro/config.py           |  4 ++
 dimos/robot/galaxea/r1pro/lio.py              | 59 +++++++++++++--
 dimos/robot/galaxea/r1pro/test_lio.py         | 71 +++++++++++++++++++
 pyproject.toml                                |  2 +-
 uv.lock                                       | 12 ++--
 10 files changed, 210 insertions(+), 29 deletions(-)
 create mode 100644 dimos/robot/galaxea/r1pro/test_lio.py

diff --git a/dimos/mapping/dim_slam/dim_slam.py b/dimos/mapping/dim_slam/dim_slam.py
index 6fd17b0bbe5..a23f2992e1b 100644
--- a/dimos/mapping/dim_slam/dim_slam.py
+++ b/dimos/mapping/dim_slam/dim_slam.py
@@ -109,9 +109,8 @@ class SourceConfig(BaseModel):
     being fused, so a fixed value is usually the right answer."""
 
     # The transform this source's estimates carry: each message's header.frame_id and
-    # child_frame_id. Both halves are needed because two sources can share a parent. A
-    # source whose parent is odom_frame_id is fused absolutely; anything else is fused as
-    # filter-anchored deltas, since its own pose has drifted.
+    # child_frame_id. Both halves are needed because two sources can share a parent. Every
+    # source is fused as filter-anchored deltas, so the parent only tells sources apart.
     parent_frame_id: str = ""
     child_frame_id: str = ""
     pose_variances: Covariance = Field(default_factory=Covariance)
@@ -172,8 +171,9 @@ class DimSlamConfig(NativeModuleConfig):
     # Caps the filter's own state rather than an incoming reading. 0 disables it.
     max_position_m: float = 10000.0
 
-    # Empty disables inertial propagation: the filter is seeded level from the first source
-    # message and holds its pose between them.
+    # Empty disables inertial propagation: the filter is seeded level at the origin, and
+    # each source's deltas are blended by inverse variance among the sources heard from in
+    # the last 0.5 s.
     imus: list[ImuConfig] = Field(default_factory=list)
     # m/s^2. Why is gravity a config?
     # 1) different IMUs have different bias
diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock
index 9308b22cb40..564de6d3e57 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.lock
+++ b/dimos/mapping/dim_slam/rust/Cargo.lock
@@ -567,7 +567,7 @@ dependencies = [
 [[package]]
 name = "dim_slam"
 version = "0.1.0"
-source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.1#7a8fc571fbd2b43fbdf9b99dc4f6eb5c014376d9"
+source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.2#34dd548f81d3b0584a399285ed3dd6cef7515c82"
 dependencies = [
  "cu_vslam_rs",
  "nalgebra",
diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml
index 8df62977ea2..acd2bbe4514 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.toml
+++ b/dimos/mapping/dim_slam/rust/Cargo.toml
@@ -11,7 +11,7 @@ path = "src/main.rs"
 [dependencies]
 # tag, not rev: nix's fetchGit only searches the ref the url names, so a bare rev is
 # unfetchable. Cargo.lock still pins the exact commit.
-dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.4.1" }
+dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.4.2" }
 # Unused directly, but cargo hands DEP_CUVSLAM_LIB_DIR only to immediate dependents
 # and build.rs needs it for the rpath.
 cu_vslam_rs = "0.2.0"
diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index 0ba5f72eed9..1e452293c30 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -35,6 +35,7 @@
 from dimos.core.global_config import global_config
 from dimos.core.transport import ZenohTransport
 from dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints import mid360_for_pointlio
+from dimos.mapping.dim_slam.dim_slam import Covariance, DimSlam, SourceConfig
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.nav_msgs.Odometry import Odometry
@@ -48,6 +49,7 @@
 from dimos.robot.galaxea.r1pro.config import (
     R1PRO_CHASSIS_LIDAR_HOST_IP,
     R1PRO_CHASSIS_LIDAR_IP,
+    R1PRO_WHEEL_ODOM_FRAME,
 )
 from dimos.robot.galaxea.r1pro.connection import R1PRO_UPPER_BODY_JOINTS, R1ProConnection
 from dimos.robot.galaxea.r1pro.head_cameras import (
@@ -55,7 +57,7 @@
     HeadLeftCamera,
     HeadRightCamera,
 )
-from dimos.robot.galaxea.r1pro.lio import R1ProLio, R1ProLioConfig, R1ProPointLio
+from dimos.robot.galaxea.r1pro.lio import PoseFromOdometry, R1ProLio, R1ProLioConfig, R1ProPointLio
 from dimos.visualization.rerun.bridge import RerunBridgeModule
 from dimos.visualization.rerun.websocket_server import RerunWebSocketServer
 
@@ -287,7 +289,7 @@ def r1pro_control(
     )
 
 
-def r1pro_lidar_odometry() -> Blueprint:
+def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
     """Our Mid-360 driver (per-point times, its own IMU) into Point-LIO, plus the mount tf."""
     lidar_frame = R1ProLioConfig.model_fields["lidar_frame"].default
     return autoconnect(
@@ -301,12 +303,62 @@ def r1pro_lidar_odometry() -> Blueprint:
             # Standing still at 0.01 the pose stepped ~7 mm a scan (the robot jittered in the viewer); 1.0 took it to 2.5 mm.
             lidar_meas_cov=1.0,
         ).remappings([(R1ProPointLio, "odometry", "pointlio_odometry")]),
-        R1ProLio.blueprint(),
+        R1ProLio.blueprint(mount_under_base=mount_under_base),
     ).remappings(
         [
             (R1ProLio, "odometry", "pointlio_odometry"),
+            # The name the planners already read, unless something else owns base_link's pose.
+            (R1ProLio, "pose", "pointlio_base_pose" if mount_under_base else "chassis_odom"),
+        ]
+    )
+
+
+# Without an IMU dimSLAM blends the sources' deltas by inverse variance: Point-LIO gets ~99% of x, y and yaw, and z,
+# roll and pitch outright. The wheels mostly carry the pose while Point-LIO is silent (dimSLAM's 0.5 s timeout). Replaying
+# a 30 m R1 drive, the fused pose stayed within 2.5 cm (mean) of Point-LIO's.
+POINTLIO_SOURCE = SourceConfig(
+    parent_frame_id="odom",
+    child_frame_id="base_link",
+    pose_variances=Covariance(x=1e-5, y=1e-5, z=1e-5, roll=1e-5, pitch=1e-5, yaw=1e-5),
+)
+WHEEL_SOURCE = SourceConfig(
+    parent_frame_id=R1PRO_WHEEL_ODOM_FRAME,
+    child_frame_id="base_link",
+    pose_variances=Covariance(x=1e-3, y=1e-3, yaw=1e-3),
+)
+
+
+def r1pro_fused_odometry() -> Blueprint:
+    """Point-LIO (carried to base_link) and wheel odometry into dimSLAM, which owns odom -> base_link.
+
+    Needs ``r1pro_control(odom_frame_id=R1PRO_WHEEL_ODOM_FRAME)`` with its ``odometry`` remapped to ``slam_sources``.
+    No cameras or IMUs: cuVSLAM on the head stereo would compete with Point-LIO for the Orin's cores. To add them, set
+    ``cameras`` and remap ``dimslam_image``/``dimslam_camera_info`` onto the head stereo streams.
+    """
+    return autoconnect(
+        r1pro_lidar_odometry(mount_under_base=True),
+        DimSlam.blueprint(
+            # Required, but nothing feeds the camera inputs, so the tracker never starts.
+            camera_mode="stereo",
+            odom_sources=[POINTLIO_SOURCE, WHEEL_SOURCE],
+        ),
+        PoseFromOdometry.blueprint(),
+    ).remappings(
+        [
+            # Point-LIO's own odom -> lidar edge would give base_link's subtree a second root.
+            (R1ProPointLio, "tf", "pointlio_tf"),
+            (R1ProLio, "base_odometry", "slam_sources"),
+            (DimSlam, "odom_sources", "slam_sources"),
+            (DimSlam, "odometry", "slam_odometry"),
+            (DimSlam, "image", "dimslam_image"),
+            (DimSlam, "camera_info", "dimslam_camera_info"),
+            (DimSlam, "depth_image", "dimslam_depth_image"),
+            (DimSlam, "depth_camera_info", "dimslam_depth_camera_info"),
+            (DimSlam, "depth_cloud", "dimslam_depth_cloud"),
+            (DimSlam, "imu", "dimslam_imu"),
+            (PoseFromOdometry, "odometry", "slam_odometry"),
             # The name the planners already read.
-            (R1ProLio, "pose", "chassis_odom"),
+            (PoseFromOdometry, "pose", "chassis_odom"),
         ]
     )
 
diff --git a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
index dc81832a03f..2be8d13f8eb 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""R1 Pro 3D navigation on Point-LIO (lidar + lidar-anchored head depth into one ray-traced map) with arm planning: ``dimos run r1pro-nav``."""
+"""R1 Pro 3D navigation on dimSLAM fusing Point-LIO and wheel odometry (lidar + lidar-anchored head depth into one ray-traced map) with arm planning: ``dimos run r1pro-nav``."""
 
 from __future__ import annotations
 
@@ -28,7 +28,7 @@
 from dimos.perception.depth2depth_cloud.module import Depth2DepthCloud
 from dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator import (
     r1pro_control,
-    r1pro_lidar_odometry,
+    r1pro_fused_odometry,
 )
 from dimos.robot.galaxea.r1pro.config import (
     R1PRO_CHASSIS_WIDTH_M,
@@ -36,8 +36,10 @@
     R1PRO_OVERHEAD_CLEARANCE_M,
     R1PRO_ROTATION_DIAMETER_M,
     R1PRO_URDF_MANAGER_CONFIG,
+    R1PRO_WHEEL_ODOM_FRAME,
     make_r1pro_model_config,
 )
+from dimos.robot.galaxea.r1pro.connection import R1ProConnection
 from dimos.robot.galaxea.r1pro.head_depth import r1pro_head_depth
 from dimos.robot.galaxea.r1pro.lio import R1ProPointLio
 from dimos.robot.galaxea.r1pro.rerun import r1pro_nav_rerun_config
@@ -59,9 +61,14 @@
         viewer_backend=global_config.viewer,
         rerun_config=r1pro_nav_rerun_config(VOXEL_SIZE_M, WALL_CLEARANCE_M, PLANNER_VIZ_HZ),
     ),
-    # Point-LIO owns odom -> base_link, so the connection's wheel odometry stays off tf.
-    r1pro_control(publish_odom_tf=False, enable_wrist_color=False, enable_imu=False),
-    r1pro_lidar_odometry().remappings([(R1ProPointLio, "lidar", "lidar_with_robot")]),
+    # dimSLAM owns odom -> base_link, so the connection's wheel odometry stays off tf and goes to it in a frame of its own.
+    r1pro_control(
+        publish_odom_tf=False,
+        odom_frame_id=R1PRO_WHEEL_ODOM_FRAME,
+        enable_wrist_color=False,
+        enable_imu=False,
+    ).remappings([(R1ProConnection, "odometry", "slam_sources")]),
+    r1pro_fused_odometry().remappings([(R1ProPointLio, "lidar", "lidar_with_robot")]),
     # Every joint on tf (the head cameras ride the torso), and the robot's own body cut out of the Mid-360's scans.
     UrdfManager.blueprint(
         **R1PRO_URDF_MANAGER_CONFIG,
diff --git a/dimos/robot/galaxea/r1pro/config.py b/dimos/robot/galaxea/r1pro/config.py
index b0c81ba148d..5230f106561 100644
--- a/dimos/robot/galaxea/r1pro/config.py
+++ b/dimos/robot/galaxea/r1pro/config.py
@@ -167,3 +167,7 @@ def make_r1pro_planar_model_config() -> RobotModelConfig:
         collision_exclusion_pairs=list(R1PRO_COLLISION_EXCLUSIONS),
         home_joints=[0.0] * len(R1PRO_PLANNING_JOINTS),
     )
+
+
+# The connection's wheel odometry's world frame where dimSLAM owns odom, which tells it apart from Point-LIO's.
+R1PRO_WHEEL_ODOM_FRAME = "wheel_odom"
diff --git a/dimos/robot/galaxea/r1pro/lio.py b/dimos/robot/galaxea/r1pro/lio.py
index ed4bf42d4b1..f182883b141 100644
--- a/dimos/robot/galaxea/r1pro/lio.py
+++ b/dimos/robot/galaxea/r1pro/lio.py
@@ -24,8 +24,10 @@
 from reactivex.disposable import Disposable
 
 from dimos.core.core import rpc
+from dimos.core.module import Module
 from dimos.core.stream import In, Out
 from dimos.hardware.sensors.lidar.pointlio.module import PointLio
+from dimos.msgs.geometry_msgs.Pose import Pose
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Quaternion import Quaternion
 from dimos.msgs.geometry_msgs.Transform import Transform
@@ -42,6 +44,8 @@ class R1ProLioConfig(StaticTfPublisherConfig):
     lidar_frame: str = "lidar_pointlio_link"
     # The URDF's name for the same lidar.
     chassis_lidar_frame: str = "lidar_chassis_left_link"
+    # On when something else (dimSLAM) owns odom -> base_link: the mount then hangs under base_link instead of over it.
+    mount_under_base: bool = False
 
 
 @cache
@@ -62,15 +66,15 @@ def joint_transform(model: LoadedRobotModel, child_link: str) -> Transform:
 
 
 def mount_transforms(config: R1ProLioConfig) -> list[Transform]:
-    """Rooted at Point-LIO's frame, plus the URDF's chassis-lidar edge."""
+    """Point-LIO's frame to base_link, then the URDF's chassis-lidar edge; the first is lidar -> base unless ``mount_under_base``."""
     model = lio_model(config.lidar_frame, config.chassis_lidar_frame)
     base_to_chassis_lidar = joint_transform(model, config.chassis_lidar_frame)
     assert base_to_chassis_lidar.frame_id == config.base_frame
     base_to_lidar = base_to_chassis_lidar + joint_transform(model, config.lidar_frame)
-    return [-base_to_lidar, base_to_chassis_lidar]
+    return [base_to_lidar if config.mount_under_base else -base_to_lidar, base_to_chassis_lidar]
 
 
-def base_pose(odometry: Odometry, lidar_to_base: Transform) -> PoseStamped:
+def base_transform(odometry: Odometry, lidar_to_base: Transform) -> Transform:
     """Point-LIO's lidar pose carried through the mount to base_link."""
     odom_to_lidar = Transform(
         translation=odometry.pose.position,
@@ -78,7 +82,11 @@ def base_pose(odometry: Odometry, lidar_to_base: Transform) -> PoseStamped:
         frame_id=odometry.frame_id,
         ts=odometry.ts,
     )
-    odom_to_base = odom_to_lidar + lidar_to_base
+    return odom_to_lidar + lidar_to_base
+
+
+def base_pose(odometry: Odometry, lidar_to_base: Transform) -> PoseStamped:
+    odom_to_base = base_transform(odometry, lidar_to_base)
     return PoseStamped(
         ts=odometry.ts,
         frame_id=odometry.frame_id,
@@ -87,13 +95,26 @@ def base_pose(odometry: Odometry, lidar_to_base: Transform) -> PoseStamped:
     )
 
 
+def base_odometry(odometry: Odometry, lidar_to_base: Transform, base_frame: str) -> Odometry:
+    """The same pose as an Odometry (no twist: Point-LIO's is the lidar's, not base_link's)."""
+    odom_to_base = base_transform(odometry, lidar_to_base)
+    return Odometry(
+        ts=odometry.ts,
+        frame_id=odometry.frame_id,
+        child_frame_id=base_frame,
+        pose=Pose(odom_to_base.translation, odom_to_base.rotation),
+    )
+
+
 class R1ProLio(StaticTfPublisher):
-    """Hangs base_link under Point-LIO's lidar frame, and publishes base_link's pose from Point-LIO's odometry."""
+    """Hangs base_link under Point-LIO's lidar frame (or the reverse), and publishes base_link's pose from Point-LIO's odometry."""
 
     config: R1ProLioConfig
 
     odometry: In[Odometry]
     pose: Out[PoseStamped]
+    # The same pose as an Odometry, for a fusion module such as dimSLAM.
+    base_odometry: Out[Odometry]
 
     def transforms(self) -> list[Transform]:
         return mount_transforms(self.config)
@@ -101,11 +122,37 @@ def transforms(self) -> list[Transform]:
     @rpc
     def start(self) -> None:
         super().start()
-        self._lidar_to_base = mount_transforms(self.config)[0]
+        base_to_lidar = mount_transforms(self.config.model_copy(update={"mount_under_base": True}))[
+            0
+        ]
+        self._lidar_to_base = -base_to_lidar
         self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry)))
 
     def _on_odometry(self, msg: Odometry) -> None:
         self.pose.publish(base_pose(msg, self._lidar_to_base))
+        self.base_odometry.publish(base_odometry(msg, self._lidar_to_base, self.config.base_frame))
+
+
+class PoseFromOdometry(Module):
+    """An Odometry's pose as a PoseStamped, for the planners that read one."""
+
+    odometry: In[Odometry]
+    pose: Out[PoseStamped]
+
+    @rpc
+    def start(self) -> None:
+        super().start()
+        self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry)))
+
+    def _on_odometry(self, msg: Odometry) -> None:
+        self.pose.publish(
+            PoseStamped(
+                ts=msg.ts,
+                frame_id=msg.frame_id,
+                position=msg.pose.position,
+                orientation=msg.pose.orientation,
+            )
+        )
 
 
 # Sharing the Orin's cores at load ~23, Point-LIO waited for a core ~60% of the time, ran 0.4-2 s behind and dropped scans: the robot stuttered in the viewer.
diff --git a/dimos/robot/galaxea/r1pro/test_lio.py b/dimos/robot/galaxea/r1pro/test_lio.py
new file mode 100644
index 00000000000..cc05041ceb1
--- /dev/null
+++ b/dimos/robot/galaxea/r1pro/test_lio.py
@@ -0,0 +1,71 @@
+# 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.
+
+import pytest
+
+from dimos.mapping.dim_slam.dim_slam import DimSlam
+from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.geometry_msgs.Quaternion import Quaternion
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.msgs.nav_msgs.Odometry import Odometry
+from dimos.robot.galaxea.r1pro.blueprints.navigation.r1pro_nav import r1pro_nav
+from dimos.robot.galaxea.r1pro.lio import (
+    R1ProLioConfig,
+    base_odometry,
+    base_pose,
+    mount_transforms,
+)
+
+
+def test_mount_under_base_reverses_the_lidar_edge() -> None:
+    over, chassis = mount_transforms(R1ProLioConfig())
+    under, same_chassis = mount_transforms(R1ProLioConfig(mount_under_base=True))
+    assert (over.frame_id, over.child_frame_id) == ("lidar_pointlio_link", "base_link")
+    assert (under.frame_id, under.child_frame_id) == ("base_link", "lidar_pointlio_link")
+    assert (over + under).translation.length() == pytest.approx(0.0, abs=1e-9)
+    assert chassis.child_frame_id == same_chassis.child_frame_id == "lidar_chassis_left_link"
+
+
+def test_base_odometry_is_base_pose() -> None:
+    lidar_to_base = mount_transforms(R1ProLioConfig())[0]
+    lidar = Odometry(
+        ts=3.0,
+        frame_id="odom",
+        child_frame_id="lidar_pointlio_link",
+        pose=Pose(Vector3(1.0, 2.0, 0.5), Quaternion.from_euler(Vector3(0.0, 0.0, 0.7))),
+    )
+    pose = base_pose(lidar, lidar_to_base)
+    odometry = base_odometry(lidar, lidar_to_base, "base_link")
+    assert (odometry.frame_id, odometry.child_frame_id, odometry.ts) == ("odom", "base_link", 3.0)
+    assert (odometry.position - pose.position).length() == pytest.approx(0.0, abs=1e-12)
+    assert odometry.orientation.to_euler().z == pytest.approx(pose.orientation.to_euler().z)
+
+
+def topic_users(direction: str, topic: str) -> set[str]:
+    """Modules with a ``direction`` ("in"/"out") stream on ``topic`` in r1pro_nav."""
+    users = set()
+    for atom in r1pro_nav.active_blueprints:
+        for stream in atom.streams:
+            name = r1pro_nav.remapping_map.get((atom.name, stream.name), stream.name)
+            if name == topic and stream.direction == direction:
+                users.add(atom.module.__name__)
+    return users
+
+
+def test_dimslam_takes_pointlio_and_wheels_and_alone_drives_the_planners() -> None:
+    assert topic_users("out", "slam_sources") == {"R1ProLio", "R1ProConnection"}
+    assert topic_users("in", "slam_sources") == {DimSlam.__name__}
+    assert topic_users("out", "chassis_odom") <= {"PoseFromOdometry", "R1ProConnection"}
+    # Off tf, so dimSLAM's odom -> base_link is base_link's only parent.
+    assert "R1ProPointLio" not in topic_users("out", "tf")
diff --git a/pyproject.toml b/pyproject.toml
index 30490697f59..b6fa4e656d1 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -503,7 +503,7 @@ tests-self-hosted = [
     "mcap>=1.2.0",
     # Needed to compile the in-tree extensions.
     "pybind11>=2.12",
-    "dim-odom>=0.4.1",
+    "dim-odom>=0.4.2",
 ]
 docs = [
     "mkdocs-awesome-nav>=3.3.0",
diff --git a/uv.lock b/uv.lock
index 399f583ddb0..fd89b4d08f2 100644
--- a/uv.lock
+++ b/uv.lock
@@ -1788,7 +1788,7 @@ wheels = [
 
 [[package]]
 name = "dim-odom"
-version = "0.4.1"
+version = "0.4.2"
 source = { registry = "https://pypi.org/simple" }
 dependencies = [
     { name = "nvidia-cublas", marker = "sys_platform == 'linux'" },
@@ -1797,10 +1797,10 @@ dependencies = [
     { name = "nvidia-cusparse", marker = "sys_platform == 'linux'" },
 ]
 wheels = [
-    { url = "https://files.pythonhosted.org/packages/a0/fd/7e4f24f8f6989fced595f95165e431f32f724ab7bfc3225646dfe05dce1a/dim_odom-0.4.1-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:dcc4d877147c0cc9f5dfc37435478b20bf602cd409b13a09ca0c49a59c04bab1", size = 6974856, upload-time = "2026-09-02T21:29:14.014Z" },
-    { url = "https://files.pythonhosted.org/packages/0e/6a/35fb6f6596222f2c3d35b561d6f1f4ea952728d2ca426f7fe83b9b446c26/dim_odom-0.4.1-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:d95aef4ebf184b1304afe826ebc0897d5bff37836ff87ef36e4adef8274e6f5a", size = 86695321, upload-time = "2026-09-02T21:28:11.583Z" },
-    { url = "https://files.pythonhosted.org/packages/e6/90/1c07b7b07c88aa11875cf24a0ce89ea9be4b4570fcd18a1ae25bc9c3911a/dim_odom-0.4.1-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:931c397f497256d92c44a26d9637139b9dc710ece29a10db1ddebd9ce5ce56d2", size = 85493366, upload-time = "2026-09-02T21:29:02.237Z" },
-    { url = "https://files.pythonhosted.org/packages/3f/10/70139307b3303f120afd41cae7615bf8f55008c7fa0c8628f861657ba4f8/dim_odom-0.4.1-py3-none-any.whl", hash = "sha256:417cfc94351fdb730e4745c3fad3d2051a85b77a2fd0c271a19ced85096b20d6", size = 1412, upload-time = "2026-09-02T21:27:38.713Z" },
+    { url = "https://files.pythonhosted.org/packages/ed/7f/a5d35789da6a78d9cd2283387fce4029069f61e92aa4cb29fd670a1d78ce/dim_odom-0.4.2-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:6d506b8dca4b1592bdd3da69a28b3c69ba84785b25ec391b121f97623baf720b", size = 6975797, upload-time = "2026-10-16T15:46:22.966Z" },
+    { url = "https://files.pythonhosted.org/packages/5b/30/5e4738bd2165187908ac27b2592f19b9eed2c2a436a9d87e3e93127fcea1/dim_odom-0.4.2-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:6cbb8e6d74315d99d8601fddf2e40416780d3731549d2a0a68c255a830e23a16", size = 86698178, upload-time = "2026-10-16T15:46:34.213Z" },
+    { url = "https://files.pythonhosted.org/packages/e6/0f/15773c43292b03690ba3ed8481f133130902850c1bcb8ca8b2b02a93d087/dim_odom-0.4.2-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:d9a5b76cb4932d27deb2b9733f5ec64ef9a10a58ad0b348db245286bf03514f8", size = 85495518, upload-time = "2026-10-16T15:46:48.071Z" },
+    { url = "https://files.pythonhosted.org/packages/e0/d0/c7d15b665e5bf9ecce0936283defbc08209b8bba47e25b21fb70de6e63c2/dim_odom-0.4.2-py3-none-any.whl", hash = "sha256:3a61913b44334e3de5420c637707297c5fde7cc03d78eac22571f98979b35ade", size = 1412, upload-time = "2026-10-16T15:46:51.201Z" },
 ]
 
 [[package]]
@@ -2716,7 +2716,7 @@ tests-self-hosted = [
     { name = "chromadb", specifier = ">=1.0.0" },
     { name = "coacd", specifier = ">=1.0.0" },
     { name = "coverage", specifier = ">=7.0" },
-    { name = "dim-odom", specifier = ">=0.4.1" },
+    { name = "dim-odom", specifier = ">=0.4.2" },
     { name = "dimos", extras = ["apriltag", "mapping", "drone", "cpu", "learning"] },
     { name = "dimos", extras = ["manipulation", "misc", "unitree"] },
     { name = "dimos", extras = ["web", "visualization", "webrtc"] },

From 3df13bd9bd60830b7b58f53e27cdd5333c33cb53 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Fri, 16 Oct 2026 08:52:31 -0700
Subject: [PATCH 02/21] dim_slam: the nix hash for dimSLAM v0.4.2

---
 dimos/mapping/dim_slam/rust/crate-hashes.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/dimos/mapping/dim_slam/rust/crate-hashes.json b/dimos/mapping/dim_slam/rust/crate-hashes.json
index 980a12b1ffc..76055f50382 100644
--- a/dimos/mapping/dim_slam/rust/crate-hashes.json
+++ b/dimos/mapping/dim_slam/rust/crate-hashes.json
@@ -1,5 +1,5 @@
 {
-  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.1#dim_slam@0.1.0": "sha256-jx3QjYNclt3JFJOonGIAzfoJeJ5bhowx70a7LumF6IA=",
+  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.2#dim_slam@0.1.0": "sha256-V7L92jW9sacgvz2qsRMJdWsEstalQNHRpbTdUoZtAPw=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",

From 3b19ff7734c674136d137bde8534ab8ea3ca4c3b Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Fri, 16 Oct 2026 09:17:28 -0700
Subject: [PATCH 03/21] all_blueprints: register PoseFromOdometry

---
 dimos/robot/all_blueprints.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py
index e3957a11cb6..67c90891a6f 100644
--- a/dimos/robot/all_blueprints.py
+++ b/dimos/robot/all_blueprints.py
@@ -324,6 +324,7 @@
     "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio",
     "point-nav-skill-container": "dimos.agents.skills.point_nav.PointNavSkillContainer",
     "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder",
+    "pose-from-odometry": "dimos.robot.galaxea.r1pro.lio.PoseFromOdometry",
     "r1-pro-connection": "dimos.robot.galaxea.r1pro.connection.R1ProConnection",
     "r1-pro-lio": "dimos.robot.galaxea.r1pro.lio.R1ProLio",
     "r1-pro-point-lio": "dimos.robot.galaxea.r1pro.lio.R1ProPointLio",

From 3df457a42b3413b1317b8c1a2778cbfe8bf1941d Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 19:57:36 -0700
Subject: [PATCH 04/21] dim_slam: dimSLAM v0.5.0-beta1, one ESKF with source
 kinds, publishing on a timer

---
 dimos/mapping/dim_slam/dim_slam.py            |  38 +++--
 dimos/mapping/dim_slam/rust/Cargo.lock        |   2 +-
 dimos/mapping/dim_slam/rust/Cargo.toml        |   2 +-
 dimos/mapping/dim_slam/rust/crate-hashes.json |   2 +-
 dimos/mapping/dim_slam/rust/src/main.rs       | 157 ++++++++++++------
 dimos/robot/diy/alfred/blueprints/vis_nav.py  |   2 +-
 .../navigation/visual_nav_setup.md            |  16 +-
 7 files changed, 151 insertions(+), 68 deletions(-)

diff --git a/dimos/mapping/dim_slam/dim_slam.py b/dimos/mapping/dim_slam/dim_slam.py
index a23f2992e1b..fac9a0e8948 100644
--- a/dimos/mapping/dim_slam/dim_slam.py
+++ b/dimos/mapping/dim_slam/dim_slam.py
@@ -99,9 +99,20 @@ class InitialStds(BaseModel):
     position: float = 0.01
     velocity: float = 0.1
     rotation: float = 0.05
+    # Without an IMU only.
+    angular_velocity: float = 0.1
     bias: float = 0.05
 
 
+class ProcessNoise(BaseModel):
+    """How fast the body's velocity can change unannounced; only used without an IMU."""
+
+    # m/s^2/sqrt(Hz).
+    linear_acceleration: float = 1.0
+    # rad/s^2/sqrt(Hz).
+    angular_acceleration: float = 1.0
+
+
 class SourceConfig(BaseModel):
     """One external odometry source and how much it is trusted. A variance below zero takes
     the message covariance, zero drops that dimension, above zero is a fixed variance. A
@@ -109,13 +120,18 @@ class SourceConfig(BaseModel):
     being fused, so a fixed value is usually the right answer."""
 
     # The transform this source's estimates carry: each message's header.frame_id and
-    # child_frame_id. Both halves are needed because two sources can share a parent. Every
-    # source is fused as filter-anchored deltas, so the parent only tells sources apart.
+    # child_frame_id. Both halves are needed because two sources can share a parent.
     parent_frame_id: str = ""
     child_frame_id: str = ""
+    # pose: a pose that does not drift (lidar-inertial odometry). twist: only the twist is used
+    # (wheels). pose_delta: a pose that drifts, fused by its message-to-message motion.
+    kind: Literal["pose", "twist", "pose_delta"] = "pose_delta"
     pose_variances: Covariance = Field(default_factory=Covariance)
     # Body frame.
     twist_variances: Covariance = Field(default_factory=Covariance)
+    # pose only: a source rejected this long without a break is re-aligned to the fused pose;
+    # 0 never re-aligns.
+    realign_after_seconds: float = 0.0
 
 
 class DimSlamConfig(NativeModuleConfig):
@@ -162,7 +178,7 @@ class DimSlamConfig(NativeModuleConfig):
     # Off when something downstream owns odom -> output_frame_id.
     publish_tf: bool = True
 
-    # The filter itself steps at the IMU rate; this is only how often it emits.
+    # Output cadence, on a timer: each output is the filter predicted to its own stamp.
     publish_rate: float = 50.0
     replay_buffer_seconds: float = 0.5
     # Outlier gate in variance units, per measurement dimension. Smaller is more
@@ -171,15 +187,15 @@ class DimSlamConfig(NativeModuleConfig):
     # Caps the filter's own state rather than an incoming reading. 0 disables it.
     max_position_m: float = 10000.0
 
-    # Empty disables inertial propagation: the filter is seeded level at the origin, and
-    # each source's deltas are blended by inverse variance among the sources heard from in
-    # the last 0.5 s.
+    # Empty predicts by constant velocity instead (see process_noise), seeded level at the
+    # origin from the first source message.
     imus: list[ImuConfig] = Field(default_factory=list)
     # m/s^2. Why is gravity a config?
     # 1) different IMUs have different bias
     # 2) gravity changes with latitude by a non-trivial amount for IMUs (0.05 m/s^2)
     initial_gravity_estimate: float = 9.8
     initial_stds: InitialStds = Field(default_factory=InitialStds)
+    process_noise: ProcessNoise = Field(default_factory=ProcessNoise)
 
     # Trust in the tracker's pose, per SourceConfig semantics; the module registers the
     # tracker as a fusion source itself, so it never appears in `odom_sources`. The tracker
@@ -190,10 +206,12 @@ class DimSlamConfig(NativeModuleConfig):
     # One entry per external source (wheel odometry, ...), matched against each message's
     # header.frame_id and child_frame_id.
     odom_sources: list[SourceConfig] = Field(default_factory=list)
-    # A virtual zero-twist measurement applied with every source message, for the
-    # directions the platform cannot move in. Above zero pulls that dimension toward
-    # zero with this variance; zero leaves it free.
-    per_dimension_error_variance: Covariance = Field(default_factory=Covariance)
+    # A ground body: z, roll and pitch held at zero, sources fused in x, y and yaw only.
+    planar: bool = False
+    # Body twist [vx vy vz wx wy wz]: above zero is a soft zero-velocity measurement of that
+    # axis with this variance, fused with every source message (vy for a base that cannot
+    # slide sideways); zero leaves it free.
+    zero_velocity_variances: Covariance = Field(default_factory=Covariance)
 
     @model_validator(mode="after")
     def _multisensor_lists_its_rig(self) -> DimSlamConfig:
diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock
index 564de6d3e57..052f550e24f 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.lock
+++ b/dimos/mapping/dim_slam/rust/Cargo.lock
@@ -567,7 +567,7 @@ dependencies = [
 [[package]]
 name = "dim_slam"
 version = "0.1.0"
-source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.2#34dd548f81d3b0584a399285ed3dd6cef7515c82"
+source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta1#fd61806b2e7ae69b1f874e51e08f40f31def5095"
 dependencies = [
  "cu_vslam_rs",
  "nalgebra",
diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml
index acd2bbe4514..d8b30c7a957 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.toml
+++ b/dimos/mapping/dim_slam/rust/Cargo.toml
@@ -11,7 +11,7 @@ path = "src/main.rs"
 [dependencies]
 # tag, not rev: nix's fetchGit only searches the ref the url names, so a bare rev is
 # unfetchable. Cargo.lock still pins the exact commit.
-dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.4.2" }
+dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.5.0-beta1" }
 # Unused directly, but cargo hands DEP_CUVSLAM_LIB_DIR only to immediate dependents
 # and build.rs needs it for the rpath.
 cu_vslam_rs = "0.2.0"
diff --git a/dimos/mapping/dim_slam/rust/crate-hashes.json b/dimos/mapping/dim_slam/rust/crate-hashes.json
index 76055f50382..38735659a44 100644
--- a/dimos/mapping/dim_slam/rust/crate-hashes.json
+++ b/dimos/mapping/dim_slam/rust/crate-hashes.json
@@ -1,5 +1,5 @@
 {
-  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.2#dim_slam@0.1.0": "sha256-V7L92jW9sacgvz2qsRMJdWsEstalQNHRpbTdUoZtAPw=",
+  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta1#dim_slam@0.1.0": "sha256-S7blHMbnroVcp0FSmryuykDejNbDHvIDhJ1AvMhxyy0=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
diff --git a/dimos/mapping/dim_slam/rust/src/main.rs b/dimos/mapping/dim_slam/rust/src/main.rs
index 23b8318f935..d2e45d03860 100644
--- a/dimos/mapping/dim_slam/rust/src/main.rs
+++ b/dimos/mapping/dim_slam/rust/src/main.rs
@@ -14,8 +14,12 @@
 
 mod msg_convert;
 
+use std::sync::{Arc, Mutex};
+use std::time::{Duration, Instant};
+
 use dim_slam::{
-    CameraConfig, DimSlamCore, DimSlamCoreConfig, ImuConfig, InitialStds, SourceConfig,
+    CameraConfig, DimSlamCore, DimSlamCoreConfig, ImuConfig, InitialStds, ProcessNoise,
+    SourceConfig, SourceKind,
 };
 use dimos_module::{native_config, run_with_transport, Input, Module, Output, Tf};
 use lcm_msgs::nav_msgs::Odometry;
@@ -51,8 +55,10 @@ impl Covariance {
 struct OdomSourceConfig {
     parent_frame_id: String,
     child_frame_id: String,
+    kind: SourceKind,
     pose_variances: Covariance,
     twist_variances: Covariance,
+    realign_after_seconds: f64,
 }
 
 /// One IMU, identified by the frame its samples carry.
@@ -75,6 +81,7 @@ struct InitialStdsConfig {
     position: f64,
     velocity: f64,
     rotation: f64,
+    angular_velocity: f64,
     bias: f64,
 }
 
@@ -104,11 +111,13 @@ struct DimSlamConfig {
     imus: Vec<ImuSourceConfig>,
     initial_gravity_estimate: f64,
     initial_stds: InitialStdsConfig,
+    process_noise: ProcessNoise,
     /// Trust in the in-process visual odometry source.
     visual_odom_pose_variances: Covariance,
     /// External sources, each identified by the transform its estimates carry.
     odom_sources: Vec<OdomSourceConfig>,
-    per_dimension_error_variance: Covariance,
+    planar: bool,
+    zero_velocity_variances: Covariance,
 }
 
 #[derive(Module)]
@@ -139,7 +148,30 @@ struct DimSlam {
     #[config]
     config: DimSlamConfig,
 
-    slam: Option<DimSlamCore>,
+    slam: Option<Arc<Mutex<Slam>>>,
+}
+
+/// The core plus the data clock: the newest input stamp and when it arrived, so the timer
+/// can run on the inputs' clock, live or replayed.
+struct Slam {
+    core: DimSlamCore,
+    newest_stamp: Option<(i64, Instant)>,
+}
+
+impl Slam {
+    fn saw(&mut self, timestamp_ns: i64) {
+        if self
+            .newest_stamp
+            .is_none_or(|(newest, _)| timestamp_ns > newest)
+        {
+            self.newest_stamp = Some((timestamp_ns, Instant::now()));
+        }
+    }
+
+    fn data_now_ns(&self) -> Option<i64> {
+        let (stamp, arrived) = self.newest_stamp?;
+        Some(stamp + arrived.elapsed().as_nanos() as i64)
+    }
 }
 
 impl DimSlam {
@@ -180,8 +212,10 @@ impl DimSlam {
                 position: self.config.initial_stds.position,
                 velocity: self.config.initial_stds.velocity,
                 rotation: self.config.initial_stds.rotation,
+                angular_velocity: self.config.initial_stds.angular_velocity,
                 bias: self.config.initial_stds.bias,
             },
+            process_noise: self.config.process_noise,
             odom_sources: self
                 .config
                 .odom_sources
@@ -189,85 +223,110 @@ impl DimSlam {
                 .map(|source| SourceConfig {
                     parent_frame_id: source.parent_frame_id.clone(),
                     child_frame_id: source.child_frame_id.clone(),
+                    kind: source.kind,
                     pose_variances: source.pose_variances.to_array(),
                     twist_variances: source.twist_variances.to_array(),
+                    realign_after_seconds: source.realign_after_seconds,
                 })
                 .collect(),
-            per_dimension_error_variance: self.config.per_dimension_error_variance.to_array(),
+            planar: self.config.planar,
+            zero_velocity_variances: self.config.zero_velocity_variances.to_array(),
+        });
+        let slam = Arc::new(Mutex::new(Slam {
+            core: slam.unwrap_or_else(|error| panic!("{error}")),
+            newest_stamp: None,
+        }));
+        self.slam = Some(Arc::clone(&slam));
+        let (odometry, tf, publish_tf) = (
+            self.odometry.clone(),
+            self.tf.clone(),
+            self.config.publish_tf,
+        );
+        let mut ticks = tokio::time::interval(Duration::from_secs_f64(
+            1.0 / self.config.publish_rate.max(1.0),
+        ));
+        tokio::spawn(async move {
+            loop {
+                ticks.tick().await;
+                let estimate = {
+                    let mut slam = slam.lock().expect("slam lock");
+                    let Some(now_ns) = slam.data_now_ns() else {
+                        continue;
+                    };
+                    slam.core.maybe_publish_at(now_ns)
+                };
+                let Some(estimate) = estimate else { continue };
+                odometry.publish(&to_odometry_msg(&estimate)).await.ok();
+                if publish_tf {
+                    tf.publish(&[to_transform(&estimate)]).await.ok();
+                }
+            }
         });
-        self.slam = Some(slam.unwrap_or_else(|error| panic!("{error}")));
     }
 
-    async fn handle_camera_info(&mut self, info: CameraInfo) {
-        let Self { slam, tf, .. } = self;
-        slam.as_mut()
+    fn slam(&self) -> std::sync::MutexGuard<'_, Slam> {
+        self.slam
+            .as_ref()
             .expect("setup ran")
-            .handle_camera_info(to_camera_model(info), &tf_lookup(tf));
+            .lock()
+            .expect("slam lock")
+    }
+
+    async fn handle_camera_info(&mut self, info: CameraInfo) {
+        let lookup = tf_lookup(&self.tf);
+        self.slam()
+            .core
+            .handle_camera_info(to_camera_model(info), &lookup);
     }
 
     async fn handle_image(&mut self, img: Image) {
-        let Self { slam, tf, .. } = self;
-        slam.as_mut()
-            .expect("setup ran")
-            .handle_image(to_image_frame(img), &tf_lookup(tf));
-        self.publish().await;
+        let lookup = tf_lookup(&self.tf);
+        let frame = to_image_frame(img);
+        let mut slam = self.slam();
+        slam.saw(frame.timestamp_ns);
+        slam.core.handle_image(frame, &lookup);
     }
 
     async fn handle_depth_image(&mut self, img: Image) {
-        let Self { slam, tf, .. } = self;
-        let cloud = slam
-            .as_mut()
-            .expect("setup ran")
-            .handle_depth_image(to_image_frame(img), &tf_lookup(tf));
+        let lookup = tf_lookup(&self.tf);
+        let frame = to_image_frame(img);
+        let cloud = {
+            let mut slam = self.slam();
+            slam.saw(frame.timestamp_ns);
+            slam.core.handle_depth_image(frame, &lookup)
+        };
         if let Some(cloud) = cloud {
             self.depth_cloud
                 .publish(&to_point_cloud2(&cloud))
                 .await
                 .ok();
         }
-        self.publish().await;
     }
 
     async fn handle_depth_camera_info(&mut self, info: CameraInfo) {
-        self.slam
-            .as_mut()
-            .expect("setup ran")
+        self.slam()
+            .core
             .handle_depth_camera_info(to_camera_model(info));
     }
 
     async fn handle_imu(&mut self, msg: Imu) {
-        let Self { slam, tf, .. } = self;
-        slam.as_mut()
-            .expect("setup ran")
-            .handle_imu(&to_imu_sample(&msg), &tf_lookup(tf));
-        self.publish().await;
+        let lookup = tf_lookup(&self.tf);
+        let sample = to_imu_sample(&msg);
+        let mut slam = self.slam();
+        slam.saw(sample.timestamp_ns);
+        slam.core.handle_imu(&sample, &lookup);
     }
 
     async fn handle_odom_sources(&mut self, msg: Odometry) {
-        self.slam
-            .as_mut()
-            .expect("setup ran")
-            .handle_source(&to_estimate(&msg));
-        self.publish().await;
-    }
-
-    async fn publish(&mut self) {
-        let Some(estimate) = self.slam.as_mut().expect("setup ran").maybe_publish() else {
-            return;
-        };
-        self.odometry
-            .publish(&to_odometry_msg(&estimate))
-            .await
-            .ok();
-        if !self.config.publish_tf {
-            return;
-        }
-        self.tf.publish(&[to_transform(&estimate)]).await.ok();
+        let estimate = to_estimate(&msg);
+        let mut slam = self.slam();
+        slam.saw(estimate.timestamp_ns);
+        slam.core.handle_source(&estimate);
     }
 
     async fn report(&mut self) {
-        if let Some(slam) = &self.slam {
-            slam.report();
+        if self.slam.is_some() {
+            self.slam().core.report();
         }
     }
 }
diff --git a/dimos/robot/diy/alfred/blueprints/vis_nav.py b/dimos/robot/diy/alfred/blueprints/vis_nav.py
index a71c61d2134..ac1e87880ec 100644
--- a/dimos/robot/diy/alfred/blueprints/vis_nav.py
+++ b/dimos/robot/diy/alfred/blueprints/vis_nav.py
@@ -160,7 +160,7 @@ def vis_nav(
             # while driving normally, so no threshold separates good frames from bad.
             covariance_gate_translation_std=0.0,
             # Alfred is holonomic in the plane.
-            per_dimension_error_variance=Covariance(z=0.01, roll=0.01, pitch=0.01),
+            zero_velocity_variances=Covariance(z=0.01, roll=0.01, pitch=0.01),
             # Wheel odometry crosses the wifi link and can land seconds late.
             replay_buffer_seconds=2.0,
             # Every IMU publishes on the one imu topic; the filter sorts them by frame_id and
diff --git a/docs/capabilities/navigation/visual_nav_setup.md b/docs/capabilities/navigation/visual_nav_setup.md
index 09e00d42465..405ec32e5d7 100644
--- a/docs/capabilities/navigation/visual_nav_setup.md
+++ b/docs/capabilities/navigation/visual_nav_setup.md
@@ -140,7 +140,7 @@ DimSlam.blueprint(
     # so disable the covariance gate.
     covariance_gate_translation_std=0.0,
     # Alfred is holonomic in the plane.
-    per_dimension_error_variance=Covariance(
+    zero_velocity_variances=Covariance(
         z=0.01,
         roll=0.01,
         pitch=0.01,
@@ -184,14 +184,20 @@ These values are per-axis variances:
 Start by telling dimSLAM that the robot stays on the ground. An indoor ground robot normally does not move vertically, roll, or pitch.
 
 ```python skip
-per_dimension_error_variance=Covariance(
+planar=True
+```
+
+This holds z, roll, and pitch at zero and fuses every source in x, y, and yaw only. For a robot that drives on ramps, leave `planar` off and add soft zero-velocity constraints instead:
+
+```python skip
+zero_velocity_variances=Covariance(
     z=1e-6,
     roll=1e-6,
     pitch=1e-6,
 )
 ```
 
-This acts as a virtual zero-twist measurement whenever dimSLAM receives a source message. It prevents sensor error from making the robot sink, rise, or tilt. Use weaker constraints when the robot drives on ramps.
+This acts as a virtual zero-velocity measurement whenever dimSLAM receives a source message.
 
 ## Visual odometry
 
@@ -236,7 +242,7 @@ Wheel encoders are good at forward velocity and turn rate. They are weaker as a
 
 A negative source variance uses the message covariance. A fixed positive variance is usually better for a drifting source because its reported covariance describes accumulated drift, not the measured delta.
 
-A source whose parent is `odom_frame_id` contributes absolute measurements. A source with another parent contributes dimSLAM-anchored deltas because its own pose has drifted.
+A source's `kind` says what its pose means. `pose` is a pose that does not drift, such as lidar-inertial odometry: its first message aligns its frame to dimSLAM's, and later messages are absolute measurements. `pose_delta`, the default, is a pose that drifts, such as wheel or visual odometry: only the motion between consecutive messages is fused. `twist` uses only the source's velocity.
 
 ## IMUs
 
@@ -274,7 +280,7 @@ Before driving, verify:
 | Tracking works but the robot moves in the wrong direction | The extrinsics are wrong. A few millimetres of error causes small constant drift. A wrong optical-frame rotation produces complete nonsense. Check tf and the URDF first. |
 | Position drifts badly from startup when using an IMU | The robot moved during gyro-bias initialization. Leave it still for about one second. Initialization restarts automatically after it becomes still. |
 | Bias initialization never completes | `init_gyro_limit` is below the gyro's noise floor at rest. Raise it above the gyro bias while keeping it below any real rotation. |
-| The map has two floors, or the robot appears rolled | z, roll, or pitch are random-walking. Configure `per_dimension_error_variance`. If wheel poses report z, roll, and pitch as zero, keep those dimensions as absolute anchors. |
+| The map has two floors, or the robot appears rolled | z, roll, or pitch are random-walking. Set `planar=True`, or configure `zero_velocity_variances`. If wheel poses report z, roll, and pitch as zero, keep those dimensions as absolute anchors. |
 | The path jumps sharply or resets | The covariance gate is firing. A CPU-only visual estimate's translation standard deviation can start above 1.0 and grow past 9 during normal driving, leaving no useful threshold. Set `covariance_gate_translation_std=0.0` for that case. |
 | Gravity is misaligned and the estimate diverges | The IMU mount is uncalibrated. One bad mount produced about 2.4 m/s² of gravity misalignment. Correct the tf transform. |
 | Wheel odometry appears to be ignored | Its `parent_frame_id` or `child_frame_id` does not exactly match `header.frame_id` or `child_frame_id` in the messages. Unmatched sources are dropped. |

From 5e35cb0171b7ecc5d1b53a2b03086a25bd577d96 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 19:58:56 -0700
Subject: [PATCH 05/21] r1pro: Point-LIO as dimSLAM's pose source and the
 wheels as its twist, with the Point-LIO core pinning gone

---
 dimos/robot/all_blueprints.py                 |  1 -
 .../blueprints/basic/r1pro_coordinator.py     | 24 +++++++-----
 .../r1pro/blueprints/navigation/r1pro_nav.py  |  4 +-
 dimos/robot/galaxea/r1pro/lio.py              | 38 -------------------
 dimos/robot/galaxea/r1pro/test_lio.py         | 11 +-----
 5 files changed, 17 insertions(+), 61 deletions(-)

diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py
index 67c90891a6f..58e6c08d25b 100644
--- a/dimos/robot/all_blueprints.py
+++ b/dimos/robot/all_blueprints.py
@@ -327,7 +327,6 @@
     "pose-from-odometry": "dimos.robot.galaxea.r1pro.lio.PoseFromOdometry",
     "r1-pro-connection": "dimos.robot.galaxea.r1pro.connection.R1ProConnection",
     "r1-pro-lio": "dimos.robot.galaxea.r1pro.lio.R1ProLio",
-    "r1-pro-point-lio": "dimos.robot.galaxea.r1pro.lio.R1ProPointLio",
     "raw-robot-bridge": "dimos.robot.raw_robot_bridge.RawRobotBridge",
     "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap",
     "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera",
diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index 1e452293c30..946a15a5c8e 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -34,8 +34,9 @@
 from dimos.core.coordination.blueprints import Blueprint, TransportSpec, autoconnect
 from dimos.core.global_config import global_config
 from dimos.core.transport import ZenohTransport
+from dimos.hardware.sensors.lidar.pointlio.module import PointLio
 from dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints import mid360_for_pointlio
-from dimos.mapping.dim_slam.dim_slam import Covariance, DimSlam, SourceConfig
+from dimos.mapping.dim_slam.dim_slam import Covariance, DimSlam, ProcessNoise, SourceConfig
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.nav_msgs.Odometry import Odometry
@@ -57,7 +58,7 @@
     HeadLeftCamera,
     HeadRightCamera,
 )
-from dimos.robot.galaxea.r1pro.lio import PoseFromOdometry, R1ProLio, R1ProLioConfig, R1ProPointLio
+from dimos.robot.galaxea.r1pro.lio import PoseFromOdometry, R1ProLio, R1ProLioConfig
 from dimos.visualization.rerun.bridge import RerunBridgeModule
 from dimos.visualization.rerun.websocket_server import RerunWebSocketServer
 
@@ -298,11 +299,11 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
             lidar_ip=R1PRO_CHASSIS_LIDAR_IP,
             host_ip=R1PRO_CHASSIS_LIDAR_HOST_IP,
         ),
-        R1ProPointLio.blueprint(
+        PointLio.blueprint(
             sensor_frame_id=lidar_frame,
             # Standing still at 0.01 the pose stepped ~7 mm a scan (the robot jittered in the viewer); 1.0 took it to 2.5 mm.
             lidar_meas_cov=1.0,
-        ).remappings([(R1ProPointLio, "odometry", "pointlio_odometry")]),
+        ).remappings([(PointLio, "odometry", "pointlio_odometry")]),
         R1ProLio.blueprint(mount_under_base=mount_under_base),
     ).remappings(
         [
@@ -313,18 +314,19 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
     )
 
 
-# Without an IMU dimSLAM blends the sources' deltas by inverse variance: Point-LIO gets ~99% of x, y and yaw, and z,
-# roll and pitch outright. The wheels mostly carry the pose while Point-LIO is silent (dimSLAM's 0.5 s timeout). Replaying
-# a 30 m R1 drive, the fused pose stayed within 2.5 cm (mean) of Point-LIO's.
+# Replaying a 30 m R1 drive: 4.8 mm / 0.25 deg jitter against Point-LIO's own 13.8 mm / 0.33 deg.
 POINTLIO_SOURCE = SourceConfig(
     parent_frame_id="odom",
     child_frame_id="base_link",
-    pose_variances=Covariance(x=1e-5, y=1e-5, z=1e-5, roll=1e-5, pitch=1e-5, yaw=1e-5),
+    kind="pose",
+    pose_variances=Covariance(x=2e-4, y=2e-4, yaw=3.3e-5),
+    realign_after_seconds=1.0,
 )
 WHEEL_SOURCE = SourceConfig(
     parent_frame_id=R1PRO_WHEEL_ODOM_FRAME,
     child_frame_id="base_link",
-    pose_variances=Covariance(x=1e-3, y=1e-3, yaw=1e-3),
+    kind="twist",
+    twist_variances=Covariance(x=5e-4, y=5e-4, yaw=6e-4),
 )
 
 
@@ -341,12 +343,14 @@ def r1pro_fused_odometry() -> Blueprint:
             # Required, but nothing feeds the camera inputs, so the tracker never starts.
             camera_mode="stereo",
             odom_sources=[POINTLIO_SOURCE, WHEEL_SOURCE],
+            planar=True,
+            process_noise=ProcessNoise(linear_acceleration=2.0, angular_acceleration=1.0),
         ),
         PoseFromOdometry.blueprint(),
     ).remappings(
         [
             # Point-LIO's own odom -> lidar edge would give base_link's subtree a second root.
-            (R1ProPointLio, "tf", "pointlio_tf"),
+            (PointLio, "tf", "pointlio_tf"),
             (R1ProLio, "base_odometry", "slam_sources"),
             (DimSlam, "odom_sources", "slam_sources"),
             (DimSlam, "odometry", "slam_odometry"),
diff --git a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
index 2be8d13f8eb..29f1b880784 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
@@ -18,6 +18,7 @@
 
 from dimos.core.coordination.blueprints import autoconnect
 from dimos.core.global_config import global_config
+from dimos.hardware.sensors.lidar.pointlio.module import PointLio
 from dimos.manipulation.manipulation_module import ManipulationModule
 from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig
 from dimos.mapping.ray_tracing.module import RayTracingVoxelMap
@@ -41,7 +42,6 @@
 )
 from dimos.robot.galaxea.r1pro.connection import R1ProConnection
 from dimos.robot.galaxea.r1pro.head_depth import r1pro_head_depth
-from dimos.robot.galaxea.r1pro.lio import R1ProPointLio
 from dimos.robot.galaxea.r1pro.rerun import r1pro_nav_rerun_config
 from dimos.robot.urdf_manager import UrdfManager
 from dimos.visualization.vis_module import vis_module
@@ -68,7 +68,7 @@
         enable_wrist_color=False,
         enable_imu=False,
     ).remappings([(R1ProConnection, "odometry", "slam_sources")]),
-    r1pro_fused_odometry().remappings([(R1ProPointLio, "lidar", "lidar_with_robot")]),
+    r1pro_fused_odometry().remappings([(PointLio, "lidar", "lidar_with_robot")]),
     # Every joint on tf (the head cameras ride the torso), and the robot's own body cut out of the Mid-360's scans.
     UrdfManager.blueprint(
         **R1PRO_URDF_MANAGER_CONFIG,
diff --git a/dimos/robot/galaxea/r1pro/lio.py b/dimos/robot/galaxea/r1pro/lio.py
index f182883b141..6e7296c408f 100644
--- a/dimos/robot/galaxea/r1pro/lio.py
+++ b/dimos/robot/galaxea/r1pro/lio.py
@@ -16,17 +16,13 @@
 
 from __future__ import annotations
 
-import contextlib
 from functools import cache
-import os
 
-import psutil
 from reactivex.disposable import Disposable
 
 from dimos.core.core import rpc
 from dimos.core.module import Module
 from dimos.core.stream import In, Out
-from dimos.hardware.sensors.lidar.pointlio.module import PointLio
 from dimos.msgs.geometry_msgs.Pose import Pose
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Quaternion import Quaternion
@@ -153,37 +149,3 @@ def _on_odometry(self, msg: Odometry) -> None:
                 orientation=msg.pose.orientation,
             )
         )
-
-
-# Sharing the Orin's cores at load ~23, Point-LIO waited for a core ~60% of the time, ran 0.4-2 s behind and dropped scans: the robot stuttered in the viewer.
-POINTLIO_RESERVED_CORES = 2
-
-
-def set_affinity(pid: int, cores: set[int]) -> None:
-    """Every thread of ``pid``: an affinity is per thread, and threads made later inherit their creator's."""
-    with contextlib.suppress(FileNotFoundError):
-        for tid in os.listdir(f"/proc/{pid}/task"):
-            with contextlib.suppress(OSError):
-                os.sched_setaffinity(int(tid), cores)
-
-
-def run_pids() -> list[int]:
-    """This dimos run: the coordinator (this worker's forkserver's parent) and everything under it."""
-    coordinator = psutil.Process(psutil.Process().parent().ppid())
-    return [coordinator.pid, *(p.pid for p in coordinator.children(recursive=True))]
-
-
-class R1ProPointLio(PointLio):
-    """Point-LIO on the last cores, with the rest of the run moved off them."""
-
-    @rpc
-    def start(self) -> None:
-        super().start()
-        if not hasattr(os, "sched_setaffinity") or self._process is None:
-            return
-        cpus = set(range(os.cpu_count() or 1))
-        reserved = set(sorted(cpus)[-POINTLIO_RESERVED_CORES:])
-        if reserved == cpus:
-            return
-        for pid in run_pids():
-            set_affinity(pid, reserved if pid == self._process.pid else cpus - reserved)
diff --git a/dimos/robot/galaxea/r1pro/test_lio.py b/dimos/robot/galaxea/r1pro/test_lio.py
index 3ba20f46ee6..7d0853ed207 100644
--- a/dimos/robot/galaxea/r1pro/test_lio.py
+++ b/dimos/robot/galaxea/r1pro/test_lio.py
@@ -12,7 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
 
 import numpy as np
 import pytest
@@ -32,7 +31,6 @@
     base_odometry,
     base_pose,
     mount_transforms,
-    set_affinity,
 )
 from dimos.robot.galaxea.r1pro.rerun import r1pro_nav_rerun_config, render_cloud
 
@@ -81,13 +79,6 @@ def test_r1prolio_publishes_base_link_pose() -> None:
         lio.stop()
 
 
-@pytest.mark.skipif(not hasattr(os, "sched_setaffinity"), reason="Linux only")
-def test_set_affinity_pins_every_thread() -> None:
-    cores = os.sched_getaffinity(0)
-    set_affinity(os.getpid(), cores)
-    assert os.sched_getaffinity(0) == cores
-
-
 @pytest.mark.parametrize("frame_id", ["lidar_pointlio_link", HEAD_LEFT_FRAME, "other"])
 def test_render_cloud_hangs_each_cloud_off_its_frame(frame_id: str) -> None:
     cloud = PointCloud2.from_numpy(
@@ -138,4 +129,4 @@ def test_dimslam_takes_pointlio_and_wheels_and_alone_drives_the_planners() -> No
     assert topic_users("in", "slam_sources") == {DimSlam.__name__}
     assert topic_users("out", "chassis_odom") <= {"PoseFromOdometry", "R1ProConnection"}
     # Off tf, so dimSLAM's odom -> base_link is base_link's only parent.
-    assert "R1ProPointLio" not in topic_users("out", "tf")
+    assert "PointLio" not in topic_users("out", "tf")

From fd1169577acc5862adcb360e312ff40b28d1a142 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 20:05:20 -0700
Subject: [PATCH 06/21] dim_odom 0.5.0b1: the fusion replay test on the new
 config, with a diff drive's sideways slide pinned

---
 dimos/mapping/dim_slam/test_dim_odom_replay.py |  7 ++++---
 pyproject.toml                                 |  2 +-
 uv.lock                                        | 12 ++++++------
 3 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/dimos/mapping/dim_slam/test_dim_odom_replay.py b/dimos/mapping/dim_slam/test_dim_odom_replay.py
index 0b6eb8e073e..f8998894d5e 100644
--- a/dimos/mapping/dim_slam/test_dim_odom_replay.py
+++ b/dimos/mapping/dim_slam/test_dim_odom_replay.py
@@ -60,11 +60,12 @@
         {
             "parent_frame_id": "wheel_odom",
             "child_frame_id": "base_link",
+            "kind": "twist",
             "twist_variances": [0.01, 0.0, 0.0, 0.0, 0.0, 0.02],
         }
     ],
-    # Ground robot: pin vz, roll and pitch rates.
-    "per_dimension_error_variance": [0.0, 0.0, 1e-6, 1e-6, 1e-6, 0.0],
+    # Diff drive on the ground: no sideways slide, no vz, roll or pitch rate.
+    "zero_velocity_variances": [0.0, 1e-4, 1e-6, 1e-6, 1e-6, 0.0],
 }
 
 
@@ -215,5 +216,5 @@ def test_dim_odom_fusion_replay():
 
     assert len(first) == 1203
     displacement = math.dist(first[0][1], first[-1][1])
-    assert displacement == pytest.approx(2.437007550968355)
+    assert displacement == pytest.approx(2.1581220240116283)
     assert first == second, "replaying the same data twice diverged"
diff --git a/pyproject.toml b/pyproject.toml
index b6fa4e656d1..c0b11841aea 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -503,7 +503,7 @@ tests-self-hosted = [
     "mcap>=1.2.0",
     # Needed to compile the in-tree extensions.
     "pybind11>=2.12",
-    "dim-odom>=0.4.2",
+    "dim-odom>=0.5.0b1",
 ]
 docs = [
     "mkdocs-awesome-nav>=3.3.0",
diff --git a/uv.lock b/uv.lock
index fd89b4d08f2..5023307f1e7 100644
--- a/uv.lock
+++ b/uv.lock
@@ -1788,7 +1788,7 @@ wheels = [
 
 [[package]]
 name = "dim-odom"
-version = "0.4.2"
+version = "0.5.0b1"
 source = { registry = "https://pypi.org/simple" }
 dependencies = [
     { name = "nvidia-cublas", marker = "sys_platform == 'linux'" },
@@ -1797,10 +1797,10 @@ dependencies = [
     { name = "nvidia-cusparse", marker = "sys_platform == 'linux'" },
 ]
 wheels = [
-    { url = "https://files.pythonhosted.org/packages/ed/7f/a5d35789da6a78d9cd2283387fce4029069f61e92aa4cb29fd670a1d78ce/dim_odom-0.4.2-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:6d506b8dca4b1592bdd3da69a28b3c69ba84785b25ec391b121f97623baf720b", size = 6975797, upload-time = "2026-10-16T15:46:22.966Z" },
-    { url = "https://files.pythonhosted.org/packages/5b/30/5e4738bd2165187908ac27b2592f19b9eed2c2a436a9d87e3e93127fcea1/dim_odom-0.4.2-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:6cbb8e6d74315d99d8601fddf2e40416780d3731549d2a0a68c255a830e23a16", size = 86698178, upload-time = "2026-10-16T15:46:34.213Z" },
-    { url = "https://files.pythonhosted.org/packages/e6/0f/15773c43292b03690ba3ed8481f133130902850c1bcb8ca8b2b02a93d087/dim_odom-0.4.2-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:d9a5b76cb4932d27deb2b9733f5ec64ef9a10a58ad0b348db245286bf03514f8", size = 85495518, upload-time = "2026-10-16T15:46:48.071Z" },
-    { url = "https://files.pythonhosted.org/packages/e0/d0/c7d15b665e5bf9ecce0936283defbc08209b8bba47e25b21fb70de6e63c2/dim_odom-0.4.2-py3-none-any.whl", hash = "sha256:3a61913b44334e3de5420c637707297c5fde7cc03d78eac22571f98979b35ade", size = 1412, upload-time = "2026-10-16T15:46:51.201Z" },
+    { url = "https://files.pythonhosted.org/packages/a1/c7/487e853f6c223404db88aac71302d69507ef475c98e8c28fb205436a52ee/dim_odom-0.5.0b1-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:c20166ca474061d8fe57d317372e6e77cce9aee87ad7d547e95a458fe8689c1e", size = 6989748, upload-time = "2026-10-18T03:02:24.037Z" },
+    { url = "https://files.pythonhosted.org/packages/2d/72/a45a0531de82413c6c470636a265153e68a27ab81be2be9f886253bdd448/dim_odom-0.5.0b1-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:b3de74aa7dbac027be22ba68db0b2f23f06de36ecc36458ae25eea50522e16ac", size = 86714180, upload-time = "2026-10-18T03:02:43.803Z" },
+    { url = "https://files.pythonhosted.org/packages/c8/54/48577cd64de41a8abdf817751ffac9ec52ea3713cc53b3524f6e5aaf54a6/dim_odom-0.5.0b1-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:847f92ec8f468c49d61fb4d9df25134f41acc25dc19f7a1a0bc0b61e8a0b2448", size = 85512037, upload-time = "2026-10-18T03:02:32.866Z" },
+    { url = "https://files.pythonhosted.org/packages/14/e4/fe594cd1190a7b207dd6c35ed3530b3acc409432d297241eab3da858e3ed/dim_odom-0.5.0b1-py3-none-any.whl", hash = "sha256:f11aef5eca7e90a1a9688f4ebf38207804d4586cf80f2fd28b519be5724250d3", size = 1431, upload-time = "2026-10-18T03:02:35.822Z" },
 ]
 
 [[package]]
@@ -2716,7 +2716,7 @@ tests-self-hosted = [
     { name = "chromadb", specifier = ">=1.0.0" },
     { name = "coacd", specifier = ">=1.0.0" },
     { name = "coverage", specifier = ">=7.0" },
-    { name = "dim-odom", specifier = ">=0.4.2" },
+    { name = "dim-odom", specifier = ">=0.5.0b1" },
     { name = "dimos", extras = ["apriltag", "mapping", "drone", "cpu", "learning"] },
     { name = "dimos", extras = ["manipulation", "misc", "unitree"] },
     { name = "dimos", extras = ["web", "visualization", "webrtc"] },

From 7f2e8de656784b2c00c58f607981262834b5e2a0 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 20:09:35 -0700
Subject: [PATCH 07/21] dim_slam, r1pro: one-line comments

---
 dimos/mapping/dim_slam/dim_slam.py               | 16 +++++-----------
 dimos/mapping/dim_slam/rust/src/main.rs          |  3 +--
 .../r1pro/blueprints/basic/r1pro_coordinator.py  |  7 +------
 3 files changed, 7 insertions(+), 19 deletions(-)

diff --git a/dimos/mapping/dim_slam/dim_slam.py b/dimos/mapping/dim_slam/dim_slam.py
index fac9a0e8948..c3039910cee 100644
--- a/dimos/mapping/dim_slam/dim_slam.py
+++ b/dimos/mapping/dim_slam/dim_slam.py
@@ -119,18 +119,15 @@ class SourceConfig(BaseModel):
     drifting source's covariance describes its accumulated drift rather than the delta
     being fused, so a fixed value is usually the right answer."""
 
-    # The transform this source's estimates carry: each message's header.frame_id and
-    # child_frame_id. Both halves are needed because two sources can share a parent.
+    # The transform each message carries (header.frame_id -> child_frame_id); two sources can share a parent.
     parent_frame_id: str = ""
     child_frame_id: str = ""
-    # pose: a pose that does not drift (lidar-inertial odometry). twist: only the twist is used
-    # (wheels). pose_delta: a pose that drifts, fused by its message-to-message motion.
+    # pose: a pose that does not drift (lidar odometry); twist: twist only (wheels); pose_delta: a drifting pose.
     kind: Literal["pose", "twist", "pose_delta"] = "pose_delta"
     pose_variances: Covariance = Field(default_factory=Covariance)
     # Body frame.
     twist_variances: Covariance = Field(default_factory=Covariance)
-    # pose only: a source rejected this long without a break is re-aligned to the fused pose;
-    # 0 never re-aligns.
+    # pose only: re-align a source to the fused pose after it is rejected this long; 0 never re-aligns.
     realign_after_seconds: float = 0.0
 
 
@@ -187,8 +184,7 @@ class DimSlamConfig(NativeModuleConfig):
     # Caps the filter's own state rather than an incoming reading. 0 disables it.
     max_position_m: float = 10000.0
 
-    # Empty predicts by constant velocity instead (see process_noise), seeded level at the
-    # origin from the first source message.
+    # Empty predicts by constant velocity instead (see process_noise).
     imus: list[ImuConfig] = Field(default_factory=list)
     # m/s^2. Why is gravity a config?
     # 1) different IMUs have different bias
@@ -208,9 +204,7 @@ class DimSlamConfig(NativeModuleConfig):
     odom_sources: list[SourceConfig] = Field(default_factory=list)
     # A ground body: z, roll and pitch held at zero, sources fused in x, y and yaw only.
     planar: bool = False
-    # Body twist [vx vy vz wx wy wz]: above zero is a soft zero-velocity measurement of that
-    # axis with this variance, fused with every source message (vy for a base that cannot
-    # slide sideways); zero leaves it free.
+    # Per body-twist axis, the variance of a soft zero-velocity measurement fused with every source message; 0 is free.
     zero_velocity_variances: Covariance = Field(default_factory=Covariance)
 
     @model_validator(mode="after")
diff --git a/dimos/mapping/dim_slam/rust/src/main.rs b/dimos/mapping/dim_slam/rust/src/main.rs
index d2e45d03860..fc51aee80cc 100644
--- a/dimos/mapping/dim_slam/rust/src/main.rs
+++ b/dimos/mapping/dim_slam/rust/src/main.rs
@@ -151,8 +151,7 @@ struct DimSlam {
     slam: Option<Arc<Mutex<Slam>>>,
 }
 
-/// The core plus the data clock: the newest input stamp and when it arrived, so the timer
-/// can run on the inputs' clock, live or replayed.
+/// The core plus the newest input stamp and when it arrived: the timer runs on the inputs' clock, live or replayed.
 struct Slam {
     core: DimSlamCore,
     newest_stamp: Option<(i64, Instant)>,
diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index 946a15a5c8e..01bae024d9b 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -331,12 +331,7 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
 
 
 def r1pro_fused_odometry() -> Blueprint:
-    """Point-LIO (carried to base_link) and wheel odometry into dimSLAM, which owns odom -> base_link.
-
-    Needs ``r1pro_control(odom_frame_id=R1PRO_WHEEL_ODOM_FRAME)`` with its ``odometry`` remapped to ``slam_sources``.
-    No cameras or IMUs: cuVSLAM on the head stereo would compete with Point-LIO for the Orin's cores. To add them, set
-    ``cameras`` and remap ``dimslam_image``/``dimslam_camera_info`` onto the head stereo streams.
-    """
+    """Point-LIO and wheel odometry (``r1pro_control`` odometry remapped to ``slam_sources``) into dimSLAM, which owns odom -> base_link."""
     return autoconnect(
         r1pro_lidar_odometry(mount_under_base=True),
         DimSlam.blueprint(

From 314393e83bd7b9d63b6d2a3ecfd2237c00764057 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 20:13:59 -0700
Subject: [PATCH 08/21] dim_odom test: fuse the wheels' measured vy rather than
 assume none (Alfred is holonomic)

---
 dimos/mapping/dim_slam/test_dim_odom_replay.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/dimos/mapping/dim_slam/test_dim_odom_replay.py b/dimos/mapping/dim_slam/test_dim_odom_replay.py
index f8998894d5e..cbacba3ff08 100644
--- a/dimos/mapping/dim_slam/test_dim_odom_replay.py
+++ b/dimos/mapping/dim_slam/test_dim_odom_replay.py
@@ -55,17 +55,17 @@
             "accel_random_walk": 1e-4,
         }
     ],
-    # Wheel twist only: vx and wz are what a diff drive measures.
+    # Wheel twist only: Alfred is holonomic, so vx, vy and wz; vy unfused leaves the IMU's velocity free to drift sideways.
     "odom_sources": [
         {
             "parent_frame_id": "wheel_odom",
             "child_frame_id": "base_link",
             "kind": "twist",
-            "twist_variances": [0.01, 0.0, 0.0, 0.0, 0.0, 0.02],
+            "twist_variances": [0.01, 0.01, 0.0, 0.0, 0.0, 0.02],
         }
     ],
-    # Diff drive on the ground: no sideways slide, no vz, roll or pitch rate.
-    "zero_velocity_variances": [0.0, 1e-4, 1e-6, 1e-6, 1e-6, 0.0],
+    # Ground robot: pin vz, roll and pitch rates.
+    "zero_velocity_variances": [0.0, 0.0, 1e-6, 1e-6, 1e-6, 0.0],
 }
 
 
@@ -216,5 +216,5 @@ def test_dim_odom_fusion_replay():
 
     assert len(first) == 1203
     displacement = math.dist(first[0][1], first[-1][1])
-    assert displacement == pytest.approx(2.1581220240116283)
+    assert displacement == pytest.approx(2.111061075971029)
     assert first == second, "replaying the same data twice diverged"

From 9389da63f17ac69c01a37de09643b753a0178937 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 20:26:12 -0700
Subject: [PATCH 09/21] dim_slam: dimSLAM v0.5.0-beta2 (an IMU no longer has
 its gyro bias dragged by angular zero-velocity entries), dim-odom 0.5.0b2

---
 dimos/mapping/dim_slam/rust/Cargo.lock         |  2 +-
 dimos/mapping/dim_slam/rust/Cargo.toml         |  2 +-
 dimos/mapping/dim_slam/rust/crate-hashes.json  |  2 +-
 dimos/mapping/dim_slam/test_dim_odom_replay.py |  4 ++--
 pyproject.toml                                 |  2 +-
 uv.lock                                        | 12 ++++++------
 6 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock
index 052f550e24f..f20a7443e17 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.lock
+++ b/dimos/mapping/dim_slam/rust/Cargo.lock
@@ -567,7 +567,7 @@ dependencies = [
 [[package]]
 name = "dim_slam"
 version = "0.1.0"
-source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta1#fd61806b2e7ae69b1f874e51e08f40f31def5095"
+source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta2#103206e75a8cfa5ba28dfc3328921bd22c47a528"
 dependencies = [
  "cu_vslam_rs",
  "nalgebra",
diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml
index d8b30c7a957..d356d9ebea7 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.toml
+++ b/dimos/mapping/dim_slam/rust/Cargo.toml
@@ -11,7 +11,7 @@ path = "src/main.rs"
 [dependencies]
 # tag, not rev: nix's fetchGit only searches the ref the url names, so a bare rev is
 # unfetchable. Cargo.lock still pins the exact commit.
-dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.5.0-beta1" }
+dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.5.0-beta2" }
 # Unused directly, but cargo hands DEP_CUVSLAM_LIB_DIR only to immediate dependents
 # and build.rs needs it for the rpath.
 cu_vslam_rs = "0.2.0"
diff --git a/dimos/mapping/dim_slam/rust/crate-hashes.json b/dimos/mapping/dim_slam/rust/crate-hashes.json
index 38735659a44..08d1cffeecf 100644
--- a/dimos/mapping/dim_slam/rust/crate-hashes.json
+++ b/dimos/mapping/dim_slam/rust/crate-hashes.json
@@ -1,5 +1,5 @@
 {
-  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta1#dim_slam@0.1.0": "sha256-S7blHMbnroVcp0FSmryuykDejNbDHvIDhJ1AvMhxyy0=",
+  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta2#dim_slam@0.1.0": "sha256-YixD62KdVtkOjP4i0WesXmpyGPMCF5JiyPGz1fCmPqg=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
diff --git a/dimos/mapping/dim_slam/test_dim_odom_replay.py b/dimos/mapping/dim_slam/test_dim_odom_replay.py
index cbacba3ff08..24a7fba2379 100644
--- a/dimos/mapping/dim_slam/test_dim_odom_replay.py
+++ b/dimos/mapping/dim_slam/test_dim_odom_replay.py
@@ -55,7 +55,7 @@
             "accel_random_walk": 1e-4,
         }
     ],
-    # Wheel twist only: Alfred is holonomic, so vx, vy and wz; vy unfused leaves the IMU's velocity free to drift sideways.
+    # Wheel twist only: Alfred is holonomic, so vx, vy and wz.
     "odom_sources": [
         {
             "parent_frame_id": "wheel_odom",
@@ -216,5 +216,5 @@ def test_dim_odom_fusion_replay():
 
     assert len(first) == 1203
     displacement = math.dist(first[0][1], first[-1][1])
-    assert displacement == pytest.approx(2.111061075971029)
+    assert displacement == pytest.approx(2.121304539069407)
     assert first == second, "replaying the same data twice diverged"
diff --git a/pyproject.toml b/pyproject.toml
index c0b11841aea..ed00c24a324 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -503,7 +503,7 @@ tests-self-hosted = [
     "mcap>=1.2.0",
     # Needed to compile the in-tree extensions.
     "pybind11>=2.12",
-    "dim-odom>=0.5.0b1",
+    "dim-odom>=0.5.0b2",
 ]
 docs = [
     "mkdocs-awesome-nav>=3.3.0",
diff --git a/uv.lock b/uv.lock
index 5023307f1e7..7309c402dd1 100644
--- a/uv.lock
+++ b/uv.lock
@@ -1788,7 +1788,7 @@ wheels = [
 
 [[package]]
 name = "dim-odom"
-version = "0.5.0b1"
+version = "0.5.0b2"
 source = { registry = "https://pypi.org/simple" }
 dependencies = [
     { name = "nvidia-cublas", marker = "sys_platform == 'linux'" },
@@ -1797,10 +1797,10 @@ dependencies = [
     { name = "nvidia-cusparse", marker = "sys_platform == 'linux'" },
 ]
 wheels = [
-    { url = "https://files.pythonhosted.org/packages/a1/c7/487e853f6c223404db88aac71302d69507ef475c98e8c28fb205436a52ee/dim_odom-0.5.0b1-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:c20166ca474061d8fe57d317372e6e77cce9aee87ad7d547e95a458fe8689c1e", size = 6989748, upload-time = "2026-10-18T03:02:24.037Z" },
-    { url = "https://files.pythonhosted.org/packages/2d/72/a45a0531de82413c6c470636a265153e68a27ab81be2be9f886253bdd448/dim_odom-0.5.0b1-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:b3de74aa7dbac027be22ba68db0b2f23f06de36ecc36458ae25eea50522e16ac", size = 86714180, upload-time = "2026-10-18T03:02:43.803Z" },
-    { url = "https://files.pythonhosted.org/packages/c8/54/48577cd64de41a8abdf817751ffac9ec52ea3713cc53b3524f6e5aaf54a6/dim_odom-0.5.0b1-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:847f92ec8f468c49d61fb4d9df25134f41acc25dc19f7a1a0bc0b61e8a0b2448", size = 85512037, upload-time = "2026-10-18T03:02:32.866Z" },
-    { url = "https://files.pythonhosted.org/packages/14/e4/fe594cd1190a7b207dd6c35ed3530b3acc409432d297241eab3da858e3ed/dim_odom-0.5.0b1-py3-none-any.whl", hash = "sha256:f11aef5eca7e90a1a9688f4ebf38207804d4586cf80f2fd28b519be5724250d3", size = 1431, upload-time = "2026-10-18T03:02:35.822Z" },
+    { url = "https://files.pythonhosted.org/packages/5d/ad/e9894f8b73314473795d6210a59bac9d285a4a94c49bc82aee75115ce7e7/dim_odom-0.5.0b2-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:2b2a9bc331ae8f0228ec0eac769583a093d17e99baca933814e7416f5e1a6e65", size = 6990154, upload-time = "2026-10-18T03:25:06.383Z" },
+    { url = "https://files.pythonhosted.org/packages/15/f4/13ae41f0c5bc4a860214e574c6ae83a595d4ab4c0a73f8e5dce9a05fc35f/dim_odom-0.5.0b2-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:95bbbd65fbdaf1c8771e76af12c91458798a1123a605de7cc295644ccccc2a54", size = 86713864, upload-time = "2026-10-18T03:25:33.288Z" },
+    { url = "https://files.pythonhosted.org/packages/cf/24/2872d116fd8f83df79accd8277efffd55d62edf383ec6bdcf23698bcdc84/dim_odom-0.5.0b2-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:28afc351505992187d6af45e1059cd1d33d301878c64e661323c82666c9f4dc0", size = 85511911, upload-time = "2026-10-18T03:25:19.517Z" },
+    { url = "https://files.pythonhosted.org/packages/37/88/c8b8d96ad1d8b2b17f4a4ea122c30656b3a9240e304808d243337d4eae99/dim_odom-0.5.0b2-py3-none-any.whl", hash = "sha256:882c3716ef38913747f1dfa9f53f1e188855785bb0a99835291fe7638adb760f", size = 1430, upload-time = "2026-10-18T03:25:22.215Z" },
 ]
 
 [[package]]
@@ -2716,7 +2716,7 @@ tests-self-hosted = [
     { name = "chromadb", specifier = ">=1.0.0" },
     { name = "coacd", specifier = ">=1.0.0" },
     { name = "coverage", specifier = ">=7.0" },
-    { name = "dim-odom", specifier = ">=0.5.0b1" },
+    { name = "dim-odom", specifier = ">=0.5.0b2" },
     { name = "dimos", extras = ["apriltag", "mapping", "drone", "cpu", "learning"] },
     { name = "dimos", extras = ["manipulation", "misc", "unitree"] },
     { name = "dimos", extras = ["web", "visualization", "webrtc"] },

From f8642677a43e50e525e3e47445c140c1ec25cc57 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 21:23:41 -0700
Subject: [PATCH 10/21] r1pro: dimSLAM loads the Orin SDK's own CUDA libraries,
 since JetPack 6.0's are older

---
 dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index 01bae024d9b..4c28aacfc96 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -340,6 +340,8 @@ def r1pro_fused_odometry() -> Blueprint:
             odom_sources=[POINTLIO_SOURCE, WHEEL_SOURCE],
             planar=True,
             process_noise=ProcessNoise(linear_acceleration=2.0, angular_acceleration=1.0),
+            # No tracker runs, and JetPack 6.0's CUDA 12.2 lacks symbols the Orin SDK (12.6) needs: load the SDK's own.
+            extra_env={"LD_LIBRARY_PATH": ""},
         ),
         PoseFromOdometry.blueprint(),
     ).remappings(

From 353fa971afe828580d83861e64a330565322f0a6 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 21:42:07 -0700
Subject: [PATCH 11/21] r1pro: Point-LIO keeps every 6th raw point, halving the
 cloud downstream (Point-LIO 24 -> 16% CPU, same jitter)

---
 dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index 4c28aacfc96..8097d633401 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -303,6 +303,8 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
             sensor_frame_id=lidar_frame,
             # Standing still at 0.01 the pose stepped ~7 mm a scan (the robot jittered in the viewer); 1.0 took it to 2.5 mm.
             lidar_meas_cov=1.0,
+            # Every 6th raw point (default 3): half the cloud for the self-filter, ray tracer and depth anchors.
+            point_filter_num=6,
         ).remappings([(PointLio, "odometry", "pointlio_odometry")]),
         R1ProLio.blueprint(mount_under_base=mount_under_base),
     ).remappings(

From 4f38894b7db4b083057d1187dabd28fe9f888051 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 21:42:40 -0700
Subject: [PATCH 12/21] UrdfManager: a repeated joint state skips the FK until
 a keepalive is due (82% of a standing R1's states)

---
 dimos/robot/test_urdf_manager.py | 21 +++++++++++++++++++++
 dimos/robot/urdf_manager.py      | 10 ++++++++++
 2 files changed, 31 insertions(+)

diff --git a/dimos/robot/test_urdf_manager.py b/dimos/robot/test_urdf_manager.py
index 04851476bf4..54941f76c86 100644
--- a/dimos/robot/test_urdf_manager.py
+++ b/dimos/robot/test_urdf_manager.py
@@ -425,3 +425,24 @@ def state_at(t: float, shoulder: float) -> None:
     )  # the tool's keepalive is due (0.3 s since sent); the shoulder's isn't (0.2 s)
 
     assert sent == [{"arm", "tool"}, {"arm"}, {"tool"}]
+
+
+def test_a_repeated_state_skips_the_fk_until_a_keepalive_is_due(make_filter, monkeypatch):
+    module = make_filter(
+        urdf_xml=_joint_robot("prismatic"), joint_tf_hz=1000.0, unchanged_tf_hz=4.0
+    )
+    fk_calls = []
+    joint_transforms = module.joint_transforms
+    monkeypatch.setattr(
+        module,
+        "joint_transforms",
+        lambda state: fk_calls.append(state.ts) or joint_transforms(state),
+    )
+    clock = [100.0]
+    monkeypatch.setattr("dimos.robot.urdf_manager.time.monotonic", lambda: clock[0])
+
+    for t in (0.0, 0.1, 0.2, 0.3):
+        clock[0] = 100.0 + t
+        module.add_joint_state(JointState(ts=t, name=["shoulder"], position=[0.3]))
+
+    assert fk_calls == [0.0, 0.3]
diff --git a/dimos/robot/urdf_manager.py b/dimos/robot/urdf_manager.py
index 0baff6ca1ef..65e15a40c16 100644
--- a/dimos/robot/urdf_manager.py
+++ b/dimos/robot/urdf_manager.py
@@ -98,6 +98,7 @@ def __init__(self, **kwargs: object) -> None:
         self._next_joint_tf_t = 0.0
         # Per edge: the last transform sent (translation + rotation) and when.
         self._sent_edges: dict[tuple[str, str], tuple[tuple[float, ...], float]] = {}
+        self._fk_state: tuple[tuple[str, ...], tuple[float, ...]] | None = None
         self._clouds_in = self._clouds_dropped = self._points_in = self._points_removed = 0
         self._next_stats_t = time.monotonic() + self.config.filter_stats_interval_s
 
@@ -146,6 +147,15 @@ def add_joint_state(self, state: JointState) -> None:
         if not self.config.publish_joint_tf or time.monotonic() < self._next_joint_tf_t:
             return
         self._next_joint_tf_t = time.monotonic() + 1.0 / self.config.joint_tf_hz
+        # A standing robot repeats its state: skip the FK until an edge's keepalive is due.
+        fk_state = (tuple(state.name), tuple(state.position))
+        oldest_sent = min((sent for _, sent in self._sent_edges.values()), default=0.0)
+        if (
+            fk_state == self._fk_state
+            and time.monotonic() < oldest_sent + 1.0 / self.config.unchanged_tf_hz
+        ):
+            return
+        self._fk_state = fk_state
         # The scene's FK state is shared with the cloud filter's thread.
         with self._filter_lock:
             transforms = self._edges_to_send(self.joint_transforms(state))

From e6c787421744f15c262528c0663a2d1f722712ff Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 22:03:49 -0700
Subject: [PATCH 13/21] Revert "UrdfManager: a repeated joint state skips the
 FK until a keepalive is due (82% of a standing R1's states)"

This reverts commit 4f38894b7db4b083057d1187dabd28fe9f888051.
---
 dimos/robot/test_urdf_manager.py | 21 ---------------------
 dimos/robot/urdf_manager.py      | 10 ----------
 2 files changed, 31 deletions(-)

diff --git a/dimos/robot/test_urdf_manager.py b/dimos/robot/test_urdf_manager.py
index 54941f76c86..04851476bf4 100644
--- a/dimos/robot/test_urdf_manager.py
+++ b/dimos/robot/test_urdf_manager.py
@@ -425,24 +425,3 @@ def state_at(t: float, shoulder: float) -> None:
     )  # the tool's keepalive is due (0.3 s since sent); the shoulder's isn't (0.2 s)
 
     assert sent == [{"arm", "tool"}, {"arm"}, {"tool"}]
-
-
-def test_a_repeated_state_skips_the_fk_until_a_keepalive_is_due(make_filter, monkeypatch):
-    module = make_filter(
-        urdf_xml=_joint_robot("prismatic"), joint_tf_hz=1000.0, unchanged_tf_hz=4.0
-    )
-    fk_calls = []
-    joint_transforms = module.joint_transforms
-    monkeypatch.setattr(
-        module,
-        "joint_transforms",
-        lambda state: fk_calls.append(state.ts) or joint_transforms(state),
-    )
-    clock = [100.0]
-    monkeypatch.setattr("dimos.robot.urdf_manager.time.monotonic", lambda: clock[0])
-
-    for t in (0.0, 0.1, 0.2, 0.3):
-        clock[0] = 100.0 + t
-        module.add_joint_state(JointState(ts=t, name=["shoulder"], position=[0.3]))
-
-    assert fk_calls == [0.0, 0.3]
diff --git a/dimos/robot/urdf_manager.py b/dimos/robot/urdf_manager.py
index 65e15a40c16..0baff6ca1ef 100644
--- a/dimos/robot/urdf_manager.py
+++ b/dimos/robot/urdf_manager.py
@@ -98,7 +98,6 @@ def __init__(self, **kwargs: object) -> None:
         self._next_joint_tf_t = 0.0
         # Per edge: the last transform sent (translation + rotation) and when.
         self._sent_edges: dict[tuple[str, str], tuple[tuple[float, ...], float]] = {}
-        self._fk_state: tuple[tuple[str, ...], tuple[float, ...]] | None = None
         self._clouds_in = self._clouds_dropped = self._points_in = self._points_removed = 0
         self._next_stats_t = time.monotonic() + self.config.filter_stats_interval_s
 
@@ -147,15 +146,6 @@ def add_joint_state(self, state: JointState) -> None:
         if not self.config.publish_joint_tf or time.monotonic() < self._next_joint_tf_t:
             return
         self._next_joint_tf_t = time.monotonic() + 1.0 / self.config.joint_tf_hz
-        # A standing robot repeats its state: skip the FK until an edge's keepalive is due.
-        fk_state = (tuple(state.name), tuple(state.position))
-        oldest_sent = min((sent for _, sent in self._sent_edges.values()), default=0.0)
-        if (
-            fk_state == self._fk_state
-            and time.monotonic() < oldest_sent + 1.0 / self.config.unchanged_tf_hz
-        ):
-            return
-        self._fk_state = fk_state
         # The scene's FK state is shared with the cloud filter's thread.
         with self._filter_lock:
             transforms = self._edges_to_send(self.joint_transforms(state))

From 9038357972fbe31ee112592e3768005493722df4 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 22:05:27 -0700
Subject: [PATCH 14/21] ControlCoordinator: an optional joint_state_hz cap; the
 R1 publishes joint state at 30 Hz

---
 dimos/control/coordinator.py                  | 23 ++++++++++++++++++-
 dimos/control/test_coordinator_commands.py    | 16 ++++++++++++-
 .../blueprints/basic/r1pro_coordinator.py     |  2 ++
 3 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py
index dcb7db3cc93..7271fbfbb67 100644
--- a/dimos/control/coordinator.py
+++ b/dimos/control/coordinator.py
@@ -95,6 +95,8 @@ class ControlCoordinatorConfig(ModuleConfig):
 
     tick_rate: float = 100.0
     publish_joint_state: bool = True
+    # At most this often (None: every tick); each subscriber pays per message.
+    joint_state_hz: float | None = None
     # Transitional: goes away once every consumer reads per-robot streams.
     publish_robot_joint_states: bool = False
     joint_state_frame_id: str = "coordinator"
@@ -902,7 +904,9 @@ def start(self) -> None:
 
         # Create and start tick loop
         publish_cb = (
-            self.coordinator_joint_state.publish if self.config.publish_joint_state else None
+            _rate_limited(self.coordinator_joint_state.publish, self.config.joint_state_hz)
+            if self.config.publish_joint_state
+            else None
         )
         publish_robot_cb = (
             self._publish_robot_joint_state if self.config.publish_robot_joint_states else None
@@ -975,3 +979,20 @@ def stop(self) -> None:
     def get_tick_count(self) -> int:
         """Get the number of ticks since start."""
         return self._tick_loop.tick_count if self._tick_loop else 0
+
+
+def _rate_limited(
+    publish: "Callable[[JointState], None]", hz: float | None
+) -> "Callable[[JointState], None]":
+    """``publish``, dropping calls that come sooner than 1 / ``hz`` after the last one passed."""
+    if hz is None:
+        return publish
+    next_t = [0.0]
+
+    def limited(msg: JointState) -> None:
+        now = time.monotonic()
+        if now >= next_t[0]:
+            next_t[0] = now + 1.0 / hz
+            publish(msg)
+
+    return limited
diff --git a/dimos/control/test_coordinator_commands.py b/dimos/control/test_coordinator_commands.py
index edae7d196f7..c4bfe06dc6c 100644
--- a/dimos/control/test_coordinator_commands.py
+++ b/dimos/control/test_coordinator_commands.py
@@ -34,7 +34,7 @@
 import pytest
 
 import dimos.control.coordinator as coord_mod
-from dimos.control.coordinator import ControlCoordinator
+from dimos.control.coordinator import ControlCoordinator, _rate_limited
 from dimos.control.task import (
     BaseControlTask,
     CoordinatorState,
@@ -357,3 +357,17 @@ def test_g1_reports_twist_stream_and_reset_command(self, coordinator):
         assert ("twist_command", "broadcast") in desc["streams"]
         assert "reset_runtime_state" in desc["commands"]
         assert desc["commands"]["reset_runtime_state"]["params"] == ["reactivate"]
+
+
+def test_rate_limited_publish_drops_calls_within_the_period(monkeypatch):
+    sent = []
+    clock = [10.0]
+    monkeypatch.setattr("dimos.control.coordinator.time.monotonic", lambda: clock[0])
+    publish = _rate_limited(sent.append, 30.0)
+    for t in (0.0, 0.01, 0.02, 0.034, 0.05, 0.07):
+        clock[0] = 10.0 + t
+        publish(t)
+
+    assert sent == [0.0, 0.034, 0.07]
+    publish_all = sent.append
+    assert _rate_limited(publish_all, None) is publish_all
diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index 8097d633401..cdedec18b7a 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -208,6 +208,8 @@ def r1pro_control(
             *_head_cameras(),
             ControlCoordinator.blueprint(
                 tick_rate=100,
+                # UrdfManager and the arm planner each spent ~40% of a core receiving it at 100 Hz.
+                joint_state_hz=30.0,
                 hardware=[
                     HardwareComponent(
                         hardware_id="r1pro",

From 6ce81bc9eae6ca31f84bf7aecb9ddec39225a0ad Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 22:29:34 -0700
Subject: [PATCH 15/21] Depth2DepthCloud: an optional max_rate_hz frame cap;
 the R1 head depth takes 5 fps

---
 dimos/perception/depth2depth_cloud/module.py       |  2 ++
 .../depth2depth_cloud/rust/src/module.rs           | 14 +++++++++++++-
 dimos/robot/galaxea/r1pro/head_depth.py            |  2 ++
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/dimos/perception/depth2depth_cloud/module.py b/dimos/perception/depth2depth_cloud/module.py
index 36d2cdc4d0d..11c4f2b5cdc 100644
--- a/dimos/perception/depth2depth_cloud/module.py
+++ b/dimos/perception/depth2depth_cloud/module.py
@@ -76,6 +76,8 @@ class Depth2DepthCloudConfig(NativeModuleConfig):
     max_range_m: float = Field(default=6.0, ge=0.0, le=1000.0)
     decimation: int = Field(default=4, ge=1, le=64)
     max_points: int = Field(default=0, ge=0, le=10000000)
+    # Frames taken per second at most; the rest are dropped on arrival. 0 takes every one.
+    max_rate_hz: float = Field(default=0.0, ge=0.0, le=1000.0)
     frame_id: str = ""
     # Keep only points whose height in height_frame (e.g. base_link) is within [min, max]; "" keeps all.
     height_frame: str = ""
diff --git a/dimos/perception/depth2depth_cloud/rust/src/module.rs b/dimos/perception/depth2depth_cloud/rust/src/module.rs
index 3e40d6498c4..1fa04600dff 100644
--- a/dimos/perception/depth2depth_cloud/rust/src/module.rs
+++ b/dimos/perception/depth2depth_cloud/rust/src/module.rs
@@ -111,6 +111,9 @@ pub struct Config {
     decimation: i64,
     #[validate(range(min = 0, max = 10000000))]
     max_points: i64,
+    /// Frames taken per second at most; the rest are dropped on arrival. 0 takes every one.
+    #[validate(range(min = 0.0, max = 1000.0))]
+    max_rate_hz: f64,
     /// Frame of the cloud; empty takes the CameraInfo's, then the image's.
     frame_id: String,
     /// Keep only points whose height in `height_frame` is within [min, max]; empty frame keeps all.
@@ -144,6 +147,7 @@ pub struct Depth2DepthCloud {
     tasks: Vec<JoinHandle<()>>,
     to_model: Arc<Latest<Picked>>,
     model_thread: Option<std::thread::JoinHandle<()>>,
+    last_frame_stamp: f64,
 }
 
 /// What the handlers hand the worker.
@@ -275,7 +279,15 @@ impl Depth2DepthCloud {
     }
 
     async fn on_image(&mut self, msg: CompressedImage) {
-        let oldest = seconds(&msg.header.stamp) - self.config.max_tf_lag_s;
+        let stamp = seconds(&msg.header.stamp);
+        // 10 % slack: camera stamps jitter a few ms.
+        if self.config.max_rate_hz > 0.0
+            && stamp - self.last_frame_stamp < 0.9 / self.config.max_rate_hz
+        {
+            return;
+        }
+        self.last_frame_stamp = stamp;
+        let oldest = stamp - self.config.max_tf_lag_s;
         let mut frames = self.shared.frames.lock().unwrap();
         frames.push_back(msg);
         while frames
diff --git a/dimos/robot/galaxea/r1pro/head_depth.py b/dimos/robot/galaxea/r1pro/head_depth.py
index 47d873778a1..a037f962d6b 100644
--- a/dimos/robot/galaxea/r1pro/head_depth.py
+++ b/dimos/robot/galaxea/r1pro/head_depth.py
@@ -43,6 +43,8 @@ def r1pro_head_depth(tf_tolerance_s: float, **overrides: object) -> Blueprint:
             "decode_scale": 4,
             # One lidar anchor per 4x4 px block: ~31k anchors per frame was most of the calibration time.
             "anchor_step": 4,
+            # Uncapped it ran ~9.5 fps at 54% of a core.
+            "max_rate_hz": 5.0,
             **overrides,
         }
     ).remappings(

From e7a97f47de42321f89efe7dc39fe6051f4516eb6 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 23:44:04 -0700
Subject: [PATCH 16/21] dim_slam: dimSLAM v0.5.0-beta3 zero-velocity update;
 the R1's wheels hold the pose while standing

---
 dimos/mapping/dim_slam/dim_slam.py                          | 4 ++++
 dimos/mapping/dim_slam/rust/Cargo.lock                      | 2 +-
 dimos/mapping/dim_slam/rust/Cargo.toml                      | 2 +-
 dimos/mapping/dim_slam/rust/crate-hashes.json               | 2 +-
 dimos/mapping/dim_slam/rust/src/main.rs                     | 6 ++++++
 .../galaxea/r1pro/blueprints/basic/r1pro_coordinator.py     | 4 ++++
 6 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/dimos/mapping/dim_slam/dim_slam.py b/dimos/mapping/dim_slam/dim_slam.py
index c3039910cee..c699ff633b1 100644
--- a/dimos/mapping/dim_slam/dim_slam.py
+++ b/dimos/mapping/dim_slam/dim_slam.py
@@ -129,6 +129,10 @@ class SourceConfig(BaseModel):
     twist_variances: Covariance = Field(default_factory=Covariance)
     # pose only: re-align a source to the fused pose after it is rejected this long; 0 never re-aligns.
     realign_after_seconds: float = 0.0
+    # Zero-velocity update: a twist at or under these (m/s, rad/s) is fused as standing still; 0 variance disables.
+    still_variance: float = 0.0
+    still_speed: float = 0.0
+    still_angular_speed: float = 0.0
 
 
 class DimSlamConfig(NativeModuleConfig):
diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock
index f20a7443e17..27555212335 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.lock
+++ b/dimos/mapping/dim_slam/rust/Cargo.lock
@@ -567,7 +567,7 @@ dependencies = [
 [[package]]
 name = "dim_slam"
 version = "0.1.0"
-source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta2#103206e75a8cfa5ba28dfc3328921bd22c47a528"
+source = "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta3#4a65da197d7ff6bf66287b3124fcf62afee30da0"
 dependencies = [
  "cu_vslam_rs",
  "nalgebra",
diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml
index d356d9ebea7..020c871b4e6 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.toml
+++ b/dimos/mapping/dim_slam/rust/Cargo.toml
@@ -11,7 +11,7 @@ path = "src/main.rs"
 [dependencies]
 # tag, not rev: nix's fetchGit only searches the ref the url names, so a bare rev is
 # unfetchable. Cargo.lock still pins the exact commit.
-dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.5.0-beta2" }
+dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.5.0-beta3" }
 # Unused directly, but cargo hands DEP_CUVSLAM_LIB_DIR only to immediate dependents
 # and build.rs needs it for the rpath.
 cu_vslam_rs = "0.2.0"
diff --git a/dimos/mapping/dim_slam/rust/crate-hashes.json b/dimos/mapping/dim_slam/rust/crate-hashes.json
index 08d1cffeecf..9e8fe84ee68 100644
--- a/dimos/mapping/dim_slam/rust/crate-hashes.json
+++ b/dimos/mapping/dim_slam/rust/crate-hashes.json
@@ -1,5 +1,5 @@
 {
-  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta2#dim_slam@0.1.0": "sha256-YixD62KdVtkOjP4i0WesXmpyGPMCF5JiyPGz1fCmPqg=",
+  "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.5.0-beta3#dim_slam@0.1.0": "sha256-cnt5MFlYfC/0SODg9glSCnR/UpyWqIl8X+H0pmg0Cvo=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
diff --git a/dimos/mapping/dim_slam/rust/src/main.rs b/dimos/mapping/dim_slam/rust/src/main.rs
index fc51aee80cc..910fd67e4d5 100644
--- a/dimos/mapping/dim_slam/rust/src/main.rs
+++ b/dimos/mapping/dim_slam/rust/src/main.rs
@@ -59,6 +59,9 @@ struct OdomSourceConfig {
     pose_variances: Covariance,
     twist_variances: Covariance,
     realign_after_seconds: f64,
+    still_variance: f64,
+    still_speed: f64,
+    still_angular_speed: f64,
 }
 
 /// One IMU, identified by the frame its samples carry.
@@ -226,6 +229,9 @@ impl DimSlam {
                     pose_variances: source.pose_variances.to_array(),
                     twist_variances: source.twist_variances.to_array(),
                     realign_after_seconds: source.realign_after_seconds,
+                    still_variance: source.still_variance,
+                    still_speed: source.still_speed,
+                    still_angular_speed: source.still_angular_speed,
                 })
                 .collect(),
             planar: self.config.planar,
diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index cdedec18b7a..e20a774bb08 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -331,6 +331,10 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
     child_frame_id="base_link",
     kind="twist",
     twist_variances=Covariance(x=5e-4, y=5e-4, yaw=6e-4),
+    # Wheels at rest read exactly zero; standing, the pose then settles instead of following Point-LIO's mm of noise.
+    still_variance=1e-6,
+    still_speed=1e-3,
+    still_angular_speed=1e-3,
 )
 
 

From 9c569a1704e70f95c8d0affcc7cbec613da64451 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sat, 17 Oct 2026 23:46:51 -0700
Subject: [PATCH 17/21] r1pro: Point-LIO's world frame is pointlio_odom where
 dimSLAM owns odom, so its poses aren't drawn offset in dimSLAM's

---
 .../robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py | 4 +++-
 dimos/robot/galaxea/r1pro/config.py                           | 2 ++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index e20a774bb08..d7f6d83588b 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -50,6 +50,7 @@
 from dimos.robot.galaxea.r1pro.config import (
     R1PRO_CHASSIS_LIDAR_HOST_IP,
     R1PRO_CHASSIS_LIDAR_IP,
+    R1PRO_POINTLIO_ODOM_FRAME,
     R1PRO_WHEEL_ODOM_FRAME,
 )
 from dimos.robot.galaxea.r1pro.connection import R1PRO_UPPER_BODY_JOINTS, R1ProConnection
@@ -302,6 +303,7 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
             host_ip=R1PRO_CHASSIS_LIDAR_HOST_IP,
         ),
         PointLio.blueprint(
+            frame_id=R1PRO_POINTLIO_ODOM_FRAME if mount_under_base else "odom",
             sensor_frame_id=lidar_frame,
             # Standing still at 0.01 the pose stepped ~7 mm a scan (the robot jittered in the viewer); 1.0 took it to 2.5 mm.
             lidar_meas_cov=1.0,
@@ -320,7 +322,7 @@ def r1pro_lidar_odometry(mount_under_base: bool = False) -> Blueprint:
 
 # Replaying a 30 m R1 drive: 4.8 mm / 0.25 deg jitter against Point-LIO's own 13.8 mm / 0.33 deg.
 POINTLIO_SOURCE = SourceConfig(
-    parent_frame_id="odom",
+    parent_frame_id=R1PRO_POINTLIO_ODOM_FRAME,
     child_frame_id="base_link",
     kind="pose",
     pose_variances=Covariance(x=2e-4, y=2e-4, yaw=3.3e-5),
diff --git a/dimos/robot/galaxea/r1pro/config.py b/dimos/robot/galaxea/r1pro/config.py
index 5230f106561..b833914eab2 100644
--- a/dimos/robot/galaxea/r1pro/config.py
+++ b/dimos/robot/galaxea/r1pro/config.py
@@ -171,3 +171,5 @@ def make_r1pro_planar_model_config() -> RobotModelConfig:
 
 # The connection's wheel odometry's world frame where dimSLAM owns odom, which tells it apart from Point-LIO's.
 R1PRO_WHEEL_ODOM_FRAME = "wheel_odom"
+# Point-LIO's world frame (its origin is where the lidar started) where dimSLAM owns odom: a shared name drew its poses offset.
+R1PRO_POINTLIO_ODOM_FRAME = "pointlio_odom"

From 7f9244648139f83c89e0fbbd9c972fa693e8e9c9 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sun, 18 Oct 2026 00:24:10 -0700
Subject: [PATCH 18/21] dim-odom 0.5.0b3

---
 pyproject.toml |  2 +-
 uv.lock        | 12 ++++++------
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index ed00c24a324..08a857b6a31 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -503,7 +503,7 @@ tests-self-hosted = [
     "mcap>=1.2.0",
     # Needed to compile the in-tree extensions.
     "pybind11>=2.12",
-    "dim-odom>=0.5.0b2",
+    "dim-odom>=0.5.0b3",
 ]
 docs = [
     "mkdocs-awesome-nav>=3.3.0",
diff --git a/uv.lock b/uv.lock
index 7309c402dd1..2234684273a 100644
--- a/uv.lock
+++ b/uv.lock
@@ -1788,7 +1788,7 @@ wheels = [
 
 [[package]]
 name = "dim-odom"
-version = "0.5.0b2"
+version = "0.5.0b3"
 source = { registry = "https://pypi.org/simple" }
 dependencies = [
     { name = "nvidia-cublas", marker = "sys_platform == 'linux'" },
@@ -1797,10 +1797,10 @@ dependencies = [
     { name = "nvidia-cusparse", marker = "sys_platform == 'linux'" },
 ]
 wheels = [
-    { url = "https://files.pythonhosted.org/packages/5d/ad/e9894f8b73314473795d6210a59bac9d285a4a94c49bc82aee75115ce7e7/dim_odom-0.5.0b2-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:2b2a9bc331ae8f0228ec0eac769583a093d17e99baca933814e7416f5e1a6e65", size = 6990154, upload-time = "2026-10-18T03:25:06.383Z" },
-    { url = "https://files.pythonhosted.org/packages/15/f4/13ae41f0c5bc4a860214e574c6ae83a595d4ab4c0a73f8e5dce9a05fc35f/dim_odom-0.5.0b2-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:95bbbd65fbdaf1c8771e76af12c91458798a1123a605de7cc295644ccccc2a54", size = 86713864, upload-time = "2026-10-18T03:25:33.288Z" },
-    { url = "https://files.pythonhosted.org/packages/cf/24/2872d116fd8f83df79accd8277efffd55d62edf383ec6bdcf23698bcdc84/dim_odom-0.5.0b2-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:28afc351505992187d6af45e1059cd1d33d301878c64e661323c82666c9f4dc0", size = 85511911, upload-time = "2026-10-18T03:25:19.517Z" },
-    { url = "https://files.pythonhosted.org/packages/37/88/c8b8d96ad1d8b2b17f4a4ea122c30656b3a9240e304808d243337d4eae99/dim_odom-0.5.0b2-py3-none-any.whl", hash = "sha256:882c3716ef38913747f1dfa9f53f1e188855785bb0a99835291fe7638adb760f", size = 1430, upload-time = "2026-10-18T03:25:22.215Z" },
+    { url = "https://files.pythonhosted.org/packages/57/2b/b90380a0a1ffc6445f75883ebf59561ec63e8f24bae18c644691cac26192/dim_odom-0.5.0b3-cp310-abi3-macosx_13_0_arm64.whl", hash = "sha256:abe92bc7ee3aff217d86605212c14be59e58690f18e2c8de5e18da268809f79f", size = 6990938, upload-time = "2026-10-18T06:46:19.811Z" },
+    { url = "https://files.pythonhosted.org/packages/5b/d1/149b56b02786b34c4740e61d4e92554b281da7f6fa41bb2e60bc29df5ebd/dim_odom-0.5.0b3-cp310-abi3-manylinux_2_35_aarch64.whl", hash = "sha256:2fbe8a11494bce10af163da7a077988cbfcd59cc0b0906a4705677ffc4fc103e", size = 86714821, upload-time = "2026-10-18T06:46:40.924Z" },
+    { url = "https://files.pythonhosted.org/packages/0e/86/ee174a272b7a19e3d98b1bcaf8dd6835f543148da928d13539f8fab24b4d/dim_odom-0.5.0b3-cp310-abi3-manylinux_2_35_x86_64.whl", hash = "sha256:1e3004e12e67469a6c35e9c41da13056d768eb24f60a27d5976fb60f18c9c302", size = 85512426, upload-time = "2026-10-18T06:46:29.53Z" },
+    { url = "https://files.pythonhosted.org/packages/42/17/8991369ed984c897bbf8e2ed52cb97288ead6db3b9d675e14e04a96e56c8/dim_odom-0.5.0b3-py3-none-any.whl", hash = "sha256:0280444d073bb25607b689fc4867c563f5cbe38c06dc34c78c9228c9ea444565", size = 1430, upload-time = "2026-10-18T06:46:31.817Z" },
 ]
 
 [[package]]
@@ -2716,7 +2716,7 @@ tests-self-hosted = [
     { name = "chromadb", specifier = ">=1.0.0" },
     { name = "coacd", specifier = ">=1.0.0" },
     { name = "coverage", specifier = ">=7.0" },
-    { name = "dim-odom", specifier = ">=0.5.0b2" },
+    { name = "dim-odom", specifier = ">=0.5.0b3" },
     { name = "dimos", extras = ["apriltag", "mapping", "drone", "cpu", "learning"] },
     { name = "dimos", extras = ["manipulation", "misc", "unitree"] },
     { name = "dimos", extras = ["web", "visualization", "webrtc"] },

From c2d81ebe12486aa3cd4c6ad7b740d0c485513617 Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sun, 18 Oct 2026 04:49:06 -0700
Subject: [PATCH 19/21] r1pro-nav: the ray tracer emits every 2nd cloud; MLS
 rebuilt its surface on each (32 -> 9% CPU)

---
 dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
index 29f1b880784..bc48506f920 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
@@ -88,7 +88,8 @@
         max_range=10.0,
         # Live, heavier head depth fell ~10 s behind; skipping stale clouds means it never works through a queue.
         max_cloud_age_s=1.0,
-        emit_every=1,
+        # Every 2nd cloud (~7 Hz with head depth) is plenty for MLS, which rebuilt its surface on each one at ~32% of a core.
+        emit_every=2,
         global_emit_every=50,
         support_min=4,
         worker_threads=3,

From 2339b379d28327dc211f8f67de9f78ea85f9e69f Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sun, 18 Oct 2026 08:00:54 -0700
Subject: [PATCH 20/21] r1pro: r1pro_control can leave the right head eye
 unread; r1pro-nav does, since head depth reads only the left

---
 .../r1pro/blueprints/basic/r1pro_coordinator.py | 17 ++++++++++++-----
 .../r1pro/blueprints/navigation/r1pro_nav.py    |  2 ++
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
index d7f6d83588b..4cc9cbdb477 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py
@@ -173,20 +173,27 @@ def _zenoh_transport(
     )
 
 
-def _head_cameras() -> list[Blueprint]:
-    """Both head eyes and their intrinsics; see ``head_cameras``."""
-    return [HeadLeftCamera.blueprint(), HeadRightCamera.blueprint(), HeadCameraInfo.blueprint()]
+def _head_cameras(head_right: bool) -> list[Blueprint]:
+    """The head eyes and their intrinsics; see ``head_cameras``."""
+    eyes = (
+        [HeadLeftCamera.blueprint(), HeadRightCamera.blueprint()]
+        if head_right
+        else [HeadLeftCamera.blueprint()]
+    )
+    return [*eyes, HeadCameraInfo.blueprint()]
 
 
 def r1pro_control(
     *,
     tasks: Sequence[TaskConfig] | None = None,
+    head_right: bool = True,
     **connection: Any,
 ) -> Blueprint:
     """R1ProConnection, ControlCoordinator and the head cameras.
 
     ``tasks`` overrides the default task set (whole-body trajectory + chassis
-    velocity); transports and remappings stay identical either way.
+    velocity); transports and remappings stay identical either way. ``head_right``
+    off leaves the right head eye unread.
     Other keyword args are ``R1ProConnectionConfig`` fields.
     """
     resolved_tasks = (
@@ -206,7 +213,7 @@ def r1pro_control(
     return (
         autoconnect(
             R1ProConnection.blueprint(**connection),
-            *_head_cameras(),
+            *_head_cameras(head_right),
             ControlCoordinator.blueprint(
                 tick_rate=100,
                 # UrdfManager and the arm planner each spent ~40% of a core receiving it at 100 Hz.
diff --git a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
index bc48506f920..c9c2e093f3b 100644
--- a/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
+++ b/dimos/robot/galaxea/r1pro/blueprints/navigation/r1pro_nav.py
@@ -67,6 +67,8 @@
         odom_frame_id=R1PRO_WHEEL_ODOM_FRAME,
         enable_wrist_color=False,
         enable_imu=False,
+        # Head depth reads only the left eye.
+        head_right=False,
     ).remappings([(R1ProConnection, "odometry", "slam_sources")]),
     r1pro_fused_odometry().remappings([(PointLio, "lidar", "lidar_with_robot")]),
     # Every joint on tf (the head cameras ride the torso), and the robot's own body cut out of the Mid-360's scans.

From 4563a3ff5bf7fb2ed87d1159d065adb84dcc8f9a Mon Sep 17 00:00:00 2001
From: Jeff Hykin <jeff.hykin@gmail.com>
Date: Sun, 18 Oct 2026 08:10:23 -0700
Subject: [PATCH 21/21] dim_slam: zenoh 1.10.1 with upstream's startup-deadlock
 fix (zenoh-vendor patch.2); it wedged dim_slam's session on R1 launches

---
 dimos/mapping/dim_slam/rust/Cargo.lock        | 3 +--
 dimos/mapping/dim_slam/rust/Cargo.toml        | 4 ++++
 dimos/mapping/dim_slam/rust/crate-hashes.json | 3 ++-
 3 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock
index 27555212335..fd0b592fc04 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.lock
+++ b/dimos/mapping/dim_slam/rust/Cargo.lock
@@ -3776,8 +3776,7 @@ dependencies = [
 [[package]]
 name = "zenoh"
 version = "1.10.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "453ba5d28a1197653aae4bb024fd74a9d5c7051d5a19a77f3a43d83f40c22584"
+source = "git+https://github.com/jeff-hykin/zenoh-vendor?tag=zenoh-1.10.1-patch.2#083bc2d17d5382e9e425113d9c3cb9c15d1a2153"
 dependencies = [
  "ahash",
  "arc-swap",
diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml
index 020c871b4e6..f9de5bec410 100644
--- a/dimos/mapping/dim_slam/rust/Cargo.toml
+++ b/dimos/mapping/dim_slam/rust/Cargo.toml
@@ -26,4 +26,8 @@ validator = { version = "0.21", features = ["derive"] }
 [profile.release]
 lto = true
 
+# zenoh 1.10.1 plus upstream's startup-deadlock fix (eclipse-zenoh/zenoh issue 2581), which no release has yet.
+[patch.crates-io]
+zenoh = { git = "https://github.com/jeff-hykin/zenoh-vendor", tag = "zenoh-1.10.1-patch.2" }
+
 [workspace]
diff --git a/dimos/mapping/dim_slam/rust/crate-hashes.json b/dimos/mapping/dim_slam/rust/crate-hashes.json
index 9e8fe84ee68..116c030de1f 100644
--- a/dimos/mapping/dim_slam/rust/crate-hashes.json
+++ b/dimos/mapping/dim_slam/rust/crate-hashes.json
@@ -3,5 +3,6 @@
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=",
   "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
-  "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA="
+  "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=",
+  "git+https://github.com/jeff-hykin/zenoh-vendor?tag=zenoh-1.10.1-patch.2#zenoh@1.10.1": "sha256-WeW+O7IMZXqYtQK20aRnQm/zy2kmf4Lr1eb4Djk13iI="
 }
\ No newline at end of file