推荐的旋转组合方式答案

【问题标题】：Recommended way to compose rotations推荐的旋转组合方式
【发布时间】：2013-12-14 12:43:22
【问题描述】：

我有 2 个旋转表示为偏航、俯仰、滚动（Tait-Brian 固有右手）。构造与它们两者等效的单个旋转的推荐方法是什么？编辑：如果我从答案中理解正确，我必须首先将偏航、俯仰、滚动转换为矩阵或四元数，组合它们，然后将结果转换回偏航、俯仰、滚动表示。此外，我的首要任务是简单，然后是数值稳定性和效率。谢谢:)

【问题讨论】：

您想使用四元数还是传统的矩阵旋转？ Quarternions 被认为更快更健壮。
你考虑过使用Eigen这样的库吗？
@Potatoswatter 在这一点上我宁愿避免委托给第 3 方，因为即便如此，如何表示轮换也是由我来决定的，对吧？
@dkrikun 决定从实际填充矩阵/四元数/欧拉角向量和理解数学，到任意选择适合您的口味。
@dkrikun 是的；只需点击我第一条评论中的链接即可。

标签： c++ geometry

【解决方案1】：

作为一般答案，如果您为两个旋转中的每一个创建一个旋转矩阵，则可以创建一个作为两者乘积的矩阵（顺序很重要！）来表示应用两个旋转的效果。

可以设想“万向节锁定”可能会使某些角度的数值不稳定（通常涉及非常接近 90 度的角度）。

使用四元数更快更稳定。你可以在http://www.genesis3d.com/~kdtop/Quaternions-UsingToRepresentRotation.htm 看到一个很好的处理——总而言之，每个旋转都可以用一个四元数表示，而多次旋转只用四元数的乘积表示。它们往往具有更好的稳定性。

可以在http://en.m.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles找到执行此操作的公式

更新使用http://planning.cs.uiuc.edu/node102.html 提供的公式，您可以修改以下代码来执行一系列旋转。虽然代码是用 C++ 编写（并编译为）C++，但我没有利用某些可能使代码更优雅的内置 C++ 类型和方法 - 在这里展示我的 C 根源。重点是展示旋转方程的工作原理，以及如何连接多个旋转。

两个关键函数是calcRot，它计算给定偏航、俯仰和滚动的旋转矩阵；和mMult 将两个矩阵相乘。当你有两个连续的旋转时，它们的旋转矩阵的乘积就是“复合”旋转——你必须注意你做事的顺序。我使用的示例显示了这一点。首先，我将向量旋转两个单独的旋转；然后我计算一个结合两个旋转的矩阵并得到相同的结果；最后我颠倒了旋转的顺序，得到了不同的结果。所有这些都应该可以帮助您解决问题。

确保我使用的约定对您有意义。

#include <iostream>
#include <cmath>

#define PI (2.0*acos(0.0))
//#define DEBUG

void calcRot(double ypr[3], double M[3][3]) {
// extrinsic rotations: using the world frame of reference
// ypr: yaw, pitch, roll in radians
  double cy, sy, cp, sp, cr, sr;
// compute sin and cos of each just once:
  cy = cos(ypr[0]);
  sy = sin(ypr[0]);
  cp = cos(ypr[1]);
  sp = sin(ypr[1]);
  cr = cos(ypr[2]);
  sr = sin(ypr[2]);

// compute this rotation matrix:
// source: http://planning.cs.uiuc.edu/node102.html
  M[0][0] =  cy*cp;
  M[0][1] =  cy*sp*sr - sy*cr;
  M[0][2] =  cy*sp*cr + sy*sr;
  M[1][0] =  sy*cp;
  M[1][1] =  sy*sp*sr + cy*cr;
  M[1][2] =  sy*sp*sr - cy*sr;
  M[2][0] = -sp;
  M[2][1] =  cp*sr;
  M[2][2] =  cp*cr;
}

