おもちゃのロボットアームでフィジカルAIを網羅的に勉強したい 〜 その４：ロボットの目編

はじめに：ロボットの目

前回の記事で触れた通り、ロボットアームの制御において、コントローラへ送信されるコマンド列の多くは具体的な座標値によって構成されます。

Gemini Robotics-ERなどのVLM(Vision Language Model)においても、ロボット前方をキャプチャした画像を参照し、人間からの指示（テキストや音声）に従ってロボットを動かすための軌道（座標の配列）を生成します。そして、それがコマンド列や各関節の角度へ翻訳されコントローラへ送信されます。この際、画像から特定した対象物の位置を、現実世界の座標へと正確に変換する必要があります。ここで重要となるのが、 「カメラの画像歪み」の除去と、「カメラ姿勢推定」 の精度です。

本記事では、ロボットの「目」の精度を高めるための一連の画像処理プロセスをまとめます。

カメラにおける「歪み」の現状

まず、安価な広角USBカメラ（水平FOV 120度）と、一般的なスマートフォン（Pixel 9a）のキャプチャ画像を比較します。対象は100円ショップで購入したA4サイズのカッターマットです。

USBカメラによる撮影

USBカメラの画像では、レンズの特性上、周辺部に向かって大きな樽型の歪みが生じていることが確認できます。このままの状態では、画像上のピクセル座標を等間隔な物理座標として扱うことができません。

スマートフォン（Pixel 9a）の広角レンズによる撮影

一方で、スマートフォンのカメラでは、内部の画像処理エンジンによって歪みが高度に補正されており、格子状の目盛りが直線として捉えられています。

以前取り組んだUnityによるシミュレータ環境では、カメラ画像(FOV 60度)に歪みが存在しないため、画像上の座標 $(u, v)$ をテーブル上の物理座標へ容易に変換できました。

しかし、今回の安価な広角USBカメラによる構成では、ソフトウェア側での補正が不可欠となります。

今回は広角カメラをロボットの目とする

VLMモデルの研究では Intel RealSense（水平FOV 86°前後）等のカメラが一般的なようですが、本プロジェクトでは水平FOV 120度を選択しましす。これは、多様なユースケースを考える場合、固定的な視点、固定的な作業エリアではなく、移動・歩行ロボットまでを見据えた方が良いと考えたからです。

「俯瞰」と「注視」を使い分ける

かつての建設現場における物体検出の実験経験から、広角で捉えた画像を 切り出す（クロッピング） 手法が、精度と安定性の両立に最も有効だと考えています。

• 広域（周辺視野）の役割：コンテキスト把握
  自身の腕・対象・目標地点を同一フレームに収める。

• 注視（中心視野）の役割：リソース最適化
  特定領域のみを高精度に切り出し、計算負荷を抑えつつ緻密に解析。

人間の目が周辺視野で状況を掴み、中心視野で凝視するように、まずは「空間を広く定義する」ことが、動的な実環境でAIを機能させるための土台となるのではと考えました。

深度情報について

ロボット用カメラとしてはステレオカメラ等の採用が一般的ですが、本プロジェクトでは、コストを優先し、標準的なWebカメラのみで深度情報を取得する手法を模索します。

過去の経験を思い出せば、スマートフォンを用いた建設現場向けAI物体検出アプリ試作時にも同様の課題がありました。一般にスマホはLiDARを搭載していないのが標準であり、たとえ搭載していたとしても、その有効測定距離は最大8m程度に留まります。広大な現場で8mでは役に立ちません。

そこで、当時の開発では 、水平で平坦な場所にしか適用出来ない制約はありましたが、「AIが検知した物体のバウンディングボックス」を手がかりに深度を推定する手法を試み、良好な結果を得ました。また、大きさが既知の物体であれば、3Dで座標を推定することも可能でした（スタジア測量の応用）。

また、建設現場向けARアプリを試作し現場で実験した際、QRコードのマーカーを用いたスマホカメラ姿勢推定、および、ARが取得した点群情報より、対象物の3D座標を高精度（cm精度)で取得出来たのが印象的でした。今回も、マーカーを用いたカメラ位置推定や深度推定を模索します。

カメラ姿勢推定の重要性

画像上のピクセル座標を実際の物理座標と一致させるには、その画像を「どの位置から、どの向きで」撮影したかというカメラ姿勢（位置と向き）を特定しなければなりません。

