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

输入数据：

 

[ 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

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

　　

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