欧拉角姿态变换

姿态B相对于姿态A的变换：

欧拉角为 rx, ry, rz, 绕 Z-Y-X 轴进行旋转。

那么姿态A相对于姿态B的变换：

欧拉角为 -rx, -ry, -rz, 绕 X-Y-Z 轴进行旋转。

  double rx, ry, rz, px, py, pz;
  rx = 10;
  ry = 20;
  rz = 30;
  px = 1;
  py = 2;
  pz = 3;

  std::cout << "rx: " << rx << " ry: " << ry << " rz: " << rz << "\n";
  std::cout << "px: " << px << " py: " << py << " pz: " << pz << std::endl;

  Eigen::AngleAxis roll1(rx / 180 * M_PI, Eigen::Vector3d::UnitX());
  Eigen::AngleAxis pitch1(ry / 180 * M_PI, Eigen::Vector3d::UnitY());
  Eigen::AngleAxis yaw1(rz / 180 * M_PI, Eigen::Vector3d::UnitZ());
  Eigen::Matrix3d matrix1 = (yaw1 * pitch1 * roll1).toRotationMatrix();
  std::cout << "matrix1: Z-Y-X" << "\n";
  std::cout << matrix1 << "\n" << std::endl;
  std::cout << "matrix1.inv: " << "\n";
  std::cout << matrix1.inverse() << "\n" << std::endl;

  rx *= -1;
  ry *= -1;
  rz *= -1;

  std::cout << "rx: " << rx << " ry: " << ry << " rz: " << rz << "\n";

  Eigen::AngleAxis roll2(rx / 180 * M_PI, Eigen::Vector3d::UnitX());
  Eigen::AngleAxis pitch2(ry / 180 * M_PI, Eigen::Vector3d::UnitY());
  Eigen::AngleAxis yaw2(rz / 180 * M_PI, Eigen::Vector3d::UnitZ());
  Eigen::Matrix3d matrix2 = (roll2 * pitch2 * yaw2).toRotationMatrix();
  std::cout << "matrix2: X-Y-Z" << "\n";
  std::cout << matrix2 << "\n" << std::endl;
  std::cout << "matrix2.inv: " << "\n";
  std::cout << matrix2.inverse() << "\n" << std::endl;

输出：

rx: 10 ry: 20 rz: 30
px: 1 py: 2 pz: 3
matrix1: Z-Y-X
 0.813798  -0.44097  0.378522
 0.469846  0.882564 0.0180283
 -0.34202  0.163176  0.925417

matrix1.inv: 
 0.813798  0.469846  -0.34202
 -0.44097  0.882564  0.163176
 0.378522 0.0180283  0.925417

rx: -10 ry: -20 rz: -30
matrix2: X-Y-Z
 0.813798  0.469846  -0.34202
 -0.44097  0.882564  0.163176
 0.378522 0.0180283  0.925417

matrix2.inv: 
 0.813798  -0.44097  0.378522
 0.469846  0.882564 0.0180283
 -0.34202  0.163176  0.925417