perception/fiducial: add marker-map robot localization (world->map TF correction)

[AaryanAgrawal]

Problem

RelocalizationModule corrects world->map drift from lidar against a frozen
premap. A robot without lidar, or one operating where lidar degrades
(elevator cabs, glass, feature-poor corridors), has no equivalent. The nav
deep dive is explicit about the gap: "We don't have proper loop closure and
stable odometry... does drift eventually"
(docs/capabilities/navigation/deep_dive.md). This adds the same class of fix
using a camera and known fiducial markers instead of a lidar premap.

What this adds

• dimos/perception/fiducial/marker_localization.py: pure-function core.
  Known marker-map pose (map frame) + a live ArUco detection -> camera pose
  via solvePnP run in reverse, gated on reprojection error and tag count.
• dimos/perception/fiducial/marker_localization_module.py: thin Module.
  color_image In, static CameraInfo, publishes TF world->map -- the same job
  as RelocalizationModule, camera swapped in for lidar.
• dimos/perception/fiducial/test_marker_localization.py: 18 tests on real
  cv2.aruco-rendered frames at known poses, end to end through detection and
  pose recovery, plus the boundaries: empty frames, unmapped tags,
  corrupted-corner rejection, multi-tag selection, map-file errors
  (missing/malformed YAML), and config validation.

3 files, 484 insertions (181 implementation, 303 tests), no changes to
existing code.

What this reuses

create_aruco_detector, estimate_marker_pose (SOLVEPNP_IPPE_SQUARE),
camera_info_to_cv_matrices, marker_reprojection_error, and
rvec_tvec_to_transform, all from marker_pose.py -- no new detection or PnP
code. TF publish + resolve_named_path map-file loading follow
RelocalizationModule's pattern exactly.

Validation

Beyond the in-PR tests: a synthetic ground-truth harness (a rendered room,
15 wall tags, a drifting-odometry model, the same estimator running on real
cv2.aruco detections frame by frame) measures ATE 1.75 m raw odometry ->
0.33 m corrected, a 5.3x reduction. When several tags pass the gate in one
frame, the estimate with the lowest reprojection error wins, and an
order-independence regression test pins that behavior. The demo GIF below
shows the run: detections on the left, raw vs corrected trajectory on the
right. The harness is standalone and not part of this diff; happy to share
it.

81% reduction in trajectory error (ATE 1.75 m -> 0.33 m, 5.3x) on the synthetic ground-truth run shown above.

Follow-ups

Fisheye intrinsics, multi-tag weighted fusion (currently best single tag by
reprojection error), smoothing/hold-last-good, a demo blueprint + fixture
map.

Limitations

Validated on synthetic ground truth and rendered cv2.aruco frames, not yet
on a physical rig.

[greptile-apps]

Greptile Summary

This PR adds camera-based marker-map localization for correcting world -> map. The main changes are:

• New pure localization core for ArUco detections against a known marker map.
• New module that consumes color images and publishes the corrected transform.
• Shared solvePnP candidate handling for IPPE ambiguity checks.
• Marker localization tests for detection, gating, map loading, and module config.
• Blueprint registration for the new module.

Confidence Score: 5/5

This looks safe to merge.

• No blocking issues found in the changed code.
• The latest changes wire the marker count and camera frame settings through the module config.
• The marker map loader now rejects the malformed pose values covered by the reported failure cases.

Important Files Changed

Filename	Overview
dimos/perception/fiducial/marker_localization.py	Adds marker-map pose loading, validation, detection filtering, and camera pose recovery.
dimos/perception/fiducial/marker_localization_module.py	Adds the image-handling module and exposes marker count, camera frame, and gate settings through config.
dimos/perception/fiducial/marker_pose.py	Adds shared solvePnP input preparation and multi-candidate marker pose estimation.
dimos/perception/fiducial/test_marker_localization.py	Adds coverage for marker localization, rejection paths, map validation, and module configuration.
dimos/robot/all_blueprints.py	Registers the marker localization module in the blueprint module map.

_{Reviews (3): Last reviewed commit: "chore(robot): register marker-localizati..." | Re-trigger Greptile}

[greptile-apps]

Single Tag Gate Is Forced

LocalizationConfig has a min_tags gate, but the module never exposes it and always builds self._cfg with the default of one tag. A deployment that needs two mapped tags for corroboration cannot configure that safety check, so one noisy or spurious passing detection can publish a new world -> map correction.

Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!

[greptile-apps]

Camera Frame Name Is Fixed

handle_color_image always looks up world -> camera_optical, while real camera streams can publish TF under names such as a front or color optical frame. With a valid marker map and detections from any differently named camera frame, this branch returns without publishing the correction, so localization silently stays inactive.

