Eigen中欧拉角,旋转向量,旋转矩阵,四元数的转换

三维空间的旋转可以用欧拉角,旋转向量,旋转矩阵,四元数来表示。
首先是欧拉角表示法,我们可以用绕某个轴旋转来表示。
旋转向量就是用一个旋转轴和一个旋转角来表示旋转。
旋转矩阵用一个矩阵来表示空间中的旋转变换关系。
四元数用4个变量来表示旋转(增加一个纬度),可以避免万向节锁现象。

具体转换公式可参考《视觉slam十四讲》这本书。

下面的程序为使用Eigen库进行转换。
(1)旋转向量->旋转矩阵
(2)旋转向量->四元数
(3)旋转向量->欧拉角

#include 
#include 
#include 

#include 
#include 
#include 
#include 
using namespace std;
int main()
{
    //1.rotation vector to  rotation matrix
    Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();
    rotationMatrix=rotationVector.toRotationMatrix();
    cout<<"rotationMatrix \n"<//2.rotation vector to quaterniond
    Eigen::Quaterniond q=Eigen::Quaterniond( rotationVector );
    cout<<"rotation quaternion \n"<//3.rotaion vector to eulerAngles
    Eigen::Vector3d eulerAngle=rotationVector.matrix().eulerAngles(0,1,2);
    cout<<"eulerAngle roll pitch yaw\n"<<180*eulerAngle/M_PI<return 0;
}

你可能感兴趣的:(SLAM)