Eigen se3初始化矩阵

Eigen中的SE3表示三维空间中的刚体变换，可以使用Eigen::Isometry3d类型来表示，它本质上是一个4x4的变换矩阵，其中前三行前三列是旋转矩阵，第四列是平移向量。SE3初始化矩阵可以通过以下方式实现：

#include <Eigen/Geometry>

Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 初始化为单位矩阵
T.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); // 绕z轴旋转45度
T.pretranslate(Eigen::Vector3d(1, 2, 3)); // 平移向量为(1,2,3)

// 输出变换矩阵
std::cout << "Transformation matrix = \n" << T.matrix() << std::endl;

上述代码中，我们首先通过`Eigen::Isometry3d::Identity()`来初始化一个单位矩阵，然后使用`rotate()`函数绕z轴旋转45度，最后使用`pretranslate()`函数设置平移向量为(1,2,3)。最终得到的变换矩阵可以使用`matrix()`函数输出。

相关问题

Eigen 创建se3矩阵

要创建一个SE(3)矩阵，需要使用Eigen库的Isometry3d类。这个类可以表示一个欧几里得变换，包括旋转和平移。以下是一个创建SE(3)矩阵的示例代码：

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

int main()
{
    // 创建一个旋转矩阵，表示绕Z轴旋转90度
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    rotation_matrix(0, 0) = cos(M_PI / 2);
    rotation_matrix(0, 1) = -sin(M_PI / 2);
    rotation_matrix(1, 0) = sin(M_PI / 2);
    rotation_matrix(1, 1) = cos(M_PI / 2);

    // 创建一个平移向量，表示在X轴方向平移1米
    Eigen::Vector3d translation_vector(1, 0, 0);

    // 创建一个SE(3)矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(rotation_matrix);
    T.pretranslate(translation_vector);

    std::cout << "SE(3)矩阵:" << std::endl << T.matrix() << std::endl;

    return 0;
}

在上面的示例代码中，我们首先创建一个3x3的旋转矩阵，表示绕Z轴旋转90度。然后创建一个3维的平移向量，表示在X轴方向平移1米。最后，我们通过Isometry3d类创建一个SE(3)矩阵，并将旋转矩阵和平移向量分别赋值给它。最终输出SE(3)矩阵。

用四元数初始化 Eigen 的se3编程举例

在Eigen中，SE(3)（欧氏群）可以用Matrix4d或Isometry3d类型表示。而四元数（Quaternion）可以用Quaterniond类型表示。因此，我们可以使用Quaterniond类型来初始化Isometry3d类型。

以下是一个示例代码：

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

using namespace std;
using namespace Eigen;

int main()
{
    Quaterniond q(0.35, 0.2, 0.3, 0.1); // 以wxyz的顺序初始化四元数
    q.normalize(); // 归一化

    Isometry3d T = Isometry3d::Identity(); // 初始化为单位矩阵
    T.rotate(q); // 通过四元数设置旋转部分
    T.pretranslate(Vector3d(0.3, 0.1, 0.1)); // 设置平移部分

    cout << "Transform matrix = \n" << T.matrix() << endl;

    return 0;
}

在上面的代码中，我们首先构造了一个四元数q，并对它进行了归一化。然后我们创建了一个Isometry3d类型的变量T，并将它初始化为单位矩阵。接着，我们使用T.rotate(q)函数通过四元数设置了旋转部分，使用T.pretranslate函数设置了平移部分。最后，我们输出了变换矩阵T的值。

需要注意的是，如果我们使用Matrix4d类型来表示欧氏变换，则可以使用Matrix4d::Identity()函数来初始化为单位矩阵，并使用Matrix4d::block函数来设置旋转和平移部分。