dynamic homography一本化と、5 trial が示した補正の効きムラ

射影画像生成と検出用homographyを一本化して座標統一を完了させた。続けて補正ロジックの本評価を5点で実施。Trial 2では93.6mmという大改善を確認した一方、Trial 5/7ではほぼ横ばい。補正は「効くケース」と「効かないケース」が明確に分かれた。静的な閉ループ補正の限界点が、5サンプルで早くも見えてきた。

💡 Key Takeaway

射影画像と検出用homographyが別行列で動いていたため、検出マーカーが射影画像上で正しく重ならない問題を解消した。M_in_to_out = M_mm_to_out @ _homography で一本化。続いて補正ロジックを5 trialで検証。Trial 2は補正前107.1mm→補正後13.5mm（改善+93.6mm）、Trial 5/7はほぼ横ばい。補正効果には強い位置依存があった。

座標統一：2つのhomographyを合成する

前回の終盤で気づいた問題から始まった。射影画像（1240×900pxのトップダウン俯瞰）に検出マーカーを描くとズレる。原因はシンプルだった。

検出側は _homography（pixel→mm）を使っていた。表示側はマット四隅から1240×900pxへの変換行列を別途作っていた。同じカメラ画像から作られた2つの行列が、射影画像上では別物として動いていた。

修正は1行の合成で済んだ。

def capture_rectified(cap):
    for _ in range(5):
        cap.read()
    ret, last_frame = cap.read()
    if not ret:
        return None, None

    rect_corners_mm = np.array([
        [+310, -89], [-310, -89], [-310, +361], [+310, +361]
    ], dtype=np.float32)
    rect_corners_out = np.array([
        [0, 0], [OUT_W, 0], [OUT_W, OUT_H], [0, OUT_H]
    ], dtype=np.float32)
    M_mm_to_out = cv2.getPerspectiveTransform(rect_corners_mm, rect_corners_out)

    # _homography（pixel→mm）と合成
    M_in_to_out = M_mm_to_out @ _homography

    rectified = cv2.warpPerspective(last_frame, M_in_to_out, (OUT_W, OUT_H))
    draw_overlays(rectified)
    return rectified, M_in_to_out

M_mm_to_out @ _homography の合成で、入力pixel → 出力pixel が1つの行列で表現される。検出側と表示側が同じ座標系を共有する状態になった。

目視確認した結果、wrist/claw_a/claw_b/TIPの各マーカーは、射影画像上で実物の青シール位置と完全に一致した。座標統一は完了。

キー入力フリーズの問題

座標統一の確認後、補正ロジックの本評価に進もうとしたところで別のトラブルにぶつかった。

n キーで次のtrialに進む設計だったが、SSH通信中はOpenCVのウィンドウがキー入力を受け付けない。cv2.waitKey() はメインスレッドで動くため、paramiko のSSH送受信がブロックしている間、キーイベントが取れずレインボーカーソルが回るだけになる。

修正方針は単純で、キー入力をターミナルの input() に置き換えた。OpenCVウィンドウはプレビュー表示のみ、進行制御はターミナル側。

# 各trial完了後
resp = input("次のtrialへ進む場合はEnter / 終了する場合は 'q' + Enter: ").strip().lower()
if resp == 'q':
    cv2.destroyAllWindows()
    print("\n[終了] HOME 復帰...")
    send_home()
    cap.release()
    if results:
        save_csv(results)
    return

これでSSH通信中もEnterキーが正しく受け取られる。

5 trialの本評価

座標統一とキー入力修正を終えて、補正ロジックの本評価を実施。赤ブロックをマット上の異なる位置に置き、各位置で1 trialずつ動作させる。1 trialの流れは以下の通り。

1. カメラで赤ブロック検出 → Unity座標
2. fk_scan_motion_based.csv（512,087点）からlookup → 角度
3. APPROACH_PICK → PICK 動作
4. 青シール3点でグリッパー先端実測 → FKエラー算出
5. lookup目標を補正（pick - fk_error）→ 再lookup
6. APPROACH_PICK(補正) → PICK(補正) 動作
7. 再度実測 → 補正効果を計測

実行ログから5 trialの結果を抜粋する。

Trial 1（補正後検出失敗）

[CAM] 赤ブロック Unity座標: (0.2658, 0.1820)
[Mac] PICK 姿勢制約後候補数: 172点
[LOOKUP] 角度: [-45.0, 110.0, -45.0, -7.0, -35.0, -20.0]
[LOOKUP] FK予測 ee: (0.2507, -0.0471, 0.1664)

  カメラ検出 赤ブロック Unity座標: (0.2658, 0.1820)
  FK予測 Unity座標: (0.2507, 0.1664)
  実測 Unity座標(自動): (0.2595, 0.2202)
  DX: +8.8mm  DZ: +53.8mm  |err|: 54.5mm

  === 補正処理 ===
  FKエラー: DX=+8.8mm  DZ=+53.8mm
  補正後lookup目標: (0.2570, 0.1283)
[Mac] CORRECT 姿勢制約後候補数: 172点
  補正後角度: [-60.0, 120.0, -35.0, 13.0, -55.0, -20.0]
[AUTO] 青シール3点検出失敗
  [SKIP] 補正後の青シール検出失敗

  === 比較 ===
  補正前: TIP vs 赤ブロック DX=-6.3mm DZ=+38.1mm |err|=38.6mm

