unity→instant-ngp座標変換

[h-cha]

nerf記事

2.2.2: ポーズ行列修正 COLMAP出力の回転行列は世界座標系からカメラ座標系への変換を表す形式です。NeRFでは反対にカメラ座標系から世界座標系への座標変換を行うため、ポーズ行列を修正する必要があります。この修正では、元の座標変換と逆の変換を獲得するため、単純に逆行列を計算することになります。

COLMAP出力の回転行列 世界座標系→カメラ座標系への座標変換

NeRFのtransform.jsonで必要なの回転行列 カメラ座標系→世界座標系への座標変換

charucoは？？？

[h-cha]

colmap2nerf.py

elems=line.split(" ") # 1-4 is quat, 5-7 is trans, 9ff is filename (9, if filename contains no spaces)
#name = str(PurePosixPath(Path(IMAGE_FOLDER, elems[9])))
# why is this requireing a relitive path while using ^
image_rel = os.path.relpath(IMAGE_FOLDER)
name = str(f"./{image_rel}/{'_'.join(elems[9:])}")
b = sharpness(name)
print(name, "sharpness=",b)
image_id = int(elems[0])
qvec = np.array(tuple(map(float, elems[1:5])))
tvec = np.array(tuple(map(float, elems[5:8])))
R = qvec2rotmat(-qvec)
t = tvec.reshape([3,1])
m = np.concatenate([np.concatenate([R, t], 1), bottom], 0)
c2w = np.linalg.inv(m) 

↓逆行列を計算 c2w = np.linalg.inv(m) これにより、COLMAP出力の回転行列（世界座標系→カメラ座標系への座標変換）をNeRFのtransform.jsonで必要なの回転行列（カメラ座標系→世界座標系への座標変換）にしている

[h-cha]

ngpの座標 transform.jsonに入力した座標(0.8, 1.0, 1.2)→実際にngpに表示される座標(1, 1.2, 0.8)

[h-cha]

blender -> transform.json

def generate_transform_matrix(pos, rot):
    def Rx(theta):
      return np.matrix([[ 1, 0            , 0            ],
                        [ 0, np.cos(theta),-np.sin(theta)],
                        [ 0, np.sin(theta), np.cos(theta)]])
    def Ry(theta):
      return np.matrix([[ np.cos(theta), 0, np.sin(theta)],
                        [ 0            , 1, 0            ],
                        [-np.sin(theta), 0, np.cos(theta)]])
    def Rz(theta):
      return np.matrix([[ np.cos(theta), -np.sin(theta), 0 ],
                        [ np.sin(theta), np.cos(theta) , 0 ],
                        [ 0            , 0             , 1 ]])

    R = Rz(rot[2]) * Ry(rot[1]) * Rx(rot[0])
    xf_rot = np.eye(4)
    xf_rot[:3,:3] = R

    xf_pos = np.eye(4)
    xf_pos[:3,3] = pos - average_position

    # barbershop_mirros_hd_dense:
    # - camera plane is y+z plane, meaning: constant x-values
    # - cameras look to +x

    # Don't ask me...
    extra_xf = np.matrix([
        [-1, 0, 0, 0],
        [ 0, 0, 1, 0],
        [ 0, 1, 0, 0],
        [ 0, 0, 0, 1]])
    # NerF will cycle forward, so lets cycle backward.
    shift_coords = np.matrix([
        [0, 0, 1, 0],
        [1, 0, 0, 0],
        [0, 1, 0, 0],
        [0, 0, 0, 1]])
    xf = shift_coords @ extra_xf @ xf_pos
    assert np.abs(np.linalg.det(xf) - 1.0) < 1e-4
    xf = xf @ xf_rot
    return xf

[h-cha]

[h-cha]

1. unityでロボットアームの座標を取得 2. ロボットアームの先端のx、y、z軸をunityの軸と揃える

3. unity(左手系) -> opencv（右手系）変換 4. quaternion （右手系）-> 回転行列（右手系）変換 5. 4x4の回転行列生成 6. opencv（右手系） -> transform.json 出力 　　　(x, y, z) -> (z, x, -y)

[h-cha]

1.unityでロボットアームの座標を取得

ロボットアームの先端の座標を取得 xarm_position_xyz.txt

1.00000  0.00031  -0.00050  0.00031　←xarm.transform.rotation　Quaternion(x, y, z, w)
-0.28  0.35  0.28　←xarm.transform.position　(x, y, z)

