OpenCV Convex Hullで、マット四隅検出が0/10から10/10になった。pixel→mmホモグラフィ変換基盤の構築

座標系をゼロから積み直した。アームの物理配置を変え、OpenCVによるマット四隅自動検出スクリプトを新規実装し、cv2.findHomography()でpixel→mmのホモグラフィ変換行列を保存するところまで到達した。最初の検出成功率は0/10だった。cv2.convexHull()という一つの判断を加えた後、10/10になった。派手な出来事はない。「基盤からやり直してよかった」という静かな納得だけがある。

全部忘めることにした、から始まる

結論が出た。固定補正では解決不能だ、と。

lookup distが0.2mmという精度で検索は完璧に動いていた。6軸全部lookupの結果を送信していることも確認済みだった。それでも爪がブロックに届かない。原因は構造的だった。Unity FKスキャンが記録するEE座標は「リンク先端（手首）」の位置であり、グリッパー爪はそこからさらに約90mm先にある。爪のオフセット方向はJ0の向きによって変わる。単一の固定補正値では、どの姿勢でも正しく補正できない。

全部忘れる。座標も、ログも、過去のhomography.npyも、「LOG154で動いた」という記憶も。

これが最終的な判断だった。以下を「判断根拠に使わない」対象として明示的に除外した。

• fk_scan_motion_based.csv（134,456点）

• motion_log_block.csv の8姿勢

• APPROACH_PICK_BASE の値

• 過去の homography.npy

• Unity の EE Transform 定義（手首位置）

• PICK_EE_Y = 0.027 の定数

• 各種オフセット値（-0.040, -0.100, -0.0903 等）

基準から順に積み上げ直す。

Unity座標系の確定

何から始めるかは自明だった。座標系。これが定まらなければ、後の全ての測定が意味を持たない。

原則: ロボット座標系 = マット座標系 = Unity座標系に一致させる。

Hierarchyで mecharm270mpi_1_0 を選択し、Scene ビューのTransformギズモとオブジェクトの実際の位置を照合した。

• RedBlock_arm1: Position(0.2401, 0.012, 0.214) → 画面の左下に位置

• WhiteBox_arm1: Position(-0.2208, 0, 0.153) → 画面の右に位置

Piカメラ上から視点での軸対応（恒久ルール）:

マットの再確認と実測

Unity上でマット（Plane1）のTransformを確認した。

GameObject: Plane1
Position: (0, 0, 0)
Rotation: (0, 0, 0)
Scale:    (0.08, 1, 0.08)
Material: Mat_ExperimentalFloor

Unity標準Plane meshは10m×10m。Scale 0.08を掛けると物理サイズは0.8m×0.8m（半辺400mm）。マット平面はUnity XZ平面(Y=0)と完全に平行。マット中心はアームベース真下でUnity原点(0,0,0)と一致。

物理マットの実測値:

物理マット実寸:
  X方向（左右）: 620mm
  Z方向（上下）: 450mm

アームベース位置（移動前）:
  画面上の辺（Z-側）から: 189mm
  画面下の辺（Z+側）から: 261mm
  X方向: 中央（左右各310mm）

カメラ配置とアーム移動（100mm上へ）

ホーム姿勢（J1〜J5=0, J6=-30）では爪がZ+方向に伸びてマット下辺の外側に出てしまい、マット全体を画角に収めると爪が消える。

解決策: アームをマット上端寄り（Z-側）へ移動する。

150mmずらすと安定性に欠けた（アーム後部がマット外に出すぎて転倒リスク）ため、100mmにした。

移動後の実測値:

アームベース位置（確定値）:
  画面上の辺（Z-側）から: 89mm
  画面下の辺（Z+側）から: 361mm
  X方向: 310mm / 310mm（中央・変更なし）

Unity座標でのマット範囲:
  X: -310mm 〜 +310mm
  Z: -89mm 〜 +361mm

Piに電源を入れ、pymycobotでホーム姿勢を実機確認した。