void mMult(double M[3][3], double R[3][3]) {
// multiply M * R, returning result in M
  double T[3][3] = {0};
  for(int ii = 0; ii < 3; ii++) {
    for(int jj = 0; jj < 3; jj++) {
      for(int kk = 0; kk < 3; kk++ ) {
        T[ii][jj] += M[ii][kk] * R[kk][jj];
      }
    }
  }
  // copy the result:
  for(int ii = 0; ii < 3; ii++) {
    for(int jj = 0; jj < 3; jj++ ) {
      M[ii][jj] = T[ii][jj];
    }
  }
}

void printRotMat(double M[3][3]) {
// print 3x3 matrix - for debug purposes
#ifdef DEBUG
  std::cout << "rotation matrix is: " << std::endl;
  for(int ii = 0; ii < 3; ii++) {
    for(int jj = 0; jj < 3; jj++ ) {
      std::cout << M[ii][jj] << " ";
    }
    std::cout << std::endl;
  }
  std::cout << std::endl;
#endif
}

void applyRot(double before[3], double after[3], double M[3][3]) {
// apply rotation matrix M to vector 'before'
// returning result in vector 'after'
  double sumBefore = 0, sumAfter = 0;
  std::cout << "Result of rotation:" << std::endl;
  for(int ii = 0; ii < 3; ii++) {
    std::cout << before[ii] << " -> ";
    sumBefore += before[ii] * before[ii];
    after[ii] = 0;
    for( int jj = 0; jj < 3; jj++) {
      after[ii] += M[ii][jj]*before[jj];
    }
    sumAfter += after[ii] * after[ii];
    std::cout << after[ii] << std::endl;
  }
  std::cout << std::endl;
  #ifdef DEBUG
    std::cout << "length before: " << sqrt(sumBefore) << "; after: " << sqrt(sumAfter) << std::endl;
  #endif
}

int main(void) {
  double r1[3] = {0, 0, PI/2}; // order: yaw, pitch, roll
  double r2[3] = {0, PI/2, 0};

  double initPoint[3] = {3,4,5}; // initial point before rotation
  double rotPoint[3], rotPoint2[3];

  // initialize rotation matrix to I
  double R[3][3];
  double R2[3][3];

  // compute first rotation matrix in-place:
  calcRot(r1, R);
  printRotMat(R);
  applyRot(initPoint, rotPoint, R);
  // apply second rotation on top of first:
  calcRot(r2, R2);
  std::cout << std::endl << "second rotation matrix: " << std::endl;
  printRotMat(R2);
  // applying second matrix to result of first rotation:
  std::cout << std::endl << "applying just the second matrix to result of first: " << std::endl;
  applyRot(rotPoint, rotPoint2, R2);
  mMult(R2, R);
  std::cout << "after multiplication: " << std::endl;
  printRotMat(R2);
  std::cout << "Applying the combined matrix to the intial vector: " << std::endl;
  applyRot(initPoint, rotPoint2, R2);

  // now in the opposite order:
  double S[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
  calcRot(r2, S);
  printRotMat(S);
  calcRot(r1, R2);
  mMult(R2, S);
  std::cout << "applying rotation in the opposite order: " << std::endl;
  printRotMat(R2);
  applyRot(initPoint, rotPoint, R2);

}

输出（#DEBUG 未定义 - 已注释掉）：

Result of rotation:
3 -> 3
4 -> -5
5 -> 4


second rotation matrix: 

applying just the second matrix to result of first: 
Result of rotation:
3 -> 4
-5 -> -5
4 -> -3

after multiplication: 
Applying the combined matrix to the intial vector: 
Result of rotation:
3 -> 4
4 -> -5
5 -> -3

请注意，最后两个给出相同的结果，表明您可以组合旋转矩阵。

applying rotation in the opposite order: 
Result of rotation:
3 -> 5
4 -> 3
5 -> 4

现在结果不同了——顺序很重要。

【讨论】：

【解决方案2】：

如果你熟悉矩阵运算，可以试试Rodrigues' rotation formula。如果您熟悉四元数，可以尝试 P' = q*P*q' 方法。

四分数数学掌握起来有点复杂，但代码更简单、更快。

【讨论】：