補正動作は実行されたが、補正後の姿勢で青シール3点を検出できなかった。補正後の効果は計測できず。

Trial 2（93.6mm改善）

[CAM] 赤ブロック Unity座標: (0.2114, 0.2834)
[LOOKUP] 角度: [-50.0, 98.0, -27.0, -6.0, -53.0, -28.0]
[LOOKUP] FK予測 ee: (0.2303, -0.0338, 0.2031)

  実測 Unity座標(自動): (0.2728, 0.1956)
  DX: +42.5mm  DZ: -7.5mm  |err|: 43.1mm

  === 補正処理 ===
  FKエラー: DX=+42.5mm  DZ=-7.5mm
  補正後lookup目標: (0.1690, 0.2909)
  補正後角度: [-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]
  補正後 実測 Unity座標: (0.2038, 0.2722)
  補正後 vs 赤ブロック: DX=-7.6mm DZ=-11.2mm |err|=13.5mm

  === 比較 ===
  補正前: TIP vs 赤ブロック DX=+61.3mm DZ=-87.8mm |err|=107.1mm
  補正後: TIP vs 赤ブロック DX=-7.6mm DZ=-11.2mm |err|=13.5mm
  改善: +93.6mm

補正前107.1mmから補正後13.5mm。改善量+93.6mm。これだけ大きな誤差を一発で補正できた。補正ロジックが機能していることを示す重要な実証データ。

Trial 3（lookup失敗）

[CAM] 赤ブロック Unity座標: (0.1589, 0.3410)
[Mac] PICK 姿勢制約後候補数: 172点
[SKIP] lookup失敗

マット端に近い位置で、姿勢制約フィルタを通過した172点の中に、目標座標から100mm以内（DIST_THRESHOLD=0.100）の点が存在しなかった。lookupテーブルの空間的なカバレッジ不足。

Trial 4（補正後検出失敗）

[CAM] 赤ブロック Unity座標: (0.2912, 0.1722)
[LOOKUP] 角度: [-45.0, 110.0, -45.0, -7.0, -35.0, -20.0]

  実測 Unity座標(自動): (0.2593, 0.2209)
  DX: +8.6mm  DZ: +54.5mm  |err|: 55.1mm

  === 補正処理 ===
  FKエラー: DX=+8.6mm  DZ=+54.5mm
  補正後lookup目標: (0.2826, 0.1177)
  補正後角度: [-60.0, 120.0, -35.0, 13.0, -55.0, -20.0]
[AUTO] 青シール3点検出失敗
  [SKIP] 補正後の青シール検出失敗

  === 比較 ===
  補正前: TIP vs 赤ブロック DX=-31.9mm DZ=+48.7mm |err|=58.2mm

Trial 1と同じく、補正後の姿勢で青シール検出失敗。補正後角度がTrial 1と完全に同じ[-60.0, 120.0, -35.0, 13.0, -55.0, -20.0]になっている点に注目。補正後lookup目標は異なる(0.2826, 0.1177) vs (0.2570, 0.1283)なのに、最近傍点が同一。

Trial 5（横ばい）

[CAM] 赤ブロック Unity座標: (0.1895, 0.2746)
[LOOKUP] 角度: [-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]

  実測 Unity座標(自動): (0.2010, 0.2738)
  DX: +18.0mm  DZ: +72.0mm  |err|: 74.2mm

  === 補正処理 ===
  FKエラー: DX=+18.0mm  DZ=+72.0mm
  補正後lookup目標: (0.1715, 0.2026)
  補正後角度: [-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]  ← 補正前と同じ
  補正後 実測 Unity座標: (0.2015, 0.2736)
  補正後 vs 赤ブロック: DX=+11.9mm DZ=-1.0mm |err|=12.0mm

  === 比較 ===
  補正前: TIP vs 赤ブロック DX=+11.5mm DZ=-0.7mm |err|=11.5mm
  補正後: TIP vs 赤ブロック DX=+11.9mm DZ=-1.0mm |err|=12.0mm
  改善: -0.5mm

補正前後で角度が完全に同一。lookupが同じ点を返している。元々誤差が11.5mmと小さい領域だったため、改善余地がほぼない。-0.5mmの差は統計的揺らぎの範囲。

Trial 6（lookup失敗）

[CAM] 赤ブロック Unity座標: (0.2075, 0.3126)
[Mac] PICK 姿勢制約後候補数: 172点
[SKIP] lookup失敗

Trial 3と同じ理由でlookup失敗。マット端〜中央寄りでカバレッジ不足。

Trial 7（横ばい）

[CAM] 赤ブロック Unity座標: (0.1278, 0.2835)
[LOOKUP] 角度: [-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]

  実測 Unity座標(自動): (0.2026, 0.2731)
  DX: +19.7mm  DZ: +71.3mm  |err|: 73.9mm

  === 補正処理 ===
  FKエラー: DX=+19.7mm  DZ=+71.3mm
  補正後lookup目標: (0.1081, 0.2122)
  補正後角度: [-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]  ← 補正前と同じ
  補正後 実測 Unity座標: (0.2026, 0.2727)
  補正後 vs 赤ブロック: DX=+74.8mm DZ=-10.8mm |err|=75.6mm

  === 比較 ===
  補正前: TIP vs 赤ブロック DX=+74.8mm DZ=-10.3mm |err|=75.5mm
  補正後: TIP vs 赤ブロック DX=+74.8mm DZ=-10.8mm |err|=75.6mm
  改善: -0.1mm