シミュレータ上では、カメラは剛性のあるフレームに固定されており、そのパラメータは既知です。

しかし、今回の4DoFロボットアームの環境では、カメラを三脚やクランプで簡易的に固定する運用を想定しています。この場合、設置のたびにカメラの位置や向きが微妙に変化するため、固定値としてのパラメータを持つことができません。

このような「動的な環境」で有効なのが、AR技術で培われた姿勢推定の技術です。

基準点としてのマーカー

移動型ロボットにおいても、カメラの自己位置を特定するための「基準点」が必要になります。私は以前、工事現場向けARアプリを開発した際、QRコードを基準点（マーカー）として現場に配置していました。 今回は、低感度かつ低解像度なカメラでも堅牢に動作させるため、QRコードよりも空間認識に特化した 「ArUco（アルコ）マーカー」 を採用します。

実装方針と要件の整理

OpenCVを活用し、以下の3点を実装します。

• 画像歪み補正（キャリブレーション）
• ArUcoマーカーを用いたカメラ姿勢推定
• 画像上の任意地点からテーブル物理座標への変換（逆投影）

生成AI時代においては、こうした要件さえ整理できれば、具体的な実装コードの多くはAIが生成してくれます。重要なのは、システム全体の網羅的な理解と、個別の課題に対する要件定義の精度です。

歪み補正の実装

物体検出によって得られた座標をロボットの逆運動学（IK）の入力とするため、まずはレンズ由来の歪みを取り除きます。

チェスボードパターンの準備

歪み補正（キャリブレーション）には、交点の位置が明確なチェスボードパターンを使用します。OpenCVのアルゴリズムは「マス目の境界（内角）」を特徴点として抽出するため、10列×7行のマス目に対して、 内部コーナー数（9×6） をパラメータとして指定し、パターンを定義します。

チェスボード作成のコード（generate_chessboard.py）

import cv2
import numpy as np
import argparse

def generate_chessboard(cols=9, rows=6, square_size_px=100, margin_px=100, darkness=0):
    """
    指定されたパラメータでチェスボード画像を生成します。
    :param cols: 列数（正方形の数）。
    :param rows: 行数（正方形の数）。
    :param square_size_px: 1つの正方形のサイズ（ピクセル単位）。
    :param margin_px: 外側の余白（ピクセル単位）。
    :param darkness: 暗い正方形のピクセル値 (0-255)。
    :return: NumPy配列としてのチェスボード画像。
    """
    # 余白を含む画像の合計サイズを計算
    width = cols * square_size_px + 2 * margin_px
    height = rows * square_size_px + 2 * margin_px
    image = np.ones((height, width), dtype=np.uint8) * 255

    # 黒い正方形を描画
    for r in range(rows):
        for c in range(cols):
            # (行 + 列) が奇数の場合、正方形を黒く塗る
            if (r + c) % 2 == 1:
                y1 = margin_px + r * square_size_px
                y2 = y1 + square_size_px
                x1 = margin_px + c * square_size_px
                x2 = x1 + square_size_px
                image[y1:y2, x1:x2] = darkness
    return image

