RICOH THETA SのためのDualfisheyeからEquirectangularに変換を行うGstreamerプラグイン

## 概要

全天球カメラRICOH THETA SからUSB経由で取得したDualfisheye形式の画像をEquirectangular形式に変換するGstreamerプラグインを作成します。Gstreamerの他のエレメントと組み合わせて使うことにより、Theta Sから取得した全天球映像をリアルタイムで画面に表示させたり、超低遅延でインターネットにライブ配信することが可能になります。

### 用語の説明

#### RICOH THETA S

リコー製の全天球カメラです。ライブストリーミングモードにしてPCに接続するとUSBカメラ(UVCカメラ)として動作させることができます。ライブストリーミングモードでは、360度映像を解像度1280x720, 15fps, Motion-JPEG Format, Dualfisheye形式で出力します。

ちなみに、リコーの上位機種としてTheta V/Z1がありますが、専用のPCアプリを使えばYoutubeへのライブ配信を行うことはできるものの、カメラ自体がUVCカメラになるモードをサポートしないので今回の用途では使用できません。

#### Gstreamer

メディア処理を行うオープンソースのフレームワークです。エレメントと呼ばれるモジュールを数珠つなぎに組み合わせて様々なマルチメディアアプリケーションを構成することができます。Gstreamerプラグインは、エレメントをGstreamerアプリケーションから呼び出しできるようにカプセル化したものです。
本記事での「Gstreamerプラグイン」は、THETA V/Z1上で動かすAndroidのプラグインとは全く関係ありません。

#### Dualfisheye形式

２個の180度半天球映像を横に並べたフォーマットです。Theta Sからはこの形式で出力されます。


#### Equirectangular形式

日本語では正距円筒図法といわれるもので、360度の映像が世界地図のメルカトル図法のように展開（正確にはちょっと違う）された形式です。VR界隈では一般的な形式なので、配信した映像をVRゴーグルで見る事もできるようになります。

## Dualfisheye --> Equirectangular変換プログラムの作成

Gstreamerプラグインを作成する前に、アルゴリズムの検証のためにまずDualfisheyeからEquirectangularに変換するプログラムを作成しました。（中身のアルゴリズムに興味のない方は、この節は飛ばしてください。）変換プログラムはPython+OpenCVで作成しています。本家Ricohのスティッチングアプリは、画像の切れ目が目立たないように重ね合わせてつなげてくれますが、今回作成する変換プログラムは、2つのカメラのつなぎ目がはっきりわかる程度の簡易的なものです。
![図3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/189951/63f32423-cabb-a5c4-4be6-ac727597d7ef.png)


