diff --git a/data/.lfs/mid360_athens_stairs.pc2.lcm.tar.gz b/data/.lfs/mid360_athens_stairs.pc2.lcm.tar.gz new file mode 100644 index 00000000000..163db45b6aa --- /dev/null +++ b/data/.lfs/mid360_athens_stairs.pc2.lcm.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:22ebef00044c18f9ad9cc722048380b486822f2172b3d8c0aa04f046a055b9d2 +size 15043365 diff --git a/dimos/mapping/cli/map.py b/dimos/mapping/cli/map.py index 6fca8a6a2e9..391de2f63d1 100644 --- a/dimos/mapping/cli/map.py +++ b/dimos/mapping/cli/map.py @@ -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 @@ -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)" ), @@ -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) diff --git a/dimos/mapping/cli/test_cli.py b/dimos/mapping/cli/test_cli.py index 53674915c6d..018dcaa4fde 100644 --- a/dimos/mapping/cli/test_cli.py +++ b/dimos/mapping/cli/test_cli.py @@ -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"]) diff --git a/dimos/mapping/relocalization/fiducial/module.py b/dimos/mapping/relocalization/fiducial/module.py new file mode 100644 index 00000000000..7e0541178c9 --- /dev/null +++ b/dimos/mapping/relocalization/fiducial/module.py @@ -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)) diff --git a/dimos/mapping/relocalization/fiducial/test_module.py b/dimos/mapping/relocalization/fiducial/test_module.py new file mode 100644 index 00000000000..b033d75feb3 --- /dev/null +++ b/dimos/mapping/relocalization/fiducial/test_module.py @@ -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) diff --git a/dimos/perception/fiducial/marker_aggregation.py b/dimos/perception/fiducial/marker_aggregation.py new file mode 100644 index 00000000000..1461b7216c7 --- /dev/null +++ b/dimos/perception/fiducial/marker_aggregation.py @@ -0,0 +1,270 @@ +# 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. + +"""One robust pose per tag visit: gate each glimpse, window per tag, Huber IRLS to one, publish once.""" + +from __future__ import annotations + +from collections import defaultdict +from collections.abc import Callable +from dataclasses import dataclass, replace +import math +from typing import TYPE_CHECKING, Any + +import numpy as np +from scipy.spatial.transform import Rotation + +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D +from dimos.protocol.service.spec import BaseConfig +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.memory.type.observation import Observation + from dimos.perception.detection.type.detection3d.marker import Detection3DMarker + +logger = setup_logger() + +Pose7 = tuple[float, float, float, float, float, float, float] # (x, y, z, qx, qy, qz, qw): m, xyzw + +HUBER_ITERATIONS = 5 # IRLS weights settle within ~5 rounds at huber_delta_m scale +# The glimpse where noise_scale == 1: close and sharp. Both terms are squared, so twice the range +# or twice the blur is four times the variance. +REF_DISTANCE_M = 0.4 +REF_REPROJ_PX = 1.0 +MIN_SCALE_DISTANCE_M = 0.2 # nearer than this the range term stops shrinking the variance +MIN_SCALE_REPROJ_PX = 0.5 # sharper than this the blur term stops shrinking the variance +MIN_NOISE_SCALE = 0.25 # floor, so no glimpse claims near-zero variance + + +class AggregationConfig(BaseConfig): + """Per-glimpse gates and the window each tag is fused over; gate values are jnav's (#2587), not re-tuned here.""" + + # deg, line of sight vs tag normal. Past this a planar tag foreshortens until IPPE's two solutions meet and the pose flips. + max_view_angle_deg: float = 45.0 + # px RMS corner reprojection: 2x REF_REPROJ_PX, the sharp-glimpse reference. + max_reproj_px: float = 2.0 + # px per tag side. 36h11 is 8 cells across, so 3 px per cell, the floor for a clean decode. + min_tag_px: float = 24.0 + # A medoid over 2 has nothing to reject; 3 is the first count where an outlier is outvoted. + min_observations: int = 3 + # m per rad in the pose distance: at REF_DISTANCE_M a 1 rad tag tilt moves the implied robot pose ~0.4 m. + rotation_weight_m_per_rad: float = 0.5 + # m residual past which a glimpse is down-weighted: the translation scatter of in-gate glimpses at REF_DISTANCE_M. + huber_delta_m: float = 0.05 + # s back from the newest glimpse. ~10 glimpses at the detector's 0.5 s quality window, and LIO drift over it stays cm-scale. + time_window_s: float = 5.0 + + +@dataclass(frozen=True) +class Glimpse: + """One sighting of a tag; a None quality field disables that gate only.""" + + ts: float + marker_id: int + pose: Pose7 # world_T_marker + distance_m: float | None = None + view_angle_deg: float | None = None + reproj_px: float | None = None + tag_px: float | None = None + + +def matrix_from_pose7(pose: tuple[float, ...]) -> np.ndarray: + """(x, y, z, qx, qy, qz, qw) -> 4x4.""" + T = np.eye(4) + T[:3, 3] = pose[:3] + T[:3, :3] = Rotation.from_quat(pose[3:7]).as_matrix() + return T + + +def tag_side_px(corners_px: np.ndarray) -> float: + """Tag side in px: sqrt of the corner quad's shoelace area.""" + x, y = np.asarray(corners_px, dtype=np.float64).reshape(4, 2).T + return math.sqrt(0.5 * abs(float(x @ np.roll(y, -1) - y @ np.roll(x, -1)))) + + +def glimpse(det: Detection3DMarker, ts: float) -> Glimpse: + """One detection's gate inputs; range and view angle only when the camera transform rode along.""" + c, o = det.center, det.orientation + pose: Pose7 = (c.x, c.y, c.z, o.x, o.y, o.z, o.w) + distance_m = view_angle_deg = None + if det.transform is not None: # smoothing_window > 0 averages the camera transform away + optical_T_marker = np.linalg.inv(det.transform.to_matrix()) @ matrix_from_pose7(pose) + t = optical_T_marker[:3, 3] + distance_m = float(np.linalg.norm(t)) + cos = abs(float(t @ optical_T_marker[:3, 2])) / (distance_m + 1e-9) # tag normal is its z + view_angle_deg = math.degrees(math.acos(min(1.0, cos))) + return Glimpse( + ts, + det.marker_id, + pose, + distance_m, + view_angle_deg, + det.reprojection_error, + tag_side_px(det.corners_px), + ) + + +def noise_scale(distance_m: float | None, reproj_px: float | None) -> float: + """Variance inflation for one fused pose, quadratic in range x blur; None reads as the reference.""" + d = REF_DISTANCE_M if distance_m is None else max(distance_m, MIN_SCALE_DISTANCE_M) + r = REF_REPROJ_PX if reproj_px is None else max(reproj_px, MIN_SCALE_REPROJ_PX) + return max((d / REF_DISTANCE_M) ** 2 * (r / REF_REPROJ_PX) ** 2, MIN_NOISE_SCALE) + + +def gate_reason(g: Glimpse, config: AggregationConfig) -> str | None: + """Why this glimpse is out, or None; a missing quality field skips its gate rather than failing shut.""" + if g.reproj_px is not None and g.reproj_px > config.max_reproj_px: + return "reproj" + if g.tag_px is not None and g.tag_px < config.min_tag_px: + return "small" + if g.view_angle_deg is not None and g.view_angle_deg > config.max_view_angle_deg: + return "oblique" + return None + + +def _pose_distance(a: Pose7, b: Pose7, rotation_weight_m_per_rad: float) -> float: + """Translation plus weighted rotation between two poses, in m.""" + translation_m = float(np.linalg.norm(np.subtract(a[:3], b[:3]))) + rotation_rad = 2.0 * math.acos(min(1.0, abs(float(np.dot(a[3:], b[3:]))))) + return translation_m + rotation_weight_m_per_rad * rotation_rad + + +def _medoid(poses: list[Pose7], rotation_weight_m_per_rad: float) -> Pose7: + """The pose closest to all the others: the robust seed the IRLS refines from.""" + costs = [sum(_pose_distance(p, q, rotation_weight_m_per_rad) for q in poses) for p in poses] + return poses[int(np.argmin(costs))] + + +def _huber_weights(residuals: np.ndarray, delta: float) -> np.ndarray: + """1 inside delta, delta / r past it. https://en.wikipedia.org/wiki/Huber_loss""" + return np.asarray(np.minimum(1.0, delta / np.maximum(residuals, 1e-12))) + + +def _huber_translation(translations: np.ndarray, seed: np.ndarray, delta_m: float) -> np.ndarray: + """IRLS weighted mean from the seed.""" + estimate = seed.copy() + for _ in range(HUBER_ITERATIONS): + w = _huber_weights(np.linalg.norm(translations - estimate, axis=1), delta_m) + estimate = (w[:, None] * translations).sum(0) / w.sum() + return estimate + + +def _huber_rotation(quaternions: np.ndarray, seed: np.ndarray, delta_rad: float) -> np.ndarray: + """IRLS Markley eigen-mean from the seed. https://ntrs.nasa.gov/citations/20070017872""" + # q and -q are one rotation: align every sample to the seed's hemisphere or the mean cancels. + signs = np.sign(quaternions @ seed) + signs[signs == 0] = 1.0 + q = quaternions * signs[:, None] + estimate = seed.copy() + for _ in range(HUBER_ITERATIONS): + w = _huber_weights(2.0 * np.arccos(np.clip(np.abs(q @ estimate), 0.0, 1.0)), delta_rad) + scatter = (w[:, None, None] * np.einsum("ni,nj->nij", q, q)).sum(0) + estimate = np.linalg.eigh(scatter)[1][:, -1] + if estimate @ seed < 0: + estimate = -estimate + return estimate + + +def robust_pose(cluster: list[Glimpse], config: AggregationConfig) -> Pose7: + """One pose for a cluster: the medoid, refined by Huber IRLS on translation and rotation.""" + poses = [g.pose for g in cluster] + seed = _medoid(poses, config.rotation_weight_m_per_rad) + if len(poses) < 2: + return seed + arr = np.array(poses, dtype=np.float64) + t = _huber_translation(arr[:, :3], np.array(seed[:3]), config.huber_delta_m) + # delta in rad, so the rotation residual shares the translation delta's robustness scale + delta_rad = config.huber_delta_m / max(config.rotation_weight_m_per_rad, 1e-9) + q = _huber_rotation(arr[:, 3:], np.array(seed[3:]), delta_rad) + return (*t.tolist(), *q.tolist()) # type: ignore[return-value] + + +def _median(values: list[float | None]) -> float | None: + """Median over the members carrying the field; median, so one in-gate outlier cannot inflate the health signal.""" + present = [v for v in values if v is not None] + return float(np.median(present)) if present else None + + +class TagAggregator: + """The detector's tap: gate each glimpse, window per tag, publish one fused pose per visit.""" + + def __init__( + self, + publish: Callable[[Detection3DArray], Any], + config: AggregationConfig, + world_frame: str = "world", + ) -> None: + self._publish = publish + self._config = config + self._world_frame = world_frame + self._window: dict[int, list[Glimpse]] = defaultdict(list) + self._published: set[int] = ( + set() + ) # tags whose current visit already published; re-armed when the window thins + + def observe(self, g: Glimpse) -> str | None: + """Keep the glimpse in its tag's window, or return why it was gated out.""" + reason = gate_reason(g, self._config) + if reason is not None: + return reason + window = self._window[g.marker_id] + window.append(g) + # Purge against this tag's newest glimpse, not wall time, so a tag out of view keeps its window. + self._window[g.marker_id] = [o for o in window if g.ts - o.ts <= self._config.time_window_s] + return None + + def fuse(self, marker_id: int) -> tuple[Pose7, float, int] | None: + """(pose, score, n) for a tag's window, or None under min_observations.""" + window = self._window.get(marker_id, []) + if len(window) < self._config.min_observations: + return None + scale = noise_scale( + _median([g.distance_m for g in window]), _median([g.reproj_px for g in window]) + ) + return robust_pose(window, self._config), min(1.0, 1.0 / scale), len(window) + + def __call__(self, obs: Observation[Detection3DMarker | None]) -> None: + det = obs.data + if det is None or self.observe(glimpse(det, obs.ts)) is not None: + return # a tag-free frame (DetectMarkers emit_empty_frames) or a gated glimpse + fused = self.fuse(det.marker_id) + if fused is None: + self._published.discard( + det.marker_id + ) # window thinned: the next full window is a new visit + return + if det.marker_id in self._published: + return # same visit: publishing per frame would fire ~2x/s + self._published.add(det.marker_id) + self._publish(self._message(det, *fused)) + + def _message( + self, det: Detection3DMarker, pose: Pose7, score: float, n: int + ) -> Detection3DArray: + """One-detection array carrying the fused world_T_marker, scored.""" + x, y, z, qx, qy, qz, qw = pose + fused = replace( + det, + center=Vector3(x, y, z), + orientation=Quaternion(qx, qy, qz, qw), + transform=None, # fused across frames: no one camera transform holds + confidence=score, # -> hypothesis.score, 0-1 + ) + logger.debug("tag fused", marker_id=det.marker_id, n=n, score=round(score, 2)) + return ImageDetections3D(det.image, [fused]).to_ros_detection3d_array( + frame_id=self._world_frame + ) diff --git a/dimos/perception/fiducial/marker_detection_stream_module.py b/dimos/perception/fiducial/marker_detection_stream_module.py index e200326c75a..e4983f275c1 100644 --- a/dimos/perception/fiducial/marker_detection_stream_module.py +++ b/dimos/perception/fiducial/marker_detection_stream_module.py @@ -39,6 +39,7 @@ from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection3d.marker import Detection3DMarker +from dimos.perception.fiducial.marker_aggregation import AggregationConfig, TagAggregator from dimos.perception.fiducial.marker_pose import camera_optical_frame_id, is_fisheye_model from dimos.perception.fiducial.marker_transformer import DetectMarkers, MarkersPerFrame from dimos.utils.logging_config import setup_logger @@ -61,6 +62,8 @@ class MarkerDetectionStreamModuleConfig(ModuleConfig): speed_limit_max_dps: float = Field(15.0, gt=0.0) tf_lookup_tolerance: float = Field(0.5, ge=0.0) camera_info: CameraInfo | None = None + # Per-glimpse gates + aggregation for aggregated_detections; first pose only after (min_observations - 1) * quality_window_s (1.0 s at defaults), the floor latency of a fix. + aggregation: AggregationConfig = Field(default_factory=AggregationConfig) class MarkerDetectionStreamModule(StreamModule[Image, Detection3DArray]): @@ -71,10 +74,17 @@ class MarkerDetectionStreamModule(StreamModule[Image, Detection3DArray]): color_image: In[Image] tf: In[TFMessage] detections: Out[Detection3DArray] + # One gated, aggregated pose per (marker, visit), not a per-frame mirror; same message type. + aggregated_detections: Out[Detection3DArray] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._warned_distortion_model = False + self._aggregate = TagAggregator( + self.aggregated_detections.publish, + self.config.aggregation, + self.config.world_frame, + ) def pipeline(self, stream: Stream[Image]) -> Stream[Detection3DArray]: result: Stream[Any] = stream.transform( @@ -101,6 +111,8 @@ def pipeline(self, stream: Stream[Image]) -> Stream[Detection3DArray]: ) ), ) + # tap yields every observation unchanged, so OpenCV runs once per frame + markers = markers.tap(self._aggregate) return markers.transform(MarkersPerFrame(frame_id=self.config.world_frame)) def _maybe_warn_distortion(self, camera_info: CameraInfo) -> None: @@ -157,9 +169,9 @@ def start(self) -> None: Module.start(self) data_inputs = {name: port for name, port in self.inputs.items() if port is not self.tf} - if len(data_inputs) != 1 or len(self.outputs) != 1: + if len(data_inputs) != 1 or len(self.outputs) != 2: raise TypeError( - f"{self.__class__.__name__} must have exactly one In and one Out port, " + f"{self.__class__.__name__} must have exactly one In and two Out ports, " f"found {len(data_inputs)} In and {len(self.outputs)} Out" ) diff --git a/dimos/perception/fiducial/test_marker_aggregation.py b/dimos/perception/fiducial/test_marker_aggregation.py new file mode 100644 index 00000000000..d92cc3e1b1f --- /dev/null +++ b/dimos/perception/fiducial/test_marker_aggregation.py @@ -0,0 +1,80 @@ +# 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 typing import Any + +import numpy as np + +from dimos.perception.fiducial.marker_aggregation import ( + AggregationConfig, + Glimpse, + TagAggregator, + gate_reason, + noise_scale, + robust_pose, + tag_side_px, +) + +TRUE = (1.0, 2.0, 0.5, 0.0, 0.0, 0.0, 1.0) +CFG = AggregationConfig() + + +def glimpse(ts: float = 0.0, pose: Any = TRUE, **quality: float) -> Glimpse: + return Glimpse(ts=ts, marker_id=7, pose=tuple(pose), **quality) + + +def test_one_gross_outlier_does_not_move_the_huber_estimate() -> None: + """A plain mean of 20 good glimpses plus one 9 m outlier moves ~0.4 m; Huber IRLS stays at the truth.""" + rng = np.random.default_rng(0) + cluster = [glimpse(pose=np.add(TRUE, [*rng.normal(0, 0.01, 3), 0, 0, 0, 0])) for _ in range(20)] + cluster.append(glimpse(pose=(6.0, 7.0, 8.0, 0.0, 0.0, 0.0, 1.0))) + x, y, z, *q = robust_pose(cluster, CFG) + np.testing.assert_allclose((x, y, z), TRUE[:3], atol=0.02) + assert abs(q[3]) > 0.999 + + +def test_each_gate_fires_on_its_own_field_and_a_missing_field_skips_it() -> None: + assert gate_reason(glimpse(reproj_px=3.0), CFG) == "reproj" + assert gate_reason(glimpse(tag_px=10.0), CFG) == "small" + assert gate_reason(glimpse(view_angle_deg=60.0), CFG) == "oblique" + assert gate_reason(glimpse(), CFG) is None + + +def test_a_fused_pose_needs_min_observations_inside_the_window() -> None: + agg = TagAggregator(lambda _msg: None, CFG) + for ts in (0.0, 1.0): + assert agg.observe(glimpse(ts=ts)) is None + assert agg.fuse(7) is None + agg.observe(glimpse(ts=2.0)) + fused = agg.fuse(7) + assert fused is not None and fused[2] == 3 + np.testing.assert_allclose(fused[0], TRUE, atol=1e-9) + agg.observe(glimpse(ts=2.0 + CFG.time_window_s + 1.0)) # purges the three above + assert agg.fuse(7) is None + + +def test_score_is_one_at_reference_and_falls_with_range_and_blur() -> None: + assert noise_scale(0.4, 1.0) == 1.0 + assert noise_scale(0.8, 1.0) == 4.0 + assert noise_scale(0.4, 2.0) == 4.0 + assert noise_scale(None, None) == 1.0 + agg = TagAggregator(lambda _msg: None, CFG) + for ts in (0.0, 1.0, 2.0): + agg.observe(glimpse(ts=ts, distance_m=0.8, reproj_px=1.0)) + fused = agg.fuse(7) + assert fused is not None and fused[1] == 0.25 + + +def test_tag_side_is_the_root_of_the_quad_area() -> None: + assert tag_side_px(np.array([[0, 0], [10, 0], [10, 10], [0, 10]])) == 10.0 diff --git a/dimos/perception/fiducial/test_marker_detection_stream_module.py b/dimos/perception/fiducial/test_marker_detection_stream_module.py index 59779c5fc24..536aa7675b3 100644 --- a/dimos/perception/fiducial/test_marker_detection_stream_module.py +++ b/dimos/perception/fiducial/test_marker_detection_stream_module.py @@ -14,6 +14,7 @@ from __future__ import annotations +import dataclasses from typing import Any import numpy as np @@ -27,6 +28,7 @@ from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection3d.marker import Detection3DMarker +from dimos.perception.fiducial.marker_aggregation import AggregationConfig, TagAggregator from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_transformer import MarkersPerFrame from dimos.perception.fiducial.test_helpers import ( @@ -94,11 +96,32 @@ def test_marker_detection_stream_module_exposes_single_stream_input() -> None: module = MarkerDetectionStreamModule(marker_length_m=0.18, camera_info=camera_info()) try: assert set(module.inputs) == {"color_image", "tf"} - assert set(module.outputs) == {"detections"} + assert set(module.outputs) == {"detections", "aggregated_detections"} finally: module.stop() +def test_the_tap_publishes_one_aggregated_pose_per_visit() -> None: + """Four glimpses of one tag publish once, on real detections rather than a stand-in.""" + image = blank_image(ts=10.0) + # The camera sits at (1, 2, 3) with identity rotation, so a tag 0.5 m up its +z axis is + # head-on and clears the view gate; _marker's own (7, 2, 3) is 90 deg off and would not. + head_on = dataclasses.replace(_marker(image, 7), center=Vector3(1.0, 2.0, 3.5)) + + published: list[Detection3DArray] = [] + aggregate = TagAggregator(published.append, AggregationConfig(), "world") + for obs_id in range(1, 5): + aggregate(_marker_obs(image, head_on, obs_id=obs_id, marker_count=1)) + + assert len(published) == 1 # min_observations fuses on the third, the fourth is the same visit + msg = published[0] + assert msg.header.frame_id == "world" + assert msg.detections_length == 1 + assert msg.detections[0].id == "7" + # 0.5 m and a sharp 0.01 px reprojection both sit under the reference, so the score saturates. + assert msg.detections[0].results[0].hypothesis.score == pytest.approx(1.0) + + def test_markers_per_frame_groups_markers_and_preserves_empty_frames() -> None: image = blank_image(ts=10.0) empty_image = blank_image(ts=11.0) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 40e2751a0a2..fa474b38657 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -130,6 +130,7 @@ "unitree-go2-cockpit": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_cockpit:unitree_go2_cockpit", "unitree-go2-coordinator": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_coordinator:unitree_go2_coordinator", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", + "unitree-go2-fiducial-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_fiducial_relocalization", "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet", "unitree-go2-holonomic-benchmark": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_holonomic_benchmark:unitree_go2_holonomic_benchmark", "unitree-go2-holonomic-controller": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_holonomic_controller:unitree_go2_holonomic_controller", @@ -199,6 +200,7 @@ "eval-module": "dimos.evals.module.EvalModule", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder", + "fiducial-relocalization": "dimos.mapping.relocalization.fiducial.module.FiducialRelocalization", "front-camera": "dimos.teleop.hosted.blueprints.cloudflare.FrontCamera", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py index 392258b3b4b..f9feebff07e 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py @@ -19,6 +19,7 @@ from dimos.core.stream import In from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import CostMapper +from dimos.mapping.relocalization.fiducial.module import FiducialRelocalization from dimos.mapping.relocalization.go2.module import Go2Relocalization from dimos.mapping.voxels.module import VoxelGridMapper from dimos.memory.module import Recorder, RecorderConfig, pose_setter_for @@ -101,6 +102,12 @@ async def _lidar_pose(self, msg: PointCloud2) -> Pose | None: Go2Relocalization.blueprint(), ).global_config(n_workers=11) +# Tag sightings only, no pointcloud premap: `-o fiducialrelocalization.marker_map_file=`. +unitree_go2_fiducial_relocalization = autoconnect( + unitree_go2_markers, + FiducialRelocalization.blueprint(), +).global_config(n_workers=12) + unitree_go2_memory = autoconnect( unitree_go2, Go2Memory.blueprint(),