导入Eigen几何模块，可以使用四元数、欧拉角和旋转矩阵。

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
 * 本程序演示了 Eigen 几何模块的使用方法
 ****************************/

int main(int argc, char **argv)
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity(); // 定义一个单位阵
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1)); //沿 Z 轴旋转 45 度

    cout.precision(3); // 保留三位小数
    cout << "rotation matrix =\n"
         << rotation_vector.matrix() << endl; //用matrix()转换成矩阵

    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // cout << rotation_matrix << endl;

    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "向量v在变换后的坐标系下表示：" << v_rotated.transpose() << endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout << "向量v在变换后的坐标系下表示：" << v_rotated.transpose() << endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
    cout << "旋转矩阵转换成欧拉角：" << euler_angles.transpose() << endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵(齐次坐标)
    cout << T.matrix() << endl;
    T.rotate(rotation_vector); // 按照rotation_vector进行旋转
    cout << "T绕rotation_vector旋转后：" << endl
         << T.matrix() << endl;

    T.pretranslate(Eigen::Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4)
    cout << "T按照(1,3,4)平移后：" << endl
         << T.matrix() << endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T * v; // 相当于R*v+t
    cout << "向量v在经过T变换后的坐标系下表示：" << v_transformed.transpose() << endl;

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout << "quaternion = \n"
         << q.coeffs() << endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond(rotation_matrix);
    cout << "四元数：" << q.coeffs() << endl;

    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q * v;
    cout << "向量v在经过四元数变换后的坐标系下表示：" << v_transformed.transpose() << endl;

    return 0;
}

CMakeLsits.txt

cmake_minimum_required( VERSION 2.8 )
project( geometry )

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

add_executable( eigenGeometry eigenGeometry.cpp )