ロボットアームの座標系、ズレを補正せず可視化する。ホモグラフィ検証からマット射影ツールへ

iPhoneカメラとOpenCVで生成したhomography.npy（pixel→mm変換行列）の精度を、キャリブレーションに使っていない7点で実測した。全7点で同じ方向に誤差が乗っていた。補正ロジックを積み上げる前に、座標系そのものを目で見える状態にすることを選んだ。

💡 Key Takeaway

iPhoneカメラ+OpenCVで生成したホモグラフィ行列の精度検証で、全7点に系統的バイアス（ΔX平均-10mm、ΔZ平均+25mm）を発見した。補正を足すのではなく、マットを正長方形に射影してアーム座標系をグリッドで可視化することを選んだ結果、「何がどこにある状態か」が目で判断できる基盤ができた。

変換行列の精度を測る

前回の作業でiPhoneカメラとOpenCVを使ってhomography.npyを完成させた。このファイルは「pixel座標をマット座標系のmm座標に変換する3×3行列」で、マット四隅のpixel検出結果とMAT_CORNERS_MM（620×450mmの実寸定義）からcv2.findHomographyで生成したものだ。

homography.npy:
[[-5.68304460e-01  -4.11477641e-03   5.61614574e+02]
 [-7.70653229e-03   5.73659311e-01  -1.36335350e+02]
 [-1.02160294e-05   2.00941764e-05   1.00000000e+00]]

精度がどの程度のものか、まだ実測していない。キャリブレーションに使った4点は当然一致する。問題は、使っていない点でどれだけの誤差が出るかだ。そこから精度検証の作業が始まった。

verify_homography.py — クリック式精度検証ツール

設計は単純だ。カメラ映像を表示し、マット上の任意の点をクリックすると、homography.npyによる予測mm座標をターミナルに表示する。そこにキャリブで使っていない既知の実測値を入力すると、誤差（ΔX、ΔZ、|err|mm）が計算される。

実装の核はpixel_to_mm関数だ。

def pixel_to_mm(px, py, H):
    pt = np.array([[[px, py]]], dtype=np.float32)
    r  = cv2.perspectiveTransform(pt, H)
    return float(r[0][0][0]), float(r[0][0][1])

cv2.perspectiveTransformに3×3行列Hと入力pixel座標を渡すだけで、変換後のmat_x（mm）とmat_z（mm）が返ってくる。座標系の定義は前回確定したものを引き継ぐ。

座標系（確定・恒久）:
  X+ = 画面左 / X- = 画面右
  Z+ = 画面下（手前）/ Z- = 画面上（奥）
  原点 = アームベース真下
  マット範囲: X=-310〜+310mm, Z=-89〜+361mm

最初に実行すると、クリックした瞬間にmacOSがレインボー（応答なし）状態になった。

原因はGUIのメインループ内でinput()を呼んでいたことだ。input()は処理を完全にブロックするため、OpenCVのウィンドウ更新が止まり、macOSが「アプリが応答していない」と判断してしまう。

Pythonのselectモジュールでstdinをノンブロッキング監視することで解決した。

# ターミナル入力をノンブロッキングでチェック
if session.pending is not None:
    ready, _, _ = select.select([sys.stdin], [], [], 0)
    if ready:
        raw = sys.stdin.readline().strip()
        if not raw:
            session.pending = None
        else:
            try:
                parts = raw.replace(" ", ",").split(",")
                session.submit(float(parts[0]), float(parts[1]))
            except:
                print("[ERROR] 形式エラー例: -150.0,+80.0")
                session.pending = None

select.selectの第4引数を0にするとタイムアウトなしのノンブロッキング動作になる。入力がなければ即座に[]を返し、メインループが止まらない。クリック後もカメラ映像は動き続け、ターミナルに座標を入力してEnterを押したタイミングだけ処理が走るようになった。

カメラ向きの混乱と根本調査

verify_homography.pyを起動すると、ウィンドウに映った映像が「アームが下・赤ブロックが上」という180°逆の向きだった。

iPhoneのIriun Webcamアプリ上の表示は正しい向きだったため、「OpenCVが逆に取得している」と判断してflip補正を追加した。cv2.flip(frame, 0)（上下反転）、cv2.flip(frame, -1)（180°回転）と変えながら試したが、何度修正してもウィンドウの映像が変わらない状態が続いた。

これはClaude側の判断が外れ続けた場面だった。補正の引数を変えることで解決しようとしたが、そもそも問題の所在を間違えていた。

根本調査として、全カメラindex（0〜4）の生画像をJPEG保存して目視比較した。

# debug_camera_orientation.py
for i in range(5):
    cap = cv2.VideoCapture(i)
    if cap.isOpened():
        for _ in range(5):
            cap.read()  # バッファ初期化
        ret, frame = cap.read()
        if ret:
            cv2.imwrite(f"cam_{i}_{frame.shape[1]}x{frame.shape[0]}.jpg", frame)
    cap.release()

index 2（Iriun Webcam）とindex 3の生画像JPEGをFinderで開いたところ、どちらもiPhone視点と同じ正しい向きで撮影されていた。アームが上・赤ブロックが左下・灰色箱が右下。

つまりflip補正を入れた状態のverifyウィンドウは「正しい向きに見えていただけ」で、実際はflipによって逆にしていた映像だった。flipをすべて除去し、生画像をそのまま表示することで問題は解決した。

この作業で確認した判断基準：コードを変える前に、何が正しい状態なのかを現物（生画像）で確認してから作業を始める。補正ロジックを積み上げた後では、どの層で問題が起きているか追いにくくなる。

