旋转矩阵与欧拉角之间的相互转换：全方位指南与 Python 和 C++ 代码实现

2023-11-05 12:34:20

引言

在计算机视觉和机器人技术中，旋转矩阵是一种广泛用于三维空间中物体旋转的数学工具。它是一个 3x3 矩阵，编码了绕三个正交轴（x、y 和 z）的旋转量。另一方面，欧拉角是一个三元组，表示绕三个顺序轴（通常记为 yaw、pitch 和 roll）的旋转量。

旋转矩阵和欧拉角之间存在着相互转换的关系，这对于理解和操纵三维空间中的旋转至关重要。本文将全面介绍这种转换，并提供 Python 和 C++ 代码的实际实现。

从旋转矩阵到欧拉角

从旋转矩阵 R 转换为欧拉角 (yaw, pitch, roll) 的过程如下：

计算 pitch ：pitch 对应于绕 y 轴的旋转，可通过以下公式计算：

pitch = atan2(-R[2][0], sqrt(R[2][1] * R[2][1] + R[2][2] * R[2][2]))

计算 yaw ：yaw 对应于绕 z 轴的旋转，可通过以下公式计算：

yaw = atan2(R[2][1], R[2][2])

计算 roll ：roll 对应于绕 x 轴的旋转，可通过以下公式计算：

roll = atan2(R[1][0], R[0][0])

从欧拉角到旋转矩阵

从欧拉角 (yaw, pitch, roll) 转换为旋转矩阵 R 的过程如下：

创建中间旋转矩阵 ：首先，分别创建绕 x、y 和 z 轴旋转的三个中间旋转矩阵：

Rx = [[1, 0, 0], [0, cos(roll), -sin(roll)], [0, sin(roll), cos(roll)]]
Ry = [[cos(pitch), 0, sin(pitch)], [0, 1, 0], [-sin(pitch), 0, cos(pitch)]]
Rz = [[cos(yaw), -sin(yaw), 0], [sin(yaw), cos(yaw), 0], [0, 0, 1]]

计算最终旋转矩阵 ：最终旋转矩阵 R 是这三个中间矩阵的乘积：

R = Rz @ Ry @ Rx

Python 实现

以下 Python 代码实现了从旋转矩阵到欧拉角以及从欧拉角到旋转矩阵的转换：

import numpy as np

def rot_to_euler(R):
  """
  从旋转矩阵转换为欧拉角

  参数：
    R：3x3 旋转矩阵

  返回：
    yaw, pitch, roll
  """

  pitch = np.arctan2(-R[2][0], np.sqrt(R[2][1] ** 2 + R[2][2] **  2))
  yaw = np.arctan2(R[2][1], R[2][2])
  roll = np.arctan2(R[1][0], R[0][0])

  return yaw, pitch, roll


def euler_to_rot(yaw, pitch, roll):
  """
  从欧拉角转换为旋转矩阵

  参数：
    yaw：绕 z 轴的旋转角度
    pitch：绕 y 轴的旋转角度
    roll：绕 x 轴的旋转角度

  返回：
    3x3 旋转矩阵
  """

  Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
  Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
  Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])

  R = Rz @ Ry @ Rx

  return R

C++ 实现

以下 C++ 代码实现了从旋转矩阵到欧拉角以及从欧拉角到旋转矩阵的转换：

#include <iostream>
#include <cmath>

using namespace std;

struct EulerAngles {
  double yaw;
  double pitch;
  double roll;
};

EulerAngles rot_to_euler(const float R[3][3]) {
  EulerAngles angles;

  angles.pitch = atan2(-R[2][0], sqrt(R[2][1] * R[2][1] + R[2][2] * R[2][2]));
  angles.yaw = atan2(R[2][1], R[2][2]);
  angles.roll = atan2(R[1][0], R[0][0]);

  return angles;
}

void euler_to_rot(const EulerAngles &angles, float R[3][3]) {
  float Rx[3][3] = {
    {1, 0, 0},
    {0, cos(angles.roll), -sin(angles.roll)},
    {0, sin(angles.roll), cos(angles.roll)}
  };

  float Ry[3][3] = {
    {cos(angles.pitch), 0, sin(angles.pitch)},
    {0, 1, 0},
    {-sin(angles.pitch), 0, cos(angles.pitch)}
  };

  float Rz[3][3] = {
    {cos(angles.yaw), -sin(angles.yaw), 0},
    {sin(angles.yaw), cos(angles.yaw), 0},
    {0, 0, 1}
  };

  // 旋转矩阵是这三个矩阵的乘积
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R[i][j] = 0;
      for (int k = 0; k < 3; k++) {
        R[i][j] += Rz[i][k] * Ry[k][j] * Rx[j][k];
      }
    }
  }
}