Trial 5と同じパターン。補正後lookupが補正前と全く同じ角度を返している。誤差が大きい領域（73.9mm）にも関わらず補正が効いていない。

5 trial 集計

完全データ（補正前後とも測定成功）は3 trial（2, 5, 7）。改善量の平均は (93.6 - 0.5 - 0.1) / 3 = +31.0mm。Trial 2の93.6mmが平均値を引き上げているが、3サンプルでは「効きムラの傾向」と断定するには弱い。

「補正後角度=補正前角度」が示すもの

Trial 5/7で補正前と補正後の角度が完全に同一になった。これは候補数172点のフィルタを通過した中で、補正後lookup目標に対する最近傍点が、補正前lookup目標と同じ点を返しているということ。

最初は「候補数172点は絞りすぎでは」と疑った。grepで確認すると、姿勢制約フィルタは以下の閾値で動いていた。

TOL_J2 = 15.0  # ±15度
TOL_J3 = 15.0
TOL_J4 = 15.0
TOL_J5 = 10.0

# フィルタ条件
abs(joints[2] - TARGET_J2) <= 15
abs(joints[3] - TARGET_J3) <= 15
abs(joints[4] - TARGET_J4) <= 15
abs(joints[5] - TARGET_J5) <= 10

意図はj2/j3/j4/j5を「PICK姿勢の角度から±10〜15度以内」で絞り込むこと。グリッパーがブロックに対して上から掴める姿勢を保ち、横向きや斜めから掴む姿勢を排除する。

これは絞りすぎではなく、PICK姿勢として妥当なバリエーション範囲。512,087点中の172点は意図された絞り込みであり、変更すべきではない。

問題は候補数ではなく、補正方向に対するlookupテーブルの空間的な疎密。補正によってlookup目標が大きく変わっても、172点の候補空間内で同じ最近傍点を引いてしまう領域が存在する。

姿勢制約フィルタの妥当性

候補数172点が「絞りすぎではない」という判断は、lookup_nearest_angles_with_ee関数の設計意図とも一致する。

def lookup_nearest_angles_with_ee(unity_x, unity_z, target_ee_y, label=""):
    pose_filtered = [
        (ee_x, ee_y, ee_z, joints) for ee_x, ee_y, ee_z, joints in _fk_table
        if abs(ee_y - target_ee_y) <= EE_Y_RANGE
        and abs(joints[2] - TARGET_J2) <= TOL_J2
        and abs(joints[3] - TARGET_J3) <= TOL_J3
        and abs(joints[4] - TARGET_J4) <= TOL_J4
        and abs(joints[5] - TARGET_J5) <= TOL_J5
    ]

最初にend-effectorのY座標で絞り（PICK_EE_Y=-0.040 ± 0.015）、続いてj2〜j5の角度で絞る。512,087点の中から、PICK姿勢として有効な候補を残す設計。

172点という数字は、この多重フィルタを通過した結果として自然な範囲にある。補正の効きムラを解決するために候補数を増やす方向は、PICK姿勢の制約を緩めることになり、本末転倒。

何が見えてきたか

5 trialで見えたのは、静的な閉ループ補正の限界。

• 効くケース：補正前誤差が大きく、補正後lookup目標が候補空間の異なる領域を指す場合（Trial 2の107.1→13.5mm）

• 効かないケース1：元々誤差が小さく、補正の余地がない場合（Trial 5）

• 効かないケース2：誤差が大きくても、補正後lookup目標が同じ最近傍点を引いてしまう場合（Trial 7）

• 検出失敗ケース：補正後の姿勢で青シールが見えない場合（Trial 1, 4）

• lookup失敗ケース：マット端でカバレッジが不足する場合（Trial 3, 6）

Trial 2で93.6mm改善という強い証拠がある一方、Trial 7のように誤差73.9mmで補正が効かない領域も存在する。「1回補正して終わり」という静的閉ループでは、効きムラを解消できない。

LOG180以降の方向性

5 trialのデータから次の段階の設計が見えてきた。視覚サーボへの移行。

静的な「1回補正して終わり」ではなく、カメラフィードバックを連続的にループさせる方向。

ループ:
  1. カメラフレーム取得
  2. 赤ブロック検出 → pixel座標
  3. グリッパーTIP検出 → pixel座標
  4. pixel差分 = ブロック - TIP
  5. 差分が閾値以下なら → PICK動作 → 終了
  6. 差分から微小角度更新を計算
  7. send_angles で微小移動
  8. ループへ

検討すべき技術課題：

• 微小角度更新の設計（Jacobian逆変換 or 経験則）

• send_anglesの応答性（SPEED=20では遅すぎる可能性）

• 収束判定の閾値設計

• 暴走防止（1回の更新角度を±2度以内に制限）

そして先に解決すべき前提として、補正後の青シール検出失敗の原因究明がある。これは静的補正でも視覚サーボでも共通の問題。手首付近の青シールが、補正後の姿勢で視野外に出るのか、HSV閾値が合わないのか、別途切り分けが必要。

verify_dynamic_pick_auto_correct.py フルコード

