第10章
ArUcoとPnPでカメラの自己位置を推定する
実寸が分かるマーカーを検出し、マーカーを基準としたカメラの3次元位置と姿勢を求めます。
この章で学ぶこと
- マーカーの四隅から位置・姿勢を推定する
- マーカー座標系とカメラ座標系を変換する
- 固定した基準に対するカメラの自己位置を記録する
- 見失った場合と推定が不安定な場合を扱う
1. 基準となるマーカーを置く
前章で生成した targets/marker-0.png を余白ごと印刷し、平らな板へ貼ります。黒い外枠の一辺を測ってください。白い余白は実寸に含めません。 80mmなら 0.080m です。
このマーカーは DICT_4X4_50 という辞書のID 0です。印刷したマーカーを壁や机に固定し、その中心を世界座標の原点とします。同じIDのマーカーを複数置かないでください。マーカーを動かすと世界座標の基準も動き、固定基準での自己位置ではなくなります。
ArUcoは識別用の模様からIDと四隅を取り出します。YOLOで物体の種類を見つける処理とは役割が異なります。OpenCVのArUco資料
2. 座標系をそろえる
| 座標系 | 原点 | 軸の向き |
|---|---|---|
| 画像 | 左上 | uは右、vは下。単位px |
| カメラ | カメラの投影中心 | Xは右、Yは下、Zは前方。単位m |
| マーカー（この教材の世界座標） | マーカー中心 | 印刷画像の右がX、上がY、表面から手前がZ。単位m |
マーカーが正面を向く姿勢でも、カメラとマーカーのY・Z方向は逆です。「カメラから見たマーカーの位置」と「マーカーから見たカメラの位置」を混同しないようにします。
この実習で推定する自己位置はカメラの位置です。ロボット本体の位置へ変換するには、カメラを本体のどこに、どの向きで取り付けたかという固定変換がさらに必要です。
3. PnPで位置と向きを求める
**PnP（Perspective-n-Point）**は、既知の3次元点と画像上の2次元点の対応、カメラの内部パラメーターから、物体とカメラの相対姿勢を求める問題です。実寸をメートルで与えることで、並進もメートルになります。
この教材は、正方形向けの SOLVEPNP_IPPE_SQUARE を使います。一辺Lのマーカーの点は、検出した左上・右上・右下・左下の順に対応させます。
(-L/2, +L/2, 0) (+L/2, +L/2, 0)
左上 ───────── 右上
│ │
左下 ───────── 右下
(-L/2, -L/2, 0) (+L/2, -L/2, 0)OpenCVから得られる回転Rと並進tは、マーカー座標の点をカメラ座標へ移すものです。
X_camera = R × X_marker + tそのため、tをそのまま自己位置として保存してはいけません。カメラの原点 X_camera = 0 を代入して逆変換すると、カメラ位置Cは次のようになります。
C_marker = -Rの転置 × t
カメラの向き（カメラ座標→マーカー座標）= Rの転置rvec はオイラー角ではなく回転ベクトルです。教材コードでは cv2.Rodrigues で回転行列に直します。OpenCVのPnP資料
4. 動かして記録する
校正したカメラと同じ設定を使います。一辺80mmの場合は次のように実行します。
python marker_localization.py --source 0 --calibration calibration.npz --marker-m 0.080 --show --max-frames 0マーカーを少し斜めから見る位置で開始し、カメラをゆっくり左右・前後へ動かします。ウィンドウの C_marker [m] が世界座標でのカメラ位置です。q で終了すると、pose-output/poses.jsonl と最後の描画画像が残ります。
保存済み画像・動画も同じカメラ設定なら使えます。
python marker_localization.py --source marker-view.jpg --calibration calibration.npz --marker-m 0.080 --output still-pose| ログの項目 | 内容 |
|---|---|
camera_in_marker_m |
マーカー座標でのカメラの位置 [X, Y, Z] |
rotation_camera_to_marker |
カメラの向きを表す3×3回転行列 |
marker_in_camera_m |
カメラ座標でのマーカー中心。PnPのt |
reprojection_error_px |
四隅の再投影RMS誤差 |
status |
推定成功・見失い・曖昧さなどの状態 |
校正画像と入力画像のサイズが違う場合は停止します。コード内で画像を勝手に縮小して続けることはしません。
5. 推定できない状態を確認する
正方形の平面からの推定は、特に正面に近い場合、複数の姿勢候補が似た投影になり得ます。教材コードはカメラ前方にある候補を選び、再投影誤差が最も小さいものを調べます。候補の誤差差が0.1px未満なら ambiguous とし、位置を出しません。これは教材用の経験的な判定で、正確さを保証する基準ではありません。
status |
意味・確認すること |
|---|---|
ok |
品質条件を満たした推定。実測との確認も必要 |
not_visible |
ID 0が見つからない。辞書・照明・白い余白を確認 |
duplicate_id |
ID 0が複数ある。一つにする |
too_small |
四隅が囲む面積が400px²未満。近づける |
ambiguous |
候補を区別しにくい。見る角度を少し変える |
high_reprojection_error |
再投影RMSが3pxを超える。校正やぶれを確認 |
pose_failed |
有効な候補がない。撮り直す |
見失ったフレームには座標を保存しません。前の位置を新しい測定結果のように表示すると、カメラが動いていても停止しているように見えるためです。小さい誤差でも、印刷サイズの入力間違いや姿勢の曖昧さを完全には防げません。
6. 実測と比較する
固定マーカーに対して、カメラを既知の位置で静止させます。カメラの投影中心を直接測るのは難しいので、レンズ付近を測った近似であることも記録します。
| 実験 | 記録するもの |
|---|---|
| 30cm程度の距離で静止 | XYZのばらつき、再投影誤差、成功率 |
| 60cm程度へ離す | 距離によるばらつきと検出率の変化 |
| マーカーに平行に10cm移動 | 推定した移動量と定規との差 |
| 一部を隠す | not_visible などになり、古い座標が出ないこと |
marker_in_camera_m のZはカメラ座標の奥行きです。一方、画面に表示する camera_in_marker_m のZはマーカー面に垂直な方向の位置です。中心までの直線距離を比べるなら sqrt(X² + Y² + Z²) を使います。軸方向の距離と混同しないようにしましょう。
やってみよう
- 実際は80mmのマーカーを160mmと入力すると、推定の距離はどうなるでしょうか。
- マーカーを動かしカメラを止めたとき、ログはどう変わるでしょうか。
- tをそのままカメラ位置として使えない理由を、座標変換の式で説明してください。
1では理想的には距離の尺度が2倍になります。2では基準そのものが動くため、カメラが動いたような相対位置の変化が出ます。固定基準での自己位置推定には、マーカーの固定が必要です。