Move the arm manually, release guidance, and wait two seconds. This collector only reads robot state. Keep the board visible and collect varied rotations.
Preview a sample before excluding it. Excluded samples are kept in the session's +excluded/ directory and are not used by the solver. Re-solve after changing samples.
+| Sample | Corners | RMS (px) | Actions |
|---|
A recorded sample is not a successful calibration. Solve and validate on new poses.
""" @@ -330,6 +445,20 @@ def do_GET(self) -> None: with collector.lock: status = collector.status() self.reply(200, json.dumps(status, allow_nan=False).encode()) + elif match := re.fullmatch(r"/samples/([0-9]+)/annotated\.png", self.path): + index = int(match[1]) + with collector.lock: + if not any(s["index"] == index for s in collector.samples): + self.send_error(404) + return + try: + data = ( + collector.root / f"sample_{index:03d}" / "annotated.png" + ).read_bytes() + except OSError as error: + self.reply(503, json.dumps({"error": str(error)}).encode()) + return + self.reply(200, data, "image/png") elif self.path.startswith("/frame.jpg"): try: self.reply(200, collector.preview(), "image/jpeg") @@ -339,16 +468,21 @@ def do_GET(self) -> None: self.send_error(404) def do_POST(self) -> None: - """Record one sample while rejecting concurrent acquisition.""" - if self.path != "/sample": + """Serialize sample capture and recoverable exclusion.""" + deletion = re.fullmatch(r"/samples/([0-9]+)/delete", self.path) + if self.path != "/sample" and deletion is None: self.send_error(404) return if not collector.lock.acquire(blocking=False): - self.reply(409, b'{"error":"sample in progress"}') + self.reply(409, b'{"error":"sample operation in progress"}') return try: try: - collector.last = collector.sample() + collector.last = ( + collector.delete_sample(int(deletion[1])) + if deletion + else collector.sample() + ) code = 200 except ( ValueError, @@ -357,7 +491,7 @@ def do_POST(self) -> None: cv2.error, subprocess.SubprocessError, ) as error: - LOGGER.warning("Sample rejected: %s", error) + LOGGER.warning("Sample operation rejected: %s", error) collector.last = {"accepted": False, "error": str(error)} code = 422 self.reply(code, json.dumps(collector.last, allow_nan=False).encode()) @@ -386,6 +520,9 @@ def add_arguments(parser: argparse.ArgumentParser) -> None: help="Optional libfranka shared-library directory on the reader host", ) parser.add_argument("--output", type=Path) + parser.add_argument( + "--resume", action="store_true", help="Resume --output with unchanged setup" + ) parser.add_argument("--port", type=int, default=8767) parser.add_argument("--squares-x", type=int, default=6) parser.add_argument("--squares-y", type=int, default=8) @@ -441,14 +578,18 @@ def main() -> None: parser = argparse.ArgumentParser(description=__doc__) add_arguments(parser) args = parser.parse_args() + if args.resume and args.output is None: + parser.error("--resume requires --output") config = build_config(args) root = args.output or Path(__file__).resolve().parent / "sessions" / ( datetime.now().strftime("%Y%m%d_%H%M%S_%f") + f"_{config.arm}_eye_to_hand" ) - if root.exists(): - parser.error("Output directory already exists; use a new session directory") + try: + collector = Collector(config, root, resume=args.resume) + except (ValueError, OSError, KeyError) as error: + parser.error(str(error)) logging.basicConfig(level=logging.INFO) - serve(Collector(config, root), args.port) + serve(collector, args.port) if __name__ == "__main__": diff --git a/calibration_tools/common.py b/calibration_tools/common.py index dc8f2a8da..1efe5b093 100644 --- a/calibration_tools/common.py +++ b/calibration_tools/common.py @@ -161,6 +161,43 @@ def check_camera(camera: Record, reference: Record) -> None: raise ValueError("Camera K must be 3 x 3 with positive focal lengths") +def sample_poses( + sample: Record, + reference: Record, + *, + arm: str, + serial: str, + board: Record, + source: Path, +) -> tuple[Array, Array]: + """Validate saved sample identity and frames, returning robot and board poses. + + Collection review and solving apply their own sample-count, image-quality, + and acquisition-drift requirements after this shared consistency check. + """ + if ( + sample.get("calibration_mode") != "eye_to_hand" + or sample.get("arm") != arm + or sample["camera"]["serial"] != serial + or sample["board"] != board + ): + raise ValueError(f"{source}: sample configuration does not match") + check_camera(sample["camera"], reference["camera"]) + for key in ("robot_before", "robot_after"): + check_state(sample[key]) + if not np.allclose( + sample[key]["F_T_EE"], + reference["robot_before"]["F_T_EE"], + rtol=0, + atol=1e-8, + ): + raise ValueError(f"{source}: end-effector frame changed") + pose = rigid_transform(sample[f"T_{arm}_base_ee"], str(source)) + if not np.allclose(pose, transform(sample["robot_before"]), rtol=0, atol=1e-8): + raise ValueError(f"{source}: stored pose disagrees with robot_before") + return pose, rigid_transform(sample["T_camera_board"], str(source)) + + def load_candidate(path: Path) -> Record: """Load a candidate only when its report and original samples still agree.""" candidate = json.loads(path.read_text(encoding="utf-8")) diff --git a/calibration_tools/solve_base_handeye.py b/calibration_tools/solve_base_handeye.py index 9619ede42..b4e092bbe 100644 --- a/calibration_tools/solve_base_handeye.py +++ b/calibration_tools/solve_base_handeye.py @@ -27,13 +27,12 @@ METHODS, Array, Record, - check_camera, check_opencv, - check_state, delta, errors, mean_pose, rigid_transform, + sample_poses, stats, transform, write_json, @@ -101,28 +100,14 @@ def load_samples( expected_serial = serial or reference["camera"]["serial"] robot_poses, board_poses = [], [] for path, sample in zip(files, samples): - if sample.get("calibration_mode") != "eye_to_hand" or sample.get("arm") != arm: - raise ValueError( - f"{path}: arm or calibration mode does not match this solver" - ) - if ( - sample["camera"]["serial"] != expected_serial - or sample["board"] != reference["board"] - ): - raise ValueError(f"{path}: camera serial or board definition changed") - check_camera(sample["camera"], reference["camera"]) - for key in ("robot_before", "robot_after"): - check_state(sample[key]) - if not np.allclose( - sample[key]["F_T_EE"], - reference["robot_before"]["F_T_EE"], - rtol=0, - atol=1e-8, - ): - raise ValueError(f"{path}: end-effector frame changed") - pose = rigid_transform(sample[f"T_{arm}_base_ee"], str(path)) - if not np.allclose(pose, transform(sample["robot_before"]), rtol=0, atol=1e-8): - raise ValueError(f"{path}: stored pose disagrees with robot_before") + pose, board_pose = sample_poses( + sample, + reference, + arm=arm, + serial=expected_serial, + board=reference["board"], + source=path, + ) drift_m, drift_rad = delta(pose, transform(sample["robot_after"])) if drift_m > 0.001 or drift_rad > 0.005: raise ValueError(f"{path}: robot moved during image acquisition") @@ -141,7 +126,7 @@ def load_samples( f"{path}: insufficient corners or excessive reprojection error" ) robot_poses.append(pose) - board_poses.append(rigid_transform(sample["T_camera_board"], str(path))) + board_poses.append(board_pose) return files, samples, np.array(robot_poses), np.array(board_poses) diff --git a/tests/unit_tests/calibration_tools/test_collector_session.py b/tests/unit_tests/calibration_tools/test_collector_session.py new file mode 100644 index 000000000..73ef61e8b --- /dev/null +++ b/tests/unit_tests/calibration_tools/test_collector_session.py @@ -0,0 +1,141 @@ +# Copyright 2026 The RPent Authors. +# +# 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 +# +# https://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. +"""Offline functional checks for reviewed and resumed calibration sessions.""" + +from __future__ import annotations + +import base64 +import copy +import importlib +import io +import json +from pathlib import Path + +import pytest + + +@pytest.fixture +def session(monkeypatch, tmp_path): + cv2 = pytest.importorskip("cv2") + if not hasattr(cv2, "aruco") or not hasattr(cv2.aruco, "CharucoDetector"): + pytest.skip("Calibration requires OpenCV with the ChArUco detector") + pytest.importorskip("scipy") + monkeypatch.syspath_prepend( + str(Path(__file__).resolve().parents[3] / "calibration_tools") + ) + collector_module = importlib.import_module("base_handeye_collect") + config = collector_module.CaptureConfig( + arm="left", + camera_serial="offline-camera", + camera_url="http://offline.invalid/frame", + reader_command=("offline-reader",), + ) + collector = collector_module.Collector(config, tmp_path / "session") + state = { + "O_T_EE": collector_module.np.eye(4).flatten(order="F").tolist(), + "dq": [0.0] * 7, + "robot_mode": 1, + "has_errors": False, + } + state["F_T_EE"] = state["O_T_EE"].copy() + image = collector.board.generateImage((680, 880), marginSize=40) + success, encoded = collector_module.cv2.imencode(".png", image) + assert success + camera = { + "serial": config.camera_serial, + "width": 680, + "height": 880, + "K": [[800.0, 0.0, 340.0], [0.0, 800.0, 440.0], [0.0, 0.0, 1.0]], + "distortion": [0.0] * 5, + "distortion_model": "distortion.brown_conrady", + "png_b64": base64.b64encode(encoded.tobytes()).decode(), + } + + def camera_response(*args, **kwargs): + return io.BytesIO( + json.dumps( + {**camera, "host_received_s": collector_module.time.time()} + ).encode() + ) + + monkeypatch.setattr( + collector_module.Collector, "read_state", lambda self: copy.deepcopy(state) + ) + monkeypatch.setattr(collector_module.time, "sleep", lambda _: None) + monkeypatch.setattr(collector_module.urllib.request, "urlopen", camera_response) + return collector_module, collector, state + + +def snapshot(folder): + return { + str(path.relative_to(folder)): path.read_bytes() + for path in folder.rglob("*") + if path.is_file() + } + + +def request_handler(module, collector, path, method="POST"): + """Exercise actual HTTP routing without opening a listening socket.""" + handler = object.__new__(module.make_handler(collector)) + handler.path = path + responses = [] + handler.reply = lambda code, body, kind="application/json": responses.append( + (code, body, kind) + ) + getattr(handler, f"do_{method}")() + assert len(responses) == 1 + return responses[0] + + +def test_review_exclude_resume_and_continue_capture(session): + collector_module, collector, state = session + code, body, _ = request_handler(collector_module, collector, "/sample") + assert code == 200 and json.loads(body)["accepted"] + original = snapshot(collector.root / "sample_001") + code, image, kind = request_handler( + collector_module, collector, "/samples/1/annotated.png", "GET" + ) + assert (code, image, kind) == (200, original["annotated.png"], "image/png") + with collector.lock: + for path in ("/sample", "/samples/1/delete"): + assert request_handler(collector_module, collector, path)[0] == 409 + code, body, _ = request_handler(collector_module, collector, "/samples/1/delete") + assert code == 200 and json.loads(body)["deleted"] + archived = collector.root / "excluded" / "sample_001" + assert snapshot(archived) == original + + resumed = collector_module.Collector(collector.config, collector.root, resume=True) + assert resumed.status()["count"] == 0 + state["O_T_EE"][12] = 0.1 + code, body, _ = request_handler(collector_module, resumed, "/sample") + result = json.loads(body) + assert code == 200 and result["accepted"] + assert Path(result["saved"]).name == "sample_002" + final = collector_module.Collector(collector.config, collector.root, resume=True) + assert [sample["index"] for sample in final.status()["samples"]] == [2] + collector = final + for index in range(1, 10): + state["O_T_EE"][12] = 0.1 + index * 0.02 + collector.sample() + root = collector.root + solver = importlib.import_module("solve_base_handeye") + assert len(solver.load_samples(root, "left")[1]) == 10 + sample_path = root / "sample_005" / "sample.json" + changed = json.loads(sample_path.read_text()) + changed["T_left_base_ee"][0][3] += 0.1 + sample_path.write_text(json.dumps(changed)) + with pytest.raises(ValueError, match="stored pose disagrees"): + solver.load_samples(root, "left") + with pytest.raises(ValueError, match="stored pose disagrees"): + collector_module.Collector(collector.config, root, resume=True)