from pymycobot import MyCobot
mc = MyCobot('/dev/ttyAMA0', 1000000)
mc.send_angles([0, 0, 0, 0, 0, -30], 10)

注意: 過去のログで from pymycobot.mecharm270 import MechArm270 を試みたが ModuleNotFoundError。このPi環境では from pymycobot import MyCobot が正しいimport文。

マット四辺がすべて画面内 + ホーム姿勢の爪先端の青シール2個も画面内。両立を確認。

mat_corner_detection.py: マット四隅検出スクリプト（新規実装）

ファイル: ~/r2r2r-scripts/mat_corner_detection.py（228行、新規作成） 過去の debug_mat_corners.py は参照せずゼロから書いた。

実装した関数一覧:

フルコード:

#!/usr/bin/env python3
"""
マット四隅検出スクリプト
Iriun Webcam（camera index 2）から複数フレームを取得し、
黒いマットの四隅を統計的に安定化して検出する。
--debug オプションで中間処理結果を画像保存。
"""

import sys
import os
import time
import argparse
import numpy as np
import cv2

CAMERA_INDEX = 2
NUM_FRAMES = 10
FRAME_INTERVAL = 0.1  # 秒
WARMUP_FRAMES = 3
OUTPUT_IMAGE = "mat_corner_detection_result.png"
DEBUG_DIR = os.path.expanduser("~/r2r2r-scripts/debug_mat_corner")


def capture_frames(cap, num_frames, warmup, interval):
    """カメラからフレームを取得する。最初のwarmupフレームは破棄。"""
    for _ in range(warmup):
        cap.read()
        time.sleep(0.05)

    frames = []
    for i in range(num_frames):
        ret, frame = cap.read()
        if ret:
            frames.append(frame)
        else:
            print(f"  フレーム {i}: 取得失敗")
        time.sleep(interval)

    return frames


def detect_corners(frame, debug=False, frame_idx=0):
    """
    1枚のフレームからマットの四隅を検出する。
    戻り値: [(x,y), (x,y), (x,y), (x,y)] 左上・右上・右下・左下の順、またはNone
    """
    h, w = frame.shape[:2]

    if debug:
        print(f"    画像サイズ: {w}x{h}")
        os.makedirs(DEBUG_DIR, exist_ok=True)
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_01_original.png"), frame)

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    if debug:
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_02_gray.png"), gray)

    # Otsu二値化（マット=黒=前景にするため反転）
    otsu_thresh, binary = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
    if debug:
        print(f"    Otsu閾値: {otsu_thresh:.1f}")
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_03_binary.png"), binary)

    # モルフォロジー演算でノイズ除去
    kernel = np.ones((15, 15), np.uint8)
    binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
    binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
    if debug:
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_04_morphology.png"), binary)

    # 輪郭抽出
    contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if debug:
        print(f"    輪郭数: {len(contours)}")
        contour_img = frame.copy()
        cv2.drawContours(contour_img, contours, -1, (0, 255, 0), 2)
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_05_contours.png"), contour_img)

    if not contours:
        return None

    # 最大面積の輪郭を選択
    largest = max(contours, key=cv2.contourArea)
    area = cv2.contourArea(largest)
    if debug:
        print(f"    最大輪郭面積: {area:.0f} px (画像全体の {100*area/(w*h):.1f}%)")
        max_contour_img = frame.copy()
        cv2.drawContours(max_contour_img, [largest], -1, (0, 0, 255), 3)
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_06_max_contour.png"), max_contour_img)

    # 面積が画像全体の5%未満なら検出失敗とみなす
    if area < w * h * 0.05:
        if debug:
            print(f"    → 面積不足で検出失敗 (閾値: {w*h*0.05:.0f})")
        return None

    # 凸包を計算（アームの凹みを無視する）
    hull = cv2.convexHull(largest)
    if debug:
        print(f"    凸包の頂点数: {len(hull)}")
        hull_img = frame.copy()
        cv2.drawContours(hull_img, [largest], -1, (0, 0, 255), 2)  # 元輪郭=赤
        cv2.drawContours(hull_img, [hull], -1, (255, 255, 0), 3)   # 凸包=シアン
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_06b_hull.png"), hull_img)

    # 4頂点に近似（凸包に対して実行）
    arc_len = cv2.arcLength(hull, True)
    epsilon = 0.02 * arc_len
    approx = cv2.approxPolyDP(hull, epsilon, True)
    used_factor = 0.02

    if debug:
        print(f"    approxPolyDP: epsilon={epsilon:.1f} (factor=0.02) → 頂点数={len(approx)}")

    # epsilonを調整して4頂点になるまでリトライ
    if len(approx) != 4:
        for factor in [0.03, 0.04, 0.05, 0.01, 0.015]:
            epsilon = factor * arc_len
            approx = cv2.approxPolyDP(hull, epsilon, True)
            if debug:
                print(f"    リトライ: epsilon={epsilon:.1f} (factor={factor}) → 頂点数={len(approx)}")
            if len(approx) == 4:
                used_factor = factor
                break

    if len(approx) != 4:
        if debug:
            print(f"    → 4頂点に近似できず検出失敗 (最終頂点数={len(approx)})")
        return None

    if debug:
        print(f"    → 4頂点近似成功 (factor={used_factor})")
        approx_img = frame.copy()
        cv2.drawContours(approx_img, [approx], -1, (0, 255, 0), 3)
        for pt in approx.reshape(-1, 2):
            cv2.circle(approx_img, (int(pt[0]), int(pt[1])), 10, (0, 0, 255), -1)
        cv2.imwrite(os.path.join(DEBUG_DIR, f"frame_{frame_idx}_07_approx.png"), approx_img)

    # 4頂点を整列（左上・右上・右下・左下の順）
    corners = sort_corners(approx.reshape(4, 2))
    return corners


