刚体运动部分（坐标、旋转、平移、李群和李代数）

1概述

本章主要涉及到三维刚体运动，包括向量的运算、旋转矩阵和旋转向量、变换矩阵、四元数、李群和李代数以及李代数的求导。主要的难点在李群和李代数部分，而向量的乘法和变换矩阵的一些基础知识后续会持续用到。李代数的主要目的是为了解决李群对加法不封闭的问题（无法使用常规的方法求极限，故而无法求导）。将李群转换为李代数，而李代数对加法是封闭的，能够解决李群的求导问题。

2向量点积和叉积

2.1向量的点积

向量的点积也叫内积，向量的点积结果是一个标量，是其中一个向量投影到另外一个向量上，然后做数乘。
$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=\left|a\right|\cdot \left|b\right|\cos ⟨a,b⟩$$

2.2 向量的叉积

向量的叉积也叫向量的外积，计算结果还是一个向量，这个新向量垂直于原始的两个向量。

其中这里定义了反对称矩阵，用于把向量变换为一个特殊的矩阵。

也定义了反对称矩阵A到向量的变换符号：

这里还有一些值得注意的演变公式，在后期很重要：
$a\times b=a^{\wedge }b=-b^{\wedge }a$
由于a和b都和叉积垂直，所以：
$a^T{a}^{\wedge }b=b^T{a}^{\wedge }b=0$

3 旋转和变换

3.1 变换矩阵与齐次坐标

旋转矩阵也叫作方向余弦矩阵，这里不再推导。给出一些约束条件，旋转矩阵的充分必要条件是该矩阵是一个行列式为1的正交矩阵，对于n维旋转矩阵定义如下：
$SO(n)=\left\{R\in {\Re }^{n\times n}|R{R}^T=I,\det (R)=1\right\}$
SO(n)是特殊正交群(Special Orthogonal Group)的意思。显然，一个旋转的相反变换为：

平移比旋转简单很多，在原坐标上加上平移向量即可实现平移。

这里要注意的是，按上式，变换是先选择再平移，显然，a’也做先旋转R.T在平移t是得不到a的。
上面的变换公式不是一个线性关系，多次变换时，由于t的存在，会很繁琐，比如：
$b=R_{1}a+t_1,c=R_{2}b+t_2$
那么a到c的变换为：
$c=R_2(R_{1}a+t_1)+t_2$
多次变换很麻烦。所以引入齐次坐标：

向坐标的末尾添加一个维度，这个维度恒为常数1，将三维坐标变成四维，称为齐次坐标。而矩阵T称为变换矩阵，它使得坐标变换变成了线性关系，等式变得简洁。这个矩阵称为特殊欧氏群(Special Euclidean Group)：

3.2 旋转向量

刚体某一次发生旋转，比如是绕着某个轴旋转了某个角度，因此可以定义一个单位向量，用来表示旋转的轴，而在单位向量上乘以常数θ表示旋转的角度，这样的一个单位长度为θ，方向为n的单位向量称为旋转向量。
$\Phi =\theta n$
旋转向量只用了3个量就表示了旋转，比旋转矩阵精简了很多。从旋转向量到旋转矩阵的变换过程由罗德里格斯公式（Rodrigues’s Formula）给出：
$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$
旋转矩阵到旋转向量的旋转角度：

关于转轴n，显然旋转轴上的向量旋转后不会发生改变，这说明：
$Rn=n$
因此，转轴n是矩阵R特征值为1的特征向量。求R的特征向量，并找出特征值为1的那一个特征向量，归一化，即可得到转轴。

3.3 四元数

4 李群和李代数

4.1 群

在前面提到SO(3)是特征正交群，SE(3)是特殊欧氏变换群，他们分别是旋转矩阵和变换矩阵。他们对乘法是封闭的，但是对加法并不封闭：
$R_{{}^1}+R_{{}^2}\notin SO(3),T_{{}^1}+T_{{}^2}\notin SE(4)$
而求极限或者求导的时候，需要使用到自变量的加法（比如求解误差关于变换矩阵T的导数），因为这两个群对加法不封闭，因此无法求解他们的导数。
李群指的是光滑连续的群，空间中旋转和变换都是联系的，因此他们也是李群。
群是一种集合加上一种运算的代数结构。我们把集合记作A，把运算记作·，那么群可以记作G=(A,·)。群有几个限制条件：

