Eigen的几何模块

转自：https://blog.csdn.net/jjjwwwjjjwww/article/details/68936090

#include 
#include 
using namespace std;

#include 
#include 

int main( )
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity( );
    Eigen::AngleAxisd rotation_vector( M_PI / 2, Eigen::Vector3d( 0, 0, 1 ) );
    cout << "rotation_matrix = \n" << rotation_vector.matrix( ) << endl;  // 转换成矩阵
    rotation_matrix = rotation_vector.toRotationMatrix( );  // 直接赋值

    // 用AngleAxis进行坐标变换
    Eigen::Vector3d v( 1, 0, 0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "( 1, 0, 0 ) after rotation = \n" << v_rotated << endl;

    // 用旋转矩阵进行坐标变换
    v_rotated = rotation_matrix * v;
    cout << "( 1, 0, 0 ) after rotation = \n" << v_rotated << endl;

    // 将旋转矩阵转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles( 0, 1, 2 );  // x-y-z 顺序，即rpy
    cout << "roll pitch yaw = \n" << euler_angles << endl;

    // 用Eigen::Isometry表示欧氏变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity( );  // 三维变换矩阵
    T.rotate( rotation_vector );  // 旋转部分赋值
    T.pretranslate( Eigen::Vector3d( 1, 0, 0 ) );  // 设置平移向量
    cout << "Transform matrix = \n" << T.matrix( ) << endl;

    // 用欧氏变换矩阵进行坐标变换，此处的乘法运算符是重载意义上的
    Eigen::Vector3d v_transformed = T * v;  // 相当于R*v + t
    cout << "v transformed = \n" << v_transformed << endl;

    // 相应地，仿射变换和摄影变换，使用Eigen::Affine3d和Eigen::Projective3d

    // 直接将旋转向量赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond( rotation_vector );
    cout << "quaternion = \n" << q.coeffs( ) << endl;  // Eigen的存储顺序为(x,y,z,w)，实部为w
    // 直接将旋转矩阵赋值给四元数
    q = Eigen::Quaterniond( rotation_matrix );  
    cout << "quaternion = \n" << q.coeffs( ) << endl;

    // 用四元数旋转一个向量，使用重载过的乘法运算符
    v_rotated = q * v;  // 相当于qvq^{-1}
    cout << "( 1, 0, 0 ) after rotation = \n" << v_rotated << endl;

    return 0;
}