[OpenCV] 100行で作るAR


## はじめに

100行のPythonスクリプトでARを作ります。
(正確には98行です)
本記事ではこのスクリプトを**`100行AR`**と呼称します。

## デモ動画

`100行AR`のデモ動画です。

<iframe width="560" height="315" src="https://www.youtube.com/embed/oaZEkQOWKgo" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

## 環境

以下の環境で実行しました。

- Windows11(カメラ付き)
- Python 3.10.0
- OpenCV 4.5.4

## 環境のセットアップ

Windows11環境のセットアップ方法です。

### Pythonをインストールする

以下のサイトからインストーラをダウンロードしインストールします。

https://www.python.org/

インストーラの以下の画面で、`Add Python 3.10 to PATH` にチェックを入れるとPythonに関するパスがWindowsの環境変数に追記されます。

![01.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/c218161a-a9f0-570f-be16-af0d78ecc183.png)

powershellを開き、pythonのバージョンが表示されれば成功です。

```powershell
PS C:\> python -V
Python 3.10.0
PS C:\>
```
### pipをアップグレードする

OpenCVをインストールする前に、pipをアップグレードしておきます

```powershell
PS C:\> python.exe -m pip install --upgrade pip
```

念のため、バージョンを確認し最新であることを確認します。

```powershell
PS C:\> pip -V
pip 21.3.1 from C:\Users\hoge\AppData\Local\Programs\Python\Python310\lib\site-packages\pip (python 3.10)
PS C:\>
```

### OpenCVをインストールする

OpenCVをインストールします。

```powershell
PS C:\> pip install opencv-contrib-python==4.5.4.60
```

以下のように、OpenCVのバージョンが確認できれば成功です。

```powershell
PS C:\> python
Python 3.10.0 (tags/v3.10.0:b494f59, Oct  4 2021, 19:00:18) [MSC v.1929 64 bit (AMD64)] on win32
Type "help", "copyright", "credits" or "license" for more information.
>>> import cv2
>>> cv2.__version__
'4.5.4'
>>> exit()
PS C:\>
```

---

以上で環境のセットアップが完了しました。

## 100行ARのスクリプトソース

お待たせしました `100行AR`のスクリプトソース(100ar.py)です。Powershellで　`python 100ar.py` と実行すれば処理が開始されます。`q`で停止します。


```python:100ar.py
import cv2
import cv2.aruco as aruco
import numpy as np

targetVideo = 0 # カメラデバイス
cap = cv2.VideoCapture( targetVideo )

# 立方体の座標
cube = np.float32([[0.025,-0.025,0], [-0.025,0.025,0], [0.025,0.025,0], [-0.025,-0.025,0],
                    [0.025,-0.025,0.05], [-0.025,0.025,0.05], [0.025,0.025,0.05], [-0.025,-0.025,0.05]
                    ])

while cap.isOpened():
    ret, img = cap.read()
    if img is None :
        break
    scale_percent = 50 # percent of original size
    width = int(img.shape[1] * scale_percent / 100)
    height = int(img.shape[0] * scale_percent / 100)
    dim = (width, height)
    # Check if frame is not empty
    if not ret:
        continue
    # Set AR Marker
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
    parameters = aruco.DetectorParameters_create()
    corners, ids, _ = aruco.detectMarkers(img, aruco_dict, parameters=parameters)
    center = (width, height)
    focal_length = center[0] / np.tan(60/2 * np.pi / 180)
    camera_matrix = np.array(
                [[focal_length, 0, center[0]],
                [0, focal_length, center[1]],
                [0, 0, 1]], dtype = "double"
            )
    dist_coeffs = np.zeros((4,1)) # レンズ歪みなしの設定
    if len(corners) > 0:
        for i, corner in enumerate(corners):
            rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corner, 0.05, camera_matrix, dist_coeffs)
            tvec = np.squeeze(tvecs)
            rvec = np.squeeze(rvecs)
            rvec_matrix = cv2.Rodrigues(rvec) # 回転ベクトルからrodoriguesへ変換
            rvec_matrix = rvec_matrix[0]      # rodoriguesから抜き出し

            # cubeの座標をARマーカに合わせる
            imgpts, jac = cv2.projectPoints(cube, rvecs, tvecs, camera_matrix, dist_coeffs)
            outpts = []
            for lp in imgpts:
                lp_int = lp.astype(np.int64)
                outpts.append( tuple(lp_int.ravel()) )

            # ARマーカに合わせたcube座標を描画:底面
            cv2.line(img,outpts[0],outpts[2],(255,0,0),2)
            cv2.line(img,outpts[1],outpts[2],(255,0,0),2)
            cv2.line(img,outpts[1],outpts[3],(255,0,0),2)
            cv2.line(img,outpts[3],outpts[0],(255,0,0),2)

            # ARマーカに合わせたcube座標を描画:上面
            cv2.line(img,outpts[4],outpts[6],(0,255,0),2)
            cv2.line(img,outpts[5],outpts[6],(0,255,0),2)
            cv2.line(img,outpts[5],outpts[7],(0,255,0),2)
            cv2.line(img,outpts[7],outpts[4],(0,255,0),2)

            # ARマーカに合わせたcube座標を描画:支柱
            cv2.line(img,outpts[0],outpts[4],(0,0,250),2)
            cv2.line(img,outpts[1],outpts[5],(0,0,250),2)
            cv2.line(img,outpts[2],outpts[6],(0,0,250),2)
            cv2.line(img,outpts[3],outpts[7],(0,0,250),2)

            # ARマーカに合わせたcube座標を描画
            cv2.circle(img, outpts[0], 10, (0, 240, 160), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '0', outpts[0], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[1], 10, (40, 200, 200), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '1', outpts[1], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[2], 10, (80, 160, 240), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '2', outpts[2], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[3], 10, (120, 120, 40), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '3', outpts[3], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[4], 10, (160, 80, 80), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '4', outpts[4], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[5], 10, (200, 40, 120), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '5', outpts[5], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[6], 10, (0, 0, 255), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '6', outpts[6], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

            cv2.circle(img, outpts[7], 10, (0, 255, 255), thickness=-1, lineType=cv2.LINE_AA)
            cv2.putText(img, '7', outpts[7], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)

    cv2.imshow('frame', img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
cap.release()
cv2.destroyAllWindows()
```