7点の精度検証と系統誤差の発見

ウィンドウの向きが正しくなったところで、マット全域に分散させた7点の精度検証を実施した。実測値はマット目盛りとアームベース原点の相対距離から計算した。

平均誤差26.9mm、最大32.6mm、RMSE≈27.2mm。

数値の大きさより、方向のパターンが重要だった。ΔXは全7点で負、ΔZは全7点で正。マット左上から右下まで、どの位置でも同じ方向にずれている。これはランダムなノイズではなく、一定方向に乗った系統的バイアスだ。

系統誤差の原因候補として3つを挙げた。①マット四隅の検出精度に問題がある、②カメラレンズの歪みが残っている、③実測値（actual座標）の計算方法がMAT_CORNERS_MMの定義と食い違っている。

mat_corner_detection.pyの検出精度確認

mat_corner_detection.pyの--debugフラグで中間処理画像を全フレーム書き出した。

検出フローはOtsu二値化 → 15×15カーネルのモルフォロジー処理 → 最大輪郭の凸包（Convex Hull）→ approxPolyDPで4頂点近似、という順だ。

10フレーム全ての近似結果を目視確認した。検出された4点はマット物理端に正確に一致していた。誤検出なし、内側への収縮もなし。

今回の検出座標：

UL: (440, 157)  UR: (1507, 157)
LL: (445, 941)  LR: (1519, 926)

UL→UR間のpixel距離は約1067px、対応する物理距離620mmから算出すると1.72px/mm。UL→LL間は784px/450mm=1.74px/mm。スケール誤差は1%程度。検出ロジックは精度の問題ではなかった。

マット正長方形射影ツール — mat_rectify.py

系統誤差の原因を数値で追うより先に、「座標系が視覚的に見える状態を作る」方針に切り替えた。

mat_rectify.pyはhomography.npyを使わない。検出した4隅pixel座標から直接cv2.getPerspectiveTransformで変換行列を計算し、マット全体を1240×900px（620×450mm、1mm=2px）の正長方形に射影する。

src = avg.astype(np.float32)  # 検出4隅のpixel座標（UL/UR/LR/LL順）
dst = np.array([
    [0,      0      ],   # UL → 画像左上
    [OUT_W,  0      ],   # UR → 画像右上
    [OUT_W,  OUT_H  ],   # LR → 画像右下
    [0,      OUT_H  ],   # LL → 画像左下
], dtype=np.float32)
M = cv2.getPerspectiveTransform(src, dst)
rectified = cv2.warpPerspective(last_frame, M, (OUT_W, OUT_H))

射影後の画像はマット実寸に対応しているため、mm→出力pxの変換は単純な比例式になる。

def mm_to_px(mat_x, mat_z):
    # (mat_x=+310, mat_z=-89) → px(0, 0) = 画像左上
    out_x = int(round((310 - mat_x) * 2))
    out_y = int(round((mat_z + 89) * 2))
    return out_x, out_y

この変換を使って4要素を重畳する。

アーム原点（0,0）マーカー： mm_to_px(0, 0) = px(620, 178) に赤い十字と円。アームベース支柱の真下を座標の起点として明示する。

100mmグリッド： X方向（-300〜+300mm）とZ方向（0〜+300mm）に100mm間隔で白線を引く。

マット物理端枠： 画像外周に白枠と四隅ラベル（UL/UR/LR/LL）。

X軸・Z軸ライン： アーム原点を通る赤（X軸）と青（Z軸）の直線。X+/X-/Z+/Z-のラベル付きで座標の向きを示す。

完成した画像では、マットの印刷目盛りと白グリッドの整合性が目視で確認できる。アーム原点マーカーがアーム支柱の真下に重なっているかどうか、各グリッドのmm値が物理的に正しいかどうかが、数値を見なくても判断できる状態になった。

系統誤差の発生仮説

可視化ツールが完成したことで、誤差の原因についても仮説が整理できた。

実測値の入力にマット目盛り（印刷領域）を使っていたのが原因の可能性が高い。物理マット実寸は620×450mmだが、印刷された目盛りの範囲は600×400mm。余白はX方向で片側10mm、Z方向で片側25mm存在する。

もし実測値を「印刷目盛り基準」で入力し、homographyは「マット物理端基準」でキャリブされていたとすると、そのまま両者にX方向10mm・Z方向25mmのオフセットが生まれる。観測された系統誤差（ΔX平均-10mm、ΔZ平均+25mm）と量的に一致する。

ただし、この仮説をコードで補正するより、次のステップでmat_rectify画像上に赤ブロックの検出位置を重畳して目視確認する方が確実だと判断した。数値の整合性を追うより、見える状態で照合する方が速い。

次のステップへ

今回得たものは精度の数値ではなく、座標系が見える状態を作るためのツールだった。

verify_homography.pyで精度測定の仕組みができた。mat_rectify.pyでマット全域の射影と座標系の可視化ができた。次のステップは、この画像の上に赤ブロックのpixel検出結果を重畳表示し、「homographyが返した座標」と「目で見た位置」が一致しているかどうかを確認することだ。

補正で数値を近づけるより、見える状態で判断する。その方針が次の出発点になる。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real2Render2Real」（Yu, Fu et al., 2025）の概念に触発されています。現実空間をデジタルツイン化し、仮想空間での試行から得た知見を実世界のモーター駆動へ還元する——その設計と実装過程を毎日記録しています。技術アプローチおよび目的は独自のものです。