目录
视觉SLAM十四讲笔记 03-李群与李代数
MizarLyc
2021年10月19日 20:54
收录于文集
共10篇

以下内容为高翔《视觉SLAM十四讲》第二版的学习笔记

回顾

上一讲介绍了三维世界中刚体运动的描述方式，包括旋转矩阵，旋转向量，欧拉角，四元数等，但SLAM中不仅需要位姿的表示，还需要位姿的优化，其核心问题是什么样的相机位姿最有可能观察到这样的观测数据，解决这类问题往往建模为优化问题，再求解最优的R，t使误差最小化。话虽如此，旋转矩阵自身有正交矩阵和行列式为1的约束，在优化中会带来许多附加的问题，为了解决这个问题，引入李群和李代数的概念。之前已经讨论了这两个特殊的群

SO(n)%3D%5C%7BR%5Cin%20R%5E%7Bn%5Ctimes%20n%7D%7CRR%5ET%3DI%2Cdet(R)%3D1%5C%7D%5C%5C%20SE(3)%3D%5C%7BT%3D%5Cbegin%7Bbmatrix%7DR%26t%5C%5C0%5ET%261%5Cend%7Bbmatrix%7D%20%5Cin%20R%5E%7B4%5Ctimes%204%7D%7C%20R%5Cin%20SO(3)%2Ct%5Cin%20R%5E3%5C%7D

群基础

群是一种集合加上一种运算的代数结构，记集合A，运算·，集合内元素为a，则群满足

%5Cforall%20a_1%2Ca_2%5Cin%20A%2Ca_1%C2%B7a_2%5Cin%20A%5C%5C%20%5Cforall%20a_1%2Ca_2%2Ca_3%5Cin%20A%2Ca_1%C2%B7(a_2%C2%B7a_3)%3D(a_1%C2%B7a_2)%C2%B7a_3%5C%5C%20%5Cexists%20a_0%5Cin%20A%2C%5Cforall%20a%5Cin%20A%2Ca%C2%B7a_0%3Da_0%C2%B7a%3Da%5C%5C%20%5Cforall%20a%5Cin%20A%2C%20%5Cexists%20a%5E%7B-1%7D%5Cin%20A%2Ca%C2%B7a%5E%7B-1%7D%3Da_0

上面四个性质分别称为封闭性，结合性，幺元和逆

可以看出，旋转矩阵的集合与矩阵乘法运算构成群，变换矩阵的集合与矩阵乘法也构成群。

李群

李群是指具有连续（光滑）性质的群，不同于类似整数群这样离散的群。旋转矩阵群和变换矩阵群表示着n维空间连续运动的结构，显然其不是离散的而是连续光滑的，也属于李群的范畴。

李代数的引入

%E7%94%B1%E4%BA%8E%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5%E7%9A%84%E6%AD%A3%E4%BA%A4%E6%80%A7%5Cquad%20RR%5ET%3DI%5C%5C%20%E5%AF%B9%E4%BA%8E%E4%BB%BB%E6%84%8F%E6%97%B6%E5%88%BB%E7%9A%84R%2C%E6%9C%89%E5%87%BD%E6%95%B0R(t)%2C%E6%98%BE%E7%84%B6%E5%9C%B0%2CR(t)R(t)%5ET%3DI%5C%5C%20%E4%B8%A4%E8%BE%B9%E5%AF%B9%E6%A0%87%E9%87%8Ft%E6%B1%82%E5%AF%BC%3A(R(t))%26%2339%3BR(t)%5ET%2B(R(t)%5ET)%26%2339%3BR(t)%3D0%5C%5C%20%E7%A7%BB%E9%A1%B9%E5%B9%B6%E7%94%B1%E5%90%91%E9%87%8F%E5%BE%AE%E7%A7%AF%E5%88%86%E6%80%A7%E8%B4%A8%3A(R(t))%26%2339%3BR(t)%5ET%3D-(R(t)%26%2339%3BR(t)%5ET)%5ET%5C%5C%20%E8%AE%B0A%3D(R(t))%26%2339%3BR(t)%5ET%2C%E5%88%99A%3D-A%5ET%2C%E8%BF%99%E6%98%AF%E4%B8%80%E4%B8%AA%E5%8F%8D%E5%AF%B9%E7%A7%B0%E7%9F%A9%E9%98%B5%5C%5C%20%E8%8B%A5%E4%B8%89%E7%BB%B4%E5%90%91%E9%87%8Fa%5E%7B%5Cwedge%7D%3DA%2C%E5%88%99%E8%AE%B0A%5E%7B%5Cvee%7D%3Da%5C%5C%20%E4%BB%8E%E8%80%8C%E5%8F%AF%E4%BB%A5%E8%AE%B0(R(t))%26%2339%3BR(t)%5ET%3D%5Cphi(t)%5E%7B%5Cwedge%7D%2C%E5%85%B6%E4%B8%AD%5Cphi(t)%E4%B8%BA%E4%B8%89%E7%BB%B4%E5%90%91%E9%87%8F%5C%5C%20%E4%B8%A4%E8%BE%B9%E5%8F%B3%E4%B9%98R(t)%3AR(t)%26%2339%3B%3D%5Cphi(t)%5E%7B%5Cwedge%7DR(t)%5C%5C

