Loop closure / map Reconstruction first pass

[leshy]

• pgo rewrite, map tool vis aditions
• full global map reconstruction takes a few seconds, we use memory2 to fetch 20% of total lidar frames based on time and space (latest only, not all)

• marker detector as a mem2 transform dimos map hk_village5 --pgo --markers

• marker detection debugger

cd dimos/utils/cli
python markers_rrd.py hk_village3 --out test3.rrd && rerun test3.rrd

# or for full map

dimos map hk_village3 --pgo --markers

[codecov]

Codecov Report

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

Files with missing lines	Patch %	Lines
dimos/utils/cli/map.py	7.82%	106 Missing ⚠️
dimos/memory2/transform.py	9.37%	29 Missing ⚠️
dimos/mapping/loop_closure/pgo.py	92.55%	11 Missing and 12 partials ⚠️
dimos/memory2/type/observation.py	91.93%	3 Missing and 2 partials ⚠️
dimos/memory2/module.py	0.00%	4 Missing ⚠️
dimos/memory2/vis/space/space.py	0.00%	4 Missing ⚠️
dimos/memory2/vis/space/svg.py	0.00%	4 Missing ⚠️
dimos/memory2/stream.py	50.00%	1 Missing and 1 partial ⚠️
dimos/msgs/geometry_msgs/Quaternion.py	80.00%	2 Missing ⚠️
dimos/memory2/observationstore/sqlite.py	87.50%	1 Missing ⚠️
... and 1 more

📢 Thoughts on this report? Let us know!

[greptile-apps]

Greptile Summary

This PR replaces the numpy-based relocalization/pgo.py with a fully redesigned loop_closure/pgo.py that wraps GTSAM ISAM2 as a memory2 Transformer, and refactors Observation so that pose is now a typed Pose property while the raw 7-tuple is stored in pose_tuple. Several bugs caught in previous review rounds have been resolved (placeholder-pose filter, qw or 1.0 corruption, rr.Points3D wrong kwarg, and obs.pose unpack crashes).

• dimos/mapping/loop_closure/pgo.py — new PGO implementation using GTSAM ISAM2; PoseGraph is itself a Transformer that applies drift corrections to arbitrary streams, replacing the old hand-rolled SLERP+voxel pass.
• dimos/memory2/type/observation.py — Observation.pose now returns a typed Pose | None; raw storage moves to pose_tuple; _to_tuple centralizes coercion from Transform / PoseStamped / tuple / list.
• dimos/perception/fiducial/marker_transformer.py + dimos/utils/cli/map.py — ArUco/AprilTag marker detection wired into the mem2 stream pipeline with optional pose smoothing and rerun visualization.

Confidence Score: 4/5

Most of the bugs from previous rounds are now fixed; the remaining unfixed crash (LookupError on empty PGO output in map.py and eval.py) is the main blocker for recordings with all-placeholder poses.

The Observation refactor, GTSAM-based PGO, and marker pipeline are all sound and well-tested. All previously flagged pose-corruption and type-crash bugs are addressed. The one active issue is that lidar.transform(PGO()).last() in map.py and eval.py still raises an unhandled LookupError when a recording yields zero valid keyframes — a condition that can occur with bad data and produces a raw traceback instead of a clean diagnostic.

dimos/utils/cli/map.py (line 285) and dimos/mapping/loop_closure/eval.py (line 95) — both call .last() on the PGO stream without a guard for empty output.

Important Files Changed

Filename	Overview
dimos/mapping/loop_closure/pgo.py	New GTSAM ISAM2-based PGO implementation as a memory2 Transformer; clean design with PoseGraph dual-role as corrector; placeholder-pose filter now correctly uses is_zero() for orientation.
dimos/memory2/type/observation.py	Major refactor: pose stored as pose_tuple, exposed as typed Pose property; _to_tuple centralizes coercion and now handles list/tuple/Transform/Pose/PoseStamped correctly with is-not-None guards.
dimos/memory2/observationstore/sqlite.py	_reconstruct_pose replaces qw or 1.0 pattern with explicit asserts; uses pose_tuple for insert/reconstruct; assert-based validation is correct for current schema but can be bypassed with -O.
dimos/utils/cli/map.py	New map CLI with spatial dedup, PGO two-pass reconstruction, and marker overlay; uses Pose API correctly; LookupError on line 285 when all frames are placeholder poses remains unhandled (pre-existing flagged issue).
dimos/perception/fiducial/marker_transformer.py	New file: ArUco/AprilTag detection as a memory2 Transformer with optional smoothing; correctly uses obs.pose (Pose object) and composes world-frame marker poses from camera-in-world transform.
dimos/memory2/transform.py	Adds SpeedLimit transformer for motion-blur gating; speed() updated to Pose API; SpeedLimit correctly drops pose-less frames and first frame (documented).
dimos/msgs/sensor_msgs/PointCloud2.py	Fixes to_rerun mode='points' to pass radii= instead of the non-existent sizes= kwarg; removes unused size parameter from signature; color carry-through comment clarified.
dimos/msgs/geometry_msgs/Quaternion.py	Adds is_zero(), angle_to(), dot(), and to_rotation_matrix() helpers; is_zero() correctly targets the all-zero uninitialized case without filtering the identity quaternion.

Sequence Diagram

sequenceDiagram
    participant CLI as dimos map CLI
    participant Lidar as Stream[PointCloud2]
    participant PGO as PGO Transformer
    participant PGOState as _PGOState (ISAM2)
    participant PoseGraph as PoseGraph
    participant Accum as _accumulate

    CLI->>Lidar: store.streams.lidar + spatial dedup
    CLI->>Lidar: lidar.tap(prog).transform(PGO())
    loop per lidar frame
        Lidar->>PGO: Observation[PointCloud2]
        PGO->>PGO: filter placeholder poses (is_zero)
        PGO->>PGOState: process(pose3, ts, cloud)
        PGOState->>PGOState: _add_keyframe (BetweenFactorPose3)
        PGOState->>PGOState: _search_for_loops (KDTree + ICP)
        alt loop accepted
            PGOState->>PGOState: _smooth_and_update (ISAM2)
            PGO-->>CLI: yield Observation[PoseGraph] snapshot
        end
    end
    PGO-->>CLI: final yield Observation[PoseGraph]
    CLI->>CLI: "graph = .last().data"
    CLI->>Accum: "_accumulate(seen.values(), graph=graph)"
    loop per deduped frame
        Accum->>PoseGraph: correction_at(obs.ts)
        PoseGraph->>PoseGraph: slerp/lerp interpolation
        PoseGraph-->>Accum: drift-correction Transform
        Accum->>Accum: cloud.transform(correction) → VoxelGrid
    end
    Accum-->>CLI: PointCloud2 (PGO map)

Loading

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