From ae5bde8a652c1066786d4a8e599e92569510a513 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 2 Sep 2026 23:26:26 +0300 Subject: [PATCH 1/5] map: --non-planar for PGO, and the athens stairs premap it builds PGOConfig's odometry priors are tuned for a Go2-class ground robot and trust relative z 100x tighter than xy - the comment in the config says so. The map CLI constructed PGO() with no arguments, so there was no way past that short of editing source, and on a stairs walk it is backwards: z is the axis a lidar-inertial estimate drifts on worst, so loop closures that need to correct height were being resisted. --non-planar trusts z like xy. --pgo-set name=value overrides any PGOConfig field for everything else (loop_search_radius is the next suspect on switchback stairs), parsed against the field's own type so a typo is a CLI error rather than a config that quietly means something else. Measured on athens: the two maps differ by 4.3 cm median, 17 cm p90, 2.5 m max, and the climb loses ~1 m of spurious height at the top. Ivan confirmed the loosened one is the better map by eye. data/mid360_athens_stairs.pc2.lcm is that map: dimos map global mid360_athens_stairs --lidar pointlio_lidar \ --voxel 0.005 --export --denoise --non-planar --- data/.lfs/mid360_athens_stairs.pc2.lcm.tar.gz | 3 ++ dimos/mapping/cli/map.py | 43 ++++++++++++++++++- dimos/mapping/cli/test_cli.py | 23 ++++++++++ 3 files changed, 67 insertions(+), 2 deletions(-) create mode 100644 data/.lfs/mid360_athens_stairs.pc2.lcm.tar.gz 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"]) From e2cec478e9c3fb0f768aece8973b0c8a5dc5131f Mon Sep 17 00:00:00 2001 From: Aaryan Agrawal Date: Wed, 2 Sep 2026 16:06:12 -0700 Subject: [PATCH 2/5] reloc: fiducial relocalization as a sibling of lidar --- .../mapping/relocalization/fiducial/module.py | 132 ++++++++ .../relocalization/fiducial/test_module.py | 91 ++++++ .../perception/fiducial/marker_aggregation.py | 288 ++++++++++++++++++ .../marker_detection_stream_module.py | 22 +- dimos/perception/fiducial/marker_map.py | 100 ++++++ .../perception/fiducial/marker_transformer.py | 65 ++++ .../test_marker_detection_stream_module.py | 2 +- dimos/robot/all_blueprints.py | 2 + .../go2/blueprints/smart/unitree_go2.py | 7 + 9 files changed, 705 insertions(+), 4 deletions(-) create mode 100644 dimos/mapping/relocalization/fiducial/module.py create mode 100644 dimos/mapping/relocalization/fiducial/test_module.py create mode 100644 dimos/perception/fiducial/marker_aggregation.py create mode 100644 dimos/perception/fiducial/marker_map.py diff --git a/dimos/mapping/relocalization/fiducial/module.py b/dimos/mapping/relocalization/fiducial/module.py new file mode 100644 index 00000000000..512f4a63780 --- /dev/null +++ b/dimos/mapping/relocalization/fiducial/module.py @@ -0,0 +1,132 @@ +# 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 + +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.perception.fiducial.marker_map import MARKER_MAP_SUFFIX, load_marker_map +from dimos.utils.data import resolve_named_path +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +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 + # Accept bar on the aggregated sighting's score, 0-1 (the detector's + # min(1, 1/noise_scale)). 0 accepts every aggregated sighting; the bar is unmeasured. + 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 = { + marker_id: tf.to_matrix() for marker_id, tf in load_marker_map(path).items() + } + 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 + c = det.bbox.center # world_T_marker, aggregated by the detector + world_T_marker = matrix_from_pose7( + ( + c.position.x, + c.position.y, + c.position.z, + c.orientation.x, + c.orientation.y, + c.orientation.z, + c.orientation.w, + ) + ) + logger.info("relocalize fiducial", marker_id=marker_id, score=round(score, 2)) + return Transform.from_matrix( + world_T_marker @ np.linalg.inv(map_T_marker), + frame_id=FRAME_WORLD, + child_frame_id=FRAME_MAP, + ) + + +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 diff --git a/dimos/mapping/relocalization/fiducial/test_module.py b/dimos/mapping/relocalization/fiducial/test_module.py new file mode 100644 index 00000000000..4e406a5af37 --- /dev/null +++ b/dimos/mapping/relocalization/fiducial/test_module.py @@ -0,0 +1,91 @@ +# 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 +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 + + +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 diff --git a/dimos/perception/fiducial/marker_aggregation.py b/dimos/perception/fiducial/marker_aggregation.py new file mode 100644 index 00000000000..4c66660e730 --- /dev/null +++ b/dimos/perception/fiducial/marker_aggregation.py @@ -0,0 +1,288 @@ +# 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. + +"""Robust multi-sighting fiducial-marker pose aggregation: time-cluster same-marker sightings, gate each, reduce each cluster to one pose via Huber-weighted IRLS (weighted-mean translation + Markley quaternion eigen-mean).""" + +from __future__ import annotations + +from collections import defaultdict +from dataclasses import dataclass +import math +from typing import TYPE_CHECKING + +import numpy as np +from scipy.spatial.transform import Rotation + +if TYPE_CHECKING: + from dimos.perception.detection.type.detection3d.marker import Detection3DMarker + +# Frame-invariant marker pose 7-vector: (x, y, z, qx, qy, qz, qw), meters + xyzw. +Pose7 = tuple[float, float, float, float, float, float, float] + +_HUBER_ITERATIONS = 5 # IRLS weights settle within ~5 iters at huber_delta_m scale + +# Reference geometry where tag_noise_scale == 1.0 (pose carries unscaled covariance). +REF_DISTANCE_M = 0.4 # camera->tag range a close, in-gate glimpse sits at +REF_REPROJ_PX = 1.0 # RMS corner misfit of a sharp glimpse +MIN_SCALE_DISTANCE_M = 0.2 # m; nearer than this the range term stops shrinking variance +MIN_SCALE_REPROJ_PX = 0.5 # px; sharper than this the reproj term stops shrinking variance +MIN_TAG_NOISE_SCALE = 0.25 # dimensionless floor, so no read claims near-zero variance + + +@dataclass(frozen=True) +class AggregationConfig: + """Per-glimpse gate thresholds (see gate_reason) + clustering/aggregation knobs.""" + + max_view_angle_deg: float = 45.0 # line-of-sight vs tag normal; grazing views mis-solve + max_reproj_px: float = 2.0 # RMS solvePnP corner reprojection error + min_tag_px: float = ( + 24.0 # tag side in px (sqrt of quad area); bounds range and tag size at once + ) + min_observations: int = 3 # clusters thinner than this are unreliable + rotation_weight_m_per_rad: float = 0.5 # 1 rad of rot ~ 0.5 m of trans in pose distance + huber_delta_m: float = 0.05 # residual (m) past which a sample is down-weighted + time_window_s: float = 5.0 # streaming: span back from the newest glimpse, not a sightings gap + + +@dataclass(frozen=True) +class TagObservation: + """One gated tag glimpse; a ``None`` quality field disables ITS gate only.""" + + ts: float + marker_id: int + pose: Pose7 + distance_m: float | None = None + view_angle_deg: float | None = None + reproj_px: float | None = None + tag_px: float | None = None + + +@dataclass(frozen=True) +class TagEstimate: + """One robust aggregated pose for a marker over a cluster/window of glimpses.""" + + pose: Pose7 # aggregated 7-vec, obs' frame + n_observations: int + # Cluster MEDIANS, not the last glimpse's -- health of the whole cluster's pose. + distance_m: float | None = None + reproj_px: float | None = None + + +def matrix_from_pose7(pose: tuple[float, ...] | list[float]) -> np.ndarray: + """``(x, y, z, qx, qy, qz, qw)`` -> 4x4 homogeneous transform.""" + matrix = np.eye(4) + matrix[:3, 3] = pose[:3] + matrix[:3, :3] = Rotation.from_quat(pose[3:7]).as_matrix() + return matrix + + +def camera_relative_quality( + det: Detection3DMarker, world_T_marker: Pose7 +) -> tuple[float | None, float | None]: + """``(distance_m, view_angle_deg)`` of this glimpse camera-relative, 0 deg == head-on; ``(None, None)`` when no camera transform rode along.""" + if det.transform is None: + # smoothing_window > 0 emits an averaged pose with transform=None, so the two camera-relative gates go quiet + return None, None + # det.transform IS world_T_optical; this inverse exactly undoes the compose that built world_T_marker. + optical_T_marker = np.linalg.inv(det.transform.to_matrix()) @ matrix_from_pose7(world_T_marker) + translation = optical_T_marker[:3, 3] + distance_m = float(np.linalg.norm(translation)) + line_of_sight = translation / (distance_m + 1e-9) # m; guards zero range + cos_angle = abs(float(line_of_sight @ optical_T_marker[:3, 2])) # tag normal IS the pose z axis + return distance_m, math.degrees(math.acos(min(1.0, cos_angle))) + + +def marker_pose7(det: Detection3DMarker) -> Pose7: + """``world_T_marker`` 7-vec off one detection.""" + c, o = det.center, det.orientation + return (c.x, c.y, c.z, o.x, o.y, o.z, o.w) + + +def tag_observation(det: Detection3DMarker, ts: float, pose: Pose7) -> TagObservation: + """One glimpse's gate inputs; ``pose`` is the frame it aggregates in -- world live, PGO-corrected in the survey.""" + distance_m, view_angle_deg = camera_relative_quality(det, marker_pose7(det)) + return TagObservation( + ts=ts, + marker_id=det.marker_id, + pose=pose, + distance_m=distance_m, + view_angle_deg=view_angle_deg, + reproj_px=det.reprojection_error, + tag_px=tag_side_px(det.corners_px), + ) + + +def tag_side_px(corners_px: np.ndarray) -> float: + """Tag side length in pixels: sqrt of the corner quad's image area (shoelace).""" + quad = np.asarray(corners_px, dtype=np.float64).reshape(4, 2) + x, y = quad[:, 0], quad[:, 1] + # shoelace: matches cv2.contourArea for a simple quad, keeps this core OpenCV-free + area = 0.5 * abs(float(np.dot(x, np.roll(y, -1)) - np.dot(y, np.roll(x, -1)))) + return math.sqrt(area) + + +def tag_noise_scale(distance_m: float | None, reproj_px: float | None) -> float: + """Dimensionless variance inflation for one aggregated tag pose (jnav tag_noise), quadratic in range x reproj_px; a ``None`` input reads as the reference.""" + distance_m = REF_DISTANCE_M if distance_m is None else distance_m + reproj_px = REF_REPROJ_PX if reproj_px is None else reproj_px + range_term = (max(distance_m, MIN_SCALE_DISTANCE_M) / REF_DISTANCE_M) ** 2 + reproj_term = (max(reproj_px, MIN_SCALE_REPROJ_PX) / REF_REPROJ_PX) ** 2 + return max(range_term * reproj_term, MIN_TAG_NOISE_SCALE) + + +def gate_reason(obs: TagObservation, config: AggregationConfig) -> str | None: + """Rejection reason, or ``None`` if the glimpse clears every gate whose input is present (a ``None`` field skips ITS gate rather than failing shut).""" + if obs.reproj_px is not None and obs.reproj_px > config.max_reproj_px: + return "reproj" + if obs.tag_px is not None and obs.tag_px < config.min_tag_px: + return "small" + if obs.view_angle_deg is not None and obs.view_angle_deg > config.max_view_angle_deg: + return "oblique" + return None + + +def _pose_distance( + a: tuple[float, ...], b: tuple[float, ...], rotation_weight_m_per_rad: float +) -> float: + """Combined translation + weighted-rotation distance between two 7-vec poses.""" + translation_m = float(np.linalg.norm(np.array(a[:3]) - np.array(b[:3]))) + rotation_rad = 2.0 * math.acos(min(1.0, abs(float(np.dot(a[3:7], b[3:7]))))) + return translation_m + rotation_weight_m_per_rad * rotation_rad + + +def cluster_medoid( + cluster: list[TagObservation], rotation_weight_m_per_rad: float +) -> TagObservation: + """The observation whose pose is most central (min total pose-distance) -- a robust seed for the Huber refinement.""" + poses = [obs.pose for obs in cluster] + best_index, best_cost = 0, float("inf") + for i in range(len(poses)): + cost = sum( + _pose_distance(poses[i], poses[j], rotation_weight_m_per_rad) + for j in range(len(poses)) + if j != i + ) + if cost < best_cost: + best_cost, best_index = cost, i + return cluster[best_index] + + +def _huber_weights(residuals: np.ndarray, delta: float) -> np.ndarray: + """IRLS Huber weights: 1 inside ``delta``, decaying as ``delta / r`` past it. https://en.wikipedia.org/wiki/Huber_loss""" + weights = np.ones_like(residuals) + outside = residuals > delta + weights[outside] = ( + delta / residuals[outside] + ) # Huber IRLS https://en.wikipedia.org/wiki/Huber_loss + return weights + + +def robust_cluster_pose( + cluster: list[TagObservation], + rotation_weight_m_per_rad: float, + huber_delta_m: float, +) -> Pose7: + """Cluster representative: the medoid refined by Huber-weighted IRLS -- weighted-mean translation + Markley quaternion eigen-mean https://ntrs.nasa.gov/citations/20070017872.""" + medoid = cluster_medoid(cluster, rotation_weight_m_per_rad) + if len(cluster) < 2: + return medoid.pose + poses = np.array([obs.pose for obs in cluster], dtype=np.float64) + translations, quaternions = poses[:, :3], poses[:, 3:7] + # Sign-align to the medoid hemisphere: q and -q are one rotation, else the mean cancels. + reference = np.array(medoid.pose[3:7], dtype=np.float64) + signs = np.sign(quaternions @ reference) + signs[signs == 0] = 1.0 + quaternions = quaternions * signs[:, None] + estimate_translation = np.array(medoid.pose[:3], dtype=np.float64) + estimate_quaternion = reference.copy() + # delta in radians so the rotation residual shares the translation delta's robustness scale. + delta_rad = huber_delta_m / max(rotation_weight_m_per_rad, 1e-9) + for _ in range(_HUBER_ITERATIONS): + weights_t = _huber_weights( + np.linalg.norm(translations - estimate_translation, axis=1), huber_delta_m + ) + estimate_translation = (weights_t[:, None] * translations).sum(0) / weights_t.sum() + angular_residual = 2.0 * np.arccos( + np.clip(np.abs(quaternions @ estimate_quaternion), 0.0, 1.0) + ) + weights_r = _huber_weights(angular_residual, delta_rad) + scatter = ( + weights_r[:, None, None] * np.einsum("ni,nj->nij", quaternions, quaternions) + ).sum(0) + estimate_quaternion = np.linalg.eigh(scatter)[1][ + :, -1 + ] # Markley quaternion eigen-mean (2007) https://ntrs.nasa.gov/citations/20070017872 + if estimate_quaternion @ reference < 0: + estimate_quaternion = -estimate_quaternion + aggregated = (*estimate_translation.tolist(), *estimate_quaternion.tolist()) + return aggregated # type: ignore[return-value] + + +def _median_present(values: list[float | None]) -> float | None: + """Median over the members that carry the field, ``None`` when none do -- median not mean, 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 + + +def aggregate_by_marker_id( + observations: list[TagObservation], config: AggregationConfig +) -> dict[int, tuple[Pose7, int]]: + """Batch: group observations by marker_id and fuse each group to one robust pose; id -> (7-vec, n).""" + by_id: dict[int, list[TagObservation]] = defaultdict(list) + for obs in observations: + by_id[obs.marker_id].append(obs) + return { + marker_id: ( + robust_cluster_pose(group, config.rotation_weight_m_per_rad, config.huber_delta_m), + len(group), + ) + for marker_id, group in by_id.items() + } + + +class TagAggregator: + """Streaming robust pose per marker over a sliding time window.""" + + def __init__(self, config: AggregationConfig) -> None: + self._config = config + self._by_marker: dict[int, list[TagObservation]] = defaultdict(list) + + def observe(self, obs: TagObservation) -> str | None: + """Feed one glimpse; return its rejection reason, or ``None`` if kept.""" + reason = gate_reason(obs, self._config) + if reason is not None: + return reason + buf = self._by_marker[obs.marker_id] + buf.append(obs) + # Purge relative to this marker's newest glimpse, not wall time, so a paused marker keeps its window. + newest = obs.ts + self._by_marker[obs.marker_id] = [ + o for o in buf if newest - o.ts <= self._config.time_window_s + ] + return None + + def robust_estimate(self, marker_id: int) -> TagEstimate | None: + """Huber-aggregated pose for a marker's in-window glimpses, or ``None`` when fewer than ``min_observations`` are available.""" + buf = self._by_marker.get(marker_id, []) + if len(buf) < self._config.min_observations: + return None + pose = robust_cluster_pose( + buf, self._config.rotation_weight_m_per_rad, self._config.huber_delta_m + ) + return TagEstimate( + pose=pose, + n_observations=len(buf), + distance_m=_median_present([o.distance_m for o in buf]), + reproj_px=_median_present([o.reproj_px for o in buf]), + ) diff --git a/dimos/perception/fiducial/marker_detection_stream_module.py b/dimos/perception/fiducial/marker_detection_stream_module.py index e200326c75a..6f6aab2163d 100644 --- a/dimos/perception/fiducial/marker_detection_stream_module.py +++ b/dimos/perception/fiducial/marker_detection_stream_module.py @@ -39,8 +39,13 @@ 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 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.perception.fiducial.marker_transformer import ( + AggregateTagBursts, + DetectMarkers, + MarkersPerFrame, +) from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -61,6 +66,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 +78,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 = AggregateTagBursts( + 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 +115,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 +173,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/marker_map.py b/dimos/perception/fiducial/marker_map.py new file mode 100644 index 00000000000..f7d5591747a --- /dev/null +++ b/dimos/perception/fiducial/marker_map.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +# 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. + +"""Survey marker map file I/O: ``map_T_tag`` per id plus the tag size it was solved at.""" + +from __future__ import annotations + +import json +from pathlib import Path +from typing import Any + +import numpy as np + +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.perception.fiducial.marker_aggregation import Pose7 + +MAP_FRAME = "map" +MARKER_MAP_SUFFIX = ".json" # what write_marker_map emits and load_marker_map parses + + +def _validated_entry(marker_id: int, entry: dict[str, Any]) -> Transform: + """Validate one survey entry into a ``map_T_marker`` Transform, failing loudly on a short translation or zero-norm quaternion.""" + translation, rotation = entry["translation"], entry["rotation"] + if not isinstance(translation, (list, tuple)) or len(translation) != 3: + raise ValueError(f"marker {marker_id}: translation must be [x, y, z], got {translation!r}") + if not isinstance(rotation, (list, tuple)) or len(rotation) != 4: + raise ValueError(f"marker {marker_id}: rotation must be [x, y, z, w], got {rotation!r}") + if not np.all(np.isfinite(np.asarray(translation, dtype=np.float64))): + raise ValueError(f"marker {marker_id}: translation must be finite, got {translation!r}") + norm = float(np.linalg.norm(np.asarray(rotation, dtype=np.float64))) + if ( + not np.isfinite(norm) or norm < 1e-6 + ): # 1e-6: unnormalizable, floor below a unit quaternion's round-off + raise ValueError(f"marker {marker_id}: rotation quaternion norm is {norm}, not usable") + return Transform( + translation=Vector3(*translation), + rotation=Quaternion(*rotation), + frame_id=MAP_FRAME, + child_frame_id=f"marker_{marker_id}", + ) + + +def load_marker_map(path: str | Path) -> dict[int, Transform]: + """``marker_id -> map_T_marker`` from a survey JSON (the format ``write_marker_map`` emits).""" + data = json.loads(Path(path).read_text()) or {} + return { + int(marker_id): _validated_entry(int(marker_id), entry) + for marker_id, entry in (data.get("markers", {}) or {}).items() + } + + +def marker_length_m_from_map(path: str | Path) -> float | None: + """The printed tag edge length the survey solved at, or ``None`` when the map carries none.""" + meta = (json.loads(Path(path).read_text()) or {}).get("meta") or {} + size = meta.get("marker_length_m") + if size is None: + return None + if float(size) <= 0: + raise ValueError(f"{path}: marker_length_m must be > 0, got {size}") + return float(size) + + +def write_marker_map( + path: Path, aggregated: dict[int, tuple[Pose7, int]], *, source: str, marker_length_m: float +) -> None: + """Serialize aggregated ``map_T_tag`` poses to the JSON ``load_marker_map`` reads.""" + doc = { + "meta": { + "schema": "map_T_tag", + "source_recording": source, + # every pose below is metric only at this tag size, so the live detector reads it back rather than being told twice + "marker_length_m": marker_length_m, + "n_detections_aggregated": { + str(mid): n for mid, (_pose, n) in sorted(aggregated.items()) + }, + }, + "markers": { + str(marker_id): { + "translation": [pose[0], pose[1], pose[2]], + "rotation": [pose[3], pose[4], pose[5], pose[6]], + } + for marker_id, (pose, _n) in sorted(aggregated.items()) + }, + } + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(json.dumps(doc, indent=2)) diff --git a/dimos/perception/fiducial/marker_transformer.py b/dimos/perception/fiducial/marker_transformer.py index 0a0f7bfd036..13618a04e09 100644 --- a/dimos/perception/fiducial/marker_transformer.py +++ b/dimos/perception/fiducial/marker_transformer.py @@ -45,6 +45,14 @@ from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.detection.type.detection3d.marker import Detection3DMarker +from dimos.perception.fiducial.marker_aggregation import ( + AggregationConfig, + TagAggregator, + TagEstimate, + marker_pose7, + tag_noise_scale, + tag_observation, +) from dimos.perception.fiducial.marker_detect import ( detect_markers_in_image as _detect_markers_in_image, ) @@ -410,3 +418,60 @@ def _source_pose( transform.rotation.w, ) return obs.pose + + +class AggregateTagBursts: + """Gate each tag glimpse and publish ONE robustly-aggregated pose per tag visit, edge-triggered.""" + + def __init__( + self, + publish: Callable[[Detection3DArray], Any], + config: AggregationConfig, + world_frame: str = "world", + ) -> None: + self._publish = publish + self._world_frame = world_frame + self._aggregator = TagAggregator(config) + # markers whose CURRENT burst already published; a marker leaves the set once its window thins under min_observations (left view > time_window_s), re-arming the edge + self._burst_counted: set[int] = set() + + def __call__(self, obs: Observation[Detection3DMarker | None]) -> None: + det = obs.data + if det is None: + return # DetectMarkers(emit_empty_frames=True) sentinel: a tag-free frame + + reason = self._aggregator.observe(tag_observation(det, obs.ts, marker_pose7(det))) + if reason is not None: + return + estimate = self._aggregator.robust_estimate(det.marker_id) + if estimate is None: + self._burst_counted.discard(det.marker_id) # window thinned -> re-arm the edge + return + if det.marker_id in self._burst_counted: + return # same burst still satisfied: publishing per frame would fire ~2x/s + self._burst_counted.add(det.marker_id) + self._publish(self._aggregated_array(det, estimate)) + + def _aggregated_array(self, det: Detection3DMarker, estimate: TagEstimate) -> Detection3DArray: + """One-detection array with this tag's aggregated world_T_marker, scored by its noise scale.""" + scale = tag_noise_scale(estimate.distance_m, estimate.reproj_px) + x, y, z, qx, qy, qz, qw = estimate.pose + aggregated = dataclasses.replace( + det, + center=Vector3(x, y, z), + orientation=Quaternion(qx, qy, qz, qw), + transform=None, # aggregated across many frames: no ONE camera transform holds + confidence=min(1.0, 1.0 / scale), # -> hypothesis.score, dimensionless 0-1 + ) + msg = ImageDetections3D(det.image, [aggregated]).to_ros_detection3d_array( + frame_id=self._world_frame + ) + logger.debug( + "tag burst aggregated", + marker_id=det.marker_id, + n_obs=estimate.n_observations, + distance_m=None if estimate.distance_m is None else round(estimate.distance_m, 2), + reproj_px=None if estimate.reproj_px is None else round(estimate.reproj_px, 2), + score=round(aggregated.confidence, 2), + ) + return msg diff --git a/dimos/perception/fiducial/test_marker_detection_stream_module.py b/dimos/perception/fiducial/test_marker_detection_stream_module.py index 59779c5fc24..8ec3d08be63 100644 --- a/dimos/perception/fiducial/test_marker_detection_stream_module.py +++ b/dimos/perception/fiducial/test_marker_detection_stream_module.py @@ -94,7 +94,7 @@ 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() 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(), From 6a97ccf3c7d96f20361f49f28e7a5f6e4a1be905 Mon Sep 17 00:00:00 2001 From: Aaryan Agrawal Date: Wed, 2 Sep 2026 21:40:03 -0700 Subject: [PATCH 3/5] reloc: rewrite tag aggregation, fold the marker map into the relocalizer --- .../mapping/relocalization/fiducial/module.py | 35 +- .../relocalization/fiducial/test_module.py | 16 +- .../perception/fiducial/marker_aggregation.py | 406 +++++++++--------- .../marker_detection_stream_module.py | 10 +- dimos/perception/fiducial/marker_map.py | 100 ----- .../perception/fiducial/marker_transformer.py | 65 --- .../fiducial/test_marker_aggregation.py | 80 ++++ 7 files changed, 319 insertions(+), 393 deletions(-) delete mode 100644 dimos/perception/fiducial/marker_map.py create mode 100644 dimos/perception/fiducial/test_marker_aggregation.py diff --git a/dimos/mapping/relocalization/fiducial/module.py b/dimos/mapping/relocalization/fiducial/module.py index 512f4a63780..f90fd405562 100644 --- a/dimos/mapping/relocalization/fiducial/module.py +++ b/dimos/mapping/relocalization/fiducial/module.py @@ -22,6 +22,8 @@ from __future__ import annotations +import json +from pathlib import Path from typing import Any import numpy as np @@ -39,19 +41,18 @@ 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.perception.fiducial.marker_map import MARKER_MAP_SUFFIX, load_marker_map 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. + # 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 - # Accept bar on the aggregated sighting's score, 0-1 (the detector's - # min(1, 1/noise_scale)). 0 accepts every aggregated sighting; the bar is unmeasured. + # 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 @@ -72,9 +73,7 @@ def start(self) -> None: 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 = { - marker_id: tf.to_matrix() for marker_id, tf in load_marker_map(path).items() - } + self._map_T_marker = load_marker_map(path) logger.info( "fiducial relocalization started", marker_map_file=str(path), @@ -130,3 +129,23 @@ 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 index 4e406a5af37..b033d75feb3 100644 --- a/dimos/mapping/relocalization/fiducial/test_module.py +++ b/dimos/mapping/relocalization/fiducial/test_module.py @@ -13,6 +13,7 @@ # limitations under the License. from collections.abc import Iterator +import json from types import SimpleNamespace from typing import Any @@ -20,7 +21,7 @@ import pytest from scipy.spatial.transform import Rotation -from dimos.mapping.relocalization.fiducial.module import FiducialRelocalization +from dimos.mapping.relocalization.fiducial.module import FiducialRelocalization, load_marker_map def pose(xyz: list[float], yaw_deg: float) -> np.ndarray: @@ -89,3 +90,16 @@ def test_relocalize_once_takes_the_first_sighting_only(module: FiducialRelocaliz ) 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 index 4c66660e730..1461b7216c7 100644 --- a/dimos/perception/fiducial/marker_aggregation.py +++ b/dimos/perception/fiducial/marker_aggregation.py @@ -12,277 +12,259 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Robust multi-sighting fiducial-marker pose aggregation: time-cluster same-marker sightings, gate each, reduce each cluster to one pose via Huber-weighted IRLS (weighted-mean translation + Markley quaternion eigen-mean).""" +"""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 dataclasses import dataclass +from collections.abc import Callable +from dataclasses import dataclass, replace import math -from typing import TYPE_CHECKING +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 -# Frame-invariant marker pose 7-vector: (x, y, z, qx, qy, qz, qw), meters + xyzw. -Pose7 = tuple[float, float, float, float, float, float, float] +logger = setup_logger() -_HUBER_ITERATIONS = 5 # IRLS weights settle within ~5 iters at huber_delta_m scale +Pose7 = tuple[float, float, float, float, float, float, float] # (x, y, z, qx, qy, qz, qw): m, xyzw -# Reference geometry where tag_noise_scale == 1.0 (pose carries unscaled covariance). -REF_DISTANCE_M = 0.4 # camera->tag range a close, in-gate glimpse sits at -REF_REPROJ_PX = 1.0 # RMS corner misfit of a sharp glimpse -MIN_SCALE_DISTANCE_M = 0.2 # m; nearer than this the range term stops shrinking variance -MIN_SCALE_REPROJ_PX = 0.5 # px; sharper than this the reproj term stops shrinking variance -MIN_TAG_NOISE_SCALE = 0.25 # dimensionless floor, so no read claims near-zero variance +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 -@dataclass(frozen=True) -class AggregationConfig: - """Per-glimpse gate thresholds (see gate_reason) + clustering/aggregation knobs.""" +class AggregationConfig(BaseConfig): + """Per-glimpse gates and the window each tag is fused over; gate values are jnav's (#2587), not re-tuned here.""" - max_view_angle_deg: float = 45.0 # line-of-sight vs tag normal; grazing views mis-solve - max_reproj_px: float = 2.0 # RMS solvePnP corner reprojection error - min_tag_px: float = ( - 24.0 # tag side in px (sqrt of quad area); bounds range and tag size at once - ) - min_observations: int = 3 # clusters thinner than this are unreliable - rotation_weight_m_per_rad: float = 0.5 # 1 rad of rot ~ 0.5 m of trans in pose distance - huber_delta_m: float = 0.05 # residual (m) past which a sample is down-weighted - time_window_s: float = 5.0 # streaming: span back from the newest glimpse, not a sightings gap + # 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 TagObservation: - """One gated tag glimpse; a ``None`` quality field disables ITS gate only.""" +class Glimpse: + """One sighting of a tag; a None quality field disables that gate only.""" ts: float marker_id: int - pose: Pose7 + 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 -@dataclass(frozen=True) -class TagEstimate: - """One robust aggregated pose for a marker over a cluster/window of glimpses.""" +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 - pose: Pose7 # aggregated 7-vec, obs' frame - n_observations: int - # Cluster MEDIANS, not the last glimpse's -- health of the whole cluster's pose. - distance_m: float | None = None - reproj_px: float | None = None + +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 matrix_from_pose7(pose: tuple[float, ...] | list[float]) -> np.ndarray: - """``(x, y, z, qx, qy, qz, qw)`` -> 4x4 homogeneous transform.""" - matrix = np.eye(4) - matrix[:3, 3] = pose[:3] - matrix[:3, :3] = Rotation.from_quat(pose[3:7]).as_matrix() - return matrix - - -def camera_relative_quality( - det: Detection3DMarker, world_T_marker: Pose7 -) -> tuple[float | None, float | None]: - """``(distance_m, view_angle_deg)`` of this glimpse camera-relative, 0 deg == head-on; ``(None, None)`` when no camera transform rode along.""" - if det.transform is None: - # smoothing_window > 0 emits an averaged pose with transform=None, so the two camera-relative gates go quiet - return None, None - # det.transform IS world_T_optical; this inverse exactly undoes the compose that built world_T_marker. - optical_T_marker = np.linalg.inv(det.transform.to_matrix()) @ matrix_from_pose7(world_T_marker) - translation = optical_T_marker[:3, 3] - distance_m = float(np.linalg.norm(translation)) - line_of_sight = translation / (distance_m + 1e-9) # m; guards zero range - cos_angle = abs(float(line_of_sight @ optical_T_marker[:3, 2])) # tag normal IS the pose z axis - return distance_m, math.degrees(math.acos(min(1.0, cos_angle))) - - -def marker_pose7(det: Detection3DMarker) -> Pose7: - """``world_T_marker`` 7-vec off one detection.""" +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 - return (c.x, c.y, c.z, o.x, o.y, o.z, o.w) - - -def tag_observation(det: Detection3DMarker, ts: float, pose: Pose7) -> TagObservation: - """One glimpse's gate inputs; ``pose`` is the frame it aggregates in -- world live, PGO-corrected in the survey.""" - distance_m, view_angle_deg = camera_relative_quality(det, marker_pose7(det)) - return TagObservation( - ts=ts, - marker_id=det.marker_id, - pose=pose, - distance_m=distance_m, - view_angle_deg=view_angle_deg, - reproj_px=det.reprojection_error, - tag_px=tag_side_px(det.corners_px), + 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 tag_side_px(corners_px: np.ndarray) -> float: - """Tag side length in pixels: sqrt of the corner quad's image area (shoelace).""" - quad = np.asarray(corners_px, dtype=np.float64).reshape(4, 2) - x, y = quad[:, 0], quad[:, 1] - # shoelace: matches cv2.contourArea for a simple quad, keeps this core OpenCV-free - area = 0.5 * abs(float(np.dot(x, np.roll(y, -1)) - np.dot(y, np.roll(x, -1)))) - return math.sqrt(area) - - -def tag_noise_scale(distance_m: float | None, reproj_px: float | None) -> float: - """Dimensionless variance inflation for one aggregated tag pose (jnav tag_noise), quadratic in range x reproj_px; a ``None`` input reads as the reference.""" - distance_m = REF_DISTANCE_M if distance_m is None else distance_m - reproj_px = REF_REPROJ_PX if reproj_px is None else reproj_px - range_term = (max(distance_m, MIN_SCALE_DISTANCE_M) / REF_DISTANCE_M) ** 2 - reproj_term = (max(reproj_px, MIN_SCALE_REPROJ_PX) / REF_REPROJ_PX) ** 2 - return max(range_term * reproj_term, MIN_TAG_NOISE_SCALE) - - -def gate_reason(obs: TagObservation, config: AggregationConfig) -> str | None: - """Rejection reason, or ``None`` if the glimpse clears every gate whose input is present (a ``None`` field skips ITS gate rather than failing shut).""" - if obs.reproj_px is not None and obs.reproj_px > config.max_reproj_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 obs.tag_px is not None and obs.tag_px < config.min_tag_px: + if g.tag_px is not None and g.tag_px < config.min_tag_px: return "small" - if obs.view_angle_deg is not None and obs.view_angle_deg > config.max_view_angle_deg: + 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: tuple[float, ...], b: tuple[float, ...], rotation_weight_m_per_rad: float -) -> float: - """Combined translation + weighted-rotation distance between two 7-vec poses.""" - translation_m = float(np.linalg.norm(np.array(a[:3]) - np.array(b[:3]))) - rotation_rad = 2.0 * math.acos(min(1.0, abs(float(np.dot(a[3:7], b[3:7]))))) +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 cluster_medoid( - cluster: list[TagObservation], rotation_weight_m_per_rad: float -) -> TagObservation: - """The observation whose pose is most central (min total pose-distance) -- a robust seed for the Huber refinement.""" - poses = [obs.pose for obs in cluster] - best_index, best_cost = 0, float("inf") - for i in range(len(poses)): - cost = sum( - _pose_distance(poses[i], poses[j], rotation_weight_m_per_rad) - for j in range(len(poses)) - if j != i - ) - if cost < best_cost: - best_cost, best_index = cost, i - return cluster[best_index] +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: - """IRLS Huber weights: 1 inside ``delta``, decaying as ``delta / r`` past it. https://en.wikipedia.org/wiki/Huber_loss""" - weights = np.ones_like(residuals) - outside = residuals > delta - weights[outside] = ( - delta / residuals[outside] - ) # Huber IRLS https://en.wikipedia.org/wiki/Huber_loss - return weights - - -def robust_cluster_pose( - cluster: list[TagObservation], - rotation_weight_m_per_rad: float, - huber_delta_m: float, -) -> Pose7: - """Cluster representative: the medoid refined by Huber-weighted IRLS -- weighted-mean translation + Markley quaternion eigen-mean https://ntrs.nasa.gov/citations/20070017872.""" - medoid = cluster_medoid(cluster, rotation_weight_m_per_rad) - if len(cluster) < 2: - return medoid.pose - poses = np.array([obs.pose for obs in cluster], dtype=np.float64) - translations, quaternions = poses[:, :3], poses[:, 3:7] - # Sign-align to the medoid hemisphere: q and -q are one rotation, else the mean cancels. - reference = np.array(medoid.pose[3:7], dtype=np.float64) - signs = np.sign(quaternions @ reference) + """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 - quaternions = quaternions * signs[:, None] - estimate_translation = np.array(medoid.pose[:3], dtype=np.float64) - estimate_quaternion = reference.copy() - # delta in radians so the rotation residual shares the translation delta's robustness scale. - delta_rad = huber_delta_m / max(rotation_weight_m_per_rad, 1e-9) - for _ in range(_HUBER_ITERATIONS): - weights_t = _huber_weights( - np.linalg.norm(translations - estimate_translation, axis=1), huber_delta_m - ) - estimate_translation = (weights_t[:, None] * translations).sum(0) / weights_t.sum() - angular_residual = 2.0 * np.arccos( - np.clip(np.abs(quaternions @ estimate_quaternion), 0.0, 1.0) - ) - weights_r = _huber_weights(angular_residual, delta_rad) - scatter = ( - weights_r[:, None, None] * np.einsum("ni,nj->nij", quaternions, quaternions) - ).sum(0) - estimate_quaternion = np.linalg.eigh(scatter)[1][ - :, -1 - ] # Markley quaternion eigen-mean (2007) https://ntrs.nasa.gov/citations/20070017872 - if estimate_quaternion @ reference < 0: - estimate_quaternion = -estimate_quaternion - aggregated = (*estimate_translation.tolist(), *estimate_quaternion.tolist()) - return aggregated # type: ignore[return-value] - - -def _median_present(values: list[float | None]) -> float | None: - """Median over the members that carry the field, ``None`` when none do -- median not mean, so one in-gate outlier cannot inflate the health signal.""" + 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 -def aggregate_by_marker_id( - observations: list[TagObservation], config: AggregationConfig -) -> dict[int, tuple[Pose7, int]]: - """Batch: group observations by marker_id and fuse each group to one robust pose; id -> (7-vec, n).""" - by_id: dict[int, list[TagObservation]] = defaultdict(list) - for obs in observations: - by_id[obs.marker_id].append(obs) - return { - marker_id: ( - robust_cluster_pose(group, config.rotation_weight_m_per_rad, config.huber_delta_m), - len(group), - ) - for marker_id, group in by_id.items() - } - - class TagAggregator: - """Streaming robust pose per marker over a sliding time window.""" - - def __init__(self, config: AggregationConfig) -> None: + """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._by_marker: dict[int, list[TagObservation]] = defaultdict(list) - - def observe(self, obs: TagObservation) -> str | None: - """Feed one glimpse; return its rejection reason, or ``None`` if kept.""" - reason = gate_reason(obs, self._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 - buf = self._by_marker[obs.marker_id] - buf.append(obs) - # Purge relative to this marker's newest glimpse, not wall time, so a paused marker keeps its window. - newest = obs.ts - self._by_marker[obs.marker_id] = [ - o for o in buf if newest - o.ts <= self._config.time_window_s - ] + 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 robust_estimate(self, marker_id: int) -> TagEstimate | None: - """Huber-aggregated pose for a marker's in-window glimpses, or ``None`` when fewer than ``min_observations`` are available.""" - buf = self._by_marker.get(marker_id, []) - if len(buf) < self._config.min_observations: + 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 - pose = robust_cluster_pose( - buf, self._config.rotation_weight_m_per_rad, self._config.huber_delta_m + 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 ) - return TagEstimate( - pose=pose, - n_observations=len(buf), - distance_m=_median_present([o.distance_m for o in buf]), - reproj_px=_median_present([o.reproj_px for o in buf]), + 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 6f6aab2163d..e4983f275c1 100644 --- a/dimos/perception/fiducial/marker_detection_stream_module.py +++ b/dimos/perception/fiducial/marker_detection_stream_module.py @@ -39,13 +39,9 @@ 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 +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 ( - AggregateTagBursts, - DetectMarkers, - MarkersPerFrame, -) +from dimos.perception.fiducial.marker_transformer import DetectMarkers, MarkersPerFrame from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -84,7 +80,7 @@ class MarkerDetectionStreamModule(StreamModule[Image, Detection3DArray]): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._warned_distortion_model = False - self._aggregate = AggregateTagBursts( + self._aggregate = TagAggregator( self.aggregated_detections.publish, self.config.aggregation, self.config.world_frame, diff --git a/dimos/perception/fiducial/marker_map.py b/dimos/perception/fiducial/marker_map.py deleted file mode 100644 index f7d5591747a..00000000000 --- a/dimos/perception/fiducial/marker_map.py +++ /dev/null @@ -1,100 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -"""Survey marker map file I/O: ``map_T_tag`` per id plus the tag size it was solved at.""" - -from __future__ import annotations - -import json -from pathlib import Path -from typing import Any - -import numpy as np - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.perception.fiducial.marker_aggregation import Pose7 - -MAP_FRAME = "map" -MARKER_MAP_SUFFIX = ".json" # what write_marker_map emits and load_marker_map parses - - -def _validated_entry(marker_id: int, entry: dict[str, Any]) -> Transform: - """Validate one survey entry into a ``map_T_marker`` Transform, failing loudly on a short translation or zero-norm quaternion.""" - translation, rotation = entry["translation"], entry["rotation"] - if not isinstance(translation, (list, tuple)) or len(translation) != 3: - raise ValueError(f"marker {marker_id}: translation must be [x, y, z], got {translation!r}") - if not isinstance(rotation, (list, tuple)) or len(rotation) != 4: - raise ValueError(f"marker {marker_id}: rotation must be [x, y, z, w], got {rotation!r}") - if not np.all(np.isfinite(np.asarray(translation, dtype=np.float64))): - raise ValueError(f"marker {marker_id}: translation must be finite, got {translation!r}") - norm = float(np.linalg.norm(np.asarray(rotation, dtype=np.float64))) - if ( - not np.isfinite(norm) or norm < 1e-6 - ): # 1e-6: unnormalizable, floor below a unit quaternion's round-off - raise ValueError(f"marker {marker_id}: rotation quaternion norm is {norm}, not usable") - return Transform( - translation=Vector3(*translation), - rotation=Quaternion(*rotation), - frame_id=MAP_FRAME, - child_frame_id=f"marker_{marker_id}", - ) - - -def load_marker_map(path: str | Path) -> dict[int, Transform]: - """``marker_id -> map_T_marker`` from a survey JSON (the format ``write_marker_map`` emits).""" - data = json.loads(Path(path).read_text()) or {} - return { - int(marker_id): _validated_entry(int(marker_id), entry) - for marker_id, entry in (data.get("markers", {}) or {}).items() - } - - -def marker_length_m_from_map(path: str | Path) -> float | None: - """The printed tag edge length the survey solved at, or ``None`` when the map carries none.""" - meta = (json.loads(Path(path).read_text()) or {}).get("meta") or {} - size = meta.get("marker_length_m") - if size is None: - return None - if float(size) <= 0: - raise ValueError(f"{path}: marker_length_m must be > 0, got {size}") - return float(size) - - -def write_marker_map( - path: Path, aggregated: dict[int, tuple[Pose7, int]], *, source: str, marker_length_m: float -) -> None: - """Serialize aggregated ``map_T_tag`` poses to the JSON ``load_marker_map`` reads.""" - doc = { - "meta": { - "schema": "map_T_tag", - "source_recording": source, - # every pose below is metric only at this tag size, so the live detector reads it back rather than being told twice - "marker_length_m": marker_length_m, - "n_detections_aggregated": { - str(mid): n for mid, (_pose, n) in sorted(aggregated.items()) - }, - }, - "markers": { - str(marker_id): { - "translation": [pose[0], pose[1], pose[2]], - "rotation": [pose[3], pose[4], pose[5], pose[6]], - } - for marker_id, (pose, _n) in sorted(aggregated.items()) - }, - } - path.parent.mkdir(parents=True, exist_ok=True) - path.write_text(json.dumps(doc, indent=2)) diff --git a/dimos/perception/fiducial/marker_transformer.py b/dimos/perception/fiducial/marker_transformer.py index 13618a04e09..0a0f7bfd036 100644 --- a/dimos/perception/fiducial/marker_transformer.py +++ b/dimos/perception/fiducial/marker_transformer.py @@ -45,14 +45,6 @@ from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.detection.type.detection3d.marker import Detection3DMarker -from dimos.perception.fiducial.marker_aggregation import ( - AggregationConfig, - TagAggregator, - TagEstimate, - marker_pose7, - tag_noise_scale, - tag_observation, -) from dimos.perception.fiducial.marker_detect import ( detect_markers_in_image as _detect_markers_in_image, ) @@ -418,60 +410,3 @@ def _source_pose( transform.rotation.w, ) return obs.pose - - -class AggregateTagBursts: - """Gate each tag glimpse and publish ONE robustly-aggregated pose per tag visit, edge-triggered.""" - - def __init__( - self, - publish: Callable[[Detection3DArray], Any], - config: AggregationConfig, - world_frame: str = "world", - ) -> None: - self._publish = publish - self._world_frame = world_frame - self._aggregator = TagAggregator(config) - # markers whose CURRENT burst already published; a marker leaves the set once its window thins under min_observations (left view > time_window_s), re-arming the edge - self._burst_counted: set[int] = set() - - def __call__(self, obs: Observation[Detection3DMarker | None]) -> None: - det = obs.data - if det is None: - return # DetectMarkers(emit_empty_frames=True) sentinel: a tag-free frame - - reason = self._aggregator.observe(tag_observation(det, obs.ts, marker_pose7(det))) - if reason is not None: - return - estimate = self._aggregator.robust_estimate(det.marker_id) - if estimate is None: - self._burst_counted.discard(det.marker_id) # window thinned -> re-arm the edge - return - if det.marker_id in self._burst_counted: - return # same burst still satisfied: publishing per frame would fire ~2x/s - self._burst_counted.add(det.marker_id) - self._publish(self._aggregated_array(det, estimate)) - - def _aggregated_array(self, det: Detection3DMarker, estimate: TagEstimate) -> Detection3DArray: - """One-detection array with this tag's aggregated world_T_marker, scored by its noise scale.""" - scale = tag_noise_scale(estimate.distance_m, estimate.reproj_px) - x, y, z, qx, qy, qz, qw = estimate.pose - aggregated = dataclasses.replace( - det, - center=Vector3(x, y, z), - orientation=Quaternion(qx, qy, qz, qw), - transform=None, # aggregated across many frames: no ONE camera transform holds - confidence=min(1.0, 1.0 / scale), # -> hypothesis.score, dimensionless 0-1 - ) - msg = ImageDetections3D(det.image, [aggregated]).to_ros_detection3d_array( - frame_id=self._world_frame - ) - logger.debug( - "tag burst aggregated", - marker_id=det.marker_id, - n_obs=estimate.n_observations, - distance_m=None if estimate.distance_m is None else round(estimate.distance_m, 2), - reproj_px=None if estimate.reproj_px is None else round(estimate.reproj_px, 2), - score=round(aggregated.confidence, 2), - ) - return msg 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 From ed44156ec2b0c31875ef9fdaf5a22064c12ed247 Mon Sep 17 00:00:00 2001 From: Aaryan Agrawal Date: Wed, 2 Sep 2026 21:54:34 -0700 Subject: [PATCH 4/5] reloc: cover the aggregation tap on real detections --- .../test_marker_detection_stream_module.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/dimos/perception/fiducial/test_marker_detection_stream_module.py b/dimos/perception/fiducial/test_marker_detection_stream_module.py index 8ec3d08be63..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 ( @@ -99,6 +101,27 @@ def test_marker_detection_stream_module_exposes_single_stream_input() -> None: 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) From aca64845fc95af335fc15b77a33337345b515602 Mon Sep 17 00:00:00 2001 From: Aaryan Agrawal Date: Wed, 2 Sep 2026 21:57:00 -0700 Subject: [PATCH 5/5] reloc: split the marker pose conversion out of _fix --- .../mapping/relocalization/fiducial/module.py | 30 +++++++++++-------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/dimos/mapping/relocalization/fiducial/module.py b/dimos/mapping/relocalization/fiducial/module.py index f90fd405562..7e0541178c9 100644 --- a/dimos/mapping/relocalization/fiducial/module.py +++ b/dimos/mapping/relocalization/fiducial/module.py @@ -105,26 +105,30 @@ def _fix(self, det: Detection3D) -> Transform | None: min_score=self.config.min_score, ) return None - c = det.bbox.center # world_T_marker, aggregated by the detector - world_T_marker = matrix_from_pose7( - ( - c.position.x, - c.position.y, - c.position.z, - c.orientation.x, - c.orientation.y, - c.orientation.z, - c.orientation.w, - ) - ) logger.info("relocalize fiducial", marker_id=marker_id, score=round(score, 2)) return Transform.from_matrix( - world_T_marker @ np.linalg.inv(map_T_marker), + _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()