def main():
    """
    引数を解析してチェスボードを生成するメイン関数。
    """
    parser = argparse.ArgumentParser(description="カメラキャリブレーション用のチェスボードパターン画像を生成します。", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument("--cols", type=int, default=10, help="列数（正方形の数）。")
    parser.add_argument("--rows", type=int, default=7, help="行数（正方形の数）。")
    parser.add_argument("--size", type=int, default=150, help="各正方形のサイズ（ピクセル単位）。")
    parser.add_argument("--margin", type=int, default=100, help="ボード周囲の余白（ピクセル単位）。")
    parser.add_argument("--darkness", type=int, default=0, help="黒い正方形の暗さレベル (0-255)。")
    parser.add_argument("--output", type=str, help="出力ファイル名。デフォルトは 'chessboard_{cols}x{rows}.png'")
    parser.add_argument("--no-display", action="store_true", help="生成された画像を表示しません。")

    args = parser.parse_args()

    # 出力ファイル名を決定
    if args.output:
        file_name = args.output
    else:
        file_name = f"chessboard_{args.cols}x{args.rows}.png"

    # キャリブレーションパターンのサイズに関するユーザーへの注記
    print(f"{args.cols}x{args.rows} の正方形ボードを生成しています。")
    print(f"注: OpenCVのキャリブレーションでは、パターンサイズは ({args.cols - 1}, {args.rows - 1}) になります。")

    # 画像を生成
    chessboard_img = generate_chessboard(
        cols=args.cols,
        rows=args.rows,
        square_size_px=args.size,
        margin_px=args.margin,
        darkness=args.darkness
    )

    # 画像を保存
    cv2.imwrite(file_name, chessboard_img)
    print(f"チェスボードを正常に保存しました: {file_name}")

    # 抑制されていない限り画像を表示
    if not args.no_display:
        cv2.imshow("Chessboard Pattern", chessboard_img)
        print("ウィンドウを閉じるには何かキーを押してください。")
        cv2.waitKey(0)
        cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

このコードを実行しチェスボードの画像を作成します。

$ python generate_chessboard.py

キャリブレーションの実行

先ほど作成したチェスボードと以下のコードにより、カメラの歪みを補正（キャリブレーション）していきます。

歪み補正のコード（camera_calib.py）

import numpy as np
import cv2
import argparse

def run_calibration(chessboard_size=(9, 6), square_size=25.0, output_filename="calibration_data.npz", camera_source=0):
    """
    チェスボードパターンを使用してカメラキャリブレーションを実行します。

    Args:
        chessboard_size (tuple): チェスボードの行と列ごとの内部コーナーの数 (corners_x, corners_y)。
        square_size (float): 正方形の1辺のサイズ（ミリメートル単位）。
        output_filename (str): キャリブレーション結果を保存するパス（.npzファイル）。
        camera_source (int or str): cv2.VideoCapture用のカメラインデックス(int)またはデバイスパス(str)。
    """
    # オブジェクトポイントを準備します。(0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) のような形式です。
    objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
    objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2) * square_size

    # 全画像のオブジェクトポイントと画像ポイントを格納する配列。
    objpoints, imgpoints = [], []
    # カメラを初期化
    cap = cv2.VideoCapture(camera_source)

    print("'s'キーで20枚の画像を撮影してください。撮影ごとにボードの角度や距離を変えてください。'q'で終了します。")
    count = 0
    gray = None
    last_captured_frame = None
    while count < 20:
        ret, frame = cap.read()
        if not ret: break
        original_frame = frame.copy()
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        # チェスボードのコーナーを検出
        ret_found, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
        if ret_found:
            cv2.drawChessboardCorners(frame, chessboard_size, corners, ret_found)
        cv2.imshow('Calibration', frame)
        key = cv2.waitKey(1) & 0xFF
        # 's'キーでキャリブレーション用に現在のフレームを保存
        if key == ord('s') and ret_found:
            objpoints.append(objp)
            imgpoints.append(corners)
            last_captured_frame = original_frame
            count += 1
            print(f"撮影完了 {count}/20。ボードを動かしてください。")
        elif key == ord('q'): break

    cap.release()
    cv2.destroyAllWindows()

    # 十分なデータがある場合、キャリブレーションを実行
    if len(objpoints) > 0 and gray is not None:
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
        # カメラ行列と歪み係数を保存
        np.savez(output_filename, mtx=mtx, dist=dist)
        print(f"{output_filename} を保存しました")

        # 最後に撮影したフレームで結果を可視化
        if last_captured_frame is not None:
            h, w = last_captured_frame.shape[:2]
            # 画像サイズに基づいてカメラ行列を調整
            newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
            # 歪み補正
            dst = cv2.undistort(last_captured_frame, mtx, dist, None, newcameramtx)
            cv2.imshow('Original', last_captured_frame)
            cv2.imshow('Undistorted', dst)
            cv2.waitKey(0)
            cv2.destroyAllWindows()