def sort_corners(pts):
    """
    4頂点を左上・右上・右下・左下の順に並び替える。
    x+yが最小 → 左上、x-yが最大 → 右上、x+yが最大 → 右下、x-yが最小 → 左下
    """
    s = pts.sum(axis=1)         # x + y
    d = pts[:, 0] - pts[:, 1]  # x - y

    ordered = np.zeros((4, 2), dtype=pts.dtype)
    ordered[0] = pts[np.argmin(s)]   # 左上: x+y 最小
    ordered[1] = pts[np.argmax(d)]   # 右上: x-y 最大
    ordered[2] = pts[np.argmax(s)]   # 右下: x+y 最大
    ordered[3] = pts[np.argmin(d)]   # 左下: x-y 最小

    return ordered


def remove_outliers_and_average(all_corners):
    """
    複数フレームの四隅座標から外れ値を除去して平均化する。
    各頂点ごとにメディアン ± 2σ の範囲外を除外。
    """
    stacked = np.array(all_corners)  # (N, 4, 2)
    n = stacked.shape[0]

    if n == 1:
        return stacked[0]

    result = np.zeros((4, 2), dtype=np.float64)

    for corner_idx in range(4):
        xs = stacked[:, corner_idx, 0].astype(np.float64)
        ys = stacked[:, corner_idx, 1].astype(np.float64)

        med_x, std_x = np.median(xs), np.std(xs)
        med_y, std_y = np.median(ys), np.std(ys)

        threshold = 2.0
        mask_x = np.abs(xs - med_x) <= max(threshold * std_x, 5.0)
        mask_y = np.abs(ys - med_y) <= max(threshold * std_y, 5.0)
        mask = mask_x & mask_y

        if np.sum(mask) == 0:
            result[corner_idx] = [med_x, med_y]
        else:
            result[corner_idx] = [np.mean(xs[mask]), np.mean(ys[mask])]

    return result


