用Matlab实现旋转矩阵

在Matlab中，你可以通过以下方式实现从四元数到旋转矩阵的转换：

% 假设你有一个四元数q = [q0, q1, q2, q3]
function rot_matrix = quat_to_rot(q)
    % 提取四元数分量
    q0 = q(1);
    q1 = q(2);
    q2 = q(3);
    q3 = q(4);

    % 根据公式计算旋转矩阵
    rot_matrix = [1 -2*q1^2 -2*q2^2,     2*q0*q1 + 2*q2*q3,       2*q0*q2 - 2*q1*q3;
                   2*q0*q1 - 2*q2*q3,     1 -2*q0^2 -2*q3^2,       2*q1*q2 + 2*q0*q3;
                   2*q0*q2 + 2*q1*q3,    -2*q1*q2 + 2*q0*q3,      1 -2*q0^2 -2*q2^2];
end

% 使用示例
quat_example = [0.7071, 0.0, 0.0, 0.7071]; % 单位四元数，代表绕Z轴顺时针90度
rot_matrix_example = quat_to_rot(quat_example);

对于欧拉角到旋转矩阵的转换，Matlab提供了`eulero2d`, `eulero3d`函数，如在《视觉SLAM十四讲》中所述。例如，单轴欧拉角到旋转矩阵：

% 假设你有三个欧拉角（通常表示俯仰、偏航、翻滚）
euler_angles = [pi/4, pi/2, 0]; % 以弧度表示
rot_matrix_euler = eulero3d(euler_angles); % 返回3x3旋转矩阵