魚眼ステレオカメラの距離推定(理論編)

# はじめに

※これは[RICORA Advent Calendar](https://adventar.org/calendars/6389) 12/22 の記事になります。

本記事はカメラのキャリブレーションから始めて距離推定を行うまでの手法について説明を行います。

続く[実装編](https://qiita.com/syunnsyunn74/items/6e248f7fbe87aa18e69d)で実際のプログラムを説明したいと思います。ある程度知識のある人はそちらから読んでもらっても大丈夫です。

情報系B3の私が現在参加しているロボット開発企業のインターンのウォームアップとして1か月の間に取り組んだ内容です。

工学部ではないのでカメラ工学の知識0から勉強しました。大きな間違いを避けるためにあえて詳細な説明は避けています。詳しくは参考にしているサイトを読んでいただけると助かります。

**目次**

 * **キャリブレーション**
 * **距離推定の仕組み**
 * **ステレオキャリブレーション**
 * **ステレオ平行化**
 * **正距円筒変換**
 * **ブロックマッチングによる視差推定**
 * **視差画像の距離変換**


## 1. キャリブレーション

カメラ画像を使って計測などを行いたいときにはまず必要な技術がキャリブレーションです、これは補正のようなものだと思ってください。

実は私たちが普段から使用しているカメラはレンズの性質上、無補正の画像は若干歪んでいます。今回の主に扱っていく魚眼カメラは言うまでもなく歪みまくっています。

右のサイトがカメラモデルと歪みについて分かりやすいと思います！ [カメラモデル(camera model)](http://www.sanko-shoko.net/note.php?id=g3ft)

レンズが作られたときのわずかな違いがそれぞれのカメラに異なる歪みをもたらしています。プログラムなどで精密な計算をしたいときにこのランダムな歪みは致命的です。

しかし、幸いにもレンズの大まかな種類によってその歪みの式は近似的に求めることが可能です。そこで式の係数を求めて補正を行い、すべてのカメラに同様の計算が適用できるようにする、これがキャリブレーションです。

キャリブレーションは一般的にチェッカーボードというものを使って行います ↓↓

![checkerbaord_fig](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/627384/5d377a78-0e63-2b73-d695-a8090592e061.png)
> 画像: [カメラキャリブレーションを視覚的に理解する](https://qiita.com/souring001/items/d4312c2bdcd19016aab4)

OpenCVではこのチェッカーボードの写真を複数枚使ってカメラの歪み係数を求めてくれる関数が存在します。


<img src="https://camo.qiitausercontent.com/88681349a9568ee816c2416151e1c25d104a840c/68747470733a2f2f71696974612d696d6167652d73746f72652e73332e61702d6e6f727468656173742d312e616d617a6f6e6177732e636f6d2f302f3432333231382f36336639396565342d623264342d336230382d396335612d6165663736633466336562362e6a706567" width="50%"><img src="https://camo.qiitausercontent.com/2e5d1393df6ccf707ea9de28787fa0ead4c432d9/68747470733a2f2f71696974612d696d6167652d73746f72652e73332e61702d6e6f727468656173742d312e616d617a6f6e6177732e636f6d2f302f3432333231382f33363637313332332d303637642d613233302d613938372d3034373934633730346637332e6a706567", width="50%>

> aaa: [左が補正前、右が補正後の画像です。魚眼レンズの特徴で端の方の歪みが強いです。]()

> 画像: [魚眼レンズの補正](https://qiita.com/hiro_o_tama/items/cb544fe64ca373750aae)

## 2. 距離推定の仕組み

カメラの歪み補正ができたら早速距離推定をしてみましょう！

...と言いたいところですが、距離推定を行うためにもその他の補正を行うことが必要になってきます。その必要性を理解するためにまずはざっくりと距離推定の仕組みについて理解してみましょう。

まず距離推定を行うためには2眼カメラが必要です。私たちが普段から物体を立体的に捉えられているのは目が二つ付いているおかげです。これを2眼カメラで再現したものをステレオカメラと言います。これを使って距離推定をしましょう。

![depth_image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/627384/ceb3a326-8288-4e5c-b585-dbf4131ec4dd.png)
> 画像: [ステレオカメラについて](https://www.zmp.co.jp/knowledge/adas_dev/adas_sensor/adas_camera/adas_stereo)

ステレオカメラと測定したい物体の距離の関係を表したものが↑の図になります。基線長とはカメラ間の距離のことです。

図より三角形の相似を使って次のように距離を求めることができます。(三角測量)
$$
\begin{align}
D = \frac{B*f}{S}
\end{align}
$$

この式を適用するためには基線長B、焦点距離f、視差Sを求める必要があります。

まず基線長と焦点距離についてです。これらはステレオキャリブレーションを行うことで取得することができます。

次に視差Sはブロックマッチングを行うことで求めることができます。

## 3. ステレオキャリブレーション

先ほど説明したキャリブレーションを行えば、プログラム中で安定して計算を行うことができそうです。しかし2眼カメラではそれぞれのカメラをキャリブレーションするだけでは少し足りません。

ステレオキャリブレーションとはキャリブレーションの2眼バージョンです。先程と同様にチェッカーボードを使います。ステレオキャリブレーションでは左右カメラの補正係数とカメラ同士の画像変換を行うための数値が計算されます。ここに基線長と焦点距離が含まれています。

## 4. ステレオ平行化

ステレオキャリブレーションによって距離を推定するためのパラメータのうち基線長と焦点距離を求めることができました。最後に視差が分かれば距離の推定を行うことができそうです。

視差を求めるためには両カメラに映る同一の点を検出することが必要になってきます。そのためにはこの後紹介する、ブロックマッチングという手法を用います。

ブロックマッチングは簡単に言えば類似度の計算から両画像のマッチングを行うアルゴリズムです。それを行いやすくするために更なる画像変換を行う必要があります。

その一つ目がステレオ平行化です。↓の画像を見ながらどういう操作か説明します。

![stereorectify.png](http://www.sanko-shoko.net/note.php?img=ph9t)
> 画像: [ステレオマッチング (stereo matching)](http://www.sanko-shoko.net/note.php?id=cbgw)

対応点の探索を行う際にどのような条件があると良いかを考えてみましょう。例えば対応点が同じ高さに必ず存在するという条件があったらどうでしょうか？

その条件がない場合に比べて大幅に計算時間を短縮できることが容易に想像できると思います。さらに探索範囲が限られるため探索の精度向上にも繋がるでしょう。この条件を用意するための操作がステレオ平行化になります。

つまりステレオ平行化では、左右の画像について対応点が同一平行線上に存在するように画像の変換を行います。この同一平行線のことをエピポーラ線と言います。(なのでステレオ平行化は別名エピポーラ変換とも言います)

しかし、いきなりこのステレオ平行化を行うことはできません。キャリブレーションで求めたパラメーターのようなものが必要になってきます。

ここでも先ほど使用したチェッカボードを使用して必要なパラメーターを求めることができます。チェッカボード上のマス目が左右画像で同じ高さに来るようになればよいというわけです。

## 5. 正距円筒変換

これで対応点探索の準備は整いました。あとは実行するだけで視差を求めることができそうです。しかし、これは通常のカメラの話であって魚眼レンズの画像にはまだ変換が必要です。ステレオ平行化前後の魚眼レンズの画像が以下になります。

![2-Figure2-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/627384/7a4f86d3-2ab4-15f0-bd2f-884c9d830ed4.png)

> 画像: [Improving the accuracy of a fisheye stereo camerawith a disparity offset map](https://www.mech.chuo-u.ac.jp/umedalab/publications/pdf/2018/0088-jf-002003.pdf)

歪みの補正+平行化によって変換後の画像は端が大きく引き伸ばされてしまっているのが分かると思います。
この状態で対応点探索を行うと中央の方は上手くいきそうですが、引き伸ばされてしまっている端の方の精度がかなり悪くなってしまいそうです。

この魚眼レンズ特有の引き延ばしを解決するための変換が正距円筒変換になります。

正距円筒図法というものをご存じでしょうか？ 実はこれ、世界地図でおなじみの緯度経度画像のことです。

この変換には次の論文で紹介されている手法を使います。↓

[正距円筒画像への変換を用いた魚眼ステレオカメラの構築](https://core.ac.uk/reader/229772600)

ここで紹介されている正距円筒画像への変換式を使うのですが、このやり方だとエピ―ポーラ線が歪んでしまいます。そこで縦と横の変換を逆にして横方向にたゆませることにします。

式については次の実装編で詳しく扱います。

## 6. ブロックマッチングによる視差推定

これでようやく対応点探索を行う準備ができました。後はマッチングアルゴリズムを左右画像に適用するだけです。

ここではセミグローバルブロックマッチング(StereoSGBM)というアルゴリズムを使っていきます。

ブロックマッチングではどのような操作を行っているか簡単に説明します。

異なる角度から撮られた写真の対応点を探すときに、まずはそれぞれの画素がどのような色かを見て似た色を対応点にしてしまうというやり方が考えられます。しかしこれだけでは相当な数のマッチングが見つかってしまうことが考えられます。
次にその画素の周辺の画素も見ることにします。ある大きさのブロックを決めてブロックが同一であれば対応点であるということにしてみましょう。すると今度は条件が厳しすぎてマッチングを見つけることが困難になってしまいます。
ブロックの考え方は悪くないはずなのでもう少し条件を易しくすることを考えます。そこでブロックの各点について中心画素より明るいか暗いかの2値のみを割り振ることにしましょう。こうしてブロックを0,1の値で表現することをセンサス変換といいます。

これでそこそこ易しい条件で周辺画素も考慮したブロックマッチングを考案することができました。

ですが、このセンサス変換は少し易しすぎます。最後に全体のつじつまが一番よくなるようなマッチングの決め方を考えます。
全体のつじつまが一番よくなるような選択を行うために動的計画法を採用します。具体的にはマッチングが合わない部分について罰則スコアを設けながら全体の罰則が最小値になるように選択を行います。
これでセミグローバルマッチングが完成しました。これらの考え方を適用することで、より滑らかに視差を求めることができるようになります。

## 7. 視差画像の距離変換

視差が求まったので最後は距離に変換するだけです。

先ほど距離は三角測量を用いることで計算できることを説明しました。しかし、あれはあくまで平行化画像について適用できる式でなので正距円筒画像に変換してしまうと同じ式を適用することができなくなってしまいます。

そこで先ほどの論文と同一の著者が書いた論文を参考にして少し異なる式で計算を行います。↓

[魚眼ステレオカメラの2眼ステレオとモーションステレオの融合による距離画像計測](https://www.jstage.jst.go.jp/article/transjsme/85/875/85_19-00069/_article/-char/ja/)

式についてはこちらも実装編で解説します。

これでようやく距離推定を行うことができました！理論を追うだけでかなり大変でしたが、実装はもっと骨が折れます(泣)

自分のように苦労する人が一人でも減ることを願って、次の[実装編](https://qiita.com/syunnsyunn74/items/6e248f7fbe87aa18e69d)に進みます！

## 参考
* [カメラキャリブレーションを視覚的に理解する](https://qiita.com/souring001/items/d4312c2bdcd19016aab4)
* [魚眼レンズの補正](https://qiita.com/hiro_o_tama/items/cb544fe64ca373750aae)
* [魚眼レンズの補正](https://qiita.com/hiro_o_tama/items/cb544fe64ca373750aae)
* [ステレオカメラについて](https://www.zmp.co.jp/knowledge/adas_dev/adas_sensor/adas_camera/adas_stereo)
* [ステレオマッチング (stereo matching)](http://www.sanko-shoko.net/note.php?id=cbgw)
* [Improving the accuracy of a fisheye stereo camerawith a disparity offset map](https://www.mech.chuo-u.ac.jp/umedalab/publications/pdf/2018/0088-jf-002003.pdf)
* [正距円筒画像への変換を用いた魚眼ステレオカメラの構築](https://core.ac.uk/reader/229772600)
* [魚眼ステレオカメラの2眼ステレオとモーションステレオの融合による距離画像計測](https://www.jstage.jst.go.jp/article/transjsme/85/875/85_19-00069/_article/-char/ja/)