実行例です。

```powershell:100ar.pyの実行方法
PS C:\Users\hoge\100ar> dir

    ディレクトリ: C:\Users\hoge\100ar

Mode                 LastWriteTime         Length Name
----                 -------------         ------ ----
-a---l        2021/11/23     11:34           4732 100ar.py

PS C:\Users\hoge\100ar> python .\100ar.py
``` 

## 仕組み

### 立方体の座標算出

`100行AR`での立方体の座標算出の流れを紹介します。

(Fig-1)の向かって左が`3次元空間に配置したARマーカー`、向かって右が`カメラで撮影した映像`です。

![Fig-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/c918f954-ccef-d85a-c17e-e0aaf9280acf.png)
**(Fig-1) 3次元空間のARマーカーとカメラ映像**

(Fig-2)は、(Fig-1)の`3次元空間に配置したARマーカー`に対しカメラの撮影位置を算出した結果を図示したものです。
`カメラで撮影した映像`から`3次元空間に配置したARマーカー`を撮影した**`カメラ位置`**の算出には、`estimatePoseSingleMarkers()`を使用します。
正確には、ARマーカーとカメラ位置の相対的な位置関係を算出します。原点(0,0,0)はARマーカーの中点のようです。

![Fig-2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/75bdf270-32d4-2633-6367-a971b69be01f.png)
**(Fig-2) カメラ位置の算出**

(Fig-2)の処理は以下になります。以降の変数`tvec`,`rvec`,`rvec_matrix`の処理は立方体の算出で使用します。

```python:100ar.py_ARマーカーとカメラ位置の相対的な位置関係を算出
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corner, 0.05, camera_matrix, dist_coeffs)
tvec = np.squeeze(tvecs)
rvec = np.squeeze(rvecs)
rvec_matrix = cv2.Rodrigues(rvec) # 回転ベクトルからrodoriguesへ変換
rvec_matrix = rvec_matrix[0]      # rodoriguesから抜き出し
```

`estimatePoseSingleMarkers()`の詳細はこちら

https://docs.opencv.org/4.x/d9/d6a/group__aruco.html#ga84dd2e88f3e8c3255eb78e0f79571bd1

ここまでで、(Fig-3)のように撮影したARマーカーの映像からカメラ位置が算出できました。

![Fig-3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/7ab3b7ca-ea46-ca41-501a-a51bd93cd2b1.png)
**(Fig-3) 算出されたカメラ位置**

ここからは立方体に対する処理になります。
(Fig-4)は、向かって左が`3次元空間に配置した立方体`、中央のカメラがさきほど`estimatePoseSingleMarkers()` で算出した`カメラの位置`、向かって右が`そのカメラで撮影した映像`です。
算出したカメラ位置から立方体を撮影するとどのような座標になるかを `projectPoints()` で算出します。