def run_view_mode(calibration_file="calibration_data.npz", camera_source=0):
    """
    キャリブレーションデータを読み込み、歪み補正されたライブ映像を表示します。
    """
    try:
        data = np.load(calibration_file)
        mtx = data['mtx']
        dist = data['dist']
        print(f"{calibration_file} からキャリブレーションデータを読み込みました")
    except FileNotFoundError:
        print(f"エラー: キャリブレーションファイル '{calibration_file}' が見つかりません。先にキャリブレーションを実行してください。")
        return
    except Exception as e:
        print(f"キャリブレーションデータの読み込みエラー: {e}")
        return

    cap = cv2.VideoCapture(camera_source)
    if not cap.isOpened():
        print(f"エラー: カメラソース {camera_source} を開けませんでした")
        return

    print("'q'キーを押して終了します。")
    newcameramtx = None

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

        h, w = frame.shape[:2]
        if newcameramtx is None:
            newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))

        dst = cv2.undistort(frame, mtx, dist, None, newcameramtx)
        
        # 横に並べて結合
        combined = np.hstack((frame, dst))
        
        cv2.imshow('Original (Left) vs Undistorted (Right)', combined)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="チェスボードパターンを使用したカメラキャリブレーション。")
    parser.add_argument("--corners-x", type=int, default=9, help="X軸方向の内部コーナーの数（デフォルト: 9）。")
    parser.add_argument("--corners-y", type=int, default=6, help="Y軸方向の内部コーナーの数（デフォルト: 6）。")
    parser.add_argument("--size", type=float, default=25.0, help="正方形の1辺のサイズ（ミリメートル単位、デフォルト: 25.0）。")
    parser.add_argument("--output", type=str, default="calibration_data.npz", help="キャリブレーションデータの出力ファイル（デフォルト: calibration_data.npz）。")
    parser.add_argument("--source", default="0", help="カメラソース（インデックスまたはデバイスパス、デフォルト: 0）。")
    parser.add_argument("--view", action="store_true", help="ビューモード: キャリブレーションデータを読み込み、歪み補正されたライブ映像を表示します。")
    args = parser.parse_args()

    chessboard_size = (args.corners_x, args.corners_y)
    
    # source引数が数字の場合はintに変換（インデックス用）、それ以外は文字列のまま（パス用）
    source = int(args.source) if args.source.isdigit() else args.source

    if args.view:
        run_view_mode(calibration_file=args.output, camera_source=source)
    else:
        print(f"{chessboard_size[0]}x{chessboard_size[1]} のコーナーパターンを探しています。")
        run_calibration(
            chessboard_size=chessboard_size,
            square_size=args.size,
            output_filename=args.output,
            camera_source=source
        )

$ python camera_calib.py --size 28.86

上記では印刷したチェスボードのマス目サイズ28.86mmを指定しています（精度を高めるため、定規で９マス分の長さを測り、９で割り算しました）。

このスクリプト実行中、作成したボードをカメラにかざし、角度や距離を変えながら20枚程度の画像をキャプチャします。OpenCVの calibrateCamera 関数により、カメラ行列（内部パラメータ）と歪み係数が算出され、.npz ファイルとして保存されます。

これにより、120度の広角カメラでも直線が直線として描写される「歪み補正済み画像」が得られるようになります。以下、左が補正前、右が補正後の画像です。

カメラ姿勢推定と座標変換の実装

歪みが補正された後、空間的な「基準」を設けるためにArUcoマーカーを導入します。

なぜArUcoマーカーなのか

ARアプリではQRコードがマーカーとして多用されますが、QRコードと比較し、ArUcoマーカーは以下の点で空間認識に優れています。

比較項目	QRコード	ArUcoマーカー
設計目的	データ格納	自己位置推定 (Pose Estimation)
推定精度	角の検出精度が限定的	サブピクセル精度で安定
ロバスト性	複雑なためボケに弱い	単純な格子模様のためボケに強い

ArUCOマーカ作成のコード（generate_aruco_markers.py）

import cv2
import numpy as np
import argparse
import sys

