由rotation转为w,x,y,z为nan，为什么呢

输入数据：

 

?

1 2 3

[ 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]

 

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17

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

　　

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54

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,             )         )

　　执行

?

1	w, x, y, z = rotation_to_quat(vcs_2_global[:3, :3])  语句后为nan<br><br><br>

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26

ipdb> rotation_matrix array([[-9.99216073e-01,  2.38020018e-04,  3.96060933e-02],        [-1.81661073e-03, -9.99205706e-01, -3.98260715e-02],        [ 3.95651262e-02, -3.98667705e-02,  9.98421375e-01]])   ipdb> lidar_pose array([[-9.99259437e-01,  1.88167320e-03,  3.84436218e-02,          1.25791009e+02],        [-3.46029554e-03, -9.99152026e-01, -4.10381479e-02,          7.06749932e+00],        [ 3.83337854e-02, -4.11407648e-02,  9.98417728e-01,         -2.21275515e+00],        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,          1.00000000e+00]])       ipdb> vcs_2_global array([[-9.99167127e-01,  2.16146910e-03,  4.07584891e-02,          1.27478973e+02],        [-3.18108674e-03, -9.99683631e-01, -2.49678490e-02,          5.73646002e+00],        [ 4.06916093e-02, -2.50766994e-02,  9.98857023e-01,         -2.46715557e+00],        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,          1.00000000e+00]])

　　

?

1 2 3 4 5 6 7 8 9

ipdb> lidar_2_vcs_arr array([[ 9.99940631e-01,  1.08074890e-02,  1.39021360e-03,          8.49000000e-01],        [-1.08185967e-02,  9.99907466e-01,  8.24725845e-03,          0.00000000e+00],        [-1.30095280e-03, -8.26180898e-03,  9.99965024e-01,          1.84418357e+00],        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,          1.00000000e+00]])