feat(calibration): resume collection and exclude invalid samples

.github/workflows/unittest.yml

@@ -33,7 +33,7 @@ jobs:
       - name: Install test dependencies
         run: uv pip install --system -e ".[test]"
       - name: Install Flywheel export dependencies
-        run: uv pip install --system -e ".[flywheel]" --torch-backend cpu
+        run: uv pip install --system -e ".[flywheel]" "opencv-python-headless==4.10.0.84" --torch-backend cpu
       - name: Run unit tests with coverage
         run: coverage run --source=rpent,robots -m pytest tests/unit_tests -v
       - name: Generate coverage report

calibration_tools/base_handeye_collect.py

@@ -19,6 +19,7 @@
 import base64
 import json
 import logging
+import re
 import shlex
 import shutil
 import subprocess
@@ -39,6 +40,7 @@
     check_opencv,
     check_state,
     delta,
+    sample_poses,
     transform,
     write_json,
 )
@@ -94,12 +96,16 @@ def board_metadata(self) -> Record:
 class Collector:
     """Own acquisition state without mutating other imported modules."""
 
-    def __init__(self, config: CaptureConfig, root: Path) -> None:
+    def __init__(
+        self, config: CaptureConfig, root: Path, *, resume: bool = False
+    ) -> None:
         """Initialize an isolated session; hardware is read only on request."""
         self.config = config
         self.root = root
         self.pose_key = f"T_{config.arm}_base_ee"
         self.samples: list[Record] = []
+        self.reference: Record | None = None
+        self.next_index = 1
         self.last: Record = {}
         self.lock = threading.Lock()
         self.board = cv2.aruco.CharucoBoard(
@@ -109,6 +115,76 @@ def __init__(self, config: CaptureConfig, root: Path) -> None:
             cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, config.dictionary)),
         )
         self.board.setLegacyPattern(False)
+        if resume:
+            self._resume()
+        elif root.exists():
+            raise FileExistsError("Output directory already exists; use --resume")
+
+    def _resume(self) -> None:
+        """Load saved samples without changing their identities or files."""
+        if not self.root.is_dir():
+            raise ValueError("--resume requires an existing session directory")
+        reference: Record | None = None
+        indices: set[int] = set()
+        for parent in (self.root, self.root / "excluded"):
+            for folder in sorted(parent.glob("sample_*")):
+                match = re.fullmatch(r"sample_([0-9]+)", folder.name)
+                if not match or not folder.is_dir() or folder.is_symlink():
+                    raise ValueError(f"Invalid sample directory: {folder}")
+                index = int(match[1])
+                if (
+                    index < 1
+                    or index in indices
+                    or folder.name != f"sample_{index:03d}"
+                ):
+                    raise ValueError(f"Invalid or duplicate sample index: {folder}")
+                indices.add(index)
+                self.next_index = max(self.next_index, index + 1)
+                record = json.loads(
+                    (folder / "sample.json").read_text(encoding="utf-8")
+                )
+                if type(record["index"]) is not int or record["index"] != index:
+                    raise ValueError(f"Sample index does not match: {folder}")
+                if reference is None:
+                    reference = record
+                sample_poses(
+                    record,
+                    reference,
+                    arm=self.config.arm,
+                    serial=self.config.camera_serial,
+                    board=self.config.board_metadata,
+                    source=folder,
+                )
+                for name in ("color.png", "annotated.png"):
+                    if not (folder / name).is_file():
+                        raise ValueError(f"Missing sample image: {folder / name}")
+                if parent == self.root:
+                    self.samples.append(record)
+        self.samples.sort(key=lambda sample: sample["index"])
+        self.reference = reference
+        self.last = {"resumed": True, "count": len(self.samples)}
+
+    def delete_sample(self, index: int) -> Record:
+        """Exclude a selected sample, preserving its files for manual recovery."""
+        if type(index) is not int or index < 1:
+            raise ValueError("Sample index must be a positive integer")
+        sample = next((s for s in self.samples if s["index"] == index), None)
+        if sample is None:
+            raise ValueError(f"No active sample with index {index}")
+        source = self.root / f"sample_{index:03d}"
+        destination = self.root / "excluded" / source.name
+        if destination.exists():
+            raise FileExistsError(f"Excluded sample already exists: {destination}")
+        destination.parent.mkdir(exist_ok=True)
+        source.rename(destination)
+        self.samples.remove(sample)
+        LOGGER.info("Excluded sample %d; files retained at %s", index, destination)
+        return {
+            "deleted": True,
+            "index": index,
+            "count": len(self.samples),
+            "archived": str(destination),
+        }
 
     def read_state(self) -> Record:
         """Run the configured readOnce probe with a bounded local timeout."""
@@ -150,9 +226,9 @@ def sample(self) -> Record:
             raise ValueError("采样窗口超过 3 秒，请检查网络后重试")
         if before["F_T_EE"] != after["F_T_EE"]:
             raise ValueError("末端坐标定义在采样期间发生变化")
