SLAM旋转Eigen的Geomety模块（旋转矩阵、旋转向量、四元数、欧式矩阵、欧拉角）

大背裤爱学习丶

于 2024-09-30 21:13:09 发布

阅读量789

点赞数 8

文章标签：矩阵算法机器学习

本文链接：https://blog.csdn.net/qq_52852434/article/details/142621879

版权

引用：

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

一、旋转向量

1、定义理解

任何旋转都是绕着一个轴旋转一定的角度，我们不妨在空间坐标系中找一个那条轴的方向向量（与轴平行，表示轴的方向，高中知识点），然后用调整这个向量模的大小（等比缩放各个坐标），这样我们只用三个数就表示了一次旋转的基本信息，那么这个向量就称为方向向量。

2、结构

$V=[x_{1},x_{2},x_{3}]$ ，其中 $\left | V \right |=x_{1}^{2}+x_{2}^{2}+x_{3}^{2}=\theta$

3、实例代码

现在我们有一个向量v=[1,0,0]与x轴平行，模长为1，将他按照z轴旋转45° ，那么我们就知道旋转轴是z轴，它的一个方向向量是[0,0,1],旋转角度是弧度制π/4，那么旋转向量V=[0,0,π/4],这样就满足了结构，最后经过一些运算，得到旋转后的坐标结果。

①先创建我们的待转向量v_orign并赋值

//创建原始向量 v_orign 坐标值为(1,0,0)

Eigen::Matrix3d v_orign(1,0,0);

②创建旋转向量v 利用AngleAxisd（角轴类）创建旋转π/4，轴为（0,0,1）的旋转矩阵

//创建旋转矩阵并赋值（旋转角度，旋转轴）
Eigen::AngleAxisd v(M_PI/4,Eien::Vector3d(0,0,1))

③创建旋转后的向量，并计算

Eigen::Vector3d v_rotated = v*v_orign;

//打印结果
cout << v_rotated.transpose() << endl;

直接旋转向量*待转向量就可以得到我们的结果，最后将结果转置打印。

二、旋转矩阵

1、定义理解

