diff --git a/dotbot/calibration/conics.py b/dotbot/calibration/conics.py index c8fbd2b5..ba768b5a 100644 --- a/dotbot/calibration/conics.py +++ b/dotbot/calibration/conics.py @@ -27,6 +27,7 @@ from dotbot.calibration.lighthouse2 import ( Calibration, + Placement, StationSolution, TrackSample, union_rect, @@ -690,6 +691,8 @@ def field_angle(centres: np.ndarray) -> float: # What a free-mode calibration records as its site's anchor: no mark in the # room is its origin. +# A site made by `site init` records the calibration it came from in its anchor +SELF_DEFINED_ANCHOR = "self-defined from spin calibration " FREE_FRAME_ANCHOR = ( "free mode: zero at the top-left of the rectangle around the spinning " "robots, grown by a robot's footprint; axes along that rectangle" @@ -787,6 +790,18 @@ def spin_field(calibration: Calibration) -> tuple[int, int]: return int(hi[0]), int(hi[1]) +def is_free_mode(calibration: Calibration) -> bool: + """Whether `calibration` is a spin calibration in its own free frame.""" + return ( + bool(calibration.stations) + and calibration.site.anchor == FREE_FRAME_ANCHOR + and all( + st.solved_from in ("conics-free", "conics-joint") + for st in calibration.stations + ) + ) + + def self_defined_site( calibration: Calibration, name: str, @@ -801,14 +816,7 @@ def self_defined_site( site extent, and a tag gets `-` appended, so the two files answer to different tags. """ - if ( - not calibration.stations - or calibration.site.anchor != FREE_FRAME_ANCHOR - or any( - st.solved_from not in ("conics-free", "conics-joint") - for st in calibration.stations - ) - ): + if not is_free_mode(calibration): raise ValueError( f"calibration {calibration.id8} is not a free-mode spin calibration; " "a site can only be defined from one" @@ -822,7 +830,7 @@ def self_defined_site( extent, field_area, staging = starter_layout((field_w, field_h), size_mm) dx, dy = field_area.x, field_area.y shift = np.array([[1, 0, dx], [0, 1, dy], [0, 0, 1.0]]) - anchor = f"self-defined from spin calibration {calibration.id8}" + anchor = f"{SELF_DEFINED_ANCHOR}{calibration.id8}" stations = [] for st in calibration.stations: H = shift @ st.matrix @@ -853,5 +861,157 @@ def self_defined_site( created_at=calibration.created_at, tag=f"{calibration.tag}-{name}" if calibration.tag else "", robot=calibration.robot, + placed_from=calibration.id, ) return site, placed + + +# Fewest circles a station must share for `align_to_calibration` to report a +# residual: a turn and a shift take two. +ALIGN_CIRCLES_MIN = 3 + + +@dataclass(frozen=True) +class Alignment: + """A free-mode calibration moved onto another one's site, and how well + the circles of the stations they share agreed.""" + + calibration: Calibration + stations: tuple[int, ...] + turn_deg: float + shift_mm: tuple[float, float] + residuals_mm: tuple[float, ...] + + @property + def rms_mm(self) -> float: + return float(np.sqrt(np.mean(np.square(self.residuals_mm)))) + + @property + def max_mm(self) -> float: + return float(np.max(self.residuals_mm)) + + +def _rigid_fit(src: np.ndarray, dst: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """The turn R and shift t minimising |R src + t - dst| (Kabsch).""" + mu_s, mu_d = src.mean(0), dst.mean(0) + U, _, Vt = np.linalg.svd((dst - mu_d).T @ (src - mu_s)) + E = np.diag([1.0, np.sign(np.linalg.det(U @ Vt)) or 1.0]) + R = U @ E @ Vt + return R, mu_d - R @ mu_s + + +def align_to_calibration( + source: Calibration, + target: Calibration, + stations: Sequence[int] = (), + tag: str | None = None, +) -> Alignment: + """`source`, a free-mode calibration, moved rigidly onto `target`'s site. + + Every source circle a shared station saw is mapped through that + station's homography in each calibration; the mean of each circle's + points in the two frames is one correspondence, and the turn and shift + are their least-squares fit. Scale is kept: both frames are metric + through the spin radius. Every station's rectangle and the fence become + `target`'s fence. `stations` picks the shared stations to align through, + and is needed when they share more than one. + """ + if not is_free_mode(source): + raise ValueError( + f"calibration {source.id8} is not a free-mode spin calibration, " + "so it is already in a site's frame and cannot be moved" + ) + if is_free_mode(target): + raise ValueError( + f"calibration {target.id8} is a free-mode spin calibration itself; " + "align to one placed on a site" + ) + shared = sorted( + {st.index for st in source.stations} & {st.index for st in target.stations} + ) + labels = ", ".join(f"{i} (channel {i + 1})" for i in shared) or "none" + if not shared: + raise ValueError( + f"calibrations {source.id8} and {target.id8} share no station" + ) + if not stations: + if len(shared) > 1: + raise ValueError( + f"calibrations {source.id8} and {target.id8} share stations " + f"{labels}: pick the one(s) to align through" + ) + stations = shared + unshared = sorted(set(stations) - set(shared)) + if unshared: + raise ValueError( + f"station(s) {', '.join(map(str, unshared))} are not in both " + f"calibrations; they share {labels}" + ) + src, dst = [], [] + for track in source.tracks: + if track.station not in stations or not track.count1: + continue + cam = track.camera_points() + src.append(apply(source.station(track.station).matrix, cam).mean(0)) + dst.append(apply(target.station(track.station).matrix, cam).mean(0)) + if len(src) < ALIGN_CIRCLES_MIN: + raise ValueError( + f"station(s) {', '.join(map(str, stations))} saw {len(src)} circle(s) " + f"of {source.id8}; aligning takes {ALIGN_CIRCLES_MIN}" + ) + src, dst = np.array(src), np.array(dst) + R, t = _rigid_fit(src, dst) + residuals = np.linalg.norm(src @ R.T + t - dst, axis=1) + turn = float(np.degrees(np.arctan2(R[1, 0], R[0, 0]))) + c, s = np.cos(np.radians(turn)), np.sin(np.radians(turn)) + T = np.array([[c, -s, t[0]], [s, c, t[1]], [0.0, 0.0, 1.0]]) + fence = tuple(int(v) for v in target.valid_mm) + moved = [] + for st in source.stations: + H = T @ st.matrix + H = H / H[2, 2] + moved.append( + StationSolution( + index=st.index, + homography=[[float(v) for v in row] for row in H], + points=st.points, + residual_mm=st.residual_mm, + solved_from=st.solved_from, + valid_mm=fence, + ) + ) + site = target.site + placements = [ + Placement( + index=p.index, + points_mm=[tuple(map(float, T[:2, :2] @ q + T[:2, 2])) for q in p.points_mm], + at=p.at, + points_from=p.points_from, + captured_at=p.captured_at, + samples=p.samples, + ) + for p in source.placements + ] + placed = Calibration( + site=Site(name=site.name, anchor=site.anchor), + valid_mm=fence, + placements=placements, + stations=moved, + tracks=source.tracks, + links=source.links, + created_at=source.created_at, + tag=( + tag + if tag is not None + else (f"{source.tag}-{site.name}" if source.tag else "") + ), + robot=source.robot, + placed_from=source.id, + ) + return Alignment( + calibration=placed, + stations=tuple(stations), + turn_deg=turn, + shift_mm=(float(t[0]), float(t[1])), + residuals_mm=tuple(float(r) for r in residuals), + ) diff --git a/dotbot/calibration/lighthouse2.py b/dotbot/calibration/lighthouse2.py index 8114ccca..c76b8541 100644 --- a/dotbot/calibration/lighthouse2.py +++ b/dotbot/calibration/lighthouse2.py @@ -257,6 +257,8 @@ class Calibration: created_at: str = "" tag: str = "" robot: str = ROBOT_DEFAULT + # The id of the free-frame spin calibration this one was moved from + placed_from: str = "" path: Optional[Path] = None # The id the file on disk declares, when this came from one. A mismatch # with `id` means the file was hand-edited into a different calibration. @@ -686,6 +688,8 @@ def render_calibration(calibration: Calibration) -> str: ] if calibration.tag: out.append(f'tag = "{toml_escape(calibration.tag)}"') + if calibration.placed_from: + out.append(f'placed_from = "{calibration.placed_from}"') out += [ "", "[site]", @@ -867,6 +871,7 @@ def read_calibration_file(path: Path) -> Calibration: created_at=metadata.get("created_at", ""), tag=metadata.get("tag", ""), robot=metadata.get("robot", ROBOT_DEFAULT), + placed_from=str(metadata.get("placed_from", "")), path=path, ) calibration.stored_id = str(metadata.get("id", "")) @@ -1043,6 +1048,15 @@ def _file_metadata(path: Path) -> dict: return {} +def calibration_site_of(spec: str) -> Optional[str]: + """The site of the one calibration file under any site that `spec` + names, None when none or several do.""" + try: + return read_calibration_file(resolve_calibration_path(spec)).site.name + except (OSError, ValueError): + return None + + def load_calibration( spec: str, root: Optional[Path] = None, diff --git a/dotbot/calibration/multi_station.py b/dotbot/calibration/multi_station.py index 8d6e3acb..30054e31 100644 --- a/dotbot/calibration/multi_station.py +++ b/dotbot/calibration/multi_station.py @@ -1174,17 +1174,33 @@ def appendable_rounds(calibration: Calibration) -> int: calibration is in a frame a re-solve would not keep. """ # pylint: disable=import-outside-toplevel - from dotbot.calibration.conics import FREE_FRAME_ANCHOR + from dotbot.calibration.conics import FREE_FRAME_ANCHOR, SELF_DEFINED_ANCHOR + from dotbot.calibration.lighthouse2 import calibration_site_of if ( not calibration.tracks or calibration.site.anchor != FREE_FRAME_ANCHOR or not all(st.solved_from.startswith("conics") for st in calibration.stations) ): - raise ValueError( + refused = ( f"calibration {calibration.id8} is not a free-mode spin calibration, " - "so a round cannot be appended to it; append to the spin calibration " - "it was made from" + "so a round cannot be appended to it" + ) + anchor = calibration.site.anchor + if calibration.placed_from: + source = calibration.placed_from + how = f"it was placed from spin calibration {source[:8]}" + elif anchor.startswith(SELF_DEFINED_ANCHOR): + source = anchor[len(SELF_DEFINED_ANCHOR) :].strip() + how = f"its site was defined from spin calibration {source[:8]}" + else: + raise ValueError( + f"{refused}; append to the spin calibration it was made from" + ) + where = calibration_site_of(source) + raise ValueError( + f"{refused}: {how}, so append to that one" + + (f" (--append {source[:8]} --site {where})" if where else "") ) return max(t.round for t in calibration.tracks) + 1 diff --git a/dotbot/cli/swarm_lh2.py b/dotbot/cli/swarm_lh2.py index 05874dec..9e8edf19 100644 --- a/dotbot/cli/swarm_lh2.py +++ b/dotbot/cli/swarm_lh2.py @@ -1116,3 +1116,68 @@ def _reframe(ctx, calibration, site_name, shift, rotate): ) click.echo(f"\nCalibration saved to {path}") click.echo(f"Calibration id {reframed.id}, site {site.name}") + + +@cmd.command( + name="place", + help=( + "Move a free-mode spin calibration onto the site of another " + "calibration, through a station both solved: the turn and shift that " + "best map its circles there are fitted, the residual printed, and the " + "moved calibration saved under ~/.dotbot/calibrations//." + ), +) +@click.argument("calibration") +@click.option( + "--to-calibration", + "target", + required=True, + metavar="ID", + help="The calibration whose site and frame to move it into: an id prefix, a tag or a path.", +) +@click.option( + "--station", + "stations", + type=int, + multiple=True, + metavar="INDEX", + help=( + "A station to align through, by index (channel - 1); repeatable. " + "Needed when the two calibrations share several." + ), +) +@click.option("--tag", default=None, help="Tag for the moved calibration.") +@click.pass_context +def _place(ctx, calibration, target, stations, tag): + if _devices(ctx): + raise click.UsageError("place sends nothing, so it takes no `dotbot swarm -d`") + from dotbot.calibration.conics import align_to_calibration + from dotbot.calibration.lighthouse2 import ( + read_calibration_file, + resolve_calibration_path, + slug_tag, + write_calibration, + ) + + try: + source = read_calibration_file(resolve_calibration_path(calibration)) + onto = read_calibration_file(resolve_calibration_path(target)) + aligned = align_to_calibration( + source, onto, stations, tag=slug_tag(tag) if tag else None + ) + except ValueError as exc: + raise click.ClickException(str(exc)) from exc + placed = aligned.calibration + path = write_calibration(placed) + through = ", ".join(f"{i} (channel {i + 1})" for i in aligned.stations) + click.echo( + f"Placed {source.id8} (site {source.site.name}) on site {placed.site.name} " + f"through station {through} of {onto.id8}: turn {aligned.turn_deg:.3f} deg, " + f"shift ({aligned.shift_mm[0]:.1f}, {aligned.shift_mm[1]:.1f}) mm" + ) + click.echo( + f" {len(aligned.residuals_mm)} circles: residual rms " + f"{aligned.rms_mm:.2f} mm, max {aligned.max_mm:.2f} mm" + ) + click.echo(f"\nCalibration saved to {path}") + click.echo(f"Calibration id {placed.id}, site {placed.site.name}") diff --git a/dotbot/tests/test_calibration_spin.py b/dotbot/tests/test_calibration_spin.py index 04a64261..205a53af 100644 --- a/dotbot/tests/test_calibration_spin.py +++ b/dotbot/tests/test_calibration_spin.py @@ -18,6 +18,7 @@ from dotbot.calibration import conics, lighthouse2, push from dotbot.calibration import spin as spin_module from dotbot.calibration.lighthouse2 import counts_for_camera_point, load_calibration +from dotbot.calibration.multi_station import appendable_rounds from dotbot.calibration.spin import ( SPIN_CHUNK_RECORDS, SPIN_TAG, @@ -746,6 +747,46 @@ def test_append_refuses_a_corner_calibration(monkeypatch, lab, tmp_path): assert fleet.calls == [] +def test_append_to_a_placed_calibration_names_the_one_it_came_from(monkeypatch, lab): + _, fleet = _fleet() + result = _collect(monkeypatch, lab, fleet) + free_id = re.search(r"--from-calibration (\w+)", result.output).group(1) + result = CliRunner().invoke( + site_cmd.cmd, + ["init", "spun", "--from-calibration", free_id], + obj={"config": lab}, + ) + placed_id = re.search(r"push (\w+) --site spun", result.output).group(1) + _, fleet = _fleet() + result = _collect(monkeypatch, lab, fleet, "--site", "spun", "--append", placed_id) + assert result.exit_code != 0 + assert "not a free-mode spin calibration" in result.output + assert f"placed from spin calibration {free_id}" in result.output + assert f"--append {free_id} --site c405" in result.output + assert fleet.calls == [] + + +def test_a_placed_file_without_its_source_falls_back_to_the_site_anchor( + monkeypatch, lab +): + _, fleet = _fleet() + result = _collect(monkeypatch, lab, fleet) + free_id = re.search(r"--from-calibration (\w+)", result.output).group(1) + result = CliRunner().invoke( + site_cmd.cmd, + ["init", "spun", "--from-calibration", free_id], + obj={"config": lab}, + ) + path = Path(re.search(r"saved to (\S+)", result.output).group(1)) + text = path.read_text() + assert f'placed_from = "{free_id}' in text + path.write_text(re.sub(r'placed_from = "\w+"\n', "", text)) + with pytest.raises(ValueError) as refused: + appendable_rounds(lighthouse2.read_calibration_file(path)) + assert f"its site was defined from spin calibration {free_id}" in str(refused.value) + assert f"--append {free_id} --site c405" in str(refused.value) + + def test_append_and_drop_station_go_with_spin(lab): for flags in (["--append", "x"], ["--drop-station", "2"]): result = CliRunner().invoke( @@ -852,3 +893,85 @@ def test_show_next_round_refuses_a_calibration_a_round_cannot_join(lab, tmp_path assert shown.exit_code != 0 assert "--next-round plans a round for collect --spin --append" in shown.output assert "not a free-mode spin calibration" in shown.output + + +# --- place --to-calibration ---------------------------------------------------- + + +def _place(lab, *args): + return CliRunner().invoke(swarm_lh2.cmd, ["place", *args], obj={"config": lab}) + + +def _saved(result) -> Path: + return Path(re.search(r"Calibration saved to (\S+)", result.output).group(1)) + + +def test_place_moves_a_spin_calibration_onto_another_through_a_station( + monkeypatch, lab +): + a, b = _two_stations() + grid = _floor_grid([a, b]) + _, fleet = _two_station_fleet([a, b], grid[:40]) + result = _collect(monkeypatch, lab, fleet, "--tag", "first") + assert result.exit_code == 0, result.output + result = CliRunner().invoke( + site_cmd.cmd, + ["init", "spun", "--from-calibration", "first"], + obj={"config": lab}, + ) + assert result.exit_code == 0, result.output + target = load_calibration("first-spun", site="spun") + # a second spin calibration over other robots, so in another free frame + _, fleet = _two_station_fleet([a, b], grid[10:]) + result = _collect(monkeypatch, lab, fleet, "--tag", "second") + assert result.exit_code == 0, result.output + source = lighthouse2.read_calibration_file(_saved(result)) + + result = _place(lab, "second", "--to-calibration", target.id8) + assert result.exit_code != 0 + assert "share stations 0 (channel 1), 1 (channel 2)" in result.output + + result = _place(lab, "second", "--to-calibration", target.id8, "--station", "0") + assert result.exit_code == 0, result.output + rms, worst = re.search( + r"residual rms (\S+) mm, max (\S+) mm", result.output + ).groups() + assert float(rms) < 1.0 and float(worst) < 2.0 + turn, dx, dy = re.search( + r"turn (\S+) deg, shift \((\S+), (\S+)\) mm", result.output + ).groups() + assert abs(float(turn)) > 1.0 or np.hypot(float(dx), float(dy)) > 50.0 + placed = lighthouse2.read_calibration_file(_saved(result)) + assert placed.site.name == "spun" and placed.site.anchor == target.site.anchor + assert placed.placed_from == source.id and placed.tag == "second-spun" + assert placed.valid_mm == target.valid_mm + assert {s.valid_mm for s in placed.stations} == {target.valid_mm} + # both put any floor point either station sees at the same place + floor = np.array([c for c in grid if a.sees(c) and b.sees(c)], dtype=float) + for st in (a, b): + cam = conics.apply(st.floor_to_cam, floor) + moved = conics.apply(placed.station(st.index).matrix, cam) + there = conics.apply(target.station(st.index).matrix, cam) + assert np.max(np.linalg.norm(moved - there, axis=1)) < 2.0 + + +def test_place_refuses_a_placed_source_and_a_free_target(monkeypatch, lab): + _, fleet = _fleet() + result = _collect(monkeypatch, lab, fleet, "--tag", "free") + assert result.exit_code == 0, result.output + result = CliRunner().invoke( + site_cmd.cmd, + ["init", "spun", "--from-calibration", "free"], + obj={"config": lab}, + ) + assert result.exit_code == 0, result.output + result = _place(lab, "free-spun", "--to-calibration", "free") + assert result.exit_code != 0 + assert "already in a site's frame" in result.output + _, fleet = _fleet(CENTRES[:-1]) # another file, not the same one retagged + result = _collect(monkeypatch, lab, fleet, "--tag", "other") + assert result.exit_code == 0, result.output + result = _place(lab, "other", "--to-calibration", "free") + assert result.exit_code != 0 and "align to one placed on a site" in result.output + result = _place(lab, "other", "--to-calibration", "free-spun", "--station", "3") + assert result.exit_code != 0 and "are not in both calibrations" in result.output