LOG139R2R2R2026/4/10

iPhoneカメラをロボットの目にした｜homography.npyでmechArm 270と接続

iPhoneを真上に固定して、ブロックの位置を実世界座標に変換するパイプラインを完成させた。

💡 Key Takeaway

ピクセル座標を実世界mmに変換するホモグラフィキャリブレーションが成立した。カッティングマットの格子を基準点にして、ロボットベースを原点とした座標系を構築。赤ブロックをどこに置いても「ロボットから見てどこにあるか」をカメラが返せるようになった。

やったこと	起きたこと	判断
iPhoneを三脚で真上に固定してキャリブレーションを実施	カッティングマットの5mm格子がそのまま基準点になった	追加マーキング不要。既存環境で完結した
4隅のシールをロボット作業エリアに合わせて貼り直した	ロボットベースを原点とした座標系に変更できた	最初のキャリブレーション範囲が作業エリアの外だったため再設計
detect_block_position.pyを新規作成した	ブロックのWorld座標(-253.1mm, 204.8mm)が即座に返ってきた	camera_judge.pyは役割が違うため分離した
9箇所でブロック位置を計測してCSVに記録した	X: -265mm〜+280mm / Y: -114mm〜+227mmをカバー	ロボットの可動域をほぼカバーするデータが取れた

ピクセル座標では、ロボットは動かせない

— 「見えている」と「わかっている」は別の話だ。

Phase 1でやり遂げたのは「固定した位置にあるブロックを掴む」ことだった。ピック位置はハードコードされた関節角度で、カメラは最後の判定にしか使っていなかった。

それでループの成立は証明できた。

でも「ブロックをどこに置いても掴める」には届いていなかった。カメラが「赤いものがある」と言えても、それが「ロボットから見てどこにあるか」は別の問いだ。

ピクセル座標と実世界座標の間には変換が必要で、その変換にはキャリブレーションが必要だ。

カッティングマットが基準点になった

— 買い足すものは何もなかった。

真上から撮影するというアプローチを選んだのは、ホモグラフィ変換の性質のためだ。

ホモグラフィは「平面 → 平面」の変換を前提にしている。カメラが斜めだと遠近歪みが入り、奥のブロックと手前のブロックで誤差のスケールが変わる。真上なら歪みがほぼゼロで、全域で誤差が均一になる。

三脚を用意して、iPhoneを真上に固定した。

撮影したとき、カッティングマットの格子が映り込んでいた。5mm格子だ。

基準点として使う条件：
  ✓ 実世界座標が既知
  ✓ 均一間隔
  ✓ 視認性が高い

3つ全部揃っていた。追加マーキングは不要だった。

calibrate_homography.py を起動して、カメラ映像上で4点をクリック。対応する実世界座標を入力するだけで、ホモグラフィ行列が homography.npy として保存された。

# calibrate_homography.py（抜粋）
cap = cv2.VideoCapture(1)  # iPhone via Iriun Webcam
cv2.setMouseCallback("Calibration", mouse_callback)

# 4点クリック後、実世界座標を入力
src_points = np.array(pixel_points, dtype=np.float32)
dst_points = np.array(world_points, dtype=np.float32)

homography_matrix, _ = cv2.findHomography(src_points, dst_points)
np.save("~/r2r2r-scripts/homography.npy", homography_matrix)

座標系を、ロボットに合わせた

— 「マット基準」では使い物にならないと気づいた。

最初のキャリブレーションはマットの左上を(0,0)とした座標系で行った。

結果を確認してすぐ問題に気づいた。ロボットが実際にブロックを掴む位置が、キャリブレーション範囲の外だった。座標がマイナスになって返ってくる。

設計としておかしい。ロボットと対話するための座標系なら、ロボットが原点でなければならない。

シールを貼り直した。

新しい座標系：
  原点 (0, 0)  = ロボットベース中心
  X+           = 右方向
  X-           = 左方向
  Y+           = 手前方向
  Y-           = 奥方向

4隅のシールをロボットの作業エリアをカバーする範囲に移動させ、マットの目盛りでロボットベースからの距離を実測した。

1（左奥）: (-300, -150)
2（右奥）: ( 300, -150)
3（左手前）: (-300,  250)
4（右手前）: ( 300,  250)

キャリブレーションをやり直した。

「見る」から「わかる」へ

— 数字が返ってきた瞬間、何かが変わった。

detect_block_position.py を実行した。

# detect_block_position.py（抜粋）
homography_matrix = np.load("~/r2r2r-scripts/homography.npy")