现在我们可以发现，对R求一次导就是左乘一个对应的三维向量φ的反对称矩阵

%E8%80%83%E8%99%91t_0%3D0%E6%97%B6%EF%BC%8C%E8%AE%BE%E6%AD%A4%E6%97%B6%E7%9A%84%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5R(t_0)%3DI%EF%BC%8C%E6%8C%89%E7%85%A7%E5%AF%BC%E6%95%B0%E6%80%A7%E8%B4%A8%E5%9C%A8t_0%E9%99%84%E8%BF%91%E8%BF%9B%E8%A1%8C%E4%B8%80%E9%98%B6%E6%B3%B0%E5%8B%92%E5%B1%95%E5%BC%80%5C%5C%20R(t)%5Capprox%20R(t_0)%2BR%26%2339%3B(t_0)(t-t_0)%3DI%2B%5Cphi(t_0)%5E%7B%5Cwedge%7DR(t_0)(t)%3DI%2B%5Cphi(t_0)%5E%7B%5Cwedge%7D(t)%5C%5C

这一步可以看出三维向量φ反映了R的导数性质。

此时再联立上述的式子

R(t)%26%2339%3B%3D%5Cphi(t)%5E%7B%5Cwedge%7DR(t)%EF%BC%8C%E5%9C%A8t%3Dt_0%E9%99%84%E8%BF%91%EF%BC%8C%E8%AE%BE%5Cphi%E4%BF%9D%E6%8C%81%E4%B8%BA%E5%B8%B8%E6%95%B0%EF%BC%8C%E5%8D%B3%5Cphi(t_0)%5E%7B%5Cwedge%7D%3D%5Cphi_0%5E%7B%5Cwedge%7D%5C%5C%E5%8F%88%E6%9C%89%E5%88%9D%E5%80%BCR(0)%3DI%2C%E8%A7%A3%E6%AD%A4%E5%BE%AE%E5%88%86%E6%96%B9%E7%A8%8B%E5%BE%97R(t)%3D%5Cexp(%5Cphi_0%5E%7B%5Cwedge%7Dt)%2Ct%E5%9C%A8t_0%E9%99%84%E8%BF%91

这一结果说明了在t=0附近，R可以由三维向量φ的反对称矩阵经过指数运算计算出来，但到目前为止，这个exp符号如何计算（毕竟这是矩阵的指数）、三维向量φ有什么性质我们还是不清楚的，因此还要继续深入对李代数的理解。

李代数的定义

李代数的定义比较复杂，由一个集合V，一个数域F和一个二元运算符[]（称为李括号）组成。

%5Cforall%20X%2CY%20%5Cin%20V%2C%5BX%2CY%5D%5Cin%20V%5C%5C%20%5Cforall%20X%2CY%2CZ%5Cin%20V%2Ca%2Cb%5Cin%20F%2C%5BaX%2BbY%2CZ%5D%3Da%5BX%2CZ%5D%2Bb%5BY%2CZ%5D%2C%5BZ%2CaX%2BbY%5D%3Da%5BZ%2CX%5D%2Bb%5BZ%2CY%5D%5C%5C%20%5Cforall%20X%5Cin%20V%2C%5BX%2CX%5D%3D0%5C%5C%20%5Cforall%20X%2CY%2CZ%5Cin%20V%2C%5BX%2C%5BY%2CZ%5D%5D%2B%5BZ%2C%5BX%2CY%5D%5D%2B%5BY%2C%5BZ%2CX%5D%5D%3D0%5C%5C