#!/usr/bin/env python3
"""
verify_dynamic_pick_auto_correct.py
赤ブロックの位置を変えながら動的PICKの実測を繰り返し、
青シール3点から自動でグリッパー先端位置を計測。
さらにFKエラーを逆算して補正lookupを1回追加し、補正前後を比較する。

操作:
  赤ブロックを置いてEnter → 初回lookup → 実測 → 補正lookup → 再実測
  Enter: 次のtrial  q: 終了してHOME復帰+CSV保存
"""

import os
import sys
import math
import csv
import time
import numpy as np
import cv2
import paramiko
from datetime import datetime
from mat_corner_detection import capture_frames, detect_corners, remove_outliers_and_average

# ---- 設定 ----------------------------------------------------------------
SSH_HOST = "192.168.2.1"
SSH_USER = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
FK_CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
CAMERA_INDEX = 1
SPEED = 20

PICK_EE_Y = -0.040
EE_Y_RANGE = 0.015
DIST_THRESHOLD = 0.100

# 姿勢制約
TARGET_J2 = -33.33
TARGET_J3 = 3.6
TARGET_J4 = -45.11
TARGET_J5 = -30.0
TOL_J2 = 15.0
TOL_J3 = 15.0
TOL_J4 = 15.0
TOL_J5 = 10.0

APPROACH_PICK_BASE = [None, 87.74, -38.33, 3.6, -70.11, -30.0]
HOME = [0, 0, 0, 0, 0, -30]

OUT_W = 1240
OUT_H = 900

MAT_CORNER_NUM_FRAMES = 10
MAT_CORNER_WARMUP_FRAMES = 3
MAT_CORNER_FRAME_INTERVAL = 0.1

WINDOW_NAME = "Dynamic PICK Verify (Auto+Correct)"

# ---- グローバル変数 --------------------------------------------------------
_fk_table = []
_homography = None
_mat_px_range = None


# ---- FK テーブル -----------------------------------------------------------
def load_fk_table():
    global _fk_table
    print("[Mac] FK scan CSV読み込み中...")
    rows = []
    with open(FK_CSV_PATH, "r") as f:
        reader = csv.reader(f)
        next(reader)
        for row in reader:
            ee_x = float(row[0])
            ee_y = float(row[1])
            ee_z = float(row[2])
            joints = [float(row[j + 3]) for j in range(6)]
            rows.append((ee_x, ee_y, ee_z, joints))
    _fk_table = rows
    print(f"[Mac] ✓ {len(_fk_table)}点ロード")


def lookup_nearest_angles_with_ee(unity_x, unity_z, target_ee_y, label=""):
    pose_filtered = [
        (ee_x, ee_y, ee_z, joints) for ee_x, ee_y, ee_z, joints in _fk_table
        if abs(ee_y - target_ee_y) <= EE_Y_RANGE
        and abs(joints[2] - TARGET_J2) <= TOL_J2
        and abs(joints[3] - TARGET_J3) <= TOL_J3
        and abs(joints[4] - TARGET_J4) <= TOL_J4
        and abs(joints[5] - TARGET_J5) <= TOL_J5
    ]
    print(f"[Mac] {label} 姿勢制約後候補数: {len(pose_filtered)}点")

    if not pose_filtered:
        return None, float("inf"), None

    best = None
    best_dist = float("inf")
    for ee_x, ee_y, ee_z, joints in pose_filtered:
        dist = math.sqrt((ee_x - unity_x)**2 + (ee_z - unity_z)**2)
        if dist < best_dist:
            best_dist = dist
            best = (ee_x, ee_y, ee_z, joints)

    if best is None or best_dist > DIST_THRESHOLD:
        return None, best_dist, None

    ee_x, ee_y, ee_z, joints = best
    return joints, best_dist, (ee_x, ee_y, ee_z)


def make_approach_angles(lookup_angles, approach_base):
    return [
        lookup_angles[0], approach_base[1], approach_base[2],
        approach_base[3], approach_base[4], approach_base[5],
    ]


# ---- Homography + マット範囲 -----------------------------------------------
def setup_homography_and_mat_range(cap):
    global _homography, _mat_px_range
    print(f"[Mac] マット四隅検出中... ({MAT_CORNER_NUM_FRAMES}フレーム)")
    frames = capture_frames(cap, MAT_CORNER_NUM_FRAMES,
                            MAT_CORNER_WARMUP_FRAMES, MAT_CORNER_FRAME_INTERVAL)
    all_corners = [c for c in (detect_corners(f) for f in frames) if c is not None]
    if not all_corners:
        print("[Mac] ✗ マット四隅検出失敗")
        return False
    corners = remove_outliers_and_average(all_corners)
    print(f"[Mac] ✓ マット四隅検出: {len(all_corners)}/{MAT_CORNER_NUM_FRAMES} フレーム成功")

    src = np.array(corners, dtype=np.float32)
    dst_mm = np.array([
        [+310, -89],   # UL
        [-310, -89],   # UR
        [-310, +361],  # LR
        [+310, +361],  # LL
    ], dtype=np.float32)
    _homography = cv2.getPerspectiveTransform(src, dst_mm)
    print(f"[Mac] ✓ 動的homography計算完了")

    min_x = int(min(corners[0][0], corners[3][0]))
    max_x = int(max(corners[1][0], corners[2][0]))
    min_y = int(min(corners[0][1], corners[1][1]))
    max_y = int(max(corners[2][1], corners[3][1]))
    _mat_px_range = (min_x, max_x, min_y, max_y)
    print(f"[Mac] ✓ マット範囲: X=[{min_x}, {max_x}] Y=[{min_y}, {max_y}]")
    return True


