eigen之三维旋转运动表达

目录

简介

用于表示三维刚体旋转运动的方法主要有:

  • 旋转向量
  • 旋转矩阵
  • 欧拉角
  • 四元数

旋转向量

#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

注: 四元数到欧拉角有时会错.

笔记

二维旋转公式

eigen之三维旋转运动表达
eigen之三维旋转运动表达

三维旋转公式

eigen之三维旋转运动表达

欧拉角

在Eigen库中, 其他运动表达转欧拉角都是通过rotation.eulerAngles(2, 1, 0)函数, 即先转为旋转矩阵然后再转为欧拉角. 这个函数有个问题就是出来欧拉角的范围不对.

  • 欧拉角的实际范围是: roll-[-pi:pi], pitch-[-pi/2:pi/2], yaw-[-pi:pi]
  • Eigen库中函数的范围是:roll-[0:pi], pitch-[-pi:pi], yaw-[-pi:pi].

eigen之三维旋转运动表达

所以推荐使用ros中的tf函数得到欧拉角,而不是eigen或者pcl中的函数

参考

上一篇:双指旋转操作的核心


下一篇:Unity和C++dll交互