上面的性质分别称为封闭性，双线性，自反性和雅可比等价。为了便于理解，一个简单的李代数例子是三维空间集合V，实数域R和叉积组成了一个李代数，读者可以自行验证上面的四条性质是否符合。下面主要展开李代数so(3)和se(3)的性质。

so(3)

李代数的引入一节中的φ就是一种李代数,且

%5Cphi%3D%5B%5Cphi_1%2C%5Cphi_2%2C%5Cphi_3%5D%2C%5CPhi%3D%5Cphi%5E%7B%5Cwedge%7D%3D%5Cbegin%7Bbmatrix%7D0%26-%5Cphi_3%26%5Cphi_2%5C%5C%5Cphi_3%260%26-%5Cphi_1%5C%5C-%5Cphi_2%26%5Cphi_1%260%5Cend%7Bbmatrix%7D%5C%5C

其二元运算符为

%5B%5Cphi_1%2C%5Cphi_2%5D%3D%5B%5CPhi_1%5CPhi_2-%5CPhi_2%5CPhi_1%5D%5E%7B%5Cvee%7D%5C%5C

上面这个反反对称运算符为从三维方阵到三维向量的映射，与反对称运算符对应的映射相反。

so(3)的元素定义为三维向量或者三维反对称矩阵（这两者本身是一一对应的）

so(3)%3D%5C%7B%5Cphi%5Cin%20R%5E3%2C%5CPhi%5Cin%20R%5E%7B3%5Ctimes%203%7D%5C%7D%5C%5C

se(3)

se(3)%3D%5C%7B%5Cxi%3D%5Cbegin%7Bbmatrix%7D%5Crho%5C%5C%5Cphi%5Cend%7Bbmatrix%7D%5Cin%20R%5E6%2C%5Crho%5Cin%20R%5E3%2C%5Cphi%5Cin%20so(3)%2C%20%5Cxi%5E%7B%5Cwedge%7D%3D%5Cbegin%7Bbmatrix%7D%5Cphi%5E%7B%5Cwedge%7D%26%5Crho%5C%5C0%5ET%260%5Cend%7Bbmatrix%7D%5Cin%20R%5E%7B4%5Ctimes%204%7D%5C%5C