if __name__ == "__main__":
    """
    ArUcoマーカー画像を生成します。
    Args:
        --id (int): マーカーID（デフォルト: 10）。
        --size (int): マーカーサイズ（デフォルト: 300）。
        --darkness (int): 暗さレベル（ピクセル値 0-255、デフォルト: 0）。
    """
    parser = argparse.ArgumentParser(description="ArUcoマーカー画像を生成します")
    parser.add_argument("--id", type=int, default=10, help="マーカーID（デフォルト: 10）")
    parser.add_argument("--size", type=int, default=300, help="マーカーサイズ（デフォルト: 300）")
    parser.add_argument("--darkness", type=int, default=0, help="暗さレベル（ピクセル値 0-255、デフォルト: 0）")

    if len(sys.argv) == 1:
        parser.print_help(sys.stderr)
        sys.exit(1)

    args = parser.parse_args()

    marker_id = args.id
    marker_size = args.size

    # 定義済みの辞書を読み込みます（4x4ビット、50マーカー）
    # OpenCV 4.7.0以降のAPI変更に対応
    try:
        aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
    except AttributeError:
        # 古いOpenCVバージョンの場合
        aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)

    # レガシーサポート用に画像を初期化（古いOpenCVバージョンではdrawMarkerに事前に割り当てられた画像が必要です）
    img = np.zeros((marker_size, marker_size, 1), dtype="uint8")

    # マーカーを生成
    # OpenCV 4.7.0以降はgenerateImageMarkerを使用し、古いバージョンはdrawMarkerを使用します
    try:
        img = cv2.aruco.generateImageMarker(aruco_dict, marker_id, marker_size)
    except AttributeError:
        # 古いOpenCVバージョンの場合
        img = cv2.aruco.drawMarker(aruco_dict, marker_id, marker_size, img, 1)

    # 暗さレベルを設定
    darkness = args.darkness
    img[img == 0] = darkness

    # 生成されたマーカー画像を保存
    output_filename = f"marker_ID_{marker_id}.png"
    cv2.imwrite(output_filename, img)

    print(f"ID {marker_id} のArUcoマーカーを生成し、{output_filename} として保存しました")

$ python generate_aruco_markers.py --id 14

上記を実行すると以下のArUCOマーカー(ID 14)が作成されます。

実装：画像座標から物理座標への逆投影

座標系の設計には、産業用ロボットのデファクトスタンダードである右手座標系を採用します。ArUcoマーカーの右下角を原点 $(0, 0, 0)$ とし、空間を $x$（前方）、$y$（左方）、$z$（上方）の軸で構成します。

実装上のコアとなるのは、画像上の2Dピクセルを物理空間の3D座標へとマッピングする 「逆投影（Inverse Projection）」 のプロセスです。具体的には、以下の3ステップを数学的に実行します。

1. 正規化: カメラの内部パラメータを用いて、2Dピクセルからレンズ歪みを除去。
2. 姿勢変換: 算出されたカメラ姿勢（回転・移動）に基づき、ピクセルから伸びる「視線ベクトル」をマーカー基準のワールド座標系へと変換。
3. レイトレーシング: このベクトルと物理平面（$Z=0$）の交点を算出。

これにより、画面上のクリック地点が、現実世界の「どの $x, y$ 座標（cm）」に相当するのかを決定論的に割り出すことが可能になります。

以下が、これらを実装したコードです。

カメラ姿勢推定のコード（camera_pose_estimator.py）

import cv2
import numpy as np
import argparse

"""
================================================================================
【汎用3次元計測・AR表示システム：技術解説版】
本プログラムは、単一カメラを用いた「単眼視における姿勢推定」の一般原理を実装しています。

1. 透視投影モデル (Pinhole Camera Model):
   3D空間の点を2D画像平面に投影する際、レンズの焦点距離や中心座標に基づき計算します。
2. 内部パラメータと歪み補正:
   レンズによる直線の歪みを数学的に補正し、理想的なピンホールカメラ状態を再現します。
3. PnP問題 (Perspective-n-Point Problem):
   既知の3Dモデル（マーカー）の4点と、画像上の4点から、カメラの相対的な
   「位置ベクトル(Translation)」と「回転行列(Rotation)」を逆算します。
4. 空間座標の相互変換:
   カメラから見た座標系を、右手座標系（右手親指がX、人差し指がY、中指がZ）に基づく
   世界座標系へ変換することで、現実の物理空間としての数値を算出します。
================================================================================
"""

# --- グローバル変数 ---
# 空間内の特定地点（マーカー平面上）の3D座標を保存する変数
clicked_3d_pos = None  

