三维空间中刚体的旋转

港控/mmm° 2023-02-13 05:54 24阅读 0赞

[从零开始一起学习SLAM | 三维空间刚体的旋转][SLAM _]  
刚体：本身不会在运动过程中产生形变的物体，运动过程中同一个向量的长度和夹角都不会发生变化。刚体变换也称欧式变换。

# 三维空间中刚体的旋转表示 #

## 1. 旋转矩阵 ##

1、是重点！！！  
2、旋转矩阵不是一般矩阵，它有比较强的约束条件。旋转矩阵R具有**正交性，R和R的转置的乘积是单位阵，且行列式值为1**。  
3、旋转矩阵R的逆矩阵表示了一个和R相反的旋转。  
4、旋转矩阵R通常和平移向量t一起组成齐次的变换矩阵T，描述了欧氏坐标变换。**引入齐次坐标是为了可以方便的描述连续的欧氏变换**。  
5、冗余。用9个元素表示3个自由度的旋转，比较冗余。

## 2. 四元数 ##

1、SLAM编程中使用频繁程度接近旋转矩阵。稍微有点抽象，不太直观，但是一定得掌握。  
2、四元数由一个实部和三个虚部组成，是一种**非常紧凑、没有奇异**的表达方式。  
3、编程时候很多坑，必须**注意**。首先，一定要注意四元素定义中**实部虚部**和打印系数的**顺序**不同，很容易出错！  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwNzkxMDk5_size_16_color_FFFFFF_t_70]  
其次，单位四元素才能描述旋转，所以四元素使用前必须**归一化**：q.normalize()。

## 3. 旋转向量 ##

1、用一个旋转轴n和旋转角θ来描述一个旋转，所以也称轴角。不过很明显，因为旋转角度有一定的周期性（360°一圈），所以这种表达方式具有奇异性。  
2、从旋转向量到旋转矩阵的转换过程称为**罗德里格斯公式**。这个推导比较麻烦，否则也不会有一个专属的名字了。OpenCV和MATLAB中都有专门的罗德里格斯函数。  
3、旋转向量本身没什么出彩的，不过**旋转向量和旋转矩阵的转换关系，其实对应于李代数和李群的映射**，这对于后面理解李代数很有帮助。

## 4. 欧拉角 ##

1、把一次旋转分解成3次绕不同坐标轴的旋转，比如航空领域经常使用的“偏航-俯仰-滚转”（yaw，pitch，roll）就是一种欧拉角。该表达方式最大的优势就是直观。  
2、欧拉角在SLAM中用的很少，原因是它的一个致命缺点：**万向锁**。也就是在俯仰角为±90°时，第一次和第3次旋转使用的是同一个坐标轴，会丢失一个自由度，引起奇异性。事实上，想要表达三维旋转，至少需要4个变量。  
了解了四种旋转的表达方式，那么编程时如何使用呢？

# 矩阵线性代数运算库Eigen #

事实上，上述几种旋转的表达方式在一个第三方库Eigen中已经定义好啦。Eigen是一个C++开源线性代数库，安装非常方便  
1、Eigen库不同于一般的库，它**只有头文件，没有**.so和 .a那样的**二进制库文件**，所以在CMakeLists.txt里只需要添加头文件路径，并不需要使用 target\_link\_libraries 将程序链接到库上。

2、Eigen以**矩阵为基本数据单元**，在Eigen中，所有的矩阵和向量都是Matrix模板类的对象，Matrix一般使用3个参数：数据类型、行数、列数

Eigen::Matrix<typename Scalar, int rowsNum, int colsNum>

而向量只是一种特殊的矩阵（一行或者一列）。同时，Eigen通过typedef 预先定义好了很多内置类型，如下，我们可以看到底层仍然是Eigen::Matrix

typedef Eigen::Matrix<float, 4, 4> Matrix4f;  
typedef Eigen::Matrix<float, 3, 1> Vector3f;

3、为了提高效率，对于已知大小的矩阵，使用时需要**指定矩阵的大小和类型**。如果不确定矩阵的大小，可以使用**动态矩阵**Eigen::Dynamic  
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix\_dynamic;

4、Eigen在数据类型方面“很傻很天真”。什么意思呢？就是使用Eigen时**操作数据类型必须完全一致，不能进行自动类型提升**。比如C++中，float类型加上double类型变量不会报错，编译器会自动将结果提升为double。但是在Eigen中float类型矩阵和double类型矩阵不能直接相加，必须统一为float或者double，否则会报错。

5、Eigen除了空间几何变换外，还提供了大量**矩阵分解、稀疏线性方程**求解等函数，非常方便。学习Eigen最好的方式就是官网：  
http://eigen.tuxfamily.org/dox/  
有非常多的示例参考。

上述四种旋转表达方式是可以相互转化的。在Eigen中它们之间的转化非常的方便。下图是我看的别人总结的旋转矩阵、四元素、旋转向量之间的相互转化图：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwNzkxMDk5_size_16_color_FFFFFF_t_70 1]

[SLAM _]: https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247485988&idx=1&sn=eaec2aa09cb4baeae36d2f669cc89174&chksm=97d7efb3a0a066a50e8ad29f05fe595a304dd19cc55be0edbe83969c3288b97f2fecc77cd440&scene=21#wechat_redirect
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwNzkxMDk5_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200527194627929.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwNzkxMDk5,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwNzkxMDk5_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200527195428431.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwNzkxMDk5,size_16,color_FFFFFF,t_70