# HSV赤検出
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask1 = cv2.inRange(hsv, [0,100,100],   [10,255,255])
mask2 = cv2.inRange(hsv, [160,100,100], [180,255,255])
red_mask = cv2.bitwise_or(mask1, mask2)

# 重心取得
M = cv2.moments(max_contour)
cx, cy = int(M["m10"]/M["m00"]), int(M["m01"]/M["m00"])

# ホモグラフィ変換
pt = np.array([[[cx, cy]]], dtype=np.float32)
world = cv2.perspectiveTransform(pt, homography_matrix)
world_x, world_y = world[0][0]

Pixel: (326, 875)
World: (-253.1 mm, 204.8 mm)

マットの目盛りで実測した値と一致していた。

ロボットベースの左253mm、手前205mmにブロックがある。カメラがそれを見て、数字として返している。

これまでのカメラは「ある/ない」しか言えなかった。今は「どこに」まで言える。

精度より体験の成立を優先する、がこのプロジェクトの哲学だ。±数mmの誤差はある。でも「位置をカメラで読む」というループ自体は成立した。

9箇所のデータを収集した

— データが溜まり始めると、次の問いが見えてくる。

collect_block_positions.py で、ブロックをさまざまな位置に置きながらCSVに記録した。

# collect_block_positions.py（抜粋）
# CSVに自動追記
with open("block_positions.csv", "a", newline="") as f:
    writer = csv.writer(f)
    writer.writerow([trial_id, datetime.now().isoformat(),
                     f"{world_x:.1f}", f"{world_y:.1f}", ""])

# block_positions.csv
trial_id, timestamp,           block_x_mm, block_y_mm, result
1,        2026-04-10T19:19:xx, -155.7,     182.5,      
2,        2026-04-10T19:19:xx, -213.8,      58.1,
...

Trial 1: World(-155.7mm,  182.5mm)
Trial 2: World(-213.8mm,   58.1mm)
Trial 3: World(-265.3mm,  -83.9mm)
Trial 4: World(  91.7mm,  205.1mm)
Trial 5: World( 208.0mm,    6.9mm)
Trial 6: World( 207.6mm, -100.1mm)
Trial 7: World( 280.2mm,  -16.2mm)
Trial 8: World(-157.6mm,  227.2mm)
Trial 9: World(-239.0mm, -113.7mm)

X軸: -265mm から +280mm Y軸: -114mm から +227mm

ロボットの可動域をほぼカバーする範囲が取れた。

次はこの座標でロボットアームを動かすことだ。send_coords() でエンドエフェクタのXYZを直接指定できれば、自分でIKを実装しなくてもいい。

「カメラがブロックの位置を読む → ロボットがそこへ動く」

そのパイプラインの、前半が今完成した。

コード全文

calibrate_homography.py

#!/usr/bin/env python3
"""
カメラキャリブレーションスクリプト
iPhoneカメラ（Iriun Webcam、カメラインデックス1）で撮影したカッティングマット画像から、
ピクセル座標→実世界座標（mm）のホモグラフィ行列を生成・保存するスクリプト。
"""

import cv2
import numpy as np
import sys
import os


