Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions data/.lfs/mid360_athens_stairs.pc2.lcm.tar.gz
Git LFS file not shown
43 changes: 41 additions & 2 deletions dimos/mapping/cli/map.py
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,31 @@ def prepared() -> Iterable[Observation[PointCloud2]]:
return result.data if result is not None else None


def _pgo_overrides(settings: list[str], non_planar: bool = False) -> dict[str, Any]:
"""PGOConfig overrides from ``--non-planar`` and ``--pgo-set name=value`` pairs.

Values are parsed with the field's own type, so an unknown or unparseable
one is a CLI error rather than a config that silently means something else.
"""
from dimos.mapping.loop_closure.pgo import PGOConfig

fields = PGOConfig.model_fields
out: dict[str, Any] = {}
for setting in settings:
name, _, raw = setting.partition("=")
if name not in fields:
raise typer.BadParameter(
f"unknown PGO field {name!r}; known: {', '.join(sorted(fields))}"
)
out[name] = fields[name].annotation(raw) # type: ignore[misc]
if non_planar and "odom_trans_var_z" not in out:
# Explicit --pgo-set wins; and if xy was also set, match that.
out["odom_trans_var_z"] = out.get(
"odom_trans_var_xy", PGOConfig.model_fields["odom_trans_var_xy"].default
)
return out


def _denoise(cloud: PointCloud2 | None) -> PointCloud2 | None:
"""Statistical outlier removal via o3d; drops sparse floaters, keeps colors."""
from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
Expand Down Expand Up @@ -322,6 +347,19 @@ def main(
"--pgo-tol",
help="Spatial dedup tolerance (meters); applies to both raw and --pgo maps. 0 disables dedup (keep every posed frame)",
),
non_planar: bool = typer.Option(
False,
"--non-planar",
help="Trust the odometry's z like its xy. PGO's defaults are tuned for a "
"ground robot that stays level and trust z 100x tighter, which fights the "
"loop closures on anything that climbs (stairs, drones, handheld)",
),
pgo_set: list[str] = typer.Option(
[],
"--pgo-set",
help="Override any PGOConfig field, repeatable: --pgo-set loop_search_radius=4. "
"Wins over --non-planar",
),
block_count: int = typer.Option(
2_000_000, "--block-count", help="VoxelBlockGrid capacity (raw and PGO rebuilds)"
),
Expand Down Expand Up @@ -548,9 +586,10 @@ def _position(obs: Observation[Any]) -> tuple[float, float, float] | None:
pgo_path: list[tuple[float, float, float]] = []
graph: PoseGraph | None = None
if pgo:
print("running PGO twopass map...")
overrides = _pgo_overrides(pgo_set, non_planar)
print(f"running PGO twopass map{f' {overrides}' if overrides else ''}...")
with progress(total, "pgo pass 1 (optimizing)") as bar:
graph = lidar.tap(bar).transform(PGO()).last().data
graph = lidar.tap(bar).transform(PGO(**overrides)).last().data