se(3)是由一个平移(与前述的变换矩阵中的平移含义并不一致）和一个so(3)三维向量组成的六维向量，反对称符号表示的转换为一由六维向量到四维方阵的映射，而不是表示向量的反对称矩阵

其二元运算符为

%5B%5Cxi_1%2C%5Cxi_2%5D%3D%5B%5Cxi_1%5E%7B%5Cwedge%7D%5Cxi_2%5E%7B%5Cwedge%7D-%5Cxi_2%5E%7B%5Cwedge%7D%5Cxi_1%5E%7B%5Cwedge%7D%5D%5E%7B%5Cvee%7D%5C%5C

其中，反反对称运算符为从四维方阵到六维向量的一个映射

指数映射和对数映射

SO(3)与so(3)的映射

由李代数的引入一节，如果固定了时刻t，那么有李代数so(3)到李群SO(3)的映射

R%3D%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%5C%5C

对exp函数进行泰勒展开

%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%3D%5Csum_%7Bn%3D0%7D%5E%7B%5Cinfty%7D%5Cfrac%7B(%5Cphi%5E%7B%5Cwedge%7D)%5En%7D%7Bn!%7D%5C%5C

这看起来需要求一个无穷级数，还要计算李代数反对称矩阵的n次幂，不过根据相似对角化的知识，对称矩阵的对角化一定可以为高次幂带来比较简单的运算，首先，先将李代数表示如下

%5Cphi%3D%5Ctheta%20a%5C%5C

其中θ是该三维向量的模长，a是单位长度的方向向量,则有

a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3Daa%5ET-I%5C%5C%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3D-a%5E%7B%5Cwedge%7D

证明如下

a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3D%20%5Cbegin%7Bbmatrix%7D%20-a_2%5E2-a_3%5E2%26a_1a_2%26a_1a_3%5C%5C%20a_1a_2%26-a_1%5E2-a_3%5E2%26a_2a_3%5C%5C%20a_1a_3%26a_2a_3%26-a_1%5E2-a_2%5E2%20%5Cend%7Bbmatrix%7D%5C%5C%20aa%5ET%3D%5Cbegin%7Bbmatrix%7Da_1%26a_2%26a_3%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7Da_1%5C%5Ca_2%5C%5Ca_3%5Cend%7Bbmatrix%7D%3D%20%5Cbegin%7Bbmatrix%7D%20a_1%5E2%26a_1a_2%26a_1a_3%5C%5C%20a_1a_2%26a_2%5E2%26a_2a_3%5C%5C%20a_1a_3%26a_2a_3%26a_3%5E2%20%5Cend%7Bbmatrix%7D%5C%5C%20a_1%5E2%2Ba_2%5E2%2Ba_3%5E2%3D1%5C%5C%20%5Crightarrow%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3Daa%5ET-I%5C%5Ca%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3Da%5E%7B%5Cwedge%7D(aa%5ET-I)%5C%5C%20a%5E%7B%5Cwedge%7Da%3Da%5Ctimes%20a%3D0%5C%5C%20%5Crightarrow%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3Da%5E%7B%5Cwedge%7D(-I)%3D-a%5E%7B%5Cwedge%7D

有了上述的简化基础，此时再进行运算

%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%3D%5Csum_%7Bn%3D0%7D%5E%7B%5Cinfty%7D%5Cfrac%7B(%5Ctheta%20a%5E%7B%5Cwedge%7D)%5En%7D%7Bn!%7D%3D%20I%2B%5Ctheta%20a%5E%7B%5Cwedge%7D%2B%20%5Cfrac%7B%5Ctheta%5E2%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%7D%7B2!%7D%2B%20%5Cfrac%7B%5Ctheta%5E3%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%7D%7B3!%7D%2B...%3D%5C%5C%20aa%5ET-a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%2B%5Ctheta%20a%5E%7B%5Cwedge%7D%2B%20%5Cfrac%7B%5Ctheta%5E2%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%7D%7B2!%7D%2B%20%5Cfrac%7B%5Ctheta%5E3%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%7D%7B3!%7D%2B...%3D%5C%5C%20aa%5ET%2B%20(%5Ctheta-%5Cfrac%7B%5Ctheta%5E3%7D%7B3!%7D%2B%5Cfrac%7B%5Ctheta%5E5%7D%7B5!%7D%2B...)a%5E%7B%5Cwedge%7D-%20(1-%5Cfrac%7B%5Ctheta%5E2%7D%7B2!%7D%2B%5Cfrac%7B%5Ctheta%5E4%7D%7B4!%7D%2B...))a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3D%5C%5C%20aa%5ET%2Bsin%5Ctheta%20a%5E%7B%5Cwedge%7D-cos%5Ctheta%20a%5E%7B%5Cwedge%7Da%5E%7B%5Cwedge%7D%3D%20aa%5ET%2Bsin%5Ctheta%20a%5E%7B%5Cwedge%7D-cos%5Ctheta%20(aa%5ET-I)%3D%5C%5C%20cos%5Ctheta%20I%2B(1-cos%5Ctheta)aa%5ET%2Bsin%5Ctheta%20a%5E%7B%5Cwedge%7D

容易发现，这就是上一讲中描述旋转矩阵与旋转向量关系的罗德里格斯公式，李代数so(3)对应的正是旋转向量组成的空间，在角度θ限定在一个2π角度内时，可以得到李代数so(3)到李群SO(3)的一个唯一指数映射，SO(3)映射到so(3)称为对数映射，计算较为复杂。

%5Cphi%3D%5Cln(R)%5E%7B%5Cvee%7D%3D(%5Csum_%7Bn%3D0%7D%5E%7B%5Cinfty%7D%5Cfrac%7B(-1)%5En%7D%7B(n%2B1)%7D(R-I)%5E%7Bn%2B1%7D)%5E%7B%5Cvee%7D%5C%5C