class HomographyCalibrator:
    def __init__(self):
        self.points_pixel = []
        self.points_world = []
        self.frame = None
        self.frame_display = None
        self.window_name = "Calibration"

    def mouse_callback(self, event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            if len(self.points_pixel) < 4:
                self.points_pixel.append([x, y])
                cv2.circle(self.frame_display, (x, y), 5, (0, 0, 255), -1)
                cv2.putText(self.frame_display, str(len(self.points_pixel)),
                           (x + 10, y - 10), cv2.FONT_HERSHEY_SIMPLEX,
                           0.5, (0, 0, 255), 2)
                cv2.imshow(self.window_name, self.frame_display)
                print(f"Point {len(self.points_pixel)} clicked: ({x}, {y})")

    def run(self):
        cap = cv2.VideoCapture(1)
        if not cap.isOpened():
            print("エラー: カメラが見つかりません (index 1)")
            sys.exit(1)

        print("カメラが接続されました。")
        print("画像上の基準点を4点順番にクリックしてください。")
        print("キー操作:")
        print("  r: クリック点をリセット")
        print("  q: キャンセルして終了")

        cv2.namedWindow(self.window_name)
        cv2.setMouseCallback(self.window_name, self.mouse_callback)

        while True:
            ret, frame = cap.read()
            if not ret:
                continue

            self.frame = frame.copy()
            self.frame_display = frame.copy()

            for i, point in enumerate(self.points_pixel):
                cv2.circle(self.frame_display, tuple(point), 5, (0, 0, 255), -1)
                cv2.putText(self.frame_display, str(i + 1),
                           (point[0] + 10, point[1] - 10),
                           cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)

            status = f"Points: {len(self.points_pixel)}/4"
            cv2.putText(self.frame_display, status, (10, 30),
                       cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)

            cv2.imshow(self.window_name, self.frame_display)
            key = cv2.waitKey(1) & 0xFF

            if len(self.points_pixel) == 4:
                print("\n4点のクリックが完了しました。")
                break

            if key == ord('r'):
                self.points_pixel = []
                print("クリック点をリセットしました。")

            if key == ord('q'):
                cap.release()
                cv2.destroyAllWindows()
                print("キャリブレーションがキャンセルされました。")
                sys.exit(0)

        cap.release()
        cv2.destroyAllWindows()
        self.input_world_coordinates()
        self.calculate_homography()
        self.save_homography()

    def input_world_coordinates(self):
        print("\n対応する実世界座標（mm）を入力してください。")
        print("形式: x,y（例: 0,0 → 100,0 → 100,100 → 0,100）")

        for i in range(4):
            while True:
                try:
                    user_input = input(f"Point {i + 1} の実世界座標 (x,y): ").strip()
                    parts = user_input.split(',')
                    if len(parts) != 2:
                        print("エラー: 形式が不正です。x,y の形式で入力してください。")
                        continue
                    x = float(parts[0].strip())
                    y = float(parts[1].strip())
                    self.points_world.append([x, y])
                    print(f"  入力: ({x}, {y})")
                    break
                except ValueError:
                    print("エラー: 数値を入力してください。")

    def calculate_homography(self):
        src_points = np.array(self.points_pixel, dtype=np.float32)
        dst_points = np.array(self.points_world, dtype=np.float32)
        self.homography_matrix, status = cv2.findHomography(src_points, dst_points)
        if self.homography_matrix is None:
            print("エラー: ホモグラフィ行列の計算に失敗しました。")
            sys.exit(1)
        print("\nホモグラフィ行列が計算されました:")
        print(self.homography_matrix)

    def save_homography(self):
        output_path = os.path.expanduser("~/r2r2r-scripts/homography.npy")
        try:
            np.save(output_path, self.homography_matrix)
            print(f"\nホモグラフィ行列を保存しました: {output_path}")
            print("キャリブレーションが完了しました。")
        except Exception as e:
            print(f"エラー: ホモグラフィ行列の保存に失敗しました。詳細: {e}")
            sys.exit(1)


def main():
    calibrator = HomographyCalibrator()
    calibrator.run()


if __name__ == "__main__":
    main()

detect_block_position.py

#!/usr/bin/env python3
"""
赤ブロック位置検出スクリプト
iPhoneカメラ（インデックス1）でマット全体を撮影し、
赤ブロックの重心ピクセル座標をホモグラフィ行列で
実世界XY座標（mm）に変換して出力するスクリプト。
"""

import cv2
import numpy as np
import sys
import os


def load_homography():
    homography_path = os.path.expanduser("~/r2r2r-scripts/homography.npy")
    if not os.path.exists(homography_path):
        print("homography.npyが見つかりません。calibrate_homography.pyを先に実行してください。")
        sys.exit(1)
    try:
        return np.load(homography_path)
    except Exception as e:
        print(f"エラー: homography.npyの読み込みに失敗しました。詳細: {e}")
        sys.exit(1)


def pixel_to_world(pixel_point, homography_matrix):
    pt = np.array([[pixel_point]], dtype=np.float32)
    world = cv2.perspectiveTransform(pt, homography_matrix)
    return world[0][0]


def detect_red_block(frame):
    hsv_frame = 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])

    mask1 = cv2.inRange(hsv_frame, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv_frame, lower_red2, upper_red2)
    red_mask = cv2.bitwise_or(mask1, mask2)

    contours, _ = cv2.findContours(red_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        print("赤ブロックが検出されませんでした")
        return None

    max_contour = max(contours, key=cv2.contourArea)
    if cv2.contourArea(max_contour) < 500:
        print("赤ブロックが検出されませんでした")
        return None

    M = cv2.moments(max_contour)
    if M["m00"] == 0:
        print("赤ブロックが検出されませんでした")
        return None

    cx = int(M["m10"] / M["m00"])
    cy = int(M["m01"] / M["m00"])
    return cx, cy


def main():
    homography_matrix = load_homography()

    cap = cv2.VideoCapture(1)
    if not cap.isOpened():
        print("カメラが見つかりません (index 1)")
        sys.exit(1)

    for _ in range(30):
        cap.read()

    ret, frame = cap.read()
    if not ret:
        print("フレームの読み込みに失敗しました")
        cap.release()
        sys.exit(1)

    centroid = detect_red_block(frame)
    if centroid is None:
        cap.release()
        sys.exit(1)

    cx, cy = centroid
    world_x, world_y = pixel_to_world((cx, cy), homography_matrix)

    print(f"Pixel: ({cx}, {cy})")
    print(f"World: ({world_x:.1f} mm, {world_y:.1f} mm)")

    cap.release()


if __name__ == "__main__":
    main()

collect_block_positions.py

#!/usr/bin/env python3
"""
赤ブロック位置データ収集スクリプト
赤ブロックをさまざまな位置に置いて、
カメラで検出したXY座標をCSVに記録するデータ収集スクリプト。
"""

import cv2
import numpy as np
import sys
import os
import csv
from datetime import datetime


def load_homography():
    homography_path = os.path.expanduser("~/r2r2r-scripts/homography.npy")
    if not os.path.exists(homography_path):
        print("homography.npyが見つかりません")
        sys.exit(1)
    try:
        return np.load(homography_path)
    except Exception as e:
        print(f"エラー: homography.npyの読み込みに失敗しました。詳細: {e}")
        sys.exit(1)


def pixel_to_world(pixel_point, homography_matrix):
    pt = np.array([[pixel_point]], dtype=np.float32)
    world = cv2.perspectiveTransform(pt, homography_matrix)
    return world[0][0]


def detect_red_block(frame):
    hsv_frame = 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])

    mask1 = cv2.inRange(hsv_frame, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv_frame, lower_red2, upper_red2)
    red_mask = cv2.bitwise_or(mask1, mask2)

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

    max_contour = max(contours, key=cv2.contourArea)
    if cv2.contourArea(max_contour) < 500:
        return None

    M = cv2.moments(max_contour)
    if M["m00"] == 0:
        return None

    cx = int(M["m10"] / M["m00"])
    cy = int(M["m01"] / M["m00"])
    return cx, cy


