Open h-cha opened 1 year ago
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で必要なの回転行列(カメラ座標系→世界座標系への座標変換)にしている
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
ロボットアームの先端の座標を取得
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
方法が2つ候補がある
3軸加速度センサを用いた姿勢推定 「グローバル座標系」と「基準姿勢におけるロボットアームの先端の座標系」の座標軸の向きを揃える
基準姿勢におけるロボットアームの先端の座標系(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
回転ベクトル・回転行列・クォータニオン・オイラー角についてまとめてみた
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
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
# 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
# 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
nerf記事
COLMAP出力の回転行列 世界座標系→カメラ座標系への座標変換
NeRFのtransform.jsonで必要なの回転行列 カメラ座標系→世界座標系への座標変換
charucoは???