def pixel_to_unity(px, py):
    pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
    r = cv2.perspectiveTransform(pt, _homography)
    mat_x_mm = float(r[0][0][0])
    mat_z_mm = float(r[0][0][1])
    unity_x = mat_x_mm * 0.001
    unity_z = mat_z_mm * 0.001
    return unity_x, unity_z


# ---- 赤ブロック検出 --------------------------------------------------------
def detect_red_block(frame):
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_red1 = np.array([0, 100, 100])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([160, 100, 100])
    upper_red2 = np.array([180, 255, 255])
    m1 = cv2.inRange(hsv, lower_red1, upper_red1)
    m2 = cv2.inRange(hsv, lower_red2, upper_red2)
    mask = cv2.bitwise_or(m1, m2)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    MIN_AREA = 500
    candidates = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_AREA:
            continue
        x, y, w, h = cv2.boundingRect(cnt)
        cx = x + w // 2
        cy = y + h // 2
        if _mat_px_range is not None:
            min_x, max_x, min_y, max_y = _mat_px_range
            if not (min_x <= cx <= max_x and min_y <= cy <= max_y):
                continue
        candidates.append((area, cx, cy))

    if not candidates:
        return None

    _, cx, cy = max(candidates, key=lambda t: t[0])
    return (cx, cy)


def detect_red_block_unity(cap):
    for _ in range(5):
        cap.read()
    ret, frame = cap.read()
    if not ret:
        return None
    pixel = detect_red_block(frame)
    if pixel is None:
        return None
    cx, cy = pixel
    unity_x, unity_z = pixel_to_unity(cx, cy)
    print(f"  赤ブロック pixel=({cx}, {cy}) → Unity=({unity_x:.4f}, {unity_z:.4f})")
    return (unity_x, unity_z)


# ---- 青シール検出 ----------------------------------------------------------
def detect_blue_markers(frame, mat_px_range, debug_trial=None):
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_blue = np.array([100, 100, 50])
    upper_blue = np.array([130, 255, 255])
    mask = cv2.inRange(hsv, lower_blue, upper_blue)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

    if debug_trial is not None:
        debug_dir = os.path.expanduser("~/r2r2r-scripts/debug_auto")
        os.makedirs(debug_dir, exist_ok=True)
        mask_bgr = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
        masked = cv2.bitwise_and(frame, frame, mask=mask)
        combined = np.hstack([frame, masked, mask_bgr])
        cv2.imwrite(f"{debug_dir}/blue_mask_trial_{debug_trial:03d}.png", combined)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    MIN_AREA = 80
    MAX_AREA = 8000
    candidates = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_AREA or area > MAX_AREA:
            continue
        M = cv2.moments(cnt)
        if M["m00"] == 0:
            continue
        cx = int(M["m10"] / M["m00"])
        cy = int(M["m01"] / M["m00"])
        if mat_px_range is not None:
            min_x, max_x, min_y, max_y = mat_px_range
            if not (min_x <= cx <= max_x and min_y <= cy <= max_y):
                continue
        candidates.append((area, cx, cy))

    if len(candidates) < 3:
        return None

    candidates.sort(key=lambda t: -t[0])
    top3 = [(cx, cy) for _, cx, cy in candidates[:3]]
    return top3


def identify_blue_markers(blue_points, homography):
    sorted_by_y = sorted(blue_points, key=lambda p: p[1], reverse=True)
    claw_a_px = sorted_by_y[0]
    claw_b_px = sorted_by_y[1]
    wrist_px = sorted_by_y[2]

    def to_unity(px, py):
        pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
        r = cv2.perspectiveTransform(pt, homography)
        return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001

    wrist_ux, wrist_uz = to_unity(*wrist_px)
    claw_a_ux, claw_a_uz = to_unity(*claw_a_px)
    claw_b_ux, claw_b_uz = to_unity(*claw_b_px)

    return {
        "wrist": (wrist_px[0], wrist_px[1], wrist_ux, wrist_uz),
        "claw_a": (claw_a_px[0], claw_a_px[1], claw_a_ux, claw_a_uz),
        "claw_b": (claw_b_px[0], claw_b_px[1], claw_b_ux, claw_b_uz),
    }


def get_gripper_tip_unity(markers):
    a_x = markers["claw_a"][2]
    a_z = markers["claw_a"][3]
    b_x = markers["claw_b"][2]
    b_z = markers["claw_b"][3]
    return ((a_x + b_x) / 2, (a_z + b_z) / 2)


def detect_gripper_tip_auto(cap, trial=None):
    for _ in range(5):
        cap.read()
    ret, frame = cap.read()
    if not ret:
        return None, None, None

    blue_points = detect_blue_markers(frame, _mat_px_range, debug_trial=trial)
    if blue_points is None:
        print("[AUTO] 青シール3点検出失敗")
        return None, None, frame

    markers = identify_blue_markers(blue_points, _homography)
    tip_unity = get_gripper_tip_unity(markers)
    return tip_unity, markers, frame


