簡介

用於表示三維剛體旋轉運動的方法主要有：

旋轉向量
旋轉矩陣
歐拉角
四元數

旋轉向量

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char** argv)
{
  /**** 1. 旋轉向量 ****/ 歐拉角的實際范圍是:  roll-[-pi:pi], pitch-[-pi/2:pi/2], yaw-[-pi:pi], 但是Eigen庫中函數的范圍是[0:pi], pitch-[-pi:pi], yaw-[-pi:pi]. 


  std::cout << "\n ********** AngleAxis **********" << std::endl;
  Eigen::AngleAxisd rotation_vector1(M_PI/4, Eigen::Vector3d(0, 0, 1));
  std::cout << "rotation vector1: " << "angle is: " << rotation_vector1.angle() * (180 / M_PI) 
                                    << "  axis is: " << rotation_vector1.axis().transpose() << std::endl;
  
  // 旋轉向量 -> 旋轉矩陣
  Eigen::Matrix3d rotation_matrix1 = rotation_vector1.matrix();  //用matrix()轉換成旋轉矩陣
  std::cout << "rotation matrix1 : \n" << rotation_matrix1 << std::endl;
  Eigen::Matrix3d rotation_matrix1_1 = rotation_vector1.toRotationMatrix();  // 由羅德里格公式進行轉換
  std::cout << "rotation matrix1_1 : \n" << rotation_matrix1_1 << std::endl;

  // 旋轉向量 -> 歐拉角
  Eigen::Vector3d euler_angle1 = rotation_vector1.matrix().eulerAngles(2, 1, 0);  // 先轉為旋轉矩陣, 然后轉為歐拉角
  std::cout << "euler angle: " << euler_angle1.transpose() << std::endl;

  // 旋轉向量 -> 四元數
  Eigen::Quaterniond quaternion1(rotation_vector1);  // 直接初始化
  Eigen::Quaterniond quaternion1_1;  
  quaternion1_1 = rotation_vector1;  // 通過賦值
  std::cout << "quaternion1: " << quaternion1.coeffs().transpose() << std::endl;  // x y z w
  std::cout << "quaternion1_1: " << quaternion1_1.coeffs().transpose() << std::endl;
  return 0;
}

結果:


 ********** AngleAxis **********
rotation vector1: angle is: 45  axis is: 0 0 1
rotation matrix1 : 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
rotation matrix1_1 : 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
euler angle: 0.785398       -0        0
quaternion1:        0        0 0.382683  0.92388
quaternion1_1:        0        0 0.382683  0.92388

旋轉矩陣

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char** argv)
{
  /**** 2. 旋轉矩陣 *****/
  std::cout << "\n ********** RotationMatrix **********" << std::endl;
  Eigen::Matrix3d rotation_matrix2;
  rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;
  std::cout << "rotation matrix2 : \n" << rotation_matrix2 << std::endl;

  // 旋轉矩陣 -> 旋轉向量
  Eigen::AngleAxisd rotation_vector2;
  rotation_vector2.fromRotationMatrix(rotation_matrix2);  // fromRotationMatrix
  Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2);  // 直接初始化
  std::cout << "rotation vector2: " << "angle is: " << rotation_vector2.angle() * (180 / M_PI) 
                                    << "  axis is: " << rotation_vector2.axis().transpose() << std::endl;
  std::cout << "rotation vector2_1: " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI) 
                                      << "  axis is: " << rotation_vector2_1.axis().transpose() << std::endl;                                    
  
  // 旋轉矩陣 -> 歐拉角
  Eigen::Vector3d euler_angle2 = rotation_matrix2.eulerAngles(2, 1, 0); // ZYX順序, 即先繞x軸roll,再繞y軸pitch,最后繞z軸yaw
  std::cout << "euler angle2: " << euler_angle2.transpose() << std::endl;  // // row pitch yaw
  
  // 旋轉矩陣 -> 四元數
  Eigen::Quaterniond quaternion2(rotation_matrix2);
  Eigen::Quaterniond quaternion2_1;
  quaternion2_1 = rotation_matrix2;
  std::cout << "quaternion2: " << quaternion2.coeffs().transpose() << std::endl;  // x y z w
  std::cout << "quaternion2_1: " << quaternion2_1.coeffs().transpose() << std::endl;
  return 0;
}

