Dimos map tool by leshy · Pull Request #2241 · dimensionalOS/dimos

leshy · 2026-05-24T09:47:26Z

Purpose of this tool is to visualize maps directly from mem2 databases, do off-line loop closure and store them as pointclouds one can relocalize on

TODO for the future, add KVstore to actual mem2 database, relocalization module can consume mem2 database directly, and recompute loop closure/global map only if needed. if not, just load from kvstore. elimites the need for tooling like this. Tooling like this will stay for analysis and development, but not as a required part of relocalization flow

greptile-apps · 2026-05-24T10:59:45Z

Greptile Summary

This PR replaces the dimos export-premap command with a richer dimos map command that visualises raw and PGO-corrected maps side-by-side in rerun, adds progress callbacks to the two-pass PGO pipeline, and improves the z-height colormap with percentile clipping.

New dimos map CLI (dimos/utils/cli/map.py): builds a raw voxel map and an optional PGO-corrected map, logs both to rerun with their odometry paths, and optionally exports the corrected map to .pc2.lcm.
PGO additions (pgo.py): per-pass progress callbacks, a corrected-path output list, and timestamp sort+dedup before Slerp to avoid ValueError on duplicate keyframe timestamps.
PointCloud2 (PointCloud2.py): removes the transform() color-copy (flagged in a prior review) and clips the z-colormap to the 1st–99th percentile to prevent outliers from crushing the colour range.

Confidence Score: 4/5

Safe to merge for the visualization/tooling use-case; the transform() color-drop and the potential Slerp crash after timestamp deduplication are still open from prior reviews.

Two previously flagged defects remain unresolved: PointCloud2.transform() silently discards point colors, and pgo_then_voxels can raise a ValueError from Slerp when all keyframes share the same timestamp after deduplication.

dimos/msgs/sensor_msgs/PointCloud2.py (color dropped in transform), dimos/mapping/relocalization/pgo.py (Slerp guard after dedup)

Important Files Changed

Filename	Overview
dimos/utils/cli/map.py	New map visualization tool; global_map built unconditionally even when only the PGO export is needed (flagged in prior review, still present).
dimos/mapping/relocalization/pgo.py	Adds callback hooks and corrected-path output; adds timestamp sort+dedup before Slerp but doesn't guard the ≥2 entry requirement post-dedup (flagged in prior review, still present).
dimos/msgs/sensor_msgs/PointCloud2.py	Removes the color-copy in transform() and improves z-colormap clipping; the color-drop bug was flagged in prior review and is still unaddressed.
dimos/robot/cli/dimos.py	Renames export-premap command to map and delegates to the new map.py helper; straightforward refactor.
docs/capabilities/mapping/relocalization.md	Documentation updated to match the renamed command and new output path convention.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    A([dimos map dataset]) --> B{pgo or export?}
    B -- yes --> C[pgo_then_voxels\npass 1: build PGO graph\ncollect corrected path]
    C --> D[pgo_then_voxels\npass 2: rebuild voxel map\nwith corrected poses]
    D --> E[pgo_map + pgo_path]
    B -- no --> F[skip PGO]
    E --> G[VoxelMapTransformer\nraw global map]
    F --> G
    G --> H[global_map + raw path]
    H --> I{no_gui?}
    I -- no --> J[rerun_init\nlog raw map + path\nlog PGO map + path]
    I -- yes --> K[skip rerun]
    J --> L{export?}
    K --> L
    L -- yes --> M[write .pc2.lcm to cwd]
    L -- no --> N([done])

_{Reviews (6): Last reviewed commit: "Merge branch 'main' into feat/ivan/mapto..." | Re-trigger Greptile}

greptile-apps · 2026-05-24T10:59:48Z

+        # Sort + dedupe by timestamp: Slerp requires strictly increasing ts.
+        kps = sorted(pgo._key_poses, key=lambda kp: kp.timestamp)
+        kps = [kp for i, kp in enumerate(kps) if i == 0 or kp.timestamp > kps[i - 1].timestamp]
+        kf_ts = np.array([kp.timestamp for kp in kps])


After deduplication kps may hold fewer than 2 entries even when n_kf >= 2, so Slerp would immediately raise ValueError. Mirror the n_kf < 2 fallback here.

Suggested change

# Sort + dedupe by timestamp: Slerp requires strictly increasing ts.

kps = sorted(pgo._key_poses, key=lambda kp: kp.timestamp)

kps = [kp for i, kp in enumerate(kps) if i == 0 or kp.timestamp > kps[i - 1].timestamp]

kf_ts = np.array([kp.timestamp for kp in kps])

# Sort + dedupe by timestamp: Slerp requires strictly increasing ts.

kps = sorted(pgo._key_poses, key=lambda kp: kp.timestamp)

kps = [kp for i, kp in enumerate(kps) if i == 0 or kp.timestamp > kps[i - 1].timestamp]

if len(kps) < 2:

for obs in stream:

if pass2_on_frame is not None:

pass2_on_frame(obs)

grid.add_frame(obs.data)

return grid.get_global_pointcloud2()

kf_ts = np.array([kp.timestamp for kp in kps])

I think this is correct, right? @leshy

this also seems correct to me

codecov · 2026-05-24T11:01:44Z

Codecov Report

❌ Patch coverage is 33.33333% with 4 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines	Patch %	Lines
dimos/msgs/sensor_msgs/PointCloud2.py	0.00%	2 Missing ⚠️
dimos/robot/cli/dimos.py	50.00%	2 Missing ⚠️