4.2 李代数

李代数是李群在单位元处的正切空间，它反映了李群的导数性质。事实上，SO(3)的李代数so(3)正好是旋转矩阵R所对应的的旋转向量。so(3)是一个三维的向量，se(3)是一个六维的向量，它们对加法封闭，可以很方便地求导。
下面直接给出李群和李代数的变换关系：

可以看到李群到李代数的变换，就是求旋转矩阵R的旋转向量的过程，而李代数到李群的变换就是罗德里格斯公式。这再次说明so(3)就是SO(3)的旋转向量。

SE（3）则要复杂一些，它包含了SO(3)部分的变换。李群到李代数的变换，增加了平移部分，求解平移部分的李代数ρ，这个等式中，t和J都是可以得到的，解方程组就可以得到ρ。李代数到李群的变换中，比SO(3)也是多了平移的Jρ，求解即可。
值得注意的是，这里的变换T是针对齐次坐标系的。

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似形式

前面提到，李群对加法不封闭，因此无法直接求导。而李群在使用的时候，我们只使用乘法，那么两个李群相乘时，对应的李代数发生了什么呢。这个关系由BCH公式给出：

其中[]为李括号。BCH公式说明，当两个矩阵的指数（旋转矩阵）做乘法时，他们会产生一些由李括号组成的余项。当A或者B为小量时，二次以上的项可以忽略，给出近似公式：

其中φ是李代数。
该式子告诉我们，当对一个旋转R左乘一个小量时，可以近似地看作，在原有李代数上加上一个项，类似于泰勒展开一样。上面给出了左乘模型和右乘模型，这里只考虑左乘模型。事实上，这里的Jl就是前面李群和李代数互相变换中的J。上面式子整理下也可以写成：

对于se(3)上面的式子同样成立，只是需要把φ换成ξ，且J的形式更加复杂，这里它是一个6×6的矩阵。

4.3.2 李代数求导

我们经常需要求误差关于位姿或者运动T的导数，假设有世界坐标系下的点p，相机观测到的坐标是z，现在求相机的位姿，那么变换关系为：

其中w是随机噪声。由于他的存在，我们一般会让下面的误差尽量小来求T：
$e=z-Tp$
假设有N个这样的点，那么就是求最优的T，使得整体误差最小：
$\underset{T}{\min }J(T)=\sum _{i=1}^N{\Vert z_i-T{p}_i\Vert }_2^2$
这是典型的最小二乘问题，那么我们就要求误差关于T的雅克比矩阵。求导的思路有两种：
① 用李代数表示姿态，然后根据李代数加法对李代数求导
② 对李群左乘或右乘微小扰动，然后对扰动求导。
下面先用李代数求导来做。
为了简单，考虑只有旋转的情况，也就是SO(3)的情况。假设点p进行了旋转，得到了RP，要计算旋转后的点坐标关于旋转的导数，记为：
$\frac{\partial (Rp)}{\partial R}$
由于SO(3)没有加法，所以该导数无法按照导数的定义进行求导。设R的李代数为φ，故有：

第二行是BCH近似，第三行是泰勒展开近似，最后一行利用了叉积的性质。
这里的Jl就是前面提到的J。形式比较复杂。

4.3.3 扰动模型

对R左乘一个小的扰动△R，看结果相对于扰动的变化率。下面设△R的李代数为φ，然后对φ求导：

可见，扰动模型得到的导数比直接求导的要少了Jl，更加简洁。
对于SE(3)直接给出扰动模型的计算，设空间点p经过一次变换T（李代数为ξ），得到Tp，现在给T左乘一个扰动△T=exp(δξ^)，我们设扰动的李代数为：
$\delta \xi ={\left[\delta \rho ,\delta \varphi \right]}^T$