Ubuntu下使用Eigen旋转几何运算总结教程

如果你未安装Eigen或者完全未接触Eigen请参考下面这三篇好文

• 一文梳理Ubuntu下Eigen矩阵运算库总结教程
• Ubuntu下安装与使用Eigen矩阵运算库教程
• Ubuntu下cmake教程实践从入门到会用

#include<iostream>
using namespace std;
#include <Eigen/Core>
// 几何模块(旋转矩阵,四元数,平移等)
#include<Eigen/Geometry>
int main()
{
    // 旋转矩阵就是3x3的矩阵
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量(方向与旋转轴[0,0,1]相同,模为旋转的角度45°)
    Eigen::AngleAxisd rotation_vector (M_PI/4,Eigen::Vector3d(0,0,1));

    cout.precision(3);//指定输出的精度
    cout<<"旋转向量转为旋转矩阵为：
"<<rotation_vector.matrix()<<endl;

    // 旋转矩阵转欧拉角
    // Z-roll,Y-pitch,X-yaw顺序
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);

    // 旋转矩阵转欧几里得变换矩阵(就是齐次变换矩阵)Eigen::Isometry
    // 三维的齐次变换矩阵是4x4的矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    // 根据旋转向量进行旋转（注意旋转向量包含了旋转轴和旋转了的角度）
    T.rotate(rotation_vector);
    cout<<"旋转后的变换矩阵为：
"<<T.matrix()<<endl;
    T.pretranslate(Eigen::Vector3d(1,3,4)); // 把第四列的平移向量设置为(1,3,4)
    cout<<"设置平移量后的矩阵为：
"<<T.matrix()<<endl;

    // 定义某个坐标
    Eigen::Vector3d p (1,0,0);
    // 用变换矩阵将p这个坐标进行变换,相当于 R*p+ [1,3,4]
    Eigen::Vector3d p_transformed = T*p;
    cout<<p_transformed<<endl;

    // 旋转向量变四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    // q.coeffs()输出的四元数顺序为[x,y,z,w]
    cout<<"旋转向量变四元数：
"<<q.coeffs()<<endl;
    // 旋转矩阵变四元数
    q = Eigen::Quaterniond(rotation_matrix);
    // 四元数可以直接对向量进行旋转，它是利用了运算符重载
    p_transformed = q*p;// 数学上是qpq^{-1}
    return 0;
}

参考文献：https://github.com/gaoxiang12/slambook/blob/master/ch3/useGeometry/eigenGeometry.cpp#L30