#!/usr/bin/env python3 """camera_align.py — 上カメラの写り方をお手本に合わせる画面 (Mac 側・アームは動かさない). 上カメラの iPhone は普段使いのもので, 毎回付け外しする. 付けるたびに写り方が変わり, 2026-10-21 の実機では お手本より下に大きくずれたまま動かしてしまった (runs/pi05_policy/pi05_20261021_082645 は dy=+37.5. 1 分前のドライランは dy=-7.8 で正常). iPhone を付けたあと, この画面で写り方を合わせてから実機に進む. 画面: ライブ映像に 基準画像 (--raw-dir の最初のエピソードの 1 枚目・HOME の姿勢) を半透明で重ねる. 隅に写り方のずれ (dx, dy) と相関の強さ (response) を出し, 判定を大きく表示する: OK しきい値以内 ZURE dx か dy がしきい値の外 (= ずれ) NO MATCH 相関が弱く dx/dy が当てにならない (= 測れない) q または Esc で終了. **表示だけで, ファイルには何も書かない.** run_policy_client.py は開始時に この measure_framing / framing_verdict で同じ検査をして, OK 以外ならアームに何も送らずに終了する (--ignore-framing で続行). 前提: - 上カメラ (iPhone 連係) が見えている. アームは HOME に置いておく (基準画像が HOME の姿勢) Usage: RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes python3 pi05/camera_align.py --raw-dir $RAW python3 pi05/camera_align.py --raw-dir $RAW --alpha 0.7 --scale 2 # 基準を濃く / 画面を小さく """ import argparse import sys import time from pathlib import Path import cv2 import numpy as np _HERE = Path(__file__).resolve().parent _ROOT = _HERE.parent _PLACE = _ROOT / "dynamic_tracking_limits" / "place" / "place_modules" # camera_resolver はここ for p in (_HERE, _ROOT, _PLACE): sys.path.insert(0, str(p)) import camera_resolver # noqa: E402 IMG_LONG = 384 # build_demo_episodes.py / run_policy_client.py と同じ (学習画像の長辺) FRAMING_MAX_DY = 20.0 # |dy| がこれを超えたら「ずれ」. 下の実測値の間を取った FRAMING_MAX_DX = 20.0 # |dx| の上限 (codex R2). お手本の dx は +0.00..+7.23 (2026-10-21 実測) # response (相関の強さ) がこれ未満なら, dx/dy を信用できないので「測れない」として止める (codex R1). # 2026-10-21 実測: 正しく対応している比較は 0.411..1.000. # 無関係な画像は ランダム 0.0375 / 真っ黒 0.0000, 10/17 の 222723 の 1 枚目は 0.0505 # (dy=+8.2 / dx=-2.5 なので, 相関を見ないと「OK」で通ってしまう). FRAMING_MIN_RESPONSE = 0.20 WINDOW = "camera_align (q で終了)" def to_small_bgr(frame): """学習画像と同じ大きさに縮める (run_policy_client.grab_rgb と同じ処理).""" h, w = frame.shape[:2] s = IMG_LONG / max(h, w) return cv2.resize(frame, (round(w * s), round(h * s)), interpolation=cv2.INTER_AREA) def load_reference(raw_dir: Path): """基準画像 = --raw-dir の最初のエピソードの 1 枚目 (HOME の姿勢). 戻り値: (BGR 画像, パス).""" eps = sorted(p.parent for p in raw_dir.glob("*/episode.npz")) if not eps: sys.exit(f"[ERROR] お手本が見つからない (基準画像を決められない): {raw_dir}") frames = sorted((eps[0] / "frames").glob("*.jpg")) if not frames: sys.exit(f"[ERROR] お手本の画像が無い: {eps[0]}") bgr = cv2.imread(str(frames[0])) if bgr is None: sys.exit(f"[ERROR] 基準画像を読めない: {frames[0]}") return bgr, frames[0] def measure_framing(ref_bgr, cur_bgr): """写り方のずれを測る. 戻り値: (dx, dy, response). cv2.phaseCorrelate + ハニング窓. 2026-10-21 実測 (基準 = teach_v1 の run_0001_20261019_183454 の 1 枚目): お手本 191257 dy= +9.1 / お手本 195120 dy= -6.7 ← ブロックの位置で揺れる範囲 ドライラン 082512 dy= -7.8 ← お手本と同じ写り方. 通す 実機 082645 dy=+37.5 ← 下にずれた写り方. 止める dx はブロックの位置で ±6 ほど動く (お手本どうしで +5.0 / +5.8) ので判定には使わず, 表示だけ. """ a = cv2.cvtColor(ref_bgr, cv2.COLOR_BGR2GRAY).astype(np.float32) b = cv2.cvtColor(cur_bgr, cv2.COLOR_BGR2GRAY).astype(np.float32) if a.shape != b.shape: b = cv2.resize(b, (a.shape[1], a.shape[0]), interpolation=cv2.INTER_AREA) win = cv2.createHanningWindow((a.shape[1], a.shape[0]), cv2.CV_32F) (dx, dy), response = cv2.phaseCorrelate(a, b, win) return float(dx), float(dy), float(response) def framing_verdict(dx, dy, response): """写り方の判定. 戻り値: (verdict, 理由). verdict は "OK" / "ずれ" / "測れない". 相関が弱い (response が低い) ときは dx/dy 自体が当てにならないので, 「ずれ」ではなく「測れない」として扱う (既定ではどちらも止める). """ if not all(np.isfinite(v) for v in (dx, dy, response)): return "測れない", "dx / dy / response に非有限値" if response < FRAMING_MIN_RESPONSE: return "測れない", (f"相関が弱く dx/dy が当てにならない " f"(response={response:.3f} < {FRAMING_MIN_RESPONSE:.2f})") bad = [] if abs(dy) > FRAMING_MAX_DY: bad.append(f"|dy|={abs(dy):.1f} > {FRAMING_MAX_DY:.0f}") if abs(dx) > FRAMING_MAX_DX: bad.append(f"|dx|={abs(dx):.1f} > {FRAMING_MAX_DX:.0f}") if bad: return "ずれ", " / ".join(bad) return "OK", (f"|dy|<={FRAMING_MAX_DY:.0f} / |dx|<={FRAMING_MAX_DX:.0f} / " f"response>={FRAMING_MIN_RESPONSE:.2f}") def open_top_camera(): cams = camera_resolver.resolve_cameras(require_top=True, require_side=False, verbose=False) cap = cv2.VideoCapture(cams["top"]) if not cap.isOpened(): sys.exit("[ERROR] 上カメラを開けない") t0 = time.time() while time.time() - t0 < 8.0: # 連係カメラは映像が来るまで数秒かかることがある ok, f = cap.read() if ok and f is not None: return cap time.sleep(0.2) cap.release() sys.exit("[ERROR] 上カメラから映像が来ない. iPhone のロック解除・連係カメラの接続を確認 " "(Photo Booth の『カメラ』メニューに iPhone が出るか)") def draw_overlay(small_bgr, ref_bgr, dx, dy, resp, alpha, scale): ref = ref_bgr if ref.shape[:2] != small_bgr.shape[:2]: ref = cv2.resize(ref, (small_bgr.shape[1], small_bgr.shape[0]), interpolation=cv2.INTER_AREA) view = cv2.addWeighted(small_bgr, 1.0 - alpha, ref, alpha, 0) big = cv2.resize(view, None, fx=scale, fy=scale, interpolation=cv2.INTER_NEAREST) verdict, _why = framing_verdict(dx, dy, resp) # cv2.putText は日本語を出せないので画面はローマ字. 端末には日本語で出す. label = {"OK": "OK", "ずれ": "ZURE", "測れない": "NO MATCH"}[verdict] color = (0, 200, 0) if verdict == "OK" else (0, 0, 255) cv2.putText(big, label, (16, 56), cv2.FONT_HERSHEY_SIMPLEX, 1.8, color, 4, cv2.LINE_AA) cv2.putText(big, f"dy={dy:+6.1f} dx={dx:+6.1f} (OK: both <= {FRAMING_MAX_DY:.0f})", (16, 92), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2, cv2.LINE_AA) cv2.putText(big, f"resp={resp:.3f} (OK: >= {FRAMING_MIN_RESPONSE:.2f}) q: quit", (16, 116), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (220, 220, 220), 1, cv2.LINE_AA) return big def main(): ap = argparse.ArgumentParser(description="上カメラの写り方をお手本に合わせる (アームは動かさない・表示だけ)") ap.add_argument("--raw-dir", required=True, help="基準画像に使う学習エピソード (必須. 最初の 1 本の 1 枚目を基準にする)") ap.add_argument("--alpha", type=float, default=0.5, help="基準画像の重ね具合 (0=ライブのみ / 1=基準のみ)") ap.add_argument("--scale", type=float, default=3.0, help="画面の拡大倍率") args = ap.parse_args() ref_bgr, ref_path = load_reference(Path(args.raw_dir)) print(f"[REF ] 基準画像: {ref_path}") print(f"[ALIGN ] |dy| <= {FRAMING_MAX_DY:.0f} なら OK. アームは HOME に置いた状態で合わせる. " "q または Esc で終了 (ファイルには何も書かない)") cap = open_top_camera() last = None try: while True: for _ in range(2): # バッファに溜まった古いフレームを捨てる cap.grab() ok, frame = cap.read() if not ok or frame is None: if cv2.waitKey(100) & 0xFF in (ord("q"), 27): break continue small = to_small_bgr(frame) dx, dy, resp = measure_framing(ref_bgr, small) cv2.imshow(WINDOW, draw_overlay(small, ref_bgr, dx, dy, resp, args.alpha, args.scale)) if last is None or abs(dy - last) > 1.0: # 端末は値が動いたときだけ出す verdict, why = framing_verdict(dx, dy, resp) print(f"[ALIGN ] dy={dy:+6.1f} dx={dx:+6.1f} response={resp:.3f} " f"→ {verdict} ({why})") last = dy if cv2.waitKey(30) & 0xFF in (ord("q"), 27): break except KeyboardInterrupt: print("\n[STOP ] Ctrl-C") finally: cap.release() cv2.destroyAllWindows() print("[DONE ] 終了 (何も書いていません)") if __name__ == "__main__": main()