結果:

********** RotationMatrix **********
rotation matrix2 : 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
rotation vector2: angle is: 45  axis is: 0 0 1
rotation vector2_1: angle is: 45  axis is: 0 0 1
euler angle2: 0.785398       -0        0
quaternion2:        0        0 0.382684  0.92388
quaternion2_1:        0        0 0.382684  0.92388

注: 單位旋轉矩陣的旋轉向量為(0, 0, 0)

歐拉角

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char** argv)
{
  /**** 3. 歐拉角 ****/
  std::cout << "\n ********** EulerAngle **********" << std::endl;
  Eigen::Vector3d euler_angle3(0.785398, -0, 0);  // row pitch yaw
  std::cout << "euler angle3: " << euler_angle3.transpose() << std::endl;

  // 歐拉角 -> 旋轉矩陣
  Eigen::Matrix3d rotation_matrix3;
  rotation_matrix3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
                     Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX());
  std::cout << "rotation matrix3 : \n" << rotation_matrix3 << std::endl;                     

  // 歐拉角 -> 旋轉向量
  Eigen::AngleAxisd rotation_vector3;
  rotation_vector3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
                     Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX());
  std::cout << "rotation vector3: " << "angle is: " << rotation_vector3.angle() * (180 / M_PI) 
                                    << "  axis is: " << rotation_vector3.axis().transpose() << std::endl;
  
  // 歐拉角 -> 四元數
  Eigen::Quaterniond quaternion3;
  quaternion3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX());
  std::cout << "quaternion2: " << quaternion3.coeffs().transpose() << std::endl;  // x y z w
  return 0;
}

結果:

********** EulerAngle **********
euler angle3: 0.785398        0        0
rotation matrix3 : 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
rotation vector3: angle is: 45  axis is: 0 0 1
quaternion2:        0        0 0.382683  0.92388

注: 歐拉角轉為旋轉向量的順序

四元數

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char** argv)
{
  /**** 4.四元數 ****/
  std::cout << "\n ********** Quaternion **********" << std::endl;
  Eigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683); // w, x, y, z

  // 四元數 -> 旋轉矩陣
  Eigen::Matrix3d rotation_matrix4;
  rotation_matrix4 = quaternion4.matrix();
  Eigen::Matrix3d rotation_matrix4_1;
  rotation_matrix4_1 = quaternion4.toRotationMatrix();
  std::cout << "rotation matrix4 : \n" << rotation_matrix4 << std::endl;                     
  std::cout << "rotation matrix4_1 : \n" << rotation_matrix4_1 << std::endl;                       

  // 四元數 -> 旋轉向量
  Eigen::AngleAxisd rotation_vector4(quaternion4);
  Eigen::AngleAxisd rotation_vector4_1;
  rotation_vector4_1 = quaternion4;
  std::cout << "rotation vector4: " << "angle is: " << rotation_vector4.angle() * (180 / M_PI) 
                                    << "  axis is: " << rotation_vector4.axis().transpose() << std::endl;
  std::cout << "rotation vector4_1: " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI) 
                                      << "  axis is: " << rotation_vector4_1.axis().transpose() << std::endl;                                    
  
  // 四元數 -> 歐拉角
  Eigen::Vector3d euler_angle4 = quaternion4.matrix().eulerAngles(2, 1, 0);
  std::cout << "euler angle4: " << euler_angle4.transpose() << std::endl;
  return 0;
}

結果

********** Quaternion **********
rotation matrix4 : 
 0.707107 -0.707106         0
 0.707106  0.707107         0
        0         0         1
rotation matrix4_1 : 
 0.707107 -0.707106         0
 0.707106  0.707107         0
        0         0         1
rotation vector4: angle is: 44.9999  axis is: 0 0 1
rotation vector4_1: angle is: 44.9999  axis is: 0 0 1
euler angle4: 0.785397       -0        0

注: 四元數到歐拉角有時會錯.