0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

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

Last updated at Posted at 2023-03-29

概要

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

前提

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

.r3dファイルをzipファイルとして解凍する

https://github.com/marek-simonik/record3d/issues/27 を参考にした.
このissueでは.r3dファイルはzipファイルになっているようで,中にはmetadataやらrgb画像やらdepthファイルが入っている.

$ mv hogehoge.r3d hogehoge.zip
$ unzip hogehoge.zip

解凍した後にでできたmetadataをjsonファイルとして読み込む

https://github.com/marek-simonik/record3d/issues/27 を参考にした.
世界座標におけるカメラの位置と向きはmetadataに入っているようなので,metadataを開くことにする.
このissueによるとmetadataはjsonファイルのようなので,jsonファイルとして開けばよい.

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なので気を付けてほしい.

main.py
qx, qy, qz, qw = metadata["pose"][0][:4]
px, py, pz = metadata["pose"][0][4:]

普通はiフレームフレームだけアクセスすることはなく,0フレーム目から順にアクセスすると思う.
以下がそれぞれのフレームからクォータニオンと世界ポーズを取得するコードである.

main.py
metadata["pose"] # 二次元配列
for q in metadata["pose"]
    qx, qy, qz, qw, px, py, pz = q

クォータニオンと世界ポーズから世界座標におけるカメラの位置と向きを得る

Record3D内ではopenglで採用している世界座標系(右手座標系)を使っている.
クォータニオンから右手座標系に変換するには以下のコードを使えばよい

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)

これで世界座標におけるカメラの位置と向きが分かるようになった.
以下はカメラの位置と向きを得るための,完成したコードである.

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の座標系に変えているところがあるので,気を付けてほしい.

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()
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?