def mouse_callback(event, x, y, flags, param):
    """
    【逆投影アルゴリズム】 - 歪み補正済み画像対応版
    歪み補正済み画像上の2D座標(x, y)から、現実世界の3D座標を算出する。
    1. 2Dピクセルを正規化画像座標に変換し、方向ベクトル（光線）を作成。
    2. カメラの現在姿勢(R, camera_pos)を用いて、光線を世界座標系へ変換。
    3. 光線と特定平面（本件ではZ=0の地面）との交点を幾何学的に特定。
    """
    global clicked_3d_pos
    mtx, dist, rvec, tvec, R, camera_pos = param

    if event == cv2.EVENT_LBUTTONDOWN and rvec is not None:
        # ステップ1: 歪み補正済みピクセルから正規化画像座標(z=1の平面)への変換
        # u_norm = (x - cx) / fx, v_norm = (y - cy) / fy
        fx, fy = mtx[0, 0], mtx[1, 1]
        cx, cy = mtx[0, 2], mtx[1, 2]
        u_norm = (x - cx) / fx
        v_norm = (y - cy) / fy

        # ステップ2: カメラ座標系での光線ベクトルを定義
        ray_cam = np.array([u_norm, v_norm, 1.0])
        
        # ステップ3: カメラの回転行列(R)を用いて、光線を世界座標系へ回転させる
        # Rは「世界→カメラ」の回転なので、その転置行列 R.T は「カメラ→世界」の回転となる
        ray_world = np.dot(R.T, ray_cam)
        
        # ステップ4: 直線と平面(Z=0)の交点計算
        # カメラ位置 P から ray_world 方向に s 倍進んだ地点のZが0になる条件：
        # camera_pos.z + s * ray_world.z = 0  =>  s = -camera_pos.z / ray_world.z
        if abs(ray_world[2]) > 1e-6:
            s = -camera_pos[2] / ray_world[2]
            intersect_3d = camera_pos + s * ray_world
            # Z=0を保証して保存
            clicked_3d_pos = np.array([intersect_3d[0], intersect_3d[1], 0.0], dtype=np.float32)
            print(f"Target Position: X={clicked_3d_pos[0]:.2f} cm, Y={clicked_3d_pos[1]:.2f} cm, Z={clicked_3d_pos[2]:.2f} cm")

def get_marker_model_cm(size_cm):
    """
    【世界座標系の定義】
    右手座標系を採用：
    - X軸: 前方（マーカーの奥方向）
    - Y軸: 左方
    - Z軸: 上方
    マーカーの右下角を原点(0,0,0)として、反時計回りに各角の座標を定義する。
    """
    return np.array([
        [ size_cm,  size_cm, 0], # 0: 左上
        [ size_cm,  0,       0], # 1: 右上
        [ 0,        0,       0], # 2: 右下 (原点)
        [ 0,        size_cm, 0]  # 3: 左下
    ], dtype=np.float32)