📢 Thoughts on this report? Let us know!

The colors-copy block in `PointCloud2.transform` calls `self.pointcloud` to check `has_colors()`, which forces a tensor->legacy conversion on every invocation. With `pgo --full-pgo` rebuilding from hundreds of lidar frames, that hidden allocation dominates the per-frame cost. The colors path was lidar-irrelevant (lidar clouds have no colors anyway) and the feature it was meant to support never landed; remove it so transform() stays a clean numpy round-trip.

paul-nechifor · 2026-05-25T22:06:25Z

            return 0
        return int(self._pcd_tensor.point["positions"].shape[0])

    def to_rerun(


Unrelated to this PR, but I'd like to see what you think about this.

I think it's not sustainable to have so many methods on messages. Having to_rerun on messages (or other such complicated functions) makes the messages very heavy and mixes concerns quite a bit.

I think it would be a lot easier to have messages just be "structs" with a few precise utility functions. (By this I'm referring to things which don't use external libs and are un-opinionated, for example there's only one way to convert a radian to a degree, but there are many choices to make when converting a occupancy map to an RGB representation of it. This particular function is both opinionated and uses external libs).

An alternative would be to have dimos/msgs/conversions/rerun/to_rerun.py . It would be a single function which takes any message, uses match and spits out a converted rerun object.

This also has the effect of containing all rerun imports to dimos/msgs/conversions/rerun and not needing to import rerun inside functions like we do now to avoid unnecessary imports when just using a message class for its data with no intention to use rerun.

I'm not saying we should do this now or anytime soon, just checking the temperature, to see if this is something we should pursue latter, or maybe when new methods are added to messages.

yeah I don't mind this personally :)

paul-nechifor · 2026-05-25T22:30:24Z

+    global_map = (
+        lidar.tap(collect_path)
+        .transform(VoxelMapTransformer(voxel_size=voxel, device=device))
+        .tap(progress(lidar.count(), "reconstructing global map"))
+        .last()
+        .data


VoxelMapTransformer emits every transform. last is implemented as .order_by("ts", desc=True).first().

Would it not be simpler to just emit one (with emit_every=0)and use .first() so no need for sorting? Something like:

Suggested change

global_map = (

lidar.tap(collect_path)

.transform(VoxelMapTransformer(voxel_size=voxel, device=device))

.tap(progress(lidar.count(), "reconstructing global map"))

.last()

.data

global_map = (

lidar.tap(collect_path)

.tap(progress(lidar.count(), "reconstructing global map"))

.transform(VoxelMapTransformer(voxel_size=voxel, device=device, emit_every=0))

.first()

.data

paul-nechifor · 2026-05-25T22:38:04Z

+    if corrected_path_out is not None:
+        corrected_path_out.extend(
+            (float(kp.t_global[0]), float(kp.t_global[1]), float(kp.t_global[2]))
+            for kp in pgo._key_poses


Nit: please rename _key_poses to key_poses since it's used all over.

paul-nechifor · 2026-05-25T22:42:01Z

-            class_ids = ((z - z.min()) / (z.max() - z.min() + 1e-8) * 255).astype(np.uint8)
+
+            # Clip to robust percentiles so a few outliers don't squash the colormap range.
+            z_lo, z_hi = np.percentile(z, [1, 99])


Since this is purely cosmetic, you might want to add a small optimization for very large point clouds: only process a small subset. For example: only process the first 20000 points, or only process a random sample of the points z = z if len(z) <= 20_000 else np.random.choice(z, 1000, replace=False)

True, actually will exclude my PC2 changes from this PR, shouldn't change efficiency

leshy requested a review from arkluc May 24, 2026 09:47

leshy added the leshy_navigation label May 24, 2026

greptile-apps Bot reviewed May 24, 2026

View reviewed changes

Comment thread dimos/utils/cli/map.py Outdated

Comment thread dimos/utils/cli/map.py

arkluc reviewed May 24, 2026

View reviewed changes

Comment thread docs/capabilities/mapping/relocalization.md Outdated

arkluc reviewed May 24, 2026

View reviewed changes

Comment thread dimos/msgs/sensor_msgs/PointCloud2.py

leshy added 4 commits May 24, 2026 18:53

map tool first pass

ceb08ba

map tool wrap

4e0cec3

renamed rerun blueprint to dimos map tool

2099e00

readme cleanup

47848d4

leshy force-pushed the feat/ivan/maptool branch from a5e789c to 47848d4 Compare May 24, 2026 10:54

dimensionalOS deleted a comment from greptile-apps Bot May 24, 2026

greptile-apps Bot reviewed May 24, 2026

View reviewed changes

leshy added the PlzReview label May 24, 2026

leshy added 3 commits May 24, 2026 22:35

map cli: annotate progress / collect_path so mypy passes

517d09c

Merge branch 'main' into feat/ivan/maptool

20cb510

paul-nechifor reviewed May 25, 2026

View reviewed changes

jeff-hykin self-assigned this May 26, 2026

Conversation

leshy commented May 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

greptile-apps Bot commented May 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Greptile Summary

Confidence Score: 4/5

Important Files Changed

Flowchart

Uh oh!

greptile-apps Bot May 24, 2026

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

codecov Bot commented May 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

leshy commented May 24, 2026 •

edited

Loading

greptile-apps Bot commented May 24, 2026 •

edited

Loading

codecov Bot commented May 24, 2026 •

edited

Loading