python中的四元数运算

Posted 2023-02-27 KAI-yq

python中的四元数运算

• 四元数球面线性插值
• 四元数、欧拉角、旋转矩阵的转换
• 四元数运算（坐标系变换）

四元数球面线性插值

import tf

i = 0
j = 1
while i < len(low_freq_data.timestamp) and j < len(high_freq_data.timestamp):
    t1 = low_freq_data.timestamp[i] 
    t2 = high_freq_data.timestamp[j-1] 
    t3 = high_freq_data.timestamp[j] 
    if t1 < t3 :
        if t1 > t2:
            scale_factor = (t1 - t2) / (t3 - t2)
            v_last = [high_freq_data.t_x[j-1], high_freq_data.t_y[j-1], high_freq_data.t_z[j-1]]
            v_curr = [high_freq_data.t_x[j], high_freq_data.t_y[j], high_freq_data.t_z[j]]
            v = np.array(v_last) * (1 - scale_factor) + np.array(v_curr) * scale_factor 
                
            q_last = [high_freq_data.q_x[j-1], high_freq_data.q_y[j-1], high_freq_data.q_z[j-1], high_freq_data.q_w[j-1]]
            q_curr = [high_freq_data.q_x[j], high_freq_data.q_y[j], high_freq_data.q_z[j], high_freq_data.q_w[j]]
            q = tf.transformations.quaternion_slerp(q_last, q_curr, scale_factor)

四元数、欧拉角、旋转矩阵的转换

from scipy.spatial.transform import Rotation as R

// 四元数到欧拉角
Rq = [q_x, q_y, q_z, q_w]
r = R.from_quat(Rq)
euler = r.as_euler('xyz', degrees=True)
// 输出euler[0],euler[1],euler[2]

// 欧拉角到四元数
r = R.from_euler('xyz', [roll, pitch, yaw], degrees=True)
q = r.as_quat()
输出q[0],q[1],q[2],q[3]

参考链接：https://blog.csdn.net/m0_59839672/article/details/125934454

四元数运算（坐标系变换）

诺瓦泰坐标系到激光雷达坐标系的转换关系，同样可用于计算任意两个6自由度坐标系间的转换关系

import numpy as np
import quaternion

# 诺瓦泰到激光雷达的坐标系转换
q_novatel = np.quaternion(novatel.q_w[0],novatel.q_x[0],novatel.q_y[0],novatel.q_z[0])
q_lidar = np.quaternion(lidar.q_w[0],lidar.q_x[0],lidar.q_y[0],lidar.q_z[0])
q_n_to_l = q_lidar * q_novatel.inverse()

t_novatel = np.array([novatel.t_x[0],novatel.t_y[0],novatel.t_z[0]]) 
t_lidar = np.array([lidar.t_x[0], lidar.t_y[0], lidar.t_z[0]])
t_n_t_l = t_lidar - (np.matrix(quaternion.as_rotation_matrix(q_n_to_l)) * t_novatel.reshape(-1,1)).reshape(-1)

参考链接：https://quaternion.readthedocs.io/en/latest/#