Eigen库的四元数使用

Eigen库是一个C++的线性代数库，它提供了对四元数的支持。在Eigen中，四元数是以Quaternion类的形式表示的。下面是使用Eigen库进行四元数操作的示例代码：

#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

int main()
{
    // 创建一个四元数对象
    Quaterniond q(1, 0, 0, 0); // w, x, y, z

    // 输出四元数对象的实部和虚部
    std::cout << "Real part: " << q.w() << std::endl;
    std::cout << "Imaginary part: " << q.vec().transpose() << std::endl;

    // 旋转一个向量
    Vector3d v(1, 0, 0);
    Vector3d v_rotated = q * v * q.inverse();
    std::cout << "Rotated vector: " << v_rotated.transpose() << std::endl;

    // 将旋转轴和旋转角度转换为四元数
    Vector3d axis(0, 0, 1);
    double angle = M_PI / 2;
    Quaterniond q_axis_angle(cos(angle / 2), sin(angle / 2) * axis.x(), sin(angle / 2) * axis.y(), sin(angle / 2) * axis.z());

    // 两个四元数的乘积等于它们对应的旋转的复合
    Quaterniond q_combined = q * q_axis_angle;
    std::cout << "Combined quaternion: " << q_combined.w() << " " << q_combined.vec().transpose() << std::endl;

    // 将四元数转换为旋转矩阵
    Matrix3d R = q.toRotationMatrix();
    std::cout << "Rotation matrix: " << std::endl << R << std::endl;

    return 0;
}

这个示例代码中，我们创建了一个四元数对象q，并输出了它的实部和虚部。然后我们将一个向量v绕q旋转，并输出旋转后的向量。接着我们将一个旋转轴和一个旋转角度转换为一个四元数对象q_axis_angle，并将两个四元数的乘积q_combined输出。最后，我们将四元数对象q转换为一个旋转矩阵R，并输出这个矩阵。