def draw_result(frame, corners):
    """検出結果を画像に描画する。"""
    img = frame.copy()

    colors = [(0, 0, 255), (0, 255, 0), (255, 0, 0), (0, 255, 255)]
    labels = ["UL", "UR", "LR", "LL"]

    for i in range(4):
        cx, cy = int(corners[i][0]), int(corners[i][1])
        color = colors[i]
        cv2.circle(img, (cx, cy), 12, color, -1)
        cv2.putText(img, f"{labels[i]} ({cx},{cy})", (cx + 15, cy - 10),
                   cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)

    pts = corners.astype(np.int32).reshape((-1, 1, 2))
    cv2.polylines(img, [pts], True, (255, 255, 255), 2)

    return img


def main():
    parser = argparse.ArgumentParser(description="マット四隅検出")
    parser.add_argument("--debug", action="store_true", help="中間処理結果を画像保存する")
    args = parser.parse_args()

    debug = args.debug

    print("=== マット四隅検出 ===")
    if debug:
        print(f"[DEBUG] デバッグモード: 中間画像を {DEBUG_DIR} に保存します")
        os.makedirs(DEBUG_DIR, exist_ok=True)

    cap = cv2.VideoCapture(CAMERA_INDEX)
    if not cap.isOpened():
        print(f"エラー: カメラ index {CAMERA_INDEX} に接続できませんでした。")
        sys.exit(1)

    print(f"カメラ接続成功 (index {CAMERA_INDEX})")
    print(f"フレーム取得中... ({NUM_FRAMES}枚、ウォームアップ{WARMUP_FRAMES}枚)")
    frames = capture_frames(cap, NUM_FRAMES, WARMUP_FRAMES, FRAME_INTERVAL)
    print(f"取得フレーム数: {len(frames)}")

    if len(frames) == 0:
        print("エラー: フレームが1枚も取得できませんでした。")
        cap.release()
        sys.exit(1)

    all_corners = []
    last_good_frame = None
    for i, frame in enumerate(frames):
        if debug:
            print(f"\n  --- フレーム {i} ---")
        corners = detect_corners(frame, debug=debug, frame_idx=i)
        if corners is not None:
            all_corners.append(corners)
            last_good_frame = frame
            print(f"  フレーム {i}: 検出成功")
        else:
            print(f"  フレーム {i}: 検出失敗")

    cap.release()

    success_count = len(all_corners)
    print(f"\n成功フレーム数: {success_count} / {len(frames)}")

    if success_count == 0:
        print("エラー: 1フレームも四隅を検出できませんでした。")
        print("マットがカメラに映っているか確認してください。")
        if debug:
            print(f"[DEBUG] 中間画像を確認してください: {DEBUG_DIR}")
        sys.exit(1)

    averaged_corners = remove_outliers_and_average(all_corners)

    labels = ["左上(UL)", "右上(UR)", "右下(LR)", "左下(LL)"]
    print("\n=== 検出結果 ===")
    for i, label in enumerate(labels):
        x, y = averaged_corners[i]
        print(f"  {label}: ({x:.1f}, {y:.1f})")

    if last_good_frame is not None:
        result_img = draw_result(last_good_frame, averaged_corners)
        output_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), OUTPUT_IMAGE)
        cv2.imwrite(output_path, result_img)
        print(f"\n結果画像: {output_path}")


if __name__ == "__main__":
    main()

バグ発見と修正: sort_corners()のnp.diff問題

初回実行（1/10成功）の結果画像で、URとLLのラベルが入れ替わっていることを発見した。

原因: sort_corners() 内のコード。

# バグあり（修正前）
d = np.diff(pts, axis=1).flatten()  # コメント「x - y」だが実際はy - x

np.diff(pts, axis=1) は「後の要素 - 前の要素」を計算するため、pts の形 [[x, y], ...] に対して y - x を返す。コメントと実装が逆で、右上と左下が入れ替わる。

# 修正後
d = pts[:, 0] - pts[:, 1]  # x - y（正しい）

0/10の原因: アームがOpenCV輪郭に混入していた

