Record3Dを使って世界座標におけるカメラの位置と向きを取得する

# 概要
1. `.r3d`ファイルをzipファイルとして解凍する
1. 解凍した後にでできた`metadata`をjsonファイルとして読み込む
1. `metadata`から世界座標におけるカメラの位置と向きを取得する

# 前提
1. Lidarが搭載されたiPhoneもしくはiPadを用意できること（現時点ではproシリーズのみ？）
1. Record3Dをios端末にインストールし，撮影した動画.r3dファイルを手に入れられるようにできること
1. なんらかの方法で.r3dファイルをPC上にコピーできること

# `.r3d`ファイルをzipファイルとして解凍する
https://github.com/marek-simonik/record3d/issues/27 を参考にした．
このissueでは`.r3d`ファイルは`zip`ファイルになっているようで，中には`metadata`やらrgb画像やらdepthファイルが入っている．
```bash
$ mv hogehoge.r3d hogehoge.zip
$ unzip hogehoge.zip
```

# 解凍した後にでできた`metadata`をjsonファイルとして読み込む
https://github.com/marek-simonik/record3d/issues/27 を参考にした．
世界座標におけるカメラの位置と向きは`metadata`に入っているようなので，`metadata`を開くことにする．
このissueによると`metadata`はjsonファイルのようなので，jsonファイルとして開けばよい．
```python:main.py
import json
json_file = "metadata"
with open(json_file, "r") as f:
    metadata = json.loads(f.read())
```

# `metadata`から世界座標におけるカメラの位置と向きを取得する
https://github.com/marek-simonik/record3d/issues/27 を参考にした．
### クォータニオンと世界ポーズを得る
このissueによると`metadata`内の`pose`というところに世界座標におけるカメラの位置と向きが保存されている．また，カメラの位置と向きの保存方法はクォータニオンと世界ポーズで保存されているようだ．
例えば以下のようにすれば0フレーム目（始めのフレーム）のクォータニオンと世界ポーズにアクセスできる．
クォータニオンについて，順番が`qx, qy, qz, qw`なので気を付けてほしい．
```python:main.py
qx, qy, qz, qw = metadata["pose"][0][:4]
px, py, pz = metadata["pose"][0][4:]
```

普通はiフレームフレームだけアクセスすることはなく，0フレーム目から順にアクセスすると思う．
以下がそれぞれのフレームからクォータニオンと世界ポーズを取得するコードである．
```python:main.py
metadata["pose"] # 二次元配列
for q in metadata["pose"]
    qx, qy, qz, qw, px, py, pz = q
```

### クォータニオンと世界ポーズから世界座標におけるカメラの位置と向きを得る
Record3D内ではopenglで採用している世界座標系（右手座標系）を使っている．
クォータニオンから右手座標系に変換するには以下のコードを使えばよい

```python:main.py
import quaternion
import numpy as np
def get_mat_from_pose(pose):
    matrix = np.eye(4)
    qx, qy, qz, qw, px, py, pz = pose
    matrix[:3, :3] = as_rotation_matrix(quaternion(qw, qx, qy, qz))
    matrix[:3, -1] = [px, py, pz]
    return matrix

for q in metadata["pose"]
    mat = get_mat_from_pose(q)
    obj_pts = mat_trf[:3, -1] #(x座標, y座標, z座標)
    obj_dir = -mat_trf[:3, 2] #(dx, dy, dz)
```
これで世界座標におけるカメラの位置と向きが分かるようになった．
以下はカメラの位置と向きを得るための，完成したコードである．
```python:main.py
import json
import quaternion
import numpy as np
def get_mat_from_pose(pose):
    matrix = np.eye(4)
    qx, qy, qz, qw, px, py, pz = pose
    matrix[:3, :3] = as_rotation_matrix(quaternion(qw, qx, qy, qz))
    matrix[:3, -1] = [px, py, pz]
    return matrix

json_file = "metadata"
with open(json_file, "r") as f:
    metadata = json.loads(f.read())

for q in metadata["pose"]
    mat = get_mat_from_pose(q)
    obj_pts = mat_trf[:3, -1] #(x座標, y座標, z座標)
    obj_dir = -mat_trf[:3, 2] #(dx, dy, dz)
```

### matplotlibでカメラの位置と向きを確認する
以下は余談であるので，無視してかまわない．
上記の方法で手に入れたカメラの位置と向きが正しいのか確認してみたいと思うときがあるだろう．
以下のコードで確認することができる．
openglの座標系からmatplotlibの座標系に変えているところがあるので，気を付けてほしい．
```python:main.py
import quaternion
import numpy as np
import matplotlib.pyplot as plt
def get_mat_from_pose(pose):
    matrix = np.eye(4)
    qx, qy, qz, qw, px, py, pz = pose
    matrix[:3, :3] = as_rotation_matrix(quaternion(qw, qx, qy, qz))
    matrix[:3, -1] = [px, py, pz]
    return matrix

json_file = "metadata"
with open(json_file, "r") as f:
    import json
    metadata = json.loads(f.read())

x = []
y = []
z = []
x_dir = []
y_dir = []
z_dir = []

for q in metadata["pose"]
    mat = get_mat_from_pose(q)
    obj_pts = mat_trf[:3, -1] #(x座標, y座標, z座標)
    obj_dir = -mat_trf[:3, 2] #(dx, dy, dz)

    x.append(obj_pts[0].item())
    y.append(obj_pts[1].item())
    z.append(-obj_pts[2].item())
    x_dir.append(obj_dir[0].item())
    y_dir.append(obj_dir[1].item())
    z_dir.append(-obj_dir[2].item())

fig = plt.figure()
ax = fig.add_subplot(projection='3d')
ax.scatter(x, z, y, s = 0.4, c = "red") #カメラの位置を表示する

for i in range(len(x)):
    ax.quiver(x[i], z[i], y[i], x_dir[i], z_dir[i], y_dir[i]) #カメラの向きを表示する

ax.set_aspect("equal")
plt.show()
```