![Fig-4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/b15cbf56-7c01-4e0c-094d-c7bd25be22ac.png)
**(Fig-4) 3次元空間の立方体とカメラとカメラ映像**

(Fig-4)の`3次元空間に配置した立方体`の座標は以下です。

```python:100ar.py_立方体の座標
# 立方体の座標
cube = np.float32([[0.025,-0.025,0], [-0.025,0.025,0], [0.025,0.025,0], [-0.025,-0.025,0],
                    [0.025,-0.025,0.05], [-0.025,0.025,0.05], [0.025,0.025,0.05], [-0.025,-0.025,0.05]
                    ])
```

(Fig-4)の`そのカメラで撮影した映像`が以下です。正確には映像ではなく座標です。
変数 `cube` が変換前の立方体の座標です。変換後の座標は変数 `imgpts` に格納されます。
変数 `cube` には3次元の座標が、`imgpts` には2次元の座標が格納されています。

```python:100ar.py_立方体の座標変換
# cubeの座標をARマーカに合わせる
imgpts, jac = cv2.projectPoints(cube, rvecs, tvecs, camera_matrix, dist_coeffs)
```

`projectPoints()`の詳細はこちら

http://opencv.jp/opencv-2svn/cpp/camera_calibration_and_3d_reconstruction.html#cv-projectpoints

(Fig-5)のように立方体の底面がARマーカーと一致するようにしています。

![Fig-5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/50061b61-325f-bf87-b0a4-7d872627b1b4.png)
**(Fig-5) ARマーカーと立方体**


### 立方体の描画

座標変換ができれば、あとは描画するだけです。スクリプトソースの以下のところから描画処理になります。

```python:100ar.py_底面に沿う直線を描画
# ARマーカに合わせたcube座標を描画:底面
cv2.line(img,outpts[0],outpts[2],(255,0,0),2)
cv2.line(img,outpts[1],outpts[2],(255,0,0),2)
cv2.line(img,outpts[1],outpts[3],(255,0,0),2)
cv2.line(img,outpts[3],outpts[0],(255,0,0),2)
```

以下は、立方体のポイントに円と数字`0`を描画しています。

```python:100ar.py_円と数字0を描画
# ARマーカに合わせたcube座標を描画
cv2.circle(img, outpts[0], 10, (0, 240, 160), thickness=-1, lineType=cv2.LINE_AA)
cv2.putText(img, '0', outpts[0], cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), thickness=2)
```

立方体の各ポイントに割り振った数値は(Fig-6)の通りです。

![Fig-6.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/274270/481da933-4f25-6b89-22f4-c2860861b21f.png)
**(Fig-6) 立方体の模型**

余談ですが、(Fig-6)の模型は今回のプログラムを作ために作成しました。紙に絵を描いていたのですが、分けがわからなくなったので模型を作りました。

---

`100行AR`は以上です。
可読性を高めるため描画処理を冗長にしたため100行になりましたが、最適化すればもっと短いプログラムになりそうです。

※補足 ARマーカーの作成はこちらが参考になるかも

https://qiita.com/suo-takefumi/items/30a225cef8ea42366f8e

## さらなる高みへ

ここからは、`100行AR`を改良するとどのように進化するかを紹介します。

### 箱を描画

立方体の各面について、描画順序を判定する処理を追加しました。これにより各面に画像をマッピングしても箱の形状を維持できるようになりました。

<iframe width="560" height="315" src="https://www.youtube.com/embed/u-SKp6FQqyw" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

### 凹むAR

画像処理を追加し、凹むエフェクトができました。

<iframe width="560" height="315" src="https://www.youtube.com/embed/fjfsPWGBYsU" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

### 仮想の穴

ストリーミングと組み合わせ穴が空いたようなエフェクト(**仮想穴**)ができました。この仮想穴は、私が開発を続けているもので **`ウソ穴`** と命名しています。

<iframe width="560" height="315" src="https://www.youtube.com/embed/qTkvsEdxneI" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>


### 2壁貫通の仮想穴

複数のストリーミングとAR処理のタイミングを追加し、2つの壁を貫通する仮想穴ができました。デモ動画では仮想の穴は2つですが、理論的には無数の壁を貫通できます。

<iframe width="560" height="315" src="https://www.youtube.com/embed/ZsRe1d_MD3U" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

（処理量が多すぎるためか、動画がひどくコマ落ちしています。処理の無駄を省き最適化したいところ）

## おわりに

`100行AR`のスクリプトソースを紹介しました。
`100行AR`では、立方体の座標を使用しましたが、今後は3Dオブジェクトデータの描画にも挑戦したいと思います。