-        if self.samples and not np.allclose(
+        if self.reference is not None and not np.allclose(
             before["F_T_EE"],
-            self.samples[0]["robot_before"]["F_T_EE"],
+            self.reference["robot_before"]["F_T_EE"],
             atol=1e-8,
             rtol=0,
         ):
@@ -161,7 +237,7 @@ def sample(self) -> Record:
             dm, dr = delta(a, np.array(old[self.pose_key]))
             if dm < 0.005 and dr < np.deg2rad(5):
                 raise ValueError("与已有姿态过于接近，请改变姿态后采集；不要重复点击")
-        check_camera(camera, self.samples[0]["camera"] if self.samples else camera)
+        check_camera(camera, self.reference["camera"] if self.reference else camera)
         png = base64.b64decode(camera.pop("png_b64"), validate=True)
         im = cv2.imdecode(np.frombuffer(png, np.uint8), cv2.IMREAD_COLOR)
         if im is None or im.shape[:2] != (camera["height"], camera["width"]):
@@ -203,7 +279,7 @@ def sample(self) -> Record:
         t[:3, :3] = rot
         t[:3, 3] = tv.ravel()
         record = {
-            "index": len(self.samples) + 1,
+            "index": self.next_index,
             "host_start_s": start,
             "host_end_s": time.time(),
             "robot_before": before,
@@ -241,6 +317,9 @@ def sample(self) -> Record:
             if temporary.exists():
                 shutil.rmtree(temporary)
         self.samples.append(record)
+        if self.reference is None:
+            self.reference = record
+        self.next_index += 1
         return {
             "accepted": True,
             "count": len(self.samples),
@@ -255,6 +334,14 @@ def status(self) -> Record:
             "count": len(self.samples),
             "session": str(self.root),
             "last": self.last,
+            "samples": [
+                {
+                    "index": sample["index"],
+                    "corners": sample["corners"],
+                    "reprojection_rms_px": round(sample["reprojection_rms_px"], 3),
+                }
+                for sample in self.samples
+            ],
         }
 
     def preview(self) -> bytes:
@@ -284,19 +371,47 @@ def preview(self) -> bytes:
 
 
 PAGE = b"""<!doctype html><meta charset="utf-8"><title>Hand-eye calibration</title>
-<style>body{font:18px sans-serif;max-width:900px;margin:30px auto}img{max-width:100%}button{padding:12px}</style>
+<style>body{font:18px sans-serif;max-width:900px;margin:30px auto}img{max-width:100%}button{padding:12px}
+table{border-collapse:collapse;width:100%}td,th{text-align:left;padding:8px;border-bottom:1px solid #ddd}</style>
 <h1>Stationary hand-eye calibration</h1>
 <p id="instruction"></p><p>Move the arm manually, release guidance, and wait two seconds.
 This collector only reads robot state. Keep the board visible and collect varied rotations.</p>
 <img id="view"><p><button id="button" onclick="capture()">Capture pose</button></p><pre id="status"></pre>
+<h2>Saved samples</h2>
+<p>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.</p>
+<table><thead><tr><th>Sample</th><th>Corners</th><th>RMS (px)</th><th>Actions</th></tr></thead><tbody id="samples"></tbody></table>
 <p>A recorded sample is not a successful calibration. Solve and validate on new poses.</p>
 <script>
+let busy=false, generation=0, samplesSignature='';
+function showStatus(state){
+ document.getElementById('status').textContent=JSON.stringify({count:state.count,session:state.session,last:state.last},null,2);
+ const signature=JSON.stringify(state.samples);if(signature===samplesSignature)return;samplesSignature=signature;
+ const rows=document.getElementById('samples');rows.replaceChildren();
+ for(const sample of state.samples){const row=rows.insertRow();
+  for(const value of [sample.index,sample.corners,sample.reprojection_rms_px])row.insertCell().textContent=value;
+  const actions=row.insertCell(), link=document.createElement('a');link.textContent='Preview';
+  link.href='/samples/'+sample.index+'/annotated.png';link.target='_blank';link.rel='noopener';actions.append(link,' ');
+  const button=document.createElement('button');button.textContent='Exclude';button.disabled=busy;
+  button.onclick=()=>excludeSample(sample.index);actions.append(button);
+ }
+}
+async function loadStatus(){const version=generation,state=await(await fetch('/status')).json();
+ if(version===generation&&!busy)showStatus(state);}
 async function refresh(){document.getElementById('view').src='/frame.jpg?t='+Date.now();
-try{document.getElementById('status').textContent=JSON.stringify(await(await fetch('/status')).json(),null,2);}catch(e){}
+try{if(!busy)await loadStatus();}catch(e){document.getElementById('status').textContent='Connection lost: '+e;}
 setTimeout(refresh,1000);}refresh();
-async function capture(){let b=document.getElementById('button');b.disabled=true;
-try{document.getElementById('status').textContent=JSON.stringify(await(await fetch('/sample',{method:'POST'})).json(),null,2);}
-catch(e){document.getElementById('status').textContent=String(e);}finally{b.disabled=false;}}
+async function mutate(path){if(busy)return;busy=true;generation++;
+ document.querySelectorAll('button').forEach(b=>b.disabled=true);
+ document.getElementById('status').textContent='Working...';
+ try{const response=await fetch(path,{method:'POST'}),result=await response.json();
+  document.getElementById('status').textContent=JSON.stringify(result,null,2);
+ }catch(e){document.getElementById('status').textContent=String(e);}
+ finally{busy=false;document.querySelectorAll('button').forEach(b=>b.disabled=false);
+  try{await loadStatus();}catch(e){}}
+}
+function capture(){return mutate('/sample');}
+function excludeSample(index){if(confirm('Exclude sample '+index+'? Files will be kept in excluded/.'))return mutate('/samples/'+index+'/delete');}
 </script>"""
 
 
@@ -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__":

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"))