pgo_path = [
(kf.optimized.translation.x, kf.optimized.translation.y, kf.optimized.translation.z)
Expand Down
23 changes: 23 additions & 0 deletions dimos/mapping/cli/test_cli.py
Original file line number Diff line number Diff line change
Expand Up @@ -241,3 +241,26 @@ def test_view_pc2(tmp_path: Path) -> None:
assert res.returncode == 0, res.stderr
assert "256 points" in res.stdout
assert out.exists() and out.stat().st_size > 0


def test_pgo_overrides() -> None:
"""PGO's defaults assume a level platform; these are the ways past that."""
import pytest as _pytest
import typer as _typer

from dimos.mapping.cli.map import _pgo_overrides

assert _pgo_overrides([]) == {}
# --non-planar means "trust z like xy", so it reads xy rather than hardcoding.
assert _pgo_overrides([], non_planar=True) == {"odom_trans_var_z": 1e-4}
assert _pgo_overrides(["odom_trans_var_xy=1e-3"], non_planar=True) == {
"odom_trans_var_xy": 1e-3,
"odom_trans_var_z": 1e-3,
}
# An explicit setting wins over the flag.
assert _pgo_overrides(["odom_trans_var_z=5e-5"], non_planar=True) == {"odom_trans_var_z": 5e-5}
# Parsed with the field's own type, so a typo is a CLI error, not a config
# that quietly means something else.
assert _pgo_overrides(["loop_search_radius=4"]) == {"loop_search_radius": 4.0}
with _pytest.raises(_typer.BadParameter):
_pgo_overrides(["nope=1"])
155 changes: 155 additions & 0 deletions dimos/mapping/relocalization/fiducial/module.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,155 @@
# 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.

"""Fiducial relocalization: one aggregated tag sighting against the surveyed marker map.

What is here is what only a tag matcher needs - the ``aggregated_detections``
input, the marker map, and the accept bar on the detector's score. Publishing
what comes back is the base module's. No pointcloud premap is needed: the tag's
surveyed pose is the map.
"""

from __future__ import annotations

import json
from pathlib import Path
from typing import Any

import numpy as np
from reactivex.disposable import Disposable

from dimos.core.core import rpc
from dimos.core.stream import In
from dimos.mapping.relocalization.module import (
FRAME_MAP,
FRAME_WORLD,
Config,
RelocalizationModule,
)
from dimos.msgs.geometry_msgs.Transform import Transform
from dimos.msgs.vision_msgs.Detection3D import Detection3D
from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray
from dimos.perception.fiducial.marker_aggregation import matrix_from_pose7
from dimos.utils.data import resolve_named_path
from dimos.utils.logging_config import setup_logger

logger = setup_logger()

MARKER_MAP_SUFFIX = ".json"


class FiducialConfig(Config):
# Surveyed marker map, `marker_id -> map_T_marker`; `.json` is appended if absent. Without one the module runs but never attempts a fix.
marker_map_file: str | None = None
# 0-1, the detector's min(1, 1/noise_scale) on the fused sighting. 0 accepts every one: the bar is unmeasured on our recordings.
min_score: float = 0.0


class FiducialRelocalization(RelocalizationModule):
"""``world_T_map = world_T_marker @ inv(map_T_marker)`` from one aggregated tag sighting."""

config: FiducialConfig
aggregated_detections: In[Detection3DArray]

def __init__(self, **kwargs: Any) -> None:
super().__init__(**kwargs)
self._map_T_marker: dict[int, np.ndarray] = {}

@rpc
def start(self) -> None:
super().start()
if not self.config.marker_map_file:
logger.info("fiducial relocalization disabled (no marker_map_file configured)")
return
path = resolve_named_path(self.config.marker_map_file, MARKER_MAP_SUFFIX)
self._map_T_marker = load_marker_map(path)
logger.info(
"fiducial relocalization started",
marker_map_file=str(path),
n_markers=len(self._map_T_marker),
)
unsub = self.aggregated_detections.subscribe(self._on_aggregated)
self.register_disposable(Disposable(unsub) if callable(unsub) else unsub)

def _on_aggregated(self, msg: Detection3DArray) -> None:
for det in msg.detections[: msg.detections_length]:
if not self.keep_relocalizing():
return
tf = self._fix(det)
if tf is not None:
self.submit(tf, "fiducial")

def _fix(self, det: Detection3D) -> Transform | None:
"""This sighting's ``world -> map``, or ``None`` for an unsurveyed tag or one below the bar."""
marker_id = _marker_id(det)
map_T_marker = None if marker_id is None else self._map_T_marker.get(marker_id)
if map_T_marker is None:
return None
score = det.results[0].hypothesis.score if det.results_length else 0.0
if score < self.config.min_score:
logger.info(
"relocalize fiducial: refused",
marker_id=marker_id,
score=round(score, 2),
min_score=self.config.min_score,
)
return None
logger.info("relocalize fiducial", marker_id=marker_id, score=round(score, 2))
return Transform.from_matrix(
_world_T_marker(det) @ np.linalg.inv(map_T_marker),
frame_id=FRAME_WORLD,
child_frame_id=FRAME_MAP,
)


def _world_T_marker(det: Detection3D) -> np.ndarray:
"""The aggregated marker pose the detector published, as a 4x4."""
c = det.bbox.center
return matrix_from_pose7(
(
c.position.x,
c.position.y,
c.position.z,
c.orientation.x,
c.orientation.y,
c.orientation.z,
c.orientation.w,
)
)


def _marker_id(det: Detection3D) -> int | None:
"""The tag id the detector stamped on ``id``; ``None`` for anything else."""
raw = str(det.id).strip()
return int(raw) if raw.isdigit() else None


def load_marker_map(path: Path) -> dict[int, np.ndarray]:
"""``marker_id -> map_T_marker`` (4x4) from a survey JSON of ``{"markers": {id: {"translation": [x, y, z], "rotation": [x, y, z, w]}}}``."""
markers = (json.loads(path.read_text()) or {}).get("markers") or {}
return {int(mid): _map_T_marker(int(mid), entry) for mid, entry in markers.items()}


def _map_T_marker(marker_id: int, entry: dict[str, Any]) -> np.ndarray:
"""One survey entry as a 4x4, failing loudly on a short vector or an unusable quaternion."""
t, q = entry["translation"], entry["rotation"]
if len(t) != 3 or not np.all(np.isfinite(t)):
raise ValueError(f"marker {marker_id}: translation must be 3 finite values, got {t!r}")
norm = float(np.linalg.norm(q)) if len(q) == 4 else 0.0
# Below unit-quaternion round-off there is no direction to normalize.
if not np.isfinite(norm) or norm < 1e-6:
raise ValueError(
f"marker {marker_id}: rotation must be a usable xyzw quaternion, got {q!r}"
)
return matrix_from_pose7((*t, *q))
105 changes: 105 additions & 0 deletions dimos/mapping/relocalization/fiducial/test_module.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
# Copyright 2026 Dimensional Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from collections.abc import Iterator
import json
from types import SimpleNamespace
from typing import Any

import numpy as np
import pytest
from scipy.spatial.transform import Rotation

from dimos.mapping.relocalization.fiducial.module import FiducialRelocalization, load_marker_map


def pose(xyz: list[float], yaw_deg: float) -> np.ndarray:
T = np.eye(4)
T[:3, :3] = Rotation.from_euler("z", yaw_deg, degrees=True).as_matrix()
T[:3, 3] = xyz
return T


def sighting(marker_id: int, world_T_marker: np.ndarray, score: float) -> Any:
"""What the detector's aggregated_detections carries: id, world pose, score."""
q = Rotation.from_matrix(world_T_marker[:3, :3]).as_quat()
x, y, z = world_T_marker[:3, 3]
center = SimpleNamespace(
position=SimpleNamespace(x=x, y=y, z=z),
orientation=SimpleNamespace(x=q[0], y=q[1], z=q[2], w=q[3]),
)
return SimpleNamespace(
id=str(marker_id),
bbox=SimpleNamespace(center=center),
results_length=1,
results=[SimpleNamespace(hypothesis=SimpleNamespace(score=score))],
)


# Tag 7 was surveyed 3 m +x of the map origin, facing +y. Live, the robot's
# world frame sees the same tag at (1, 2) facing +x, so world and map disagree
# by exactly world_T_marker @ inv(map_T_marker).
MAP_T_MARKER = pose([3.0, -1.0, 0.5], 90.0)
WORLD_T_MARKER = pose([1.0, 2.0, 0.5], 0.0)


@pytest.fixture
def module() -> Iterator[FiducialRelocalization]:
m = FiducialRelocalization()
m._map_T_marker = {7: MAP_T_MARKER}
yield m
m.dispose()


def test_fix_composes_world_to_map_from_one_sighting(module: FiducialRelocalization) -> None:
"""A surveyed tag's live pose against its map pose is the whole fix."""
tf = module._fix(sighting(7, WORLD_T_MARKER, score=0.9))
assert tf is not None
assert (tf.frame_id, tf.child_frame_id) == ("world", "map")
np.testing.assert_allclose(
tf.to_matrix(), WORLD_T_MARKER @ np.linalg.inv(MAP_T_MARKER), atol=1e-9
)


def test_fix_refuses_unsurveyed_tags_and_low_scores(module: FiducialRelocalization) -> None:
"""The relocalizer holds the accept decision: unknown tag or below its own bar is no fix."""
assert module._fix(sighting(8, WORLD_T_MARKER, score=0.9)) is None
module.config.min_score = 0.95
assert module._fix(sighting(7, WORLD_T_MARKER, score=0.9)) is None


def test_relocalize_once_takes_the_first_sighting_only(module: FiducialRelocalization) -> None:
"""Two tags in one burst: the first places the map, the second is not re-earned."""
got: list[Any] = []
module._world_to_map.subscribe(got.append)
module._map_T_marker[9] = pose([0.0, 0.0, 0.0], 0.0)
burst: Any = SimpleNamespace(
detections=[sighting(7, WORLD_T_MARKER, 0.9), sighting(9, WORLD_T_MARKER, 0.9)],
detections_length=2,
)
module._on_aggregated(burst)
assert len(got) == 1 and module.placed


def test_marker_map_loads_poses_and_names_the_bad_entry(tmp_path: Any) -> None:
"""A survey JSON becomes map_T_marker per id; a malformed entry fails naming its marker."""
q = Rotation.from_matrix(MAP_T_MARKER[:3, :3]).as_quat().tolist()
good = {"translation": MAP_T_MARKER[:3, 3].tolist(), "rotation": q}
path = tmp_path / "survey.json"
path.write_text(json.dumps({"markers": {"7": good}}))
np.testing.assert_allclose(load_marker_map(path)[7], MAP_T_MARKER, atol=1e-9)

path.write_text(json.dumps({"markers": {"7": {**good, "rotation": [0, 0, 0, 0]}}}))
with pytest.raises(ValueError, match="marker 7: rotation"):
load_marker_map(path)
Loading
Loading