SeeingTag：PnP測位からホモグラフィ変換へ

SeeingTag は、スマートカー競技のコースを俯瞰カメラで撮影し、車両の位置と姿勢を推定するシステムである。コース上に固定した ArUco マーカーで世界座標系を定義し、車載マーカーから車両位置とヨー角を求め、結果を UDP で Unity 側へ送信する。

現在の構成は最初から完成していたものではない。カメラ入力と通信の検証、多マーカー PnP、ホモグラフィによる平面写像という段階を経ている。方式を変更した最大の理由は、理論上の優劣ではなく、実際の会場で当初の前提が一つずつ成立しなくなったことである。

担当範囲

画像測位の主要処理、設定ファイル化、デバッグ用ツール、通信処理、後のスループット改善を継続して実装・検証した。Unity の受信側と車両全体の制御系はチームプロジェクト側に属し、SeeingTag の責務は、遅延が小さく座標規約の安定した位置・姿勢データを提供することである。

この境界を明確にすることは、原因調査にも重要だった。測位プログラムが座標を出力できることと、車両制御全体が正しく動くことは同じではない。逆に、Unity 上の遅延や姿勢ずれが、必ずしも車両制御側の問題とも限らない。そのため初期段階から、UDP リスナー、カメラ単体テスト、測位のみを行うキャリブレーションモードを分離して用意した。

Phase 0：まず処理の閉ループを確認する

最初の目標は二つだけだった。

カメラ映像から DICT_4X4_50 のマーカーを安定して検出できるか
JSON 形式の位置データを UDP で連続送信できるか

カメラ、送信側、受信側を独立したテストスクリプトに分け、パケットには seq と timestamp を追加した。これにより、「画像認識が結果を生成していない」のか、「ネットワークで届いていない」のかを切り分けられるようになった。

最初に発生した問題は誤検出である。すぐに複雑な識別器を追加するのではなく、画像上のマーカー幅を用いたフィルタを導入した。マーカーの実寸とカメラ設置高がほぼ固定された競技環境では、この単純な制約でも低コストで効果があった。

この段階では、次の項目を確認した。

同じマーカーを一度だけでなく連続して検出できるか
seq が単調増加し、受信側で欠落を確認できるか
カメラ入力失敗、未検出、送信失敗をログ上で区別できるか
最小マーカー幅を変更したとき、誤検出と遠方マーカーの見落としがどう変化するか

原型の目的は最終方式の正しさを証明することではない。大きな問題を、個別に反証可能な処理へ分解することだった。

Phase 1・2：PnP でカメラと車両を世界座標へ配置する

最初の完全な測位処理では、二段階の PnP を使用した。

複数の固定マーカーについて、既知の三次元座標と画像上の角点を結合し、カメラ姿勢を推定する
車載マーカーの相対姿勢を別の PnP で推定し、世界座標へ変換する

この方式は車両の三次元位置とヨー角を出力でき、処理構成も標準的である。しかし、カメラ内部パラメータと歪みモデルが十分に正確であるという前提を持つ。

当時はチェスボードによる正式なキャリブレーションが完了しておらず、画角から焦点距離を近似していた。処理自体は動作するが、画角の誤差が絶対位置の系統誤差につながる。現場調整を容易にするため、その後は JSON 設定、キャリブレーションモード、HUD、マーカーごとのローパスフィルタ、UDP 送信クラスを追加した。

ここでの誤判断は、「アルゴリズムが連続した数値を返す」ことを「幾何モデルが正しい」ことに近いものとして扱ってしまった点である。PnP は多くの場合に数値を返すが、結果が存在することは、幾何的に信頼できることを意味しない。内部パラメータが不正確な場合、その誤差を姿勢推定が吸収し、表示は安定して見えても絶対座標がずれる。

この経験から、後の検証では「出力があるか」だけでなく、コース範囲、既知点の写像誤差、姿勢方向の一貫性も確認するようにした。

現場で明らかになった三つの問題

方式変更の直接のきっかけはカメラだった。

スマートフォンの RTSP 映像は広い範囲を撮影できたが、5～30 秒の累積遅延が発生した。一般的な USB カメラは遅延が小さい一方、画角が狭く、一度に一つか二つの固定マーカーしか見えなかった。広角映像は範囲を確保できても、画像周辺の樽型歪みが PnP の角点対応と内部パラメータの仮定を崩した。

PnP を継続する場合、次の問題を同時に解く必要があった。

低遅延な映像入力
正確な内部パラメータと歪み補正
コース全体を覆う画角
固定マーカーの安定した可視性

しかし、この競技で必要なのは地面上の二次元位置とヨー角であり、カメラの完全な三次元姿勢ではない。この要求の差が、方式を再設計する根拠になった。

採用しなかった選択肢

