由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]]) |
【推荐】国内首个AI IDE,深度理解中文开发场景,立即下载体验Trae
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步