ArUcoマーカーでカメラ位置姿勢の取得

- 記事のお引越しを考えてるので近々非公開にするかもしれないです…
- [ChArUco マーカーボードで位置推定](https://qiita.com/namahoge/items/552da5f2495d0daef3d6) も参考になるかもしれないです
- (この記事のコードめちゃくちゃ雑です・・・悪しからず・・・)


# はじめに

- ArUcoマーカーってカメラの自己位置推定に便利な気がしてたんですが、探してもいいサンプルがなくて詰まったのでメモ
    - (もっといい書き方あれば教えてほしいです、、、)
- コード、素材、カメラパラメータ等はここに置いてます ⇨ https://github.com/naruya/aruco

# 環境
- python3.7

# 素材

- shisa.mp4

![shisa.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/1d855f77-bf99-5871-b6c1-332fe946f36b.gif)

# 出力
![sample1 (1).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/88c53333-d509-65a8-cc89-a91b9b1be2fe.gif) ![sample2 (1).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/ef0cc55e-0e8d-be8d-6380-e300034283d0.gif)

# 理論的な
- この方の記事がわかりやすかったです
    - https://mem-archive.com/2018/10/28/post-962/

- カメラの外部パラメータ( $R$ と $T$ )は以下を満たす
    - c: カメラ( **画像** )座標系
    - w: 世界座標系

```math
\begin{bmatrix}
X_c \\
Y_c \\
Z_c
\end{bmatrix}
= R
\begin{bmatrix}
X_w \\
Y_w \\
Z_w
\end{bmatrix}
+ T
```

- 世界座標系でのカメラの座標 $O_w$ を求めるには、まず $R^{-1} = R^\top$ を使って、

```math
\begin{bmatrix}
X_w \\
Y_w \\
Z_w
\end{bmatrix}
=
R^\top
\begin{pmatrix}
\begin{bmatrix}
X_c \\
Y_c \\
Z_c
\end{bmatrix}
- T
\end{pmatrix}
```

- 次に、$O_w$ はカメラ座標系では原点なので、上の式に $ \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}^\top = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}^\top $ を代入して、

```math
O_w = 
\begin{bmatrix}
O^x_w \\
O^y_w \\
O^z_w
\end{bmatrix}
=
R^\top
\begin{pmatrix}
- T
\end{pmatrix}
```

- また、カメラの正面向き $vz_w$ ( $ vz_c = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}^\top $ )は、カメラ座標 $ \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}^\top = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}^\top$ を考えることで、

```math
vz_w = 
R^\top
\begin{pmatrix}
\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix}
- T
\end{pmatrix}
- O_w
=
R^\top
\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix}
```

- 他のカメラの向き $vx_w$ ( $vx_c = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}^\top $: カメラ右向き)、 $vy_w$ ( $vy_c = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}^\top $: カメラ下向き) も同様

### 注意ポイント

- 座標系の取り方の微妙な違い(どっちも右手座標系＆それはそう感はあるけど一応)
    - 世界座標系(アルコマーカーの座標系):
        - マーカー自体がxy平面上にあって、右がx軸正、**上**がy軸正、マーカー上空がz軸正
    - カメラ座標系(OpenCVのカメラ画像に対する座標系):
        - 画像右がx軸正、画像**下**がy軸正、画像奥がz軸正
- え、カメラの歪みとか使わずにRとTで世界座標系での位置求めちゃっていいの？と思ったけどたぶんRとTはカメラの歪みとか考慮して求められてるし問題ない気がしている(とりあえず経験的には実用上問題ない)


# コード

## 1. 準備

```python
import cv2
import numpy as np
import matplotlib.pyplot as plt
from skvideo.io import vread
import moviepy.editor as mpy
from tqdm import tqdm
from mpl_toolkits.mplot3d import axes3d, Axes3D

def npy_to_gif(npy, filename):
    clip = mpy.ImageSequenceClip(list(npy), fps=10)
    clip.write_gif(filename)

vid = vread("src/shisa.mp4")
print(vid.shape)
```

(524, 1920, 1080, 3)

## 2. アルコマーカー

```python
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
aruco = cv2.aruco.drawMarker(aruco_dict, 0, 256)
cv2.imwrite("aruco.png", aruco)
```

![aruco.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/c47466e8-60f3-e2aa-229a-2c361b8e9c11.png)


- これを印刷して脚立に貼り付けました

## 3. カメラの設定

- Google Pixel3 の録画モード、倍率1、で撮りました

```python
marker_length = 0.07 # [m] ### 注意！
mtx = np.load("camera/mtx.npy")
dist = np.load("camera/dist.npy")
```

## 4. メイン

- 画像をリサイズすると `xyz` や `rpy` の値(スケール?)も変わるので注意してください
- rpy(roll pitch yaw)はplotには使わないですが一応

```python
XYZ = []
RPY = []
V_x = []
V_y = []
V_z = []

for frame in vid[:500:25]:  # 全部処理すると重いので…
    frame = frame[...,::-1]  # BGR2RGB
    corners, ids, _ = cv2.aruco.detectMarkers(frame, aruco_dict)

    if len(corners) == 0:
        continue

    rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_length, mtx, dist)

    R = cv2.Rodrigues(rvec)[0]  # 回転ベクトル -> 回転行列
    R_T = R.T
    T = tvec[0].T

    xyz = np.dot(R_T, - T).squeeze()
    XYZ.append(xyz)

    rpy = np.deg2rad(cv2.RQDecomp3x3(R_T)[0])
    RPY.append(rpy)
    
    V_x.append(np.dot(R_T, np.array([1,0,0])))
    V_y.append(np.dot(R_T, np.array([0,1,0])))
    V_z.append(np.dot(R_T, np.array([0,0,1])))

    # ---- 描画
    cv2.aruco.drawDetectedMarkers(frame, corners, ids, (0,255,255))
    cv2.aruco.drawAxis(frame, mtx, dist, rvec, tvec, marker_length/2)
    cv2.imshow('frame', frame)
    cv2.waitKey(1)
    # ----

cv2.destroyAllWindows()
```

