用四元数初始化 Eigen 的se3编程举例
时间: 2023-11-22 10:56:52 浏览: 33
在Eigen中,SE(3)(欧氏群)可以用Matrix4d或Isometry3d类型表示。而四元数(Quaternion)可以用Quaterniond类型表示。因此,我们可以使用Quaterniond类型来初始化Isometry3d类型。
以下是一个示例代码:
```cpp
#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函数来设置旋转和平移部分。