バグ修正後に再実行しても0/10が続いた。--debug モードを実装して中間処理画像を全段階で保存した。

python3 ~/r2r2r-scripts/mat_corner_detection.py --debug
# → ~/r2r2r-scripts/debug_mat_corner/ に各フレームの中間画像を保存
# 保存される画像: frame_{i}_01_original.png 〜 frame_{i}_07_approx.png

各フレームのデバッグログ（代表値）:

--- フレーム 0 ---
  画像サイズ: 1920x1080
  Otsu閾値: 134.0
  輪郭数: 9
  最大輪郭面積: 776,941 px（画像全体の 37.5%）
  approxPolyDP: epsilon=115.7（factor=0.02）→ 頂点数=7
  リトライ: factor=0.03 → 頂点数=7
  リトライ: factor=0.04 → 頂点数=7
  リトライ: factor=0.05 → 頂点数=7
  リトライ: factor=0.01 → 頂点数=9
  リトライ: factor=0.015 → 頂点数=8
  → 4頂点に近似できず検出失敗（最終頂点数=8）
フレーム 0: 検出失敗

中間画像 frame_0_06_max_contour.png を確認して原因が判明した。最大輪郭の輪郭線が、マット四辺の外側を通りながら、アーム本体に沿って内側に大きくえぐれている。

アーム本体が黒っぽいため、Otsu二値化でマットと同じ「黒領域」として結合されていた。結果として「マット＋アーム輪郭」という複雑な形が一つの輪郭になり、どのepsilonでも4頂点に収まらない。

cv2.convexHull()で解決: 1行の追加

解決策: 最大輪郭を取得した後、cv2.approxPolyDP() の前に cv2.convexHull() で凸包を計算する。

マットの外周は長方形（凸形状）だが、アームが「内側へのえぐれ」を作っている。凸包を取ることでこのえぐれが消え、マットの外側の長方形が残る。

# 修正前
max_contour = max(contours, key=cv2.contourArea)
approx = cv2.approxPolyDP(max_contour, epsilon, True)

# 修正後（1行追加のみ）
max_contour = max(contours, key=cv2.contourArea)
hull = cv2.convexHull(max_contour)  # ← 追加
approx = cv2.approxPolyDP(hull, epsilon, True)

修正後のデバッグログ（代表値）:

--- フレーム 0 ---
  Otsu閾値: 134.0 / 輪郭数: 7
  最大輪郭面積: 776,027 px（37.4%）
  凸包の頂点数: 24
  approxPolyDP: epsilon=75.8（factor=0.02）→ 頂点数=4
  → 4頂点近似成功（factor=0.02）

--- フレーム 2 ---（異常フレームの例）
  Otsu閾値: 83.0（他フレームは134。照明の瞬間変化と推測）
  輪郭数: 2 / 最大輪郭面積: 896,274 px（43.2%）
  凸包の頂点数: 29
  approxPolyDP: factor=0.02 → 頂点数=5
  リトライ: factor=0.03 → 頂点数=4 → 成功

成功フレーム数: 10 / 10

=== 検出結果 ===
  左上(UL): (425.2, 96.7)
  右上(UR): (1513.5, 89.7)
  右下(LR): (1536.2, 876.1)
  左下(LL): (429.7, 897.9)

0/10が10/10になった。

calibrate_homography.py: ホモグラフィ変換基盤の実装

ファイル: ~/r2r2r-scripts/calibrate_homography.py（新規作成）

mat_corner_detection.py から capture_frames, detect_corners, remove_outliers_and_average をimportして使う設計。

フルコード:

#!/usr/bin/env python3
"""
ホモグラフィキャリブレーションスクリプト
カメラ画像のpixel座標 → マット座標系(mm)のホモグラフィ行列を計算し、
homography.npy に保存する。

座標系（Unity/ロボット/マット共通）:
  X+ = 画面左, X- = 画面右
  Z+ = 画面下（手前）, Z- = 画面上（奥）
  原点 = アームベース真下
"""