def get_next_trial_id(csv_path):
    if not os.path.exists(csv_path):
        return 1
    try:
        with open(csv_path, 'r', encoding='utf-8') as f:
            rows = list(csv.reader(f))
            return len(rows) if len(rows) > 1 else 1
    except Exception:
        print("警告: CSVファイルの読み込みに失敗しました。trial_idを1から開始します。")
        return 1


def write_csv_header(csv_path):
    if not os.path.exists(csv_path):
        try:
            with open(csv_path, 'w', newline='', encoding='utf-8') as f:
                csv.writer(f).writerow(['trial_id', 'timestamp', 'block_x_mm', 'block_y_mm', 'result'])
        except Exception as e:
            print(f"エラー: CSVファイルの作成に失敗しました。詳細: {e}")
            sys.exit(1)


def append_csv_row(csv_path, trial_id, block_x, block_y):
    try:
        with open(csv_path, 'a', newline='', encoding='utf-8') as f:
            csv.writer(f).writerow([trial_id, datetime.now().isoformat(),
                                    f"{block_x:.1f}", f"{block_y:.1f}", ""])
    except Exception as e:
        print(f"エラー: CSVファイルへの書き込みに失敗しました。詳細: {e}")
        sys.exit(1)


def main():
    homography_matrix = load_homography()

    cap = cv2.VideoCapture(1)
    if not cap.isOpened():
        print("カメラが見つかりません (index 1)")
        sys.exit(1)

    csv_path = os.path.expanduser("~/r2r2r-scripts/block_positions.csv")
    write_csv_header(csv_path)
    trial_id = get_next_trial_id(csv_path)

    print("赤ブロック位置データ収集スクリプト")
    print(f"保存先: {csv_path}")
    print()

    while True:
        user_input = input("ブロックを置いてEnterを押してください（qで終了）: ").strip()
        if user_input.lower() == 'q':
            break

        for _ in range(30):
            cap.read()

        ret, frame = cap.read()
        if not ret:
            print("フレームの読み込みに失敗しました")
            continue

        centroid = detect_red_block(frame)
        if centroid is None:
            print("検出失敗。再度試してください。")
            continue

        cx, cy = centroid
        world_x, world_y = pixel_to_world((cx, cy), homography_matrix)

        print(f"Trial {trial_id}: Pixel({cx}, {cy}) → World({world_x:.1f}mm, {world_y:.1f}mm)")
        append_csv_row(csv_path, trial_id, world_x, world_y)
        trial_id += 1
        print()

    print(f"{trial_id - 1}件のデータを記録しました。")
    cap.release()


if __name__ == "__main__":
    main()