def main():
    global clicked_3d_pos
    parser = argparse.ArgumentParser(description='General Purpose 3D Pose Tracker')
    parser.add_argument('--id', type=int, default=0, help='追跡するID')
    parser.add_argument('--size', type=float, default=5.0, help='マーカーサイズ[cm]')
    parser.add_argument('--params', type=str, default='camera_params.npz')
    parser.add_argument('--cam', type=int, default=0)
    args = parser.parse_args()

    # カメラパラメータの読み込み（事前キャリブレーション必須）
    try:
        with np.load(args.params) as data:
            mtx, dist = data['mtx'], data['dist']
    except:
        print("Error: 指定されたカメラパラメータが見つかりません。")
        return

    # ArUco検出の設定
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
    detector = cv2.aruco.ArucoDetector(aruco_dict, cv2.aruco.DetectorParameters())
    obj_points = get_marker_model_cm(args.size)

    cap = cv2.VideoCapture(args.cam)
    win_name = '3D Pose and Target Tracker'
    cv2.namedWindow(win_name)

    # 動的な姿勢情報を保持するリスト（コールバック用）
    pose_data = [mtx, dist, None, None, None, None]

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

        # 最初にフレーム全体の歪みを補正する
        undistorted_frame = cv2.undistort(frame, mtx, dist, None, mtx)

        # 歪み補正後の画像でマーカー検出を行う
        gray = cv2.cvtColor(undistorted_frame, cv2.COLOR_BGR2GRAY)
        corners, ids, _ = detector.detectMarkers(gray)

        if ids is not None and args.id in ids:
            idx = np.where(ids == args.id)[0][0]
            
            # PnPアルゴリズムによるカメラの外部パラメータ(rvec, tvec)の算出
            ret_pnp, rvec, tvec = cv2.solvePnP(obj_points, corners[idx], mtx, np.zeros((5, 1)))

            if ret_pnp:
                # 回転ベクトル(rvec)を回転行列(R)に変換
                R, _ = cv2.Rodrigues(rvec)
                
                # 世界座標系におけるカメラ位置の算出
                # 計算式: C = -R^T * t
                camera_pos = -np.dot(R.T, tvec.flatten())
                
                # 回転角（オイラー角）の抽出
                pitch = np.degrees(np.arctan2(R[2,1], R[2,2]))
                yaw   = np.degrees(np.arctan2(-R[2,0], np.sqrt(R[2,1]**2 + R[2,2]**2)))
                roll  = np.degrees(np.arctan2(R[1,0], R[0,0]))
                
                # コールバック関数への姿勢データの受け渡し
                pose_data[2:6] = [rvec, tvec, R, camera_pos]
                cv2.setMouseCallback(win_name, mouse_callback, param=pose_data)

                # 座標軸の描画
                cv2.drawFrameAxes(undistorted_frame, mtx, np.zeros((5, 1)), rvec, tvec, args.size * 0.8)

                # AR表示: 保存された3D地点を現在のカメラ映像に投影
                target_str = "Target: [Click to set]"
                if clicked_3d_pos is not None:
                    # 3D点を2Dピクセルに再投影
                    img_pts, _ = cv2.projectPoints(clicked_3d_pos.reshape(1,1,3), rvec, tvec, mtx, np.zeros((5, 1)))
                    px, py = img_pts.ravel().astype(int)
                    
                    if 0 <= px < undistorted_frame.shape[1] and 0 <= py < undistorted_frame.shape[0]:
                        cv2.circle(undistorted_frame, (px, py), 15, (0, 0, 255), -1) # 赤いドットを表示
                        cv2.putText(undistorted_frame, "TARGET", (px + 20, py), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)
                    
                    target_str = f"Target X:{clicked_3d_pos[0]:.1f} Y:{clicked_3d_pos[1]:.1f} Z:{clicked_3d_pos[2]:.1f} (cm)"

                # --- 画面上への情報表示 (視認性向上のため大きなフォントを使用) ---
                # カメラの位置
                cv2.putText(undistorted_frame, f"x:{camera_pos[0]:.1f}, y:{camera_pos[1]:.1f}, z:{camera_pos[2]:.1f} (cm)", 
                            (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
                # カメラの姿勢角度
                cv2.putText(undistorted_frame, f"Pitch:{pitch:.1f} Roll:{roll:.1f} Yaw:{yaw:.1f} (deg)", 
                            (30, 130), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
                # クリックしたターゲットのマーカー基準座標
                cv2.putText(undistorted_frame, target_str, (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)

        # 最終的に表示するのは、すべての描画が完了した歪み補正済み画像
        cv2.imshow(win_name, undistorted_frame)
        if cv2.waitKey(1) & 0xFF == ord('q'): break

    cap.release()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()

3D姿勢推定と空間座標の検証

作成した camera_pose_estimator.py を用いて、実機での動作確認を行います。以下のコマンドを実行しますが、コマンドのパラメータとして、ArUCOマーカーのID(14)、サイズ(6.3cm)と歪み補正データのパスを指定します。

$ python camera_pose_estimator.py --id 14 --size 6.3 --params ../chessboard/calibration_data.npz

1. 空間の正規化とカメラ姿勢の取得

コマンドを実行すると、まずレンズの歪みが補正されたカッティングマットの映像が表示されます。視野内にArUcoマーカー（ID:14）を収めることで、カメラの3次元的な位置と向きがリアルタイムに算出されます。

• カメラ位置: $x=-14.7$cm, $y=-6.1$cm, $z=10.7$cm
• カメラ姿勢: Pitch $-178.1$° / Roll $-88.6$° / Yaw $-26.3$°

2. 座標プロットによる精度検証

このコードにはAR的な追従機能を実装しています。画面上の任意の点をクリックすると、その地点がカッティングマット上の物理座標（$Z=0$平面）へと投影されます。

• 定点追従: カメラの位置や角度を動かしても、プロットされた点はマット上の同じ物理位置に張り付き続けます。
• 計測精度: 画像の例では $(x, y) = (-4.0\text{cm}, -5.1\text{cm})$。実用レベルの精度が得られていることが確認できます。

別の地点 $(x, y) = (-8.9\text{cm}, -10.0\text{cm})$ を指定した場合も、同様に安定したプロットが行われました。

おわりに

100円ショップのカッティングマットや安価なUSBカメラといった簡易な機材でも、幾何学的な裏付けがあれば高精度な計測システムが構築できる点に、改めて工学的な面白さを感じました。先日開催された「国際ロボット展」でもArUcoマーカーを基準に動く実機を目にしましたが、ロボットの「視覚」が成立するプロセスを実感を伴って理解できたことは大きな収穫です。

以前、ARアプリ開発でQRコードの認識精度に苦しんだ経験がありますが、用途に合わせArUcoマーカーのような最適な技術を選択する重要性を痛感しています。

最後に、私はピアノを弾く際、眼鏡による鍵盤の歪みを避けるために眼鏡を外すことがあります。私自身の多関節アーム（腕や指）を正確に動かすには、入力となる視覚情報の整合性が不可欠だからです。どれほど高度なAIモデルを採用しても、この「視覚系の正規化」こそが、物理的なアウトプットの精度を支える土台になると思います。

次のステップ

今回構築した「ロボットの目（視覚パイプライン）」を土台に、次回はいよいよ実機への実装フェーズに突入します。

具体的には、4DoFロボットアーム、Gemini、そしてGemini Robotics-ERを統合し、以下の一連のプロセスを検証します。

• AIによる空間推論: Geminiが画像から対象物を認識し、タスクを解釈。
• 物理座標へのデプロイ: 推論結果を、今回定義したカッティングマット上の物理座標系へ変換。
• フィジカル・アクション: 算出された座標に基づき、アームが実際に物体を検知・把持・移動。

余談：光学技術の系譜を訪ねて（ニコン博物館再訪）

これまで建設現場での物体検出やARの実験を重ねてきた経験から、今回の「ArUcoマーカーを基準点とした空間座標の特定」という試みは、本質的に「測量」そのものであると感じています。

また、私自身、中学生から高校生にかけ、ニコンの一眼レフで天体撮影を行い、天体望遠鏡を2台自作した経験があるなど、光学技術には昔から深い関心がありました。先日、その原点を確認すべく、品川の「ニコン博物館」を再び訪れました。

空間を数値化するプロフェッショナル機器

工事現場でよく見かけるトータルステーションは、水平を基準にレーザー測距と分度盤による角度計測を組み合わせ、三角関数で対象の3D座標を算出する機器です。道路に埋め込まれた基準点（マーカー）をもとに位置を特定する仕組みは、今回のプログラムの実装意図と完全に合致しています。

また、ミクロの世界では、画像処理技術を用いて精密部品を0.001mm単位で自動測定するNEXIV、巨大な構造物に対しては非接触レーザーで高精度にスキャンを行う APDISなど、対象のスケールに応じた「座標特定」の解法が提示されていました。

以下、品川のニコン博物館にて撮影（2026/1/24)

工事現場でよく見かけるトータルステーション

水平を測る（レベル）、水平角と高度角を測る（トランシット、セオドライト）

NEXIVは、工場で製造される精密な部品を測るための、超高精度なデジタル顕微鏡のような測定機。x、y、zと計測位置の座標が表示されている。

APDISは航空機等を対象とした非接触レーザー計測用

測量技術の整理と自作プログラムの位置付け

展示を見ながら、今回実装したプログラムとプロ仕様の計測器を比較し、その役割を整理してみました。

対象物	機器名	役割	測定の仕組み
高低差	レベル	「高さ」の差を計測	水平線をもとに標尺（スタッフ）を読み取る
角度	セオドライト	「方向角」の計測	高精度な分度盤により水平・垂直角を割り出す
位置・距離	トータルステーション	「3D座標」の特定	角度とレーザーによる距離から位置を算出
精密部品	NEXIV	「ミクロの寸法」	画像処理により0.001mm単位で自動形状測定
巨大構造物	APDIS	「大規模3Dスキャン」	航空機等を対象とした非接触レーザー計測
AR・ロボット	自作プログラム	「空間座標のバインド」	単眼カメラとマーカーによる幾何学的な座標算出

ソースコードの公開

以下に最新版を公開します。