# ---- SSH経由アーム制御 ----------------------------------------------------
def ssh_send_angles(angles, speed=SPEED, wait=5):
    angles_str = str(angles)
    cmd = (
        f"python3 -c \""
        f"from pymycobot.mycobot import MyCobot; import time; "
        f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
        f"mc.send_angles({angles_str}, {speed}); time.sleep({wait}); "
        f"print('ANGLES:', mc.get_angles())\""
    )
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)
    stdin, stdout, stderr = client.exec_command(cmd)
    output = stdout.read().decode("utf-8", errors="ignore").strip()
    error = stderr.read().decode("utf-8", errors="ignore").strip()
    client.close()
    if output:
        print(f"  [Pi] {output}")
    if error and "no longer maintained" not in error:
        print(f"  [Pi stderr] {error}")


def send_home():
    print("[ARM] HOME へ移動...")
    ssh_send_angles(HOME, wait=4)
    print("[ARM] HOME 完了")


# ---- mat_rectify 射影 ----------------------------------------------------
def mm_to_px(mat_x, mat_z):
    out_x = int(round((310 - mat_x) * 2))
    out_y = int(round((mat_z - (-89)) * 2))
    return out_x, out_y


def draw_overlays(img):
    grid_color = (255, 255, 255)
    x_grid_values = [+300, +200, +100, -100, -200, -300]
    z_grid_values = [+100, +200, +300]
    for x_mm in x_grid_values:
        px, _ = mm_to_px(x_mm, 0)
        cv2.line(img, (px, 0), (px, OUT_H), grid_color, 2)
    for z_mm in z_grid_values:
        _, py = mm_to_px(0, z_mm)
        cv2.line(img, (0, py), (OUT_W, py), grid_color, 2)

    cv2.rectangle(img, (0, 0), (OUT_W - 1, OUT_H - 1), (255, 255, 255), 2)

    _, y_axis = mm_to_px(0, 0)
    cv2.line(img, (0, y_axis), (OUT_W, y_axis), (0, 0, 255), 2)
    cv2.putText(img, "X+", (30, y_axis - 10),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
    cv2.putText(img, "X-", (OUT_W - 60, y_axis - 10),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)

    x_axis, _ = mm_to_px(0, 0)
    cv2.line(img, (x_axis, 0), (x_axis, OUT_H), (255, 0, 0), 2)
    cv2.putText(img, "Z-", (x_axis + 10, 30),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2, cv2.LINE_AA)
    cv2.putText(img, "Z+", (x_axis + 10, OUT_H - 15),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2, cv2.LINE_AA)

    ox, oy = mm_to_px(0, 0)
    cv2.circle(img, (ox, oy), 25, (0, 0, 255), 3)
    cv2.line(img, (ox - 20, oy), (ox + 20, oy), (0, 0, 255), 3)
    cv2.line(img, (ox, oy - 20), (ox, oy + 20), (0, 0, 255), 3)
    cv2.putText(img, "ARM(0,0)", (ox + 30, oy - 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)


def capture_rectified(cap):
    for _ in range(5):
        cap.read()
    ret, last_frame = cap.read()
    if not ret:
        return None, None

    rect_corners_mm = np.array([
        [+310, -89], [-310, -89], [-310, +361], [+310, +361]
    ], dtype=np.float32)
    rect_corners_out = np.array([
        [0, 0], [OUT_W, 0], [OUT_W, OUT_H], [0, OUT_H]
    ], dtype=np.float32)
    M_mm_to_out = cv2.getPerspectiveTransform(rect_corners_mm, rect_corners_out)

    # _homography（pixel→mm）と合成
    M_in_to_out = M_mm_to_out @ _homography

    rectified = cv2.warpPerspective(last_frame, M_in_to_out, (OUT_W, OUT_H))
    draw_overlays(rectified)
    return rectified, M_in_to_out


# ---- CSV保存 --------------------------------------------------------------
def save_csv(results):
    ts = datetime.now().strftime("%Y%m%d_%H%M%S")
    csv_path = os.path.expanduser(f"~/r2r2r-scripts/dynamic_pick_correct_{ts}.csv")
    fields = [
        "trial", "angles_init", "angles_correct",
        "camera_unity_x", "camera_unity_z",
        "fk_pred_unity_x", "fk_pred_unity_z",
        "init_measured_x", "init_measured_z",
        "init_dx_mm", "init_dz_mm", "init_err_mm",
        "correct_measured_x", "correct_measured_z",
        "correct_dx_mm", "correct_dz_mm", "correct_err_mm",
    ]
    with open(csv_path, "w", newline="") as f:
        w = csv.DictWriter(f, fieldnames=fields)
        w.writeheader()
        w.writerows(results)
    print(f"\n[CSV保存] {csv_path} ({len(results)}点)")

    init_errs = [r["init_err_mm"] for r in results]
    correct_errs = [r["correct_err_mm"] for r in results if r["correct_err_mm"] != "" and r["correct_err_mm"] is not None]
    if init_errs:
        print(f"  補正前: avg_err={sum(init_errs)/len(init_errs):.1f}mm  max_err={max(init_errs):.1f}mm")
    if correct_errs:
        print(f"  補正後: avg_err={sum(correct_errs)/len(correct_errs):.1f}mm  max_err={max(correct_errs):.1f}mm")


# ---- メイン ----------------------------------------------------------------
def main():
    print("=== Dynamic PICK Verify (Auto+Correct) ===")

    load_fk_table()

    cap = cv2.VideoCapture(CAMERA_INDEX)
    if not cap.isOpened():
        print(f"[ERROR] カメラ index={CAMERA_INDEX} を開けません")
        return
    for _ in range(30):
        cap.read()

    if not setup_homography_and_mat_range(cap):
        cap.release()
        return

    results = []
    trial = 0

    try:
        send_home()

        while True:
            trial += 1
            print(f"\n{'='*60}")
            print(f"[Trial {trial}]")
            print(f"{'='*60}")
            input("赤ブロックをマット上に置いてEnterを押してください: ")

            pick_unity = detect_red_block_unity(cap)
            if pick_unity is None:
                print("[SKIP] 赤ブロック検出失敗")
                continue
            pick_x, pick_z = pick_unity
            print(f"[CAM] 赤ブロック Unity座標: ({pick_x:.4f}, {pick_z:.4f})")

            pick_angles, pick_dist, pick_ee = lookup_nearest_angles_with_ee(
                pick_x, pick_z, PICK_EE_Y, label="PICK")
            if pick_angles is None:
                print("[SKIP] lookup失敗")
                continue
            print(f"[LOOKUP] 角度: {pick_angles}")
            print(f"[LOOKUP] FK予測 ee: ({pick_ee[0]:.4f}, {pick_ee[1]:.4f}, {pick_ee[2]:.4f})")

            approach_angles = make_approach_angles(pick_angles, APPROACH_PICK_BASE)
            print(f"[ARM] APPROACH_PICK へ移動...")
            ssh_send_angles(approach_angles, wait=4)
            print(f"[ARM] PICK へ移動...")
            ssh_send_angles(pick_angles, wait=4)

            pred_unity_x = pick_ee[0]
            pred_unity_z = pick_ee[2]

            tip_unity, markers, raw_frame = detect_gripper_tip_auto(cap, trial=trial)
            if tip_unity is None:
                print(f"[SKIP] グリッパー先端検出失敗")
                send_home()
                continue

            measured_x, measured_z = tip_unity
            print(f"\n  [Trial {trial}]")
            print(f"  カメラ検出 赤ブロック Unity座標: ({pick_x:.4f}, {pick_z:.4f})")
            print(f"  FK予測 Unity座標: ({pred_unity_x:.4f}, {pred_unity_z:.4f})")
            print(f"  実測 Unity座標(自動): ({measured_x:.4f}, {measured_z:.4f})")

            dx_mm = (measured_x - pred_unity_x) * 1000
            dz_mm = (measured_z - pred_unity_z) * 1000
            err_mm = math.sqrt(dx_mm**2 + dz_mm**2)
            print(f"  DX: {dx_mm:+.1f}mm  DZ: {dz_mm:+.1f}mm  |err|: {err_mm:.1f}mm")

            print()
            print(f"  === 補正処理 ===")
            fk_error_x = measured_x - pred_unity_x
            fk_error_z = measured_z - pred_unity_z
            print(f"  FKエラー: DX={fk_error_x*1000:+.1f}mm  DZ={fk_error_z*1000:+.1f}mm")

            corrected_target_x = pick_x - fk_error_x
            corrected_target_z = pick_z - fk_error_z
            print(f"  補正後lookup目標: ({corrected_target_x:.4f}, {corrected_target_z:.4f})")

            correct_angles, correct_dist, correct_ee = lookup_nearest_angles_with_ee(
                corrected_target_x, corrected_target_z, PICK_EE_Y, label="CORRECT")
            if correct_angles is None:
                print(f"  [SKIP] 補正lookup失敗")
                correct_measured_x = None
                correct_measured_z = None
                correct_dx = None
                correct_dz = None
                correct_err = None
            else:
                print(f"  補正後角度: {correct_angles}")
                correct_approach = make_approach_angles(correct_angles, APPROACH_PICK_BASE)
                print(f"  [ARM] APPROACH_PICK(補正) へ移動...")
                ssh_send_angles(correct_approach, wait=4)
                print(f"  [ARM] PICK(補正) へ移動...")
                ssh_send_angles(correct_angles, wait=4)

                tip_unity2, markers2, raw_frame2 = detect_gripper_tip_auto(cap, trial=trial)
                if tip_unity2 is None:
                    print(f"  [SKIP] 補正後の青シール検出失敗")
                    correct_measured_x = None
                    correct_measured_z = None
                    correct_dx = None
                    correct_dz = None
                    correct_err = None
                else:
                    correct_measured_x, correct_measured_z = tip_unity2
                    correct_dx = (correct_measured_x - pick_x) * 1000
                    correct_dz = (correct_measured_z - pick_z) * 1000
                    correct_err = math.sqrt(correct_dx**2 + correct_dz**2)
                    print(f"  補正後 実測 Unity座標: ({correct_measured_x:.4f}, {correct_measured_z:.4f})")
                    print(f"  補正後 vs 赤ブロック: DX={correct_dx:+.1f}mm DZ={correct_dz:+.1f}mm |err|={correct_err:.1f}mm")

            init_dx = (measured_x - pick_x) * 1000
            init_dz = (measured_z - pick_z) * 1000
            init_err = math.sqrt(init_dx**2 + init_dz**2)
            print()
            print(f"  === 比較 ===")
            print(f"  補正前: TIP vs 赤ブロック DX={init_dx:+.1f}mm DZ={init_dz:+.1f}mm |err|={init_err:.1f}mm")
            if correct_err is not None:
                print(f"  補正後: TIP vs 赤ブロック DX={correct_dx:+.1f}mm DZ={correct_dz:+.1f}mm |err|={correct_err:.1f}mm")
                print(f"  改善: {init_err - correct_err:+.1f}mm")

            results.append({
                "trial": trial,
                "angles_init": str(pick_angles),
                "angles_correct": str(correct_angles) if correct_angles else "",
                "camera_unity_x": pick_x,
                "camera_unity_z": pick_z,
                "fk_pred_unity_x": pred_unity_x,
                "fk_pred_unity_z": pred_unity_z,
                "init_measured_x": measured_x,
                "init_measured_z": measured_z,
                "init_dx_mm": round(init_dx, 2),
                "init_dz_mm": round(init_dz, 2),
                "init_err_mm": round(init_err, 2),
                "correct_measured_x": correct_measured_x if correct_measured_x is not None else "",
                "correct_measured_z": correct_measured_z if correct_measured_z is not None else "",
                "correct_dx_mm": round(correct_dx, 2) if correct_dx is not None else "",
                "correct_dz_mm": round(correct_dz, 2) if correct_dz is not None else "",
                "correct_err_mm": round(correct_err, 2) if correct_err is not None else "",
            })

            rectified, M = capture_rectified(cap)
            if rectified is not None:
                for key, color in [("wrist", (255, 0, 0)), ("claw_a", (0, 255, 0)), ("claw_b", (0, 255, 0))]:
                    m = markers[key]
                    unity_x_m, unity_z_m = m[2], m[3]
                    mat_x_mm = unity_z_m * 1000
                    mat_z_mm = unity_x_m * 1000
                    px, py = mm_to_px(mat_x_mm, mat_z_mm)
                    if 0 <= px < OUT_W and 0 <= py < OUT_H:
                        cv2.circle(rectified, (px, py), 8, color, 2)
                        cv2.putText(rectified, key, (px + 12, py + 5),
                                    cv2.FONT_HERSHEY_SIMPLEX, 0.4, color, 1, cv2.LINE_AA)

                tip_mat_x = measured_z * 1000
                tip_mat_z = measured_x * 1000
                tx, ty = mm_to_px(tip_mat_x, tip_mat_z)
                if 0 <= tx < OUT_W and 0 <= ty < OUT_H:
                    cv2.drawMarker(rectified, (tx, ty), (0, 255, 255),
                                   cv2.MARKER_CROSS, 25, 3)
                    cv2.putText(rectified, "TIP", (tx + 15, ty + 15),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1, cv2.LINE_AA)

                pred_mat_x = pred_unity_z * 1000
                pred_mat_z = pred_unity_x * 1000
                fx, fy = mm_to_px(pred_mat_x, pred_mat_z)
                if 0 <= fx < OUT_W and 0 <= fy < OUT_H:
                    cv2.drawMarker(rectified, (fx, fy), (0, 165, 255),
                                   cv2.MARKER_TILTED_CROSS, 20, 2)
                    cv2.putText(rectified, "FK", (fx + 12, fy - 8),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 165, 255), 1, cv2.LINE_AA)

                debug_dir = os.path.expanduser("~/r2r2r-scripts/debug_auto")
                os.makedirs(debug_dir, exist_ok=True)
                debug_path = f"{debug_dir}/trial_{trial:03d}.png"
                cv2.imwrite(debug_path, rectified)
                print(f"  デバッグ画像: {debug_path}")

                corr_str = f" corr={correct_err:.1f}mm" if correct_err is not None else ""
                info_text = f"Trial {trial}: init={init_err:.1f}mm{corr_str}"
                cv2.putText(rectified, info_text, (12, 28),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (200, 200, 200), 1, cv2.LINE_AA)
                cv2.imshow(WINDOW_NAME, rectified)
                cv2.waitKey(1)

            resp = input("次のtrialへ進む場合はEnter / 終了する場合は 'q' + Enter: ").strip().lower()
            if resp == 'q':
                cv2.destroyAllWindows()
                print("\n[終了] HOME 復帰...")
                send_home()
                cap.release()
                if results:
                    save_csv(results)
                return

            cv2.destroyAllWindows()
            send_home()

    except KeyboardInterrupt:
        print("\n[Ctrl+C検出] HOME 復帰...")
        try:
            send_home()
        except Exception:
            pass
    except Exception as e:
        print(f"\n[ERROR] {e}")
        try:
            send_home()
        except Exception:
            pass
    finally:
        cap.release()
        cv2.destroyAllWindows()

    if results:
        save_csv(results)


if __name__ == "__main__":
    main()

5 trialのデータは少ない。だが「補正は効くケースと効かないケースがある」という事実は、サンプル数が増えても本質的に変わらないと思う。次の段階は、静的補正の延長線上ではなく、視覚サーボへの構造変更にある。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究"Real2Render2Real"の概念から着想を得たオマージュです。本プロジェクトは、AIの判断を物理的動作へ変換する設計を検証するもので、現実空間をデジタルツイン化し、仮想空間での大量試行（Real-to-Render）から得た最適解を、実世界のモーター駆動（to-Real）へ還元する「R2R2Rパイプライン」の実装過程を記録・考察します。