通过旋转向量一节的方法，分别算出转角和转轴，相对计算对数映射公式更为简便。

%5Ctheta%20%3D%20arccos(%5Cfrac%7Btr(R)-1%7D%7B2%7D)%5C%5C%20Rn%3Dn

SE(3)与se(3)的映射

%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)%3D%20%5Cbegin%7Bbmatrix%7D%20%5Csum_%7Bn%3D0%7D%5E%7B%5Cinfty%7D%5Cfrac%7B(%5Cphi%5E%7B%5Cwedge%7D)%5En%7D%7Bn!%7D%26%20%5Csum_%7Bn%3D0%7D%5E%7B%5Cinfty%7D%5Cfrac%7B(%5Cphi%5E%7B%5Cwedge%7D)%5En%7D%7B(n%2B1)!%7D%5Crho%5C%5C%200%5ET%261%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%20R%26J%5Crho%5C%5C%200%5ET%261%20%5Cend%7Bbmatrix%7D%5C%5C

类似上文so(3)的推导，可以比较快的得到

J%3D%5Cfrac%7Bsin%5Ctheta%7D%7B%5Ctheta%7DI%2B(1-%5Cfrac%7Bsin%5Ctheta%7D%7B%5Ctheta%7D)aa%5ET%2B(%5Cfrac%7B1-cos%5Ctheta%7D%7B%5Ctheta%7D)a%5E%7B%5Cwedge%7D%5C%5C

则两者的相互映射关系可以写作如下

se(3)%5Crightarrow%20SE(3)%3AR%3D%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%3Dcos%5Ctheta%20I%2B(1-cos%5Ctheta)aa%5ET%2Bsin%5Ctheta%20a%5E%7B%5Cwedge%7D%5C%5C%20J%3D%5Cfrac%7Bsin%5Ctheta%7D%7B%5Ctheta%7DI%2B(1-%5Cfrac%7Bsin%5Ctheta%7D%7B%5Ctheta%7D)aa%5ET%2B(%5Cfrac%7B1-cos%5Ctheta%7D%7B%5Ctheta%7D)a%5E%7B%5Cwedge%7D%2Ct%3DJ%5Crho%5C%5C%20SE(3)%5Crightarrow%20se(3)%3A%5Ctheta%20%3D%20arccos(%5Cfrac%7Btr(R)-1%7D%7B2%7D)%2CRn%3Dn%2Ct%3DJ%5Crho%5C%5C

上面介绍了李代数的定义，SLAM中最常用的两类李代数so(3)和se(3)，但切莫忘记为什么要使用李代数。目前看来，李代数可以和李群在一个2π角度内形成唯一映射，那么对于李群的乘法（也就是一次旋转），在李代数上是怎么表达与计算呢？

李代数求导

根据指数公式在标量上的表示，我们难免有一个美好的想法：

ln(%5Cexp(A)%5Cexp(B))%3DA%2BB%5C%5C

因为根据标量的指数函数就有这样的性质，然而很遗憾，对于矩阵的指数函数并没有这样的性质。

BCH公式

上面的式子实际上可由BCH公式定量说明

ln(%5Cexp(A)%5Cexp(B))%3D%20A%2BB%2B%5Cfrac%7B1%7D%7B2%7D%5BA%2CB%5D%5E%7B%5Cwedge%7D%2B...%5C%5C

近似公式

根据BCH公式，令其中一个李代数为小量，以至于其二阶以上的项都可以被忽略

ln(%5Cexp(%5Cphi_1%5E%7B%5Cwedge%7D)%5Cexp(%5Cphi_2%5E%7B%5Cwedge%7D))%5E%7B%5Cvee%7D%20%5Capprox%20%5Cbegin%7Bcases%7D%20J_l(%5Cphi_2)%5Cphi_1%5E%7B-1%7D%2B%5Cphi_2%20%5Cqquad%20%26%20%E5%BD%93%5Cphi_1%E4%B8%BA%E5%B0%8F%E9%87%8F%EF%BC%8C%E5%B7%A6%E4%B9%98%E6%A8%A1%E5%9E%8B%20%5C%5C%20J_r(%5Cphi_1)%5Cphi_2%5E%7B-1%7D%2B%5Cphi_1%20%5Cqquad%20%26%20%E5%BD%93%5Cphi_2%E4%B8%BA%E5%B0%8F%E9%87%8F%EF%BC%8C%E5%8F%B3%E4%B9%98%E6%A8%A1%E5%9E%8B%20%5Cend%7Bcases%7D%5C%5C

