輸入資料：
[ 1.73137369e+12 1.25671584e+02 7.06587582e+00 -2.20798730e+00 1.98006199e-02 -1.99310770e-02 9.99605316e-01 -5.13860500e-04 1.00000000e+00]
def quat_to_rotmat(x, y, z, w):
rot_mat = np.array(
[
[1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * z * w, 2 * x * z + 2 * y * w],
[2 * x * y + 2 * z * w, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * x * w],
[2 * x * z - 2 * y * w, 2 * y * z + 2 * x * w, 1 - 2 * x * x - 2 * y * y],
]
)
return rot_mat
def rotation_to_quat(rotation_arr):
w = ((np.trace(rotation_arr) + 1) ** 0.5) / 2
x = (rotation_arr[2][1] - rotation_arr[1][2]) / (4 * w)
y = (rotation_arr[0][2] - rotation_arr[2][0]) / (4 * w)
z = (rotation_arr[1][0] - rotation_arr[0][1]) / (4 * w)
return w, x, y, z
def prepare_vcs_2_global_for_upload_data(
source_calibration_dir_path, target_calibration_dir_path, selected_pose
):
calibration_name = f"calibration.json"
calib_file = os.path.join(source_calibration_dir_path, calibration_name)
with open(calib_file, "r") as fin:
calib_parameters = json.load(fin)
vcs_2_global_pose = []
lidar_2_vcs_arr = np.array(calib_parameters["lidar_top_2_vcs"]["T"])
for idx, pose_item in enumerate(selected_pose):
rotation_matrix = quat_to_rotmat(
pose_item[4],
pose_item[5],
pose_item[6],
pose_item[7],
)
lidar_pose = np.vstack(
(
np.hstack(
(
rotation_matrix,
np.array(
[
[
pose_item[1],
pose_item[2],
pose_item[3],
]
]
).T,
)
),
np.array([[0, 0, 0, 1]]),
)
)
vcs_2_global = np.dot(
lidar_2_vcs_arr, np.dot(lidar_pose, np.linalg.inv(lidar_2_vcs_arr))
)
w, x, y, z = rotation_to_quat(vcs_2_global[:3, :3])
vcs_2_global_pose.append(
np.array(
(
pose_item[0],
vcs_2_global[0, -1],
vcs_2_global[1, -1],
vcs_2_global[2, -1],
x,
y,
z,
w,
),
dtype=np.float64,
)
)
執行
w, x, y, z = rotation_to_quat(vcs_2_global[:3, :3]) 語句後為nan