- こんな感じ

![shisa.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/7b9cc4ee-5076-080c-ada8-97ec78540a09.png)

- ちなみに `rpy` は、 
    - `cv2.RQDecomp3x3()` を使う方法
    - `cv2.decomposeProjectionMatrix()` を使う方法
    - `arctan()` やら `arcsin()` 
- を使う方法などなどありそう(後述)

## 5. カメラ位置の描画

```python
def plot_all_frames(elev=90, azim=270):
    frames = []

    for t in tqdm(range(len(XYZ))):
        fig = plt.figure(figsize=(4,3))
        ax = Axes3D(fig)
        ax.view_init(elev=elev, azim=azim)
        ax.set_xlim(-2, 2); ax.set_ylim(-2, 2); ax.set_zlim(-2, 2)
        ax.set_xlabel("x"); ax.set_ylabel("y"); ax.set_zlabel("z")

        x, y, z = XYZ[t]
        ux, vx, wx = V_x[t]
        uy, vy, wy = V_y[t]
        uz, vz, wz = V_z[t]

        # draw marker
        ax.scatter(0, 0, 0, color="k")
        ax.quiver(0, 0, 0, 1, 0, 0, length=1, color="r")
        ax.quiver(0, 0, 0, 0, 1, 0, length=1, color="g")
        ax.quiver(0, 0, 0, 0, 0, 1, length=1, color="b")
        ax.plot([-1,1,1,-1,-1], [-1,-1,1,1,-1], [0,0,0,0,0], color="k", linestyle=":")

        # draw camera
        ax.quiver(x, y, z, ux, vx, wx, length=0.5, color="r")
        ax.quiver(x, y, z, uy, vy, wy, length=0.5, color="g")
        ax.quiver(x, y, z, uz, vz, wz, length=0.5, color="b")

        # save for animation
        fig.canvas.draw()
        frames.append(np.array(fig.canvas.renderer.buffer_rgba()))
        plt.close()

    return frames
```

```python
frames = plot_all_frames(elev=105, azim=270)
npy_to_gif(frames, "src/sample1.gif")

frames = plot_all_frames(elev=165, azim=270)
npy_to_gif(frames, "src/sample2.gif")
```

![sample1 (1).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/88c53333-d509-65a8-cc89-a91b9b1be2fe.gif) ![sample2 (1).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/ef0cc55e-0e8d-be8d-6380-e300034283d0.gif)

### デフォルトの向き

```python
ax.quiver(x, y, z, 1,0,0, length=0.5, color="r")
ax.quiver(x, y, z, 0,1,0, length=0.5, color="g")
ax.quiver(x, y, z, 0,0,1, length=0.5, color="b")
```

- ってやると、

![sample3.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/188173/9f9df930-8f76-4006-cde1-e16506440079.gif)

- アルコマーカー使った回転ベクトル/回転行列は、⇧からの回転(の逆変換)を求めてくれてる
    - `R.T` とかする必要あるので
    - (どっちが逆と言われるべきなのかはよくわからない)

## おまけ

### roll pitch yaw を求める他の方法

```python
rpy = np.deg2rad(cv2.RQDecomp3x3(R_T)[0])
print(rpy)

# Tはなんでもいい & この用途だとあんまよくない方法だと思う
rpy = cv2.decomposeProjectionMatrix(np.hstack([R_T, -T]))[6]  # [0~5]は使わない
rpy = np.deg2rad(rpy.squeeze())
print(rpy)

r = np.arctan2(-R_T[2][1], R_T[2][2])
p = np.arcsin(R_T[2][0])
y = np.arctan2(-R_T[1][0], R_T[0][0])
rpy = - np.array([r, p, y])
print(rpy)
```

- 全部同じ
- 以上、回転行列 ⇨ 回転ベクトルでした

### roll pitch yaw と回転行列の関係

```python
def eulerAnglesToRotationMatrix(euler):
    R_x = np.array([[                1,                 0,                 0],
                    [                0,  np.cos(euler[0]), -np.sin(euler[0])],
                    [                0,  np.sin(euler[0]),  np.cos(euler[0])]])
    R_y = np.array([[ np.cos(euler[1]),                 0,  np.sin(euler[1])],
                    [ 0,                                1,                 0],
                    [-np.sin(euler[1]),                 0,  np.cos(euler[1])]])
    R_z = np.array([[ np.cos(euler[2]), -np.sin(euler[2]),                 0],
                    [ np.sin(euler[2]),  np.cos(euler[2]),                 0],
                    [                0,                 0,                 1]])
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R

diff = eulerAnglesToRotationMatrix(rpy) - R_T
print(diff.astype(np.float16))

from scipy.spatial.transform import Rotation
diff = Rotation.from_euler('xyz', rpy).as_matrix() - R_T
print(diff.astype(np.float16))
```

- (diff は 0)
- `eulerAnglesToRotationMatrix()` を使えば回転行列になる(戻せる)
    - それぞれの成分の回転行列の積
- `eulerAnglesToRotationMatrix()` を `scipy` でやったverが2個目
- 以上、回転ベクトル ⇨ 回転行列の変換でした



# 以上！

よければLGTMを～