slam系列2：刚体坐标变换

线性代数的基础知识参考这篇：https://xin1999.blog.csdn.net/article/details/124779288

1. 刚体变换

刚体变换仅有旋转+平移功能，矩阵模式表达为：$a^{\prime }=Ra+t$
齐次坐标就是在原n维坐标表示上增加了一个维度，使用n+1维来表示n维，这样欧式变换的叠加就可以直接用矩阵进行计算。

使用齐次坐标表达刚体变换：

2. 旋转的方法

旋转矩阵存在不直观和有冗余的问题，因此提出如下几种表达旋转的方法：

2.1 旋转向量

一个旋转轴$u$和一个旋转角$\theta$来刻画旋转变换，可以用一个三维向量$U=u\theta$来表示。根据罗德里格斯变换公式，我们可以得到三维点/向量$v$ 在旋转向量$U$的作用下变换到 $v_{rot}$ 的关系式为：

旋转向量转为旋转矩阵表达为：

旋转矩阵转为旋转向量为：

2.2 欧拉角

欧拉角就是一种将复杂三维旋转变换分解为分别绕三个独立轴转动角度的复合变换的描述，它使用了3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。
对于x，y，z三个轴的不同旋转顺序一共有(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)等六种组合，不同的旋转顺规会带来不同的结果。我们需要明确旋转顺序，才能确定最终欧拉角所指的姿态。
外旋/静态欧拉角（常用）：旋转中的X,Y,Z轴都是固定不变的
内旋/动态欧拉角：内旋是指在物体旋转过程中绕自身坐标轴旋转，
内旋与外旋的绕轴旋转序列相互倒序时，两者等效。
欧拉角不适用于插值和迭代，往往只用于人机交互中。
欧拉角转为旋转矩阵：
沿X轴旋转时，公式如下，其他同理：

旋转矩阵与zxy外旋的关系为：

展开得到：

求解有：

2.3 四元数

类似于在二维平面中的旋转可以用一个复数来表示，在三维系统中旋转可以用四元数来表示。
首先看下二维平面中的复数和旋转矩阵之间的关系：


四元数要做的事情类似，可以和旋转建立映射关系。

2.3.1 基本定义

四元数的定义和复数非常类似，唯一的区别就是四元数一共有三个虚部，而复数只有一个。所有的四元数都可以写成如下形式：

也可以写作：

向量形式为：

虚部满足：

2.3.2 性质

四元数之间的乘法比较特殊，它们是不遵守交换律的，用矩阵形式表达如下：


上面的乘法可以表示为：

四元数的共轭为：

满足性质：

四元数的逆为：

2.3.3 旋转表达