SLAM14讲摘要笔记（1）

1. 旋转矩阵

定义推导：
为了描述两个坐标之间的关系，对等式左右同乘


由于为正交矩阵，则，所以
欧式变换除了旋转还有平移，平移就简单多了，直接在旋转后的向量坐标上加一个平移向量t就可以，把旋转和平移合在一起有：

当做两次变换时有：


为了简化形式，引入了齐次坐标和变换矩阵：

三维旋转矩阵构成特殊正交群 SO(3)： (Special Orthogonality Group)

三维变换矩阵构成特殊欧式群 SE(3)： (Special Euclidean Group)

2. 罗德里格斯公式

我们可以看到旋转轴向量可以通过得到轴的单位向量
旋转角度θ slot by tr(R)

3. 四元数相关

我们可以使用一个四元数q=((x,y,z)sinθ2, cosθ2) 来执行一个旋转。具体来说，如果我们想要把空间的一个点P绕着单位向量轴n = (x, y, z)表示的旋转轴旋转θ角度，我们首先把点P扩展到四元数空间，即四元数p = (P, 0)。那么，旋转后新的点对应的四元数（当然这个计算而得的四元数的实部为0，虚部系数就是新的坐标）为：
p′=qpq−1

其中，q=(cosθ2, (x,y,z)sinθ2) ，q−1=q∗N(q)，由于u是单位向量，因此
N(q)=1，即q−1=q∗。右边表达式包含了四元数乘法。相关的定义如下：
四元数乘法：q1q2=(v1→×v2→+w1v2→+w2v1→,w1w2−v1→⋅v2→)

共轭四元数：q∗=(−v⃗ ,w)

四元数的模：N(q) = √(x^2 + y^2 + z^2 +w^2)，即四元数到原点的距离

四元数的逆：q−1=q∗N(q)
众所周知的是，欧拉旋转是有万向节死锁(Gimbal Lock)的问题的。幸好我们有四元数(Quaternion)这种数学工具可以避免这个情况。

一般来说，我们都会用单位四元数 来表示旋转，其中 那么给定一个单位四元数，可以构造旋转矩阵



这个四元数构造的大概思路就是把四元数的旋转操作写成矩阵形式（注：给定一个用于旋转的单位四元数 和被旋转的三维向量，那么要直接用四元数旋转这个向量，则我们首先要构造一个纯四元数 ，设旋转后的向量为，旋转后的向量构造的纯四元数为 ，那么）。因为是用四元数来构造矩阵的，所以这个矩阵构造公式就没有欧拉角顺规的说法了。
反之，由旋转矩阵到四元数的转换：假设矩阵R={mij},i,j∈[1,2,3],其对应的四元数q

4. 李群李代数

4.1 初识SO(3) SE(3)

在李群中，我们使用矩阵来表达一个旋转和平移，这存在冗余的自由度。三维空间的旋转只有三自由度，旋转+平移有六自由度。因此，我们希望寻找一个没有冗余自由度（但是相应的存在奇异性）的表示，也就是李代数so(3)和se(3)：怎么将李代数和李群元素对应起来呢？我们说，从李代数到李群，通过指数映射；反之，从李群到李代数，则通过对数映射。
=R
其中∧算符把一个三维向量变换为它对应的反对称矩阵（3×3） *****************************************************************************************
可以看到这也就是Rodrigues（罗德里格斯）公式。说明李代数so(3)里的向量，其方向指向旋转轴，而模长表示转过的角度。平时用的rpy欧拉角，实际上是李代数之一。另外，指数映射是一个满射非单射。由于旋转具有周期性，多转360度表示的是一个旋转。如果我们把旋转限制在正负180度内，那么李群到李代数就是一一对应的。
反之，对数映射ln(R)∨将一个旋转矩阵转换为一个李代数。但由于对数的泰勒展开不是很优雅，通常直接按照下式计算李代数向量的大小和方向：

4.2 SE(3)中的指数映射和对数映射

记，它们各为三维向量，一共组成了六维，构成一个李代数。我们的记法里ρ为平移，ϕ为旋转。

该式左上即 SO(3) 的指数映射，右上是平移量 ρ 的线性表示。其系数J3×3的计算方式：

它与Rodrigues相似但有差别，实际上是对ϕ求了一次导数，也是一个雅可比矩阵，这个矩阵是可逆的。
现在我们说清了SE(3)上的指数映射。那么它的对数映射怎么计算呢？也就是一个SE(3)中的矩阵怎么找到对应的向量呢？