SLAM笔记（九）再谈李代数

内容接SLAM笔记（一）SLAM中的数学概览
李群：
定义：实数空间上的连续群(对乘法、逆都是连续的，解析的)
举例：如GL(n)，SO(n)，SE(n）

李代数(Lie algebra):
定义：由一个集合，一个数域，和一个二元运算[]组成；满足封闭、双线性、自反性、雅克比等价。

如3D中的叉乘：g=(R3,R,×)$g=(R^3,R,\times )$就属于一个李代数。

李代数定义于李群的正切空间上，描述了李群中元素局部性质。在刚体运动SE(3)中，李代数表征某处的速度（在后边会具体分析）
举例:so(3),se(3)

1.李群求导

运算为李括号：[ŵ ,v̂ ]=ŵ v̂ −v̂ ŵ $[\hat{w},\hat{v}]=\hat{w}\hat{v}-\hat{v}\hat{w}$

引入李代数的原因是：
对于旋转矩阵R(t)(t指代角度),由于R(0) = I,所以旋转矩阵可以一阶近似表示：

R(t0+dt)=R(t0)+dR=I+ŵ (t0)dt$$R(t_0+dt)=R(t_0)+dR=I+\hat{w}(t_0)dt$$

若t0$t_0$ = 0:

R(dt)=R(0)+dR=I+ŵ (0)dt$$R(dt)=R(0)+dR=I+\hat{w}(0)dt$$

其中ŵ (t)$\hat{w}(t)$是一个关于向量 w(t)=[u1,u2,u3]T$w(t)=[u_1,u_2,u_3{]}^T$反对称矩阵（skew-symmetric matrix）

⎧⎩⎨⎪⎪0u3−u2−u30u1u2−u10⎫⎭⎬⎪⎪$$\left\{\begin{array}{ccc}0& -u_3& u_2\\ u_3& 0& -u_1\\ -u_2& u_1& 0\end{array}\right\}$$

它的推导如下：：

这样对旋转矩阵R求导，只用直接左乘一个矩阵。如果R是单位矩阵，那它的导数就是ŵ $\hat{w}$，一个反对称矩阵。

只有反对称矩阵组成的空间，即 so(3)，我们称之为在单位矩阵处的正切空间tangent space。

为什么这么称呼它？二维曲线在某处的导数是一条切线，三维曲线在某处的导数是一个切面，所以正切空间，实际上是导数所组成的空间。

对于一般非单位矩阵的旋转矩阵R，只要求出了单位矩阵的的正切空间ŵ $\hat{w}$，然后在 ŵ $\hat{w}$右边乘上这个矩阵R就可以求得R对应的正切空间了。

由于ŵ (t)$\hat{w}(t)$反映了R的导数性质，故称它在SO(3)的正切空间(tangent space)上。

2.指数映射： so(3)→SO(3)$so(3)\to SO(3)$

由于R˙(t)=ŵ R(t)$\dot{R}(t)=\hat{w}R(t)$ , 该微分方程可以求得一般解为：
R(t)=eŵ tR(0)$R(t)=e^{\hat{w}t}R(0)$

当R(0) = I 时，上式为：
R(t)=eŵ t$R(t)=e^{\hat{w}t}$
这意味着可以通过指数映射，将so(3)(w)转换为SO(3)(R)

eŵ t$e^{\hat{w}t}$代表如下所示的矩阵指数形式：

（3-1）
由于题主懒得修改截图，此处将w$w$进行了归一化，即我们在进行指数映射时，将原来的w’分解为wt$wt$，其中w$w$模为1$。

模为1的ŵ $\hat{w}$有如下性质：
ŵ 3=−ŵ ${\hat{w}}^3=-\hat{w}$
ŵ 2=wwT−I${\hat{w}}^2=w{w}^T-I$
代入（1-1），得到：

容易看出括号里分别是sin(t)$sin(t)$和1−cos(t)$1-cos(t)$,因此上式可转变：

eŵ t=I+sin(t)ŵ +(1−cos(t))ŵ 2$$e^{\hat{w}t}=I+sin(t)\hat{w}+(1-cos(t)){\hat{w}}^2$$

（3-2）
令w=[nx,ny,nz]T$w=[n_x,n_y,n_z{]}^T$ 表示旋转方向：
相应的ŵ =$\hat{w}=$

⎧⎩⎨⎪⎪0nz−ny−nz0nxny−nx0⎫⎭⎬⎪⎪$$\left\{\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right\}$$

t表示旋转角度，则（3-2）即是从旋转方向和角度生成的旋转矩阵（2-1）。
即给定旋转方向和角度，可以得到旋转矩阵。

3.对数映射： SO(3)→so(3)$SO(3)\to so(3)$

反之，给定旋转矩阵R，也可以得到旋转方向和旋转角度：
（3-3）
上面推导的是连续时间的，并且假设||w||=1。上述表示的物理意义是在单位旋转速度w下，经过时间t后，旋转了多少。
更全面的写法是：
（3-4）
也就是对于so(3)中的向量表示w，w的模表示转过的角度，其方向指向旋转轴
当R为单位矩阵时，（3-3）中t = 0时，即无旋转。

有一种so(3)的指数映射如下：
Lie-Cartan coordinates of the first kind：

通过一些变换可以得到分开的下列各式
Lie-Cartan coordinates of the second kind：

当w1=[0,0,1]T,w2=[0,1,0]T,w3=[0,0,1]T$w_1=[0,0,1{]}^T,w_2=[0,1,0{]}^T,w_3=[0,0,1{]}^T$
时，α1,α2,α3${\alpha }_1,{\alpha }_2,{\alpha }_3$就是平时用的欧拉角，所以欧拉角实际上也是旋转群的李代数之一。

（注意，第一样式到第二样式并非单纯指数形式地展开，而有其他处理，因为当幂级数为矩阵时指数的ea+b=ea+eb$e^{a+b}=e^a+e^b$不再成立）

so(3)到SO(3)之间的指数和对数的映射不是双射，仅是满射：对每一个SO(3)都能在so(3)找到一个对应的矩阵；但可能存在很多so(3)与之对应（只要so(3)的角度相差2π$2\pi$）

细心的读者肯定发现了，在（3-4）中，如果我们把旋转限制在正负180度内，那么李群到李代数就是一一对应的。

so(3)与SO(3)的关系可视化如下：

4.写在工程经验之后：6维李代数数值上到底表征什么

实际上我们对位姿所对应的李代数（se(3),（1x6）)进行求导后，得到一个2x6的雅克比矩阵，用优化方法，如光束平差法(Bundle Adjustment)求解。每求解一次，即可得到关于一次关于位姿的增量（1x6）。然后可以根据这6维中的旋转向量（3维）得到旋转的增量dp和位移的增量dt。

也就是，李代数旋转部分的三个数值表示一个 旋转向量。

旋转向量的大小（模）表示旋转转角度，因此可以根据上面3-2式直接得到旋转矩阵（看g2o源码，会发现它也是先将旋转向量转化为旋转矩阵）