この記事は，ssl-zemi 12月(2023/12/17)で発表した内容を編集した物です．
SSL-visionの概要と課題
SSLは天井につけられたカメラの情報をもとに，各チームはロボットへ命令を送ります．天井からの視点であるので，ロボットの位置やボールの位置を検出するのは簡単です．
しかし，天井の高さやカメラの配置によっては，ロボットによってボールがみえなくなったり，照明状況によりロボットやボールの検出が安定しなかったりすることがあります．
また，ネットワークを通って，各チームで利用できるまでに，レイテンシが100 ms(60 fps，6F)程度存在します．そこで，SSL-visonシステムの性能や会場による差による影響を低減するために，ロボットにカメラを載せてみました．
載せてみた
ロボットにRaspberry piとカメラをそのまま載せてしまいます．（モバイルバッテリは開発中のもので，実際には取り外します．）
カメラ(OV5647)からYUV形式，640*480，90 fpsで画像を取得します．Raspberry pi 4により画像処理行い，検出したボール位置（ロボット座標系）をロボットを制御するマイコン(STM32H7)にUART送信します．
Raspberry pi を画像処理モジュールとして疎な結合とすることで，既存のロボットシステムへの影響を少なく，カメラシステムの載っていない機体とも共存させやすい構成としています．今後のシステムの開発状況，稼働時の安定性などによっては，結合の程度は変わる可能性があります．
ボール検出
YUVの信号は，輝度信号と2つの色差信号から構成されています．この色差信号2つの差を取ることで，オレンジっぽさにより明暗が変化するグレースケール画像を取得することができます．
この画像に対して，いくつか設定した高さで，横方向に走査しながらエッジを検出します．検出されたエッジの中点を結んだ線上を改めて，縦方向に走査しながらエッジを検出し，ボールの中心座標を特定します．この手法はTIGERs MANNHEIMの手法を参考にしています．
疎に探索することにより，高速化をしています．また，非常に並列性を上げやすい手法です．
座標変換
スクリーン座標系のボール位置から，ロボット座標系に変換します．
ロボット座標系->スクリーン座標系
スクリーン座標系からロボット座標系への逆変換を考える前に，ロボット座標系からスクリーン座標への順変換を考えます．
これは，ロボット座標系->カメラ座標系と，カメラ座標系->スクリーン座標系への変換に分割できます．
ロボット座標系->カメラ座標系の変換は視野変換と呼ばれる操作で行います．具体的には
- ベクトルを足してカメラの位置まで原点を動かす
- 回転行列を使ってカメラの向きに座標を合わせる
ことで実現できます．
カメラ座標系->スクリーン座標は透視変換と呼ばれる操作を行ったあとに原点を画面左上にずらすことで実現できます．OpenCVの場合，この変換は推定されたカメラ行列を使うことで実現されます．くわしくはカメラキャリブレーションと3次元再構成を参照してください．
スクリーン座標系-> ロボット座標系
続いて逆変換を考えます．順変換で導出した式を逆に解けばいいのですが，2次元から3次元への変換なので不良設定問題となります．
そのため「ボールのz座標は0である」という制約をかけます．
スクリーン座標からカメラ座標への変換は下のようになります．カメラ座標のz座標は通常奥行きと等しいですが，今回は決まらないので1として計算します．OpenCVの場合，カメラ行列の逆行列を推定したスクリーン座標にかけて求めます．必要ならundistortPointsを先に適用します．
カメラ座標系からワールド座標系への変換は以下です．この時にロボット座標のzが0になるようにスケール変数sを決めます．
結果
こちらがボール位置検出の様子です．モノクロ画像がエッジ処理に使われている画像で，カラー画像の黒線はエッジ処理に使う行を示しています．カラー画像に描画されている緑点が推定されたボール中心です．中心位置のx,y,z座標がcm単位でカラー画像左下に表示されています．右下に出てるのがフレーム当たりの処理時間です．GUIを表示しているせいもありますがそれほど速くないですね．
課題と展望
とりあえず以上の処理が実装できましたが，コードが十分に最適化されていないため，性能に改善の余地があります．今後効率的なコードにリファクタすることで性能を出せたらよいと考えています．また，カメラと天井照明の位置関係により，ボールの下側が影になって見えなくなってしまいます．カメラをチューンして影になっている部分の色が取れるように改善しつつ，ロボットに光源を搭載できないかと考えています．いんあでぃしょん，Visionや既存AIとの統合がいまだ実現できていません．既存AIやVisionが持つ遅れを伴いつつも高度な情報と，ローカルカメラによって得た高速で低レイテンシな情報をいかに統合するかがロボットの性能向上に極めて重要だと考えます．
参考文献
Fabio Seel & Sabolc Jut, On-Board Computer Vision for Autonomous Ball Interception, 2019, https://download.tigers-mannheim.de/papers/2019-BallIntercept_TC-Seel_Jut.pdf
OpenCV, カメラキャリブレーションと3次元再構成, http://opencv.jp/opencv-2.2/cpp/calib3d_camera_calibration_and_3d_reconstruction.html
山本紘暉，座標変換について調べてみた, 2020, https://dev.classmethod.jp/articles/convert-coordinates/