変換プログラムのコードは[こちら](https://gist.github.com/kozokomiya/701c2ca269d6afccdf7a4fd5f252fe05)です。

### 変換プログラムのアルゴリズム

DualfisheyeからEquirectangularへの変換するには、三角関数を駆使してEquirectangular上のピクセルがDualfisheye上のどのピクセルに相当するかを計算する座標変換処理を行うのですが、高速化のためにあらかじめ座標変換行列を作成しておき、OpenCVのremap関数を使って１フレーム分を一気に変換する方法を使います。

#### プログラムの概要

- 座標変換行列のファイルを読み込もうとして、ファイルが無ければ作成する
- 入力動画ファイルを１フレームずつ読み込んで、remap関数で変換して出力ファイルに書き込む

#### ソースコード

```python
# Dual-fisheye to Equirectangular Converter using OpenCV remap
import sys
import numpy as np
import cv2

try:
    # try to load map file
    xmap = np.load("xmap.npy")
    ymap = np.load("ymap.npy")
except IOError:
    # if the map files are not exist, generate new map files.
    print("Generating map miles...")
    COLS = 1280
    ROWS = 720
    xmap = np.zeros((ROWS, COLS), np.float32)
    ymap = np.zeros((ROWS, COLS), np.float32)
    DST_X = float(COLS)
    DST_Y = DST_X / 2
    SRC_CX1 = DST_X / 4
    SRC_CX2 = DST_X - SRC_CX1
    SRC_CY = DST_X / 4
    SRC_R = 0.884 * DST_X / 4
    SRC_RX = SRC_R * 1.00
    SRC_RY = SRC_R * 1.00
    #
    for y in range(COLS // 2):
        for x in range(COLS):
            ph1 = np.pi * x / DST_Y
            th1 = np.pi * y / DST_Y

            x1 = np.sin(th1) * np.cos(ph1)
            y1 = np.sin(th1) * np.sin(ph1)
            z1 = np.cos(th1)

            ph2 = np.arccos(-x1)
            th2 = (1 if y1 >= 0 else -1) * np.arccos(-z1 / np.sqrt(y1 * y1 + z1 * z1))

            if ph2 < np.pi / 2:
                r0 = ph2 / (np.pi / 2)
                xmap[y,x] = SRC_RX * r0 * np.cos(th2) + SRC_CX1
                ymap[y,x] = SRC_RY * r0 * np.sin(th2) + SRC_CY
            else:
                r0 = (np.pi - ph2) / (np.pi / 2)
                xmap[y,x] = SRC_RX * r0 * np.cos(np.pi - th2) + SRC_CX2
                ymap[y,x] = SRC_RY * r0 * np.sin(np.pi - th2) + SRC_CY

    np.save("xmap.npy", xmap)
    np.save("ymap.npy", ymap)

def convert_dualfisheye_to_equirectangular(frmae):
    return cv2.remap(frame, xmap, ymap, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)

if __name__ == '__main__':
    if len(sys.argv) < 3:
        print('Usage: Python3 dualfisheye.py <inputfile> <outputfile>')
        sys.exit(1)
    cap = cv2.VideoCapture(sys.argv[1])
    if not cap.isOpened():
        print('file not opened')
        sys.exit(1)
    fps    = cap.get(cv2.CAP_PROP_FPS)
    height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
    width  = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
    fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
    out = cv2.VideoWriter(sys.argv[2], fourcc, fps, (int(width), int(height)))
    count = 0
    while(cap.isOpened()):
        ret, frame = cap.read()
        if ret == False:
            break
        frame = convert_dualfisheye_to_equirectangular(frame)
        out.write(frame)

        if count % 30 == 0:
            print('.', end="")
            sys.stdout.flush()
        count += 1

    print()
    cap.release()
    cv2.destroyAllWindows()
```

#### remap関数について

remap関数はOpenCVに予め定義されている関数です。カメラのレンズの歪み補正などのためによく使われる関数だそうです。map1とmap2は、**変換後**の座標(xd, yd)をもとに**変換前**の座標(xs, ys)値を出力するテーブルです。逆変換のテーブルを用意しておくことがポイントです。

```
dst = cv.remap( src, map1, map2, interpolation)
```

- src:  入力画像のフレームデータ 
- map1: 変換後の座標(x_d, y_d)を与えると変換前のx座標値(x_s)を出力するテーブル

- map2: 変換後の座標(x_d, y_d)を与えると変換前のy座標値(y_s)を出力するテーブル
- interpolation: 補完方法
- dst: 出力画像のフレームデータ

#### 座標変換行列の計算

今回は、dualfisheye形式の画像をequirectangular形式の画像に変換するので、equirectangular上の座標がdualfisheye上のどこの座標に来るかを計算する式を使って、この式を1280x720のすべてのピクセルに適用することにより座標変換行列を作成します。

以下の式では、equirectangularの座標からdualfisheye上の座標に変換しています。equirectangularの2次元座標が、3次元の球体のどこに位置するかを計算し、座標を回転してから今度はdualfisheye上の2次元平面上の位置に変換し直しています。


```math
\begin{align}
& \phi_1, \theta_1 \mbox{: 球面上の経度と緯度}\\
& {\phi}_1 = \frac{{\pi}Wx_d}{2}, \quad
  {\theta}_1 = \frac{{\pi}Wy_d}{2}\\
& \mbox{ここで、W: Equirectangular図法画像の横幅 }\\
\\
& (x_1, y_1, z_1)\mbox{: 球面上の点の３次元空間での位置座標}\\
& x_1 = {\sin}{\theta_1}{\cos{\phi_1}}\\
& y_1 = {\sin}{\theta_1}{\sin{\phi_1}}\\
& z_1 = {\cos}{\theta_1}\\
\\
& \phi_2, \theta_2 \mbox{: x軸を自転軸としたときの球面上の経度と緯度}\\
& {\phi}_2={\cos}^{-1}(-x_1)\\
& {\theta}_2=
\begin{cases}
 \quad {\cos}^{-1}\frac{-z_1}{\sqrt{y_1^2 + z_1^2}} & (y_1 \ge 1) \\
 -{\cos}^{-1}\frac{-z_1}{\sqrt{y_1^2 + z_1^2}} & (y_1 \lt 1) 
\end{cases}\\
\\
& (x_s, y_s) \mbox{:dualfisheye座表上の点の位置}\\
& \mathbf{\mbox{if}}
\quad
\phi_2 \lt \frac{\pi}{2}
\quad
\mathbf{\mbox{then}}
\\
& \qquad x_s = \frac{2r_s{\phi_2}}{\pi}{\cos} {\theta_2}+x_{c1},\\
& \qquad y_s = \frac{2r_s{\phi_2}}{\pi}{\sin} {\theta_2}+y_{c1}
\\
& \mathbf{\mbox{else}}
\\
& \qquad x_s = \frac{2r_s(\pi-{\phi_2})}{\pi}{\cos}(\pi- {\theta_2})+x_{c2},\\
& \qquad y_s = \frac{2r_s(\pi-{\phi_2})}{\pi}{\sin}(\pi- {\theta_2})+y_{c2}\\

& \mbox{ここで、}(x_{c1}, y_{c1}){は左魚眼の中心座標、}(x_{c2}, y_{c2}){は右魚眼の中心座標}\\
& r_s\mbox{は、魚眼の半径}
\end{align}
```

### 変換プログラムの使い方

Theta Sで撮影した動画（Dualfisheye形式）の解像度720pに変換した動画ファイルを用意します。また、python3とopenCVをインストールしておきます。動作確認はmac上のpython(3.9.1) + opencv (4.5.0)で行いましたが、remap関数が動けば良いので多少古くても問題ないと思います。

```console
python3 dualfisheye.py <input_filename> <output_filename>
```

初回起動時のみ、変換行列のファイルを作成するので変換の開始まで数十秒かかりますが、2回目以降は作成した変換行列ファイルを読み込んで起動するのでその時間分だけ早くなります。

## Gstreamer プラグイン

作成したアルゴリズムを今度はGstreamer プラグインとして実装しました。

ソースコード一式は[こちら](https://github.com/kozokomiya/dualfisheye)。

プラグインは、mac、Ubuntu PC、raspberry piにおいて動作します。解像度は1280x720のみをサポートします。

### ビルド&実行方法

依存ライブラリとしてgstreamerとopencv4の開発パッケージ、ビルドツールとしてcmake, meson, ninjaをインストールします。linuxの場合は、USBカメラからの入力を受け取るためにv4l2-utilsもインストールします。

クローンしてビルドします。初回ビルド時は、スクリプトが座標行列変換のファイルxmap.npyとymap.npyを自動的に作成します。

Pythonで作成したnumpyのバイナリデータファイルをC++から読み込むために、rezoo氏の[numpy.hpp](http://mglab.blogspot.com/2014/03/numpyhpp-npy.html)を使わせていただいております。

```console
git clone https://github.com/kozokomiya/dualfisheye.git
cd dualfisheye
./build.sh
```

### テストパターンを変換する
テストとして、gstreamer標準のテストパターンをdualfisheye画像として入力して、画面に表示させてみます。テストパターンはDualfisheye形式ではないので、歪んだテストパターンの出力結果が表示されます。

```console
./testsrc.sh
```

./testsrc.shの中身です。環境変数GST_PLUGIN_PATHによりビルドしてできたPlutginの場所を`gst-launch-1.0`コマンドにより起動しています。パイプラインの中で今回作成したGstreamer Pluginである`dualfisheye` によりDualfisheye to Equirectangular変換を行っています。

```bash
#!/bin/bash

export GST_PLUGIN_PATH="./build:${GST_PLUGIN_PATH}"
export GST_DEBUG="1,dualfisheye:4"
# DEBUG_LEVEL  1=ERROR, 2=WARNING, 3=FIXME, 4=INFO, 5=DEBUG, 6=LOG, 7=TRACE

# force cache clear
rm -rf ~/.cache/gstreamer-1.0

# Launch dualfisheye conversion
gst-launch-1.0 -v \
  videotestsrc !\
  "video/x-raw,format=RGBx,width=1280,height=720,framerate=15/1" !\
  videoconvert !\
  dualfisheye !\
  videoconvert !\
  autovideosink
```

### THETA Sからの入力を変換して表示する

Theta Sから受け取ったDualfisheyeの画像をリアルタイムでEquirectangularに変換して画面に表示します。Theta Sをライブストリーミングモードに設定した後、PCにUSBで接続してから以下のスクリプトを実行します。

```console
./start.sh
```

スクリプト(start.sh)の中身です。macの場合はavfvideosrcを、Linuxの場合はv4l2srcによりカメラ画像をキャプチャしています。Linuxの場合はMotion JPEGで出力されるので、jpegdecによりRAW画像に変換してからdualfisheyeエレメントに渡します。

```bash
#!/bin/bash

if [ "$(uname)" == 'Darwin' ]; then   # Mac Case
  export CAMERA_SRC='avfvideosrc device-index=0 !'
elif [ "$(expr substr $(uname -s) 1 5)" == 'Linux' ]; then  # Linux Case
  if [[ $(uname -a) == *raspberry* ]]; then   # Raspberry Pi case
    export CAMERA_SRC='v4l2src device=/dev/video0 ! omxmjpegdec !'
  else                                        # Linux PC 
    export CAMERA_SRC='v4l2src device=/dev/video0 ! jpegdec !'
  fi
else
  echo 'Your platform is not supported. Only for linux or Mac.'
  exit 1
fi

export GST_PLUGIN_PATH="./build:${GST_PLUGIN_PATH}"
export GST_DEBUG="1,dualfisheye:4"
# DEBUG_LEVEL  1=ERROR, 2=WARNING, 3=FIXME, 4=INFO, 5=DEBUG, 6=LOG, 7=TRACE

# force cache clear
rm -rf ~/.cache/gstreamer-1.0

# Launch dualfisheye conversion
gst-launch-1.0 -v \
  $CAMERA_SRC \
  videoconvert !\
  dualfisheye !\
  videoconvert !\
  autovideosink

```

### RTPで配信する

このプラグインを使用するときには、環境変数`GST_PLUGIN_PATH`にプラグインのライブラリ(linuxの場合は、libgstdualfisheye.so)があるパスを記述し、プラグインのライブラリファイルと同じディレクトリにxmap.npy、ymap.npyのファイルを置いて起動します。

Linux PCでTheta Sカメラからの映像をEquirectangularに変換しVP8でエンコードしてRTP送信する場合のスクリプトは以下のようになります。環境変数`HOST`にRTPの送信先のホスト名またはIPアドレス、`PORT`にRTPのUDPポート番号を設定した上で、実行してください。

```console
export GST_PLUGIN_PATH='$(WORKDIR)/dualfisheye/build'
export HOST=xx.xx.xx.xx
export PORT=8001
gst-launch-1.0 -v \
  v4l2src device=/dev/video0 ! \
  image/jpeg,width=1280,height=720 ! \
  jpegdec ! \
  videoconvert ! \
  dualfisheye ! \
  videoconvert ! \
  vp8enc deadline=1 ! \
  rtpvp8pay pt=96 ! \
  udpsink host=${HOST} port=${PORT}
```



#### Linux / Raspberry piでビルドする場合の手順

Raspberry piにopencv4をセットアップする. 方法は[こちら](https://qiita.com/tomtum/items/f13a67a7d97ae7f051b1)

Gstreamer開発パッケージのインストール

```console
sudo apt install -y gstreamer1.0-omx gstreamer1.0-tools gstreamer1.0-plugins-* libgstreamer-plugins-base1.0-dev

```

mesonのインストール

```console
pip3 install meson ninja
```

リポジトリのクローンして、ビルド

```console
git clone https://github.com/kozokomiya/dualfisheye.git
cd dualfisheye
./build.sh
```

ただし、Raspberry pi3の場合は、カメラから入力して表示する場合1-2fps程度でした処理できませんでした。Raspberry pi4の場合は、変換処理は高速化できるもののgstreamerでのRaspberry pi4のハードウェアJPEGデコーダが動作しないため、15fpsでは動作しません。

### 動かない場合のTIPS

start.shスクリプトがうまく動かない場合として、カメラのデバイスの指定がうまく行っていない可能性があるので、以下を確認してください。

#### linux / raspberry piのカメラdeviceの確認方法 

```console
v4l2-ctl --list-devices
```

- デバイス名を読み取って、`start.sh` の７行目か９行目に指定する。

#### mac のカメラdevice-indexの確認方法

```console
ffmpeg -f avfoundation -list_devices true -i ""
```

- 出力から "RICOH THETA S" のindex番号を読み取って、`start.sh`の4行目に指定する。



### Gstreamer プラグインソースコードの作成方法

このプラグインは、[gst-plugins-bad](https://github.com/GStreamer/gst-plugins-bad)に含まれるgst-element-makerスクリプトを使って、プラグイン雛形を作成し、処理の内容を追加する方法で作成しました。ソースコード中にプラグインの情報を設定し、毎フレーム行うの変換処理を`gst_dualfisheye_transform_frame_ip()` 関数の内に記述します。