[greptile-apps]

Malformed Map Poses Are Accepted

The YAML values are unpacked directly into Vector3 and Quaternion, so a malformed entry like translation: [1.0] is accepted as (1, 0, 0) instead of failing at load time. The module can then compute and publish a world -> map correction from the wrong marker pose, and invalid rotations such as a zero quaternion can fail later during transform inversion.

[greptile-apps]

Validate finite translations

This accepts non-finite translation values from the marker map. PyYAML can load .nan or .inf as floats, and this check only verifies that translation is a 3-element list or tuple. A map entry such as translation: [.nan, 0.0, 0.0] reaches Vector3(*translation), then localize_from_detections() composes it into the selected marker pose, and the module can publish a world -> map transform containing non-finite coordinates. Please reject non-finite translation components at load time.

[leshy]

Hi, we considered fiducial marker relocalization, I didn't read the code yet but sounds roughly ok.

Can you give me instructions how to test? I'd like to print markers and test on my go2 to ensure this works before reviewing the code.

[AaryanAgrawal]

Thanks — here's everything needed to test on a Go2, end to end. No hand-surveying required: step 2 uses the existing unitree-go2-markers blueprint to let the robot author its own marker map.

1. Print tags

dimos apriltag --ids 0-5 --size-mm 100 --family tag36h11 -o markers.pdf

100 mm matches the existing markers blueprint default so no size overrides are needed anywhere. After printing, measure a tag's black border with the ruler printed on the sheet — printers rescale silently, and tag size is how PnP infers distance, so a 5% print-scale error is a systematic 5% range error on every fix. If yours printed at a different size, use the measured value for marker_length_m below. Tape tags flat on walls at roughly camera height, ideally 2-3 visible from the areas you'll drive.

2. Build the marker map with the robot (no tape measure)

Run the existing markers blueprint with the robot somewhere with good odometry (fresh boot is fine — the survey run's world frame becomes your map frame):

dimos run unitree-go2-markers

Let it see each tag from a couple of angles, then read the marker_<id> transforms off the TF tree (the rerun viewer — on by default — or your LCM tooling) and paste them into a YAML:

# office_markers.yaml — poses in the map frame, quaternion is [x, y, z, w]
markers:
  0:
    translation: [3.12, -0.40, 0.35]
    rotation: [0.5, -0.5, 0.5, 0.5]
  1:
    translation: [5.80, 2.10, 0.33]
    rotation: [0.0, 0.0, 0.707, 0.707]

If you'd rather survey by hand: translation is the tag center in meters; orientation matters more than position (a 2° orientation error displaces the computed camera pose ~3.5 cm per meter of distance), which is why I'd let the robot do it.

3. Compose and run

Add next to unitree_go2_markers in dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py:

from dimos.perception.fiducial.marker_localization_module import MarkerLocalizationModule

unitree_go2_marker_localization = autoconnect(
    unitree_go2,
    MarkerLocalizationModule.blueprint(
        marker_map_file="/home/you/office_markers.yaml",  # absolute path: ~ is not expanded
        marker_length_m=0.10,  # your measured print size
        camera_info=GO2Connection.camera_info_static,
    ),
).global_config(n_workers=11, robot_model="unitree_go2")

Then register and run:

pytest dimos/robot/test_all_blueprints_generation.py  # regenerates all_blueprints.py; it FAILS
                                                      # with "please commit" — that's expected,
                                                      # the registry is updated either way
dimos run unitree-go2-marker-localization

The Go2's front camera chain already publishes camera_optical, which is the module's default camera_optical_frame — no frame config needed on a Go2.

4. What to expect

Whenever a mapped tag clears the reprojection gate you'll see a world -> map transform publish on TF (same correction contract as RelocalizationModule); with no tag in view the last correction holds and nothing publishes. Tags seen but rejected log a MarkerLocalizationModule: gate rejected warning. The test that shows the point: drive a loop away from the tags and back — watch odometry drift accumulate, then snap out in the map frame on the first good tag sighting. Detections are solid to ~2.5-3 m and ~45° off-axis for 100 mm tags; single-frame pose is tightest inside ~1 m, so don't judge accuracy from a grazing-angle sighting down a hallway.

So far this is validated synthetically — a rendered-room harness running the real cv2.aruco detector frame-by-frame shows an 81% reduction in trajectory error against a drifting-odometry model (ATE 1.75 m -> 0.33 m). Yours would be the first real-hardware run, so I'm keen on whatever you hit — happy to iterate on anything that doesn't behave.