当坐标在一组基底 $[e_{1},e_{2},e_{3}]$ 下a的坐标为 $a=[a_{1},a_{2},a_{3}]$ ，当发生旋转后基底为 $[e_{1}^{'},e_{2}^{'},e_{3}^{'}]$ ，那么a的坐标就会变成 $a^{'}=[a_{1}^{'},a_{2}^{'},a_{3}^{'}]$ ，那么他们之间会有什么关系？

关系如图所示，可是这样我们还要考虑这些基底的运算，那我们不妨消掉一边左右同乘e的转置

就可以得到：

这样我们有了待转的坐标，只要乘上一个矩阵，就可以得到旋转之后的坐标啦！

2、结构

那么没错图上的矩阵就是旋转矩阵，有了这个矩阵我们就可以得到旋转后的坐标结果了。

3、代码

可是我们怎样得到旋转矩阵的？我们可以利用旋转向量来得到！

利用罗德里格斯公式可以实现旋转矩阵和旋转向量的相互转化，详细式子和推导我们就不展开了，来看代码：

①创建旋转矩阵R并把上面的旋转向量v转化成我们的旋转矩阵R

Eigen::Matrix3d R = v.toRotationMatrix();

只要通过toRotationMatrix函数就可以将旋转向量转化为旋转矩阵。

②用旋转矩阵R*待转矩阵v_orign就可以得到旋转之后的结果了

Eigen::Vector3d v_rotated = R*v_orign;

//打印结果
cout << v_rotated << endl;

三、欧拉角

1、定义理解

任何旋转都可以看成依次绕x，y，z旋转一定角度得到的结果，没错这些角度就是欧拉角。

但欧拉角是给人看的，我们不用它来计算。

2、代码

接下来我们把旋转向量v转化成欧拉角，并打印出来看看。

首先，欧拉角是个向量分别代表各个轴旋转的弧度

其次，旋转向量和旋转矩阵都可利用eulerAngle方法直接得到参数列表（2,1,0）代表z轴，y轴，x轴的顺序。

Eigen::Vector3d euler_angle = v.eulerAngle(2,1,0);

cout << euler_angle.transpose() << endl;

这样我们就得到了一个向量，看到各个轴应旋转的弧度。

四、欧式变换矩阵

1、定义理解

基本变换包括旋转和平移，前面都是旋转的方法和代码，再加上平移的话，我们就可以看到一个坐标系任意一个点的360°视角了，那么这个时候我们就想办法设计一个矩阵，让它能同时表示旋转和平移等信息，没错，这就是欧式变换矩阵。

2、结构

其中R是我们标题二的旋转矩阵，t是平移向量，也就是x，y，z方向分别移动多少距离。

至于为什么是这个结构，我们这里不深入拓展了，你可以用它去跟我们的待处理向量乘一下，看看得到的结果是不是旋转加平移之后的结果，注意：这里的待处理向量加了一个自由度，结构为[x,y,z,1].

3、代码

接下来我们的R旋转矩阵继续用我们的旋转向量v（用旋转矩阵，旋转向量都可以），平移向量t=[1,1,1]

首先，欧式变化矩阵的类名为Isometry，因为我们用的是三维数据，数据类型是double，所以声明类型为Isometry3d。

其次，利用rotate方法和pretranslate方法分别设置旋转矩阵和平移矩阵。

最后，计算结果时，欧式变换矩阵一定放在*号的左边，否则报错。

//创建欧式变换矩阵并初始化为单位矩阵I
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();

//将T的旋转矩阵设为v
T.rotate(v);

//将T的平移向量设为[1,1,1]
T.pretranslate(Eigen::Vector3d (1,1,1));

//计算结果
Eigen::Vector3d result = T*v;

//打印
cout << result << endl;

五、四元数

1、定义理解

我们前面描述的旋转向量要用9个量来描述3个自由度，具有冗余性；欧拉角和旋转向量虽然是紧凑的，但是具有奇异性；那我们不妨在三个自由度的基础上，加一个自由度，既不会太多，也不会出现奇异性，没错，这就是四元数，但它也有缺点，不是给人看的，或者说人很难看出来，并且计算相对来说更加复杂一点。

2、结构

这个加粗体q就是我们的四元数了， $q_{0}$ 是四元数的实部， $q_{1},q_{2},q_{3}$ 是四元数的虚部，听起来是不是很像我们初中学过的虚数，没错，就是虚数再拓展一些，我们高中的时候学过复数坐标系，而我们的四元数在原本的基础上，多加了两个坐标轴j和k，并且定义了一些它们之间的计算，如下图：

计算机通过四元数知道旋转的信息，这个时候就可以处理待转向量了，至于具体的细节我们就不再展开了，更重要的是代码实现。

你只要知道它和旋转矩阵，旋转向量一样，只是表示旋转信息的另一种表示方法，并且可以和旋转矩阵，旋转向量相互之间转换，也可以填到欧式变换矩阵的旋转矩阵R里面。

3、代码

我们把旋转向量v转化成四元数并打印，然后用四元数处理待转矩阵v_orign得到旋转后的结果。

首先，四元数的声明类为Quaternion，我们的数据类型为double，所以应用的声明类为Quaterniond。

其次，q.coeffs()打印出来的四元数，先是虚部，后为实部。

最后，计算时的q*v_orign并不是真实的运算，真实的运算为 $result=q*v*q^{-}$ ，这里的v为待转向量v_orign。

//创建四元数q并将旋转向量v转化成q
Eigen::Quaterniond q = Eigen::Quaterniond(v);

//打印四元数查看 注意（顺序为（q1,q2,q3,q0）前三者为虚部，最后一位为实部）
cout << q.coeffs() << endl;

//计算结果
Eigen::Vector3d result=q*v_orign;

//打印结果
cout << result << endl;