ホモグラフィ以外にも、広角レンズを正式にキャリブレーションして各フレームを歪み補正する方法や、カメラとキャプチャ機器を更新して PnP を継続する方法があった。

三次元測位が必要な用途では、これらが適切な場合もある。しかし本システムでは追加コストが大きかった。

キャリブレーション値がレンズ、解像度、焦点距離に依存し、機材変更時に再実施が必要
全フレームの歪み補正は処理負荷を増やすが、RTSP の古いフレーム蓄積は解決しない
高品質なカメラとキャプチャ機器は画質を改善する一方、設置と運用を複雑にする
最終的に利用するのは地面上の二次元座標で、完全な三次元姿勢を活用していない

PnP を廃止したのは、方式そのものが劣るからではない。必要な前提を維持する費用が、現在のタスクで得られる利益を上回ったためである。

Phase 3：ホモグラフィで必要な問題だけを解く

新しい方式では、固定マーカーの画像座標とコース上の既知座標からホモグラフィ行列を求め、車載マーカー中心を直接コース座標へ写像する。

固定マーカーの画像角点 + 既知のコース座標
                    ↓
          findHomography + RANSAC
                    ↓
車載マーカー中心 → perspectiveTransform → コース座標

姿勢も三次元回転行列から抽出せず、車載マーカー上の方向を表す二点をコース平面へ写像し、その方向ベクトルから計算する。

この変更により、主要処理からカメラ内部パラメータへの依存を除去できた。デバッグも単純になり、写像行列が成立しているか、出力点がコース内にあるかを俯瞰図で直接確認できる。

一方で制約もある。参照点は同一平面上にある必要があり、初期化時には四つの固定マーカーが見えなければならない。また、高さやピッチ角といった三次元情報は得られない。現在の競技要件では、これらの制約を受け入れられると判断した。

再設計をどのように評価したか

評価は単一の FPS ではなく、四つの層に分けた。

層	確認内容	異常時に優先して疑う部分
入力	実カメラ FPS、新しいフレームを取得できているか	カメラ、圧縮形式、バッファ
検出	固定・車載マーカーの検出率	照明、サイズ閾値、モーションブラー
幾何	既知点の写像、コース境界、姿勢方向	マーカー座標、角点順序、行列
出力	UDP 周波数、データ経過時間、Unity 応答	キュー、フィルタ、ネットワーク、座標規約

この分解は、特定のアルゴリズム選択以上に有効だった。「画面が遅い」「車両表示が揺れる」といった曖昧な現象を、具体的な処理区間に対応づけられるようになった。

平滑化と遅延のトレードオフ

測位が成立した後、次の課題はフレーム間の揺れだった。システムでは、マーカー検出結果に対するフィルタと、最終位置・姿勢に対する出力フィルタを分けた。二段構成によりノイズ源を区別しやすくなったが、出力側の係数を小さくしすぎると、Unity 上のヨー角が実車の旋回に遅れて追従した。

そのため、調整目標を「静止画面で最も滑らか」から「許容できる揺れの範囲で遅延を最小化する」へ変更した。車載マーカーの取り付け角による一定誤差は、永続化された car_heading_offset_degrees で補正し、フィルタや座標変換の内部へ混在させないようにした。

静止状態で最適に見える係数が、動的追従でも最適とは限らない。車両の旋回、加減速、短時間遮蔽を含めて評価しなければ、応答を過度に遅く設定してしまう。

スループットと短時間ロストへの対応

測位精度の次に、情報の新しさを改善した。入力スレッドは継続的にフレームを取得し、処理側は常に最新フレームだけを参照する。古いフレームを順番に処理する構造を避け、表示更新頻度も測位送信頻度から分離した。性能表示では、カメラ、処理、送信の各周波数を別々に記録する。

車載マーカーが短時間見えなくなった場合も、任意の履歴を無制限に外挿しない。既知の死角に入る前に手動で保護機能を有効化し、外挿時間、移動距離、姿勢変化に上限を設けた。状態は OFF / ARMED / DRIVING として映像上に表示し、ログを見なくても動作モードを確認できるようにした。

残された課題

現在の方式にも明確な限界がある。

長時間の遮蔽に対する独立した冗長系がない
精度が固定マーカー座標と地面の平面性に依存する
短時間外挿は車載オドメトリの代替にはならない
軌跡の真値がなく、位置・姿勢誤差を定量比較しにくい

今後は車輪速や IMU を用いた短時間融合と、既知の真値点を含む評価軌跡を導入したい。これにより、「見た目が安定した」という評価から、再現可能な誤差比較へ移行できる。

この開発から得た最も重要な教訓は、「ホモグラフィが PnP より優れている」ということではない。必要な情報、現場条件、維持すべき前提を照合し、使わない三次元情報を手放すことで、依存関係を短くし、検証しやすいシステムへ変更できたことである。

GitHub リポジトリ：SeeingTag-v2