Eigen 的se3编程举例

Eigen 是一种 C++ 的线性代数库，它支持各种矩阵操作和几何变换。其中，se3 表示三维空间内的刚体变换，包括旋转和平移。下面是一个使用 Eigen 实现 se3 变换的示例代码：

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

int main()
{
    // 定义一个旋转矩阵和一个平移向量
    Eigen::Matrix3d R;
    R << 0.0, -1.0, 0.0,
         1.0, 0.0, 0.0,
         0.0, 0.0, 1.0;
    Eigen::Vector3d t(1.0, 2.0, 3.0);

    // 定义一个 se3 变换
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.linear() = R;
    T.translation() = t;

    // 打印 se3 变换的矩阵形式
    std::cout << "SE3 matrix:\n" << T.matrix() << std::endl;

    // 对一个点进行变换
    Eigen::Vector3d p(4.0, 5.0, 6.0);
    Eigen::Vector3d q = T * p;

    // 打印变换前后的点坐标
    std::cout << "Point before transformation:\n" << p << std::endl;
    std::cout << "Point after transformation:\n" << q << std::endl;

    return 0;
}

这段代码定义了一个旋转矩阵和一个平移向量，然后使用 Eigen::Isometry3d 类定义了一个 se3 变换 T，并将 R 和 t 分别赋值给它的旋转和平移部分。最后，对一个点进行变换并输出变换前后的点坐标。