C++ 使用 Eigen 创建具有俯仰、偏航、滚转的旋转矩阵

Creating a rotation matrix with pitch, yaw, roll using Eigen

提问by Caesar

How do I create a rotation matrix using pitch, yaw, roll with Eigen library?

如何使用 Eigen 库使用俯仰、偏航、滚动创建旋转矩阵？

回答by Caesar

Seeing as how I couldn't find a prebuilt function that does this, I built one and here it is in case someone finds this question in the future

看到我如何找不到执行此操作的预构建函数，我构建了一个，以防将来有人发现此问题

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

回答by narcispr

Caesar answer is ok but as David Hammen says it depends on your application. For me (underwater or aerial vehicles field) the winning combination is:

Caesar 的回答没问题，但正如 David Hammen 所说，这取决于您的申请。对我来说（水下或飞行器领域）获胜组合是：

Eigen::Quaterniond
euler2Quaternion( const double roll,
                  const double pitch,
                  const double yaw )
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q;
}

回答by David Hammen

How do I create a rotation matrix using pitch, yaw, roll with Eigen library?

如何使用 Eigen 库使用俯仰、偏航、滚动创建旋转矩阵？

There are 48 ways to do this. Which one do you want? Here are the factors:

有 48 种方法可以做到这一点。你想要哪一个？以下是因素：

• Extrinsic verus intrinsic.
  Are the rotations about the axes of the fixed system (extrinsic) or are they about the rotated axes (intrinsic)?
  
  
• Rotation versus transformation.
  Do you want to represent the matrix that physically rotates some object or do you want to represent the matrix that transforms vectors from one reference frame to another?
  
  
• Astronomical sequences.
  There are six fundamental astronomical sequences. The canonical Euler sequence involves a rotation about the z axis followed by a rotation about the (rotated) x axis followed by a third rotation about (rotated again) z axis. There are five more of these astronomical-style sequences (x-y-x, x-z-x, y-x-y, y-z-y,and z-y-z) in addition to this canonical z-x-zsequence.
  
  
• Aerospace sequences.
  To add to the confusion, there are six fundamental aerospace sequences as well. For example, a pitch-yaw-roll sequence versus a roll-pitch-yaw sequence. While the astronomy community has pretty much settled on a z-x-zsequence, the same cannot be said of the aerospace community. Somewhere along the way you find people using every one of the six possible sequences. The six sequences in this group are x-y-z, x-z-y, y-z-x, y-x-z, z-x-y, and z-y-x.

• 外在对内在。
  是围绕固定系统的轴（外在）旋转还是围绕旋转轴（内在）旋转？
  
  
• 旋转与变换。
  您是要表示物理旋转某个对象的矩阵，还是要表示将向量从一个参考系转换为另一个参考系的矩阵？
  
  
• 天文序列。
  有六个基本的天文序列。规范的欧拉序列包括绕 z 轴旋转，然后绕（旋转）x 轴旋转，然后绕（再次旋转）z 轴进行第三次旋转。除了这个规范的zxz序列之外，还有五个这些天文风格的序列（xyx、xzx、yxy、yzy和zyz）。
  
  
• 航空航天序列。
  更令人困惑的是，还有六个基本的航空航天序列。例如，俯仰-偏航-滚转序列与滚转-俯仰-偏航序列。虽然天文学界几乎已经确定了zxz序列，但航空界却并非如此。一路上，你会发现人们使用六种可能的序列中的每一种。此组中的六个序列是XYZ，XZY，YZX，YXZ，ZXY，和ZYX。

回答by ApproachingDarknessFish

All you need to create a rotational matrix is the pitch, yaw, roll, and the ability to perform matrix multiplication.

创建旋转矩阵所需的只是俯仰、偏航、滚动和执行矩阵乘法的能力。

First, create three rotational matrices, one for each axis of rotation (ie one for pitch, one for yaw, one for roll). These matrices will have the values:

首先，创建三个旋转矩阵，一个用于每个旋转轴（即一个用于俯仰，一个用于偏航，一个用于滚动）。这些矩阵将具有以下值：

Pitch Matrix:

音高矩阵：

1, 0, 0, 0,
0, cos(pitch), sin(pitch), 0,
0, -sin(pitch), cos(pitch), 0,
0, 0, 0, 1

Yaw Matrix:

偏航矩阵：

cos(yaw), 0, -sin(yaw),  0,
0, 1, 0, 0,
sin(yaw), 0, cos(yaw), 0,
0, 0, 0, 1

Roll Matrix:

滚动矩阵：

cos(roll), sin(roll), 0, 0,
-sin(roll), cos(roll), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1

Next, multiply all of these together. The order here is important. For normal rotations, you will want to multiply the Roll Matrix by the Yaw Matrix first and then multiply the product by the Pitch Matrix. However, if you're trying to "undo" a rotation by going backwards, you'll want to perform the multiplications in reverse order (in addition to the angles having opposite values).

接下来，将所有这些相乘。这里的顺序很重要。对于正常旋转，您需要先将横滚矩阵乘以偏航矩阵，然后将乘积乘以俯仰矩阵。但是，如果您试图通过向后“撤消”旋转，您将需要以相反的顺序执行乘法（除了具有相反值的角度）。

回答by Ran Cheng

I translate their Java implementation to C++ from this site: Euler Angle Visualization Tool

我从这个站点将他们的 Java 实现翻译成 C++：Euler Angle Visualization Tool

#include <iostream>
#include <math.h>
#include <Eigen/Dense>

Eigen::Matrix3d rotation_from_euler(double roll, double pitch, double yaw){
    // roll and pitch and yaw in radians
    double su = sin(roll);
    double cu = cos(roll);
    double sv = sin(pitch);
    double cv = cos(pitch);
    double sw = sin(yaw);
    double cw = cos(yaw);
    Eigen::Matrix3d Rot_matrix(3, 3);
    Rot_matrix(0, 0) = cv*cw;
    Rot_matrix(0, 1) = su*sv*cw - cu*sw;
    Rot_matrix(0, 2) = su*sw + cu*sv*cw;
    Rot_matrix(1, 0) = cv*sw;
    Rot_matrix(1, 1) = cu*cw + su*sv*sw;
    Rot_matrix(1, 2) = cu*sv*sw - su*cw;
    Rot_matrix(2, 0) = -sv;
    Rot_matrix(2, 1) = su*cv;
    Rot_matrix(2, 2) = cu*cv;
    return Rot_matrix;
}



int main() {
    Eigen::Matrix3d rot_mat = rotation_from_euler(0, 0, 0.5*M_PI);
    std::cout << rot_mat << std::endl;
    return 0;
}