import sys
import os
import numpy as np
import cv2

from mat_corner_detection import (
    capture_frames,
    detect_corners,
    remove_outliers_and_average,
)

CAMERA_INDEX = 2
NUM_FRAMES = 10
FRAME_INTERVAL = 0.1
WARMUP_FRAMES = 3
HOMOGRAPHY_PATH = os.path.expanduser("~/r2r2r-scripts/homography.npy")

# マット四隅のマット座標（mm）— 変更不可
# 順序: UL, UR, LR, LL（sort_cornersの出力順に対応）
MAT_CORNERS_MM = np.array([
    [+310.0, -89.0],   # UL: 画面左上
    [-310.0, -89.0],   # UR: 画面右上
    [-310.0, +361.0],  # LR: 画面右下
    [+310.0, +361.0],  # LL: 画面左下
], dtype=np.float32)


def detect_mat_corners_averaged(cap):
    """
    複数フレームでマット四隅を検出し、外れ値除去+平均化した4点pixel座標を返す。
    戻り値: (4, 2) の numpy配列、または None
    """
    print(f"フレーム取得中... ({NUM_FRAMES}枚、ウォームアップ{WARMUP_FRAMES}枚)")
    frames = capture_frames(cap, NUM_FRAMES, WARMUP_FRAMES, FRAME_INTERVAL)
    print(f"取得フレーム数: {len(frames)}")

    if len(frames) == 0:
        return None

    all_corners = []
    for i, frame in enumerate(frames):
        corners = detect_corners(frame)
        if corners is not None:
            all_corners.append(corners)
            print(f"  フレーム {i}: 検出成功")
        else:
            print(f"  フレーム {i}: 検出失敗")

    print(f"成功フレーム数: {len(all_corners)} / {len(frames)}")

    if len(all_corners) == 0:
        return None

    averaged = remove_outliers_and_average(all_corners)
    return averaged


def compute_homography(pixel_pts, mat_pts):
    """
    pixel座標(4点) → マット座標(4点) のホモグラフィ行列を計算する。
    戻り値: (3, 3) のホモグラフィ行列 H、または None
    """
    src = pixel_pts.astype(np.float32)
    dst = mat_pts.astype(np.float32)
    H, status = cv2.findHomography(src, dst)
    if H is None:
        return None
    return H


def pixel_to_mm(pixel_x, pixel_y, H):
    """
    pixel座標をマット座標系(mm)に変換する。
    他スクリプトからimportして使えるユーティリティ関数。

    引数:
        pixel_x, pixel_y: pixel座標
        H: ホモグラフィ行列 (3x3)

    戻り値:
        (mat_x_mm, mat_z_mm): マット座標系での座標(mm)
    """
    pt = np.array([[[pixel_x, pixel_y]]], dtype=np.float32)
    transformed = cv2.perspectiveTransform(pt, H)
    mat_x = float(transformed[0][0][0])
    mat_z = float(transformed[0][0][1])
    return mat_x, mat_z


def verify_homography(pixel_pts, H):
    """検出した4点をHで変換し、期待値との誤差を表示する。"""
    labels = ["UL(左上)", "UR(右上)", "LR(右下)", "LL(左下)"]

    print("\n=== 検証: pixel → mm 変換 ===")
    print(f"{'頂点':<12} {'pixel':>16} {'変換後(mm)':>20} {'期待値(mm)':>20} {'誤差(mm)':>12}")
    print("-" * 84)

    for i, label in enumerate(labels):
        px, py = pixel_pts[i]
        mx, mz = pixel_to_mm(px, py, H)
        ex, ez = MAT_CORNERS_MM[i]
        err_x = mx - ex
        err_z = mz - ez
        err = np.sqrt(err_x ** 2 + err_z ** 2)
        print(f"  {label:<10} ({px:7.1f},{py:7.1f}) → ({mx:8.1f},{mz:8.1f})  期待({ex:8.1f},{ez:8.1f})  誤差 {err:.2f}")