以下は使ってないけど、回転行列xarm.transform.localToWorldMatrixを取得している xarm_unity_mat.txt

1.00000 -0.00028    -0.00036    -0.28008
-0.00028    -1.00000    0.00005 0.35001
-0.00036    -0.00005    -1.00000    0.28003
0.00000 0.00000 0.00000 1.00000

[h-cha]

2.ロボットアームの先端の姿勢を変換する

方法が２つ候補がある

1. ロボットアームの先端の姿勢をunityのグローバル座標系と揃える
2. ロボットアームの先端の姿勢をカメラ方向をzとした基準カメラ姿勢に変換する

３軸加速度センサを用いた姿勢推定 「グローバル座標系」と「基準姿勢におけるロボットアームの先端の座標系」の座標軸の向きを揃える

基準姿勢におけるロボットアームの先端の座標系(x, y, z)→unityグローバル座標系(-x, -y, z)

疑問

座標を揃えるときに、カメラ方向をzにしたほうが良いのか？ 基準姿勢におけるロボットアームの先端の座標系(x, y, z) -> unityグローバル座標系(-x, z, y) その場合unityグローバル座標系に揃えることができなくなる

def robot_to_global_unity(quaternion, position):
    # robot -> camera　(x, y, z) -> (-x, z, y)z軸をカメラに向ける 左手座標
    # position[0] = -position[0]
    # temp = position[1]
    # position[1] = position[2]
    # position[2] = temp

    # quaternion[0] = -quaternion[0]
    # temp = quaternion[1]
    # quaternion[1] = quaternion[2]
    # quaternion[2] = temp

    # 基準姿勢におけるロボットアームの先端の座標系(x, y, z)→unityグローバル座標系(-x, -y, z)

    position[0] = -position[0]
    position[1] = -position[1]

    quaternion[0] = -quaternion[0]
    quaternion[1] = -quaternion[1]

    return quaternion, position

[h-cha]

3.unity(左手系) -> opencv（右手系）変換

回転ベクトル・回転行列・クォータニオン・オイラー角についてまとめてみた

Python scipyのRotationモジュールで三次元回転を扱う

quaternion -> 回転行列に変換するpythonモジュール（scipyのRotationモジュール）を使用するには、左手系->右手系に変換しないといけない

注意点として、このモジュールのAPIはすべて右手座標系向けに設計されています。 もし左手系の回転表現の計算に使うなら、回転表現と座標をすべて右手系に変換してからこのモジュールで計算しその結果を左手系に変換し直す必要があります。 右手系と左手系の変換方法については、以前の記事を参照してください。

# unity （左手系）->　opencv　（右手系）
def left_to_right(quaternion, position):
    # y軸反転
    quaternion[1] = -quaternion[1]
    position[1] = -position[1]
    return quaternion, position

[h-cha]

4.quaternion （右手系）-> 回転行列（右手系）変換

Python scipyのRotationモジュールで三次元回転を扱う

# quaternion （右手系）-> 回転行列（右手系）
def q_to_mat(quaternion):
    # クォータニオンから
    # quat = np.array([0.        , 0.67385289, 0.44923526, 0.58660887])
    rot = Rotation.from_quat(quaternion)

    # 回転行列に変換
    rot = rot.as_matrix()

    return rot

[h-cha]

5. 4x4の回転行列生成

# rot(mat3x3), pos(x,y,z)回転行列作成
def mat4x4(rot, position):
    mat = np.zeros((4, 4))
    mat[0:3, 0:3] = rot
    mat[0:3, 3] = position
    mat[3, 3] = 1
    return mat

[h-cha]

6. opencv（右手系） -> transform.json 出力

# opencv　（右手系）-> nerf
def generate_transform_matrix(mat):
    shift_coords = np.matrix([
        [0, 0, 1, 0],
        [1, 0, 0, 0],
        [0, -1, 0, 0],
        [0, 0, 0, 1]])

    xf = shift_coords @ mat
    return xf

[h-cha]

• [ ] 2. ロボットアームの先端の姿勢を変換するが間違っている可能性 unityで取得したquaternionとpositionでカメラの軌跡を可視化してみる（カメラ方向がz軸になっているか？）
• [x] 6. opencv（右手系） -> transform.json 出力が間違っている可能性 検証 　　検証した結果間違ってなさそう