[Python] Transform Euler angle of RealityCapture camera alignment file

I'm trying to utilize lidar data with reality capture and POSTSHOT. My pointcloud is not ordered(structured) format, so cannot be imported into reality capture. So my idea is utilizing lidar data as original, overlaying pointcloud from reality capture with cloudcompare, then I can get 4x4 transform matrix. Then I can use it in POSTSHOT.

With this matrix, I have tried to convert camera alignment data from reality capture whicch is written in X, Y, alt, heading, pitch, yaw format. I have wrote python code to achieve this procedure, but the camera angle in the output file is strange. Can someone solve this issue or tell me better way?

My code is as below.

import numpy as np
import pandas as pd
from scipy.spatial.transform import Rotation as R
from scipy.optimize import minimize

# === 設定 ===
csv_path = "./way1.csv" # 変換前のカメラcsv
output_csv_path = csv_path.replace(".csv", "_ICP_vec_corrected.csv")

# CloudCompareなどから得た4x4 ICP行列（空白区切り、行列形式で貼り付け）
matrix_text = """
0.554002 -0.080952 0.002766 0.997178
0.080937 0.554003 0.003136 -0.088055
-0.003191 -0.002703 0.559877 0.004015
0.000000 0.000000 0.000000 1.000000
"""

def parse_matrix_from_text(text):
return np.array([[float(x) for x in line.strip().split()] for line in text.strip().splitlines()])

def load_rc_csv(csv_path):
df = pd.read_csv(csv_path)
df.columns = df.columns.str.strip().str.lower().str.replace('#', '')
return df

FRONT = np.array([0, 0, -1]) # カメラfront
UP = np.array([0, 1, 0]) # カメラup

def best_euler_for_vectors(front_goal, up_goal, order='ZXY'):
# 与えたfront/upベクトルを持つ回転行列をオイラー角で逆算
def loss(euler):
R_try = R.from_euler(order, euler, degrees=True)
f = R_try.apply(FRONT)
u = R_try.apply(UP)
return np.linalg.norm(f - front_goal) + np.linalg.norm(u - up_goal)
res = minimize(loss, np.zeros(3), method='BFGS', options={'gtol':1e-10, 'disp': False})
return res.x

def transform_camera_poses_matrix(df, T_icp):
output = []
for _, row in df.iterrows():
# カメラの元の回転行列
rot = R.from_euler('ZXY', [row['heading'], row['pitch'], row['roll']], degrees=True)
T = np.eye(4)
T[:3, :3] = rot.as_matrix()
T[:3, 3] = [row['x'], row['y'], row['alt']]
# 4x4行列で座標と回転を一括変換
T_new = T_icp @ T
R_new = R.from_matrix(T_new[:3, :3])
front_new = R_new.apply(FRONT)
up_new = R_new.apply(UP)
# 新しいfront/upベクトルを持つZXYオイラー角を逆算
new_euler = best_euler_for_vectors(front_new, up_new, order='ZXY')
outrow = dict(row)
outrow['x'] = T_new[0, 3]
outrow['y'] = T_new[1, 3]
outrow['alt'] = T_new[2, 3]
outrow['heading'] = new_euler[0]
outrow['pitch'] = new_euler[1]
outrow['roll'] = new_euler[2]
output.append(outrow)
return pd.DataFrame(output)

def apply_icp_to_csv_vec(csv_path, matrix_text):
df = load_rc_csv(csv_path)
T_icp = parse_matrix_from_text(matrix_text)
df_aligned = transform_camera_poses_matrix(df, T_icp)
df_aligned.to_csv(output_csv_path, index=False)
print(f"[INFO] ベクトル厳密一致バージョンで保存: {output_csv_path}")

if __name__ == "__main__":
apply_icp_to_csv_vec(csv_path, matrix_text)

Continue reading...