15 の nomo ( @nomotech )です。
この記事はrogy Advent Calender 2020 24日目の記事です。
RGB-Depthセンサを自作しよう【4-構造化光法編】の続きです。
深度計算
三角測量
三角測量でのキーワードは透視投影変換です。透視投影変換は以下の式で表されます。
\begin{aligned}
\omega\left(\begin{matrix}u\\v\\1\end{matrix}\right)
&= \left(\begin{matrix}
f_x & 0 & c_x\\
0 & f_y & c_y\\
0 & 0 & 1
\end{matrix}\right)
\left(\begin{matrix}
r_{11} & r_{12} & r_{13} & t_x \\
r_{21} & r_{22} & r_{23} & t_y \\
r_{31} & r_{32} & r_{33} & t_z
\end{matrix}\right)
\left(\begin{matrix}x\\y\\z\\1\end{matrix}\right) \\
&= M\left(\begin{matrix}x\\y\\z\\1\end{matrix}\right) \\
\end{aligned}
ステレオの場合、ある3次元上の点 (x \ y \ z\ 1)^t を 2 視点で観察するので、カメラからの透視投影(添え字が c ) と プロジェクタからの透視投影(添え字が p )の 2 つの式を立てることができます。
\begin{aligned} &\begin{cases} \omega_c \left(\begin{matrix}u_c\\v_c\\1\end{matrix}\right) = M_c \left(\begin{matrix}x\\y\\z\\1\end{matrix}\right) \\ \omega_p \left(\begin{matrix}u_p\\v_p\\1\end{matrix}\right) = M_p \left(\begin{matrix}x\\y\\z\\1\end{matrix}\right) \end{cases} \end{aligned}ここで、
\begin{aligned}
\omega_c &= {m_c}_{31} x + {m_c}_{32} y + {m_c}_{33} z + {m_c}_{34} \\
\omega_p &= {m_p}_{31} x + {m_p}_{32} y + {m_p}_{33} z + {m_p}_{34} \end{aligned}
より以下の4つの関係式が得られます。
ここで、今回は横縞の位相シフトのパターンを用いたので、わかっている変数は u_c , v_c , v_p の3つです。
求めたいのは x, y, z の3変数なので、3つの式があればいので、上の式のうち1, 2, 4 式の連立方程式を解けばいいです。(縦縞のパターンの場合は v_p の代わりに u_p が得られるので 1, 2, 3 式の連立方程式を解けばいい)
\begin{aligned} \left(\begin{matrix} u_c {m_c}_{31} - {m_c}_{11} & u_c {m_c}_{32} - {m_c}_{12} & u_c {m_c}_{33} - {m_c}_{13} \\ v_c {m_c}_{31} - {m_c}_{21} & u_c {m_c}_{32} - {m_c}_{22} & u_c {m_c}_{33} - {m_c}_{23} \\ v_p {m_p}_{31} - {m_p}_{21} & u_p {m_p}_{32} - {m_p}_{22} & u_p {m_p}_{33} - {m_p}_{23} \\ \end{matrix}\right) \left(\begin{matrix}x\\y\\z\end{matrix}\right) &= \left(\begin{matrix}-u_c {m_c}_{34} + {m_c}_{14}\\-v_c {m_c}_{34} + {m_c}_{24}\\ -v_p {m_p}_{34} + {m_p}_{24}\end{matrix}\right)\\ \Leftrightarrow A\left(\begin{matrix}x\\y\\z\end{matrix}\right) &= b \\ \Leftrightarrow \left(\begin{matrix}x\\y\\z\end{matrix}\right) &= A^{-1}b \end{aligned}この時 A は 3\times3 行列なのでサラスの公式によって逆行列は簡単に求められます。
この処理は画素ごとに独立して計算できるので GPU による並列処理で高速に計算できます。
以下が三角測量の結果です。深度が計算できました。
レンズゆがみ補正
カメラのレンズには円形の歪みがあります。
通常ならばとった画像にゆがみ補正を掛けて歪んでいない画像にしてから画像処理をしますが、今回は3次元計測をするので三角測量のをするときにゆがみ補正をします。
そうすることによってゆがみ補正をGPUで並列処理することができ高速化できます。
OpenCV の undistortedPoints 関数であらかじめ歪んだ画像の座標 (u, v) からゆがみ補正した座標 (u^{\prime}, v^{\prime} ) への対応を求めておきます。この補正された (u^{\prime}, v^{\prime} ) を上の三角測量の式に使えば歪みを考慮した三角測量ができます。
下の画像はゆがみ補正の結果です。
分かりづらいですが、背景の板の深度が平らになり、少し湾曲していた辺が直線になってることがわかります。
これまでの処理をGLSLのcompute shader で実装することによって処理を高速に実行できます。
実験
実験は以下のような配置で行いました。
仕様ハードウェアは第二回に紹介したものです。
PCスペックは CPU : intel Core i7-8750H、GPU : NVIDIA Geforce RTX 2060です。
実験は部屋を暗くしてプロジェクタからのパターンがわかりやすいようにしました。
撮像が 120 fps 処理時間は 150 fps で高速に 3 次元計測できました。
以下の結果は各画素からの3次元位置を vertex shader の point cloud で表示しています。12枚分の残像の影響で、変化の大きいエッジ部分では正確な深度が計算できていませんが、だいたいの部分では高速に深度を計算できています。
下の画像ではソフトソーサーを計測したものです。深度とカラーを同時に撮れていることが確認できます。
ここでソフトソーサーの黒の模様がきれいに取れているように見えますが、実際は黒い部分はプロジェクタのパターンが反射されないので計測できていないだけです。
課題
- 解像度 1440\times1080 の高解像度で撮像が 120 fps 処理時間は 150 fps で高速に 3 次元計測できています。しかし、プロジェクタのパターン更新が 60 fps なので撮像画像 12 枚分の残像が見えてしまっています。プロジェクタから 120 fps でパターンを切り替えることができれば残像が小さくなりより高精度になると思います。
- 3 次元計測の結果に横縞のノイズがのっていることが確認できます。この原因は投影時間と撮像時間とのズレが原因で正弦波パターンの影響が出ているのだと考えています。これを解決するアイデアはあるので、機会があればチャレンジしようと思います。
- プロジェクタの輝度が低いため明るい場所ではパターンがカメラにちゃんと映らないという問題がありました。そのためこの深度センサは暗い場所でしか使えないという制限があります。これは強力な光源を用意すれば解決できると思います。
- 今回は RGB のプロジェクタを使っていますが、これを赤外線単色にし、カメラを(赤外線が検知できる)モノクロカメラにすれば 3 倍の速度で投影撮像ができると思います。
- 位相シフト法は 6 枚の固定パターンでできるので、そもそもプロジェクタではなく、自作の投射機を作れば、光源をめっちゃ強くすることもでき、速度ももっと高速化することもできます、機会があればやってみたいと思います。
まとめ
以上今回まで 5 回にわたって自作深度センサの紹介記事を書いてきました。いかがでしたか？
質問があれば @nomotech に連絡くれれば応対できるかもしれないです。ソースコードは気が向いたら公開するかもしれないです。
ロボット界隈にもっと高速画像処理、高速センシングが流行ってほしいなと思います。
需要があればもっと画像処理系の記事を書きます。
それでは皆さん良い進捗を👍
RGB-Depthセンサを自作しよう【1-基礎知識編】
RGB-Depthセンサを自作しよう【2-同期撮像編】
RGB-Depthセンサを自作しよう【3-キャリブレーション編】
RGB-Depthセンサを自作しよう【4-構造化光法編】
RGB-Depthセンサを自作しよう【5-深度計算編】