def main():
    print("=== ホモグラフィキャリブレーション ===")
    print(f"座標系: X+=画面左, Z+=画面下, 原点=アームベース真下")
    print(f"マットサイズ: X=620mm, Z=450mm\n")

    cap = cv2.VideoCapture(CAMERA_INDEX)
    if not cap.isOpened():
        print(f"エラー: カメラ index {CAMERA_INDEX} に接続できませんでした。")
        sys.exit(1)
    print(f"カメラ接続成功 (index {CAMERA_INDEX})")

    pixel_pts = detect_mat_corners_averaged(cap)
    cap.release()

    if pixel_pts is None:
        print("\nエラー: マット四隅の検出に失敗しました。")
        sys.exit(1)

    labels = ["UL(左上)", "UR(右上)", "LR(右下)", "LL(左下)"]
    print("\n検出した四隅 pixel座標:")
    for i, label in enumerate(labels):
        x, y = pixel_pts[i]
        print(f"  {label}: ({x:.1f}, {y:.1f})")

    print("\nホモグラフィ行列を計算中...")
    H = compute_homography(pixel_pts, MAT_CORNERS_MM)
    if H is None:
        print("エラー: ホモグラフィ行列の計算に失敗しました。")
        sys.exit(1)

    print("ホモグラフィ行列:")
    for row in H:
        print(f"  [{row[0]:12.6f} {row[1]:12.6f} {row[2]:12.6f}]")

    verify_homography(pixel_pts, H)

    np.save(HOMOGRAPHY_PATH, H)
    print(f"\n保存完了: {HOMOGRAPHY_PATH}")


if __name__ == "__main__":
    main()

calibrate_homography.py 実行結果

=== ホモグラフィキャリブレーション ===
座標系: X+=画面左, Z+=画面下, 原点=アームベース真下
マットサイズ: X=620mm, Z=450mm

カメラ接続成功 (index 2)
成功フレーム数: 10 / 10

検出した四隅 pixel座標:
  UL(左上): (443.6, 88.9)
  UR(右上): (1525.6, 105.1)
  LR(右下): (1528.5, 888.9)
  LL(左下): (429.1, 881.1)

ホモグラフィ行列 H:
  [ -0.568304  -0.004115  561.614574]
  [ -0.007707   0.573659 -136.335350]
  [ -0.000010   0.000020    1.000000]

=== 検証: pixel → mm 変換 ===
  UL(左上)  ( 443.6,  88.9) → ( 310.0,  -89.0)  期待( 310.0,  -89.0)  誤差 0.00
  UR(右上)  (1525.6, 105.1) → (-310.0,  -89.0)  期待(-310.0,  -89.0)  誤差 0.00
  LR(右下)  (1528.5, 888.9) → (-310.0,  361.0)  期待(-310.0,  361.0)  誤差 0.00
  LL(左下)  ( 429.1, 881.1) → ( 310.0,  361.0)  期待( 310.0,  361.0)  誤差 0.00

保存完了: /Users/shimazueiichi/r2r2r-scripts/homography.npy

誤差0.00mmは4点対応のホモグラフィが数学的に完全フィットするという性質から来る。本当の精度検証は、キャリブに使っていない別の点——マットグリッドの交点など——で行う必要がある。それは次のステップだ。

後続スクリプトからの利用方法:

import numpy as np
from calibrate_homography import pixel_to_mm

H = np.load('~/r2r2r-scripts/homography.npy')
mat_x, mat_z = pixel_to_mm(px, py, H)

積み直す、という判断

「全部忘れる」と決断してから、以下の順序で積み上げてきた。

基盤からやり直してよかった。

この納得が今日の核心だ。次の検証で「キャリブに使っていない点での誤差」が出れば、ホモグラフィの実用精度が測れる。その数値が、ブロック検出からアームへの座標送信という本来の流れを再開できるかの判断基準になる。

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