其中

J_l%3D%5Cfrac%7Bsin%5Ctheta%7D%7B%5Ctheta%7DI%2B(1-%5Cfrac%7Bsin%5Ctheta%7D%7B%5Ctheta%7D)aa%5ET%2B(%5Cfrac%7B1-cos%5Ctheta%7D%7B%5Ctheta%7D)a%5E%7B%5Cwedge%7D%5C%5C%20J_l%5E%7B-1%7D%3D%5Cfrac%7B%5Ctheta%20cot(%5Ctheta%2F2)%7D%7B2%7DI%2B(1-%5Cfrac%7B%5Ctheta%20cot(%5Ctheta%2F2)%7D%7B2%7D)aa%5ET%2B(%5Cfrac%7B%5Ctheta%7D%7B2%7D)a%5E%7B%5Cwedge%7D%5C%5C%20J_r(%5Cphi)%3DJ_l(-%5Cphi)

以李群左乘一个小量为例，左乘一个微小旋转△R，则李群上为△R·R，对应到李代数上，是一个加法运算

%5Cexp(%5CDelta%5Cphi%5E%7B%5Cwedge%7D)%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%3D%5Cexp((%5Cphi%2BJ_l(%5Cphi)%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)%5C%5C

如果是在李代数上做一个微小量的加法，对应到李群上是一次左乘或右乘

%5Cexp((%5Cphi%2B%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)%3D%20%5Cexp((J_l(%5Cphi)%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%3D%20%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)%5Cexp((J_r(%5Cphi)%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)%5C%5C

对于变换矩阵群SE(3)和李代数se(3)也有类似的近似公式，不过形式更为复杂，一般工程中直接调用对应计算库计算，故不赘述。

求导

求导原因

设想一个简单的位姿估计问题，对世界坐标系的点集P，在位姿T得到了得到了观测点集Z，由前面的知识，对每一点有

z%3DTp%2Bw%5C%5C

其中w为观测数据带来的误差。

为了最小化误差，一般采用最小二乘法做最优估计，即

min_TJ(T)%3D%5Csum_%7Bi%3D1%7D%5E%7BN%7D%7C%7Cz_i-Tp_i%7C%7C_2%5E2%5C%5C

求解此问题需要用到目标函数J关于自变量T的导数，也就是要对变换矩阵群T求导，然而，变换矩阵群T上对加法并没有封闭性，导致无法直接求导，故而转向其唯一映射的李代数，李代数对加法具有封闭性，可以进行求导。

SO(3)上的李代数求导

该模型用李代数表示姿态，根据李代数加法的定义，对李代数求导。

设一点p，经过旋转矩阵R得到Rp，现在要求旋转后的坐标对旋转矩阵的导数,直接用对应的李代数表示姿态

%5Cfrac%7B%5Cpartial%20(%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p)%7D%7B%5Cpartial%20%5Cphi%7D%3D%20%5Clim_%7B%5CDelta%5Cphi%5Crightarrow0%7D%5Cfrac%7B%5Cexp((%5Cphi%2B%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5CDelta%20%5Cphi%7D%5C%5C%3D%20%5Clim_%7B%5CDelta%5Cphi%5Crightarrow0%7D%5Cfrac%7B%5Cexp((J_l(%5Cphi)%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5CDelta%20%5Cphi%7D%5C%5C%3D%20%5Clim_%7B%5CDelta%5Cphi%5Crightarrow0%7D%5Cfrac%7B(I%2B(J_l(%5Cphi)%5CDelta%5Cphi)%5E%7B%5Cwedge%7D)%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5CDelta%20%5Cphi%7D%5C%5C%3D%20%5Clim_%7B%5CDelta%5Cphi%5Crightarrow0%7D%5Cfrac%7B(J_l(%5Cphi)%5CDelta%5Cphi)%5E%7B%5Cwedge%7D%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5CDelta%20%5Cphi%7D%5C%5C%3D%20%5Clim_%7B%5CDelta%5Cphi%5Crightarrow0%7D%5Cfrac%7B-(%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p)%5E%7B%5Cwedge%7DJ_l(%5Cphi)%5CDelta%5Cphi%7D%7B%5CDelta%20%5Cphi%7D%5C%5C%3D%20-(Rp)%5E%7B%5Cwedge%7DJ_l

其中，第二式为左乘近似公式，第三式为一阶泰勒展开，第五式将反对称矩阵与一矩阵相乘看做叉积过程，交换后符号取反，详细见上一讲的数学基础部分，这样得到了李代数求导的结果，但计算上还是比较复杂的。

SO(3)左乘扰动模型

对李群左乘或者右乘微小扰动，然后对扰动求导，分为左扰动和右扰动模型，这里以左乘扰动模型为例。

使李群左乘一个扰动，并利用近似公式,对扰动的李代数求导

%E4%BB%A4%E6%89%B0%E5%8A%A8%5Ctriangle%20R%20%3D%20%5Cexp(%5Cvarphi%5E%7B%5Cwedge%7D)%2C%E5%AF%B9%E8%AF%A5%E6%89%B0%E5%8A%A8%E6%B1%82%E5%AF%BC%E5%A6%82%E4%B8%8B%5C%5C%20%5Cfrac%7B%5Cpartial%20Rp%7D%7B%5Cpartial%20%5Cvarphi%7D%3D%20%5Clim_%7B%5Cvarphi%5Crightarrow0%7D%5Cfrac%7B%5Cexp(%5Cvarphi%5E%7B%5Cwedge%7D)%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5Cvarphi%7D%5C%5C%3D%20%5Clim_%7B%5Cvarphi%5Crightarrow0%7D%5Cfrac%7B(I%2B%5Cvarphi%5E%7B%5Cwedge%7D)exp(%5Cphi%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5Cvarphi%7D%5C%5C%3D%20%5Clim_%7B%5Cvarphi%5Crightarrow0%7D%5Cfrac%7B%5Cvarphi%5E%7B%5Cwedge%7Dexp(%5Cphi%5E%7B%5Cwedge%7D)p%7D%7B%5Cvarphi%7D%5C%5C%3D%20-%5Clim_%7B%5Cvarphi%5Crightarrow0%7D%5Cfrac%7B%5Cvarphi(%5Cexp(%5Cphi%5E%7B%5Cwedge%7D)p)%5E%7B%5Cwedge%7D%7D%7B%5Cvarphi%7D%3D-(Rp)%5E%7B%5Cwedge%7D

其中，第二式用到了一阶泰勒展开，第四式将反对称矩阵与一矩阵相乘看做叉积过程，交换后符号取反，详细见上一讲数学基础部分。

显然，对比上面第一个求导模型，省去了J的计算，这也使这种模型更为实用。

SE(3)左乘扰动模型

使变换矩阵左乘一个扰动，然后对扰动求导

%5Ctriangle%20T%20%3D%20exp(%5Cdelta%5Cxi%5E%7B%5Cwedge%7D)%2C%5Cdelta%5Cxi%3D%5B%5Cdelta%5Crho%2C%5Cdelta%5Cphi%5D%5ET%5C%5C%20%5Cfrac%7B%5Cpartial%20(Tp)%7D%7B%5Cpartial%20%5CDelta%5Cxi%7D%3D%20%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B%5Cexp(%5Cdelta%5Cxi%5E%7B%5Cwedge%7D)%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)p%7D%7B%5Cdelta%5Cxi%7D%5C%5C%3D%20%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B(I%2B%5Cdelta%5Cxi%5E%7B%5Cwedge%7D)%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)p-%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)p%7D%7B%5Cdelta%5Cxi%7D%5C%5C%3D%20%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B%5Cdelta%5Cxi%5E%7B%5Cwedge%7D%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)p%7D%7B%5Cdelta%5Cxi%7D

在之前，我们已经定义了

%5Cxi%5E%7B%5Cwedge%7D%3D%5Cbegin%7Bbmatrix%7D%5Cphi%5E%7B%5Cwedge%7D%26%5Crho%5C%5C0%5ET%260%5Cend%7Bbmatrix%7D%5C%5C%20exp(%5Cxi%5E%7B%5Cwedge%7D)%3D%5Cbegin%7Bbmatrix%7DR%26t%5C%5C0%5ET%261%5Cend%7Bbmatrix%7D%5C%5C

所以有

%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B%5Cdelta%5Cxi%5E%7B%5Cwedge%7D%5Cexp(%5Cxi%5E%7B%5Cwedge%7D)p%7D%7B%5Cdelta%5Cxi%7D%3D%20%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B%5Cbegin%7Bbmatrix%7D%5Cdelta%5Cphi%5E%7B%5Cwedge%7D%26%5Cdelta%5Crho%5C%5C0%5ET%260%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7DR%26t%5C%5C0%5ET%261%5Cend%7Bbmatrix%7Dp%7D%7B%5Cdelta%5Cxi%7D%3D%20%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B%5Cbegin%7Bbmatrix%7D%5Cdelta%5Cphi%5E%7B%5Cwedge%7D%26%5Cdelta%5Crho%5C%5C0%5ET%260%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7DRp%2Bt%5C%5C1%5Cend%7Bbmatrix%7D%7D%7B%5Cdelta%5Cxi%7D%5C%5C%3D%20%5Clim_%7B%5Cdelta%5Cxi%5Crightarrow0%7D%5Cfrac%7B%5Cbegin%7Bbmatrix%7D%5Cdelta%5Cphi%5E%7B%5Cwedge%7D(Rp%2Bt)%2B%5Cdelta%5Crho%5C%5C0%5ET%5Cend%7Bbmatrix%7D%7D%7B%5B%5Cdelta%5Crho%2C%5Cdelta%5Cphi%5D%5ET%7D%3D%5Cbegin%7Bbmatrix%7DI%26(-Rp%2Bt)%5E%7B%5Cwedge%7D%5C%5C0%5ET%260%5ET%5Cend%7Bbmatrix%7D

最后一步是来源于

%5Cfrac%7Bd%5Cbegin%7Bbmatrix%7Da%5C%5Cb%5Cend%7Bbmatrix%7D%7D%7Bd%5Cbegin%7Bbmatrix%7Dx%5C%5Cy%5Cend%7Bbmatrix%7D%7D%3D%20%5Cbegin%7Bbmatrix%7D%5Cfrac%7Bda%7D%7Bdx%7D%26%5Cfrac%7Bda%7D%7Bdy%7D%5C%5C%5Cfrac%7Bdb%7D%7Bdx%7D%26%5Cfrac%7Bdb%7D%7Bdy%7D%5Cend%7Bbmatrix%7D%5C%5C

由于计算较为复杂，暂时略去这一步的推导，综上，结果如下。

%5Cfrac%7B%5Cpartial%20(Tp)%7D%7B%5Cpartial%20%5CDelta%5Cxi%7D%3D%20%5Cbegin%7Bbmatrix%7DI%26(-Rp%2Bt)%5E%7B%5Cwedge%7D%5C%5C0%5ET%260%5ET%5Cend%7Bbmatrix%7D%5C%5C

实践模块

该讲的实践模块主要是让大家学会使用Sopuhs进行李群与李代数的映射以及扰动更新模型的应用，将上面的理论付诸实际。

首先安装sophus，采用源码编译安装

代码块
HTML
自动换行
复制代码
#安装Sopuhs
#先将高翔的slambook2/3rdParty中对应版本的Sophus clone下来
cd Sophus
mkdir build
cmake ..
make
sudo make install
复制成功

然后正常编译ch4中的代码，并尝试运行。

useSophus.cpp中已经注释的十分清楚了，包含了旋转矩阵群和变换矩阵群的初始化，两类李代数的初始化，左乘扰动模型更新。

书上还有另外一个例子：评价轨迹误差。该程序将ground truth与estimated两个文件中的信息读出并转换成变换矩阵群SE(3)，其误差根据书上提供的误差定义编程实现，注释也比较清楚，这里不多赘述。

cv13649489