ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

eigen之三维旋转运动表达

2021-06-26 12:02:36  阅读：383  来源： 互联网

标签：eigen 三维 旋转 0.707107 欧拉角 rotation pi 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库中, 其他运动表达转欧拉角都是通过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].

lALPDgQ9qv5fIi3NAbPNAvs_763_435

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

参考

标签：eigen,三维,旋转,0.707107,欧拉角,rotation,pi,Eigen
来源： https://www.cnblogs.com/chrislzy/p/14933873.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有