视觉SLAM：视觉SLAM中的李群与李代数、左乘扰动模型

为什么引入李群与李代数？

SLAM中对矩阵求导数的应用场景？什么情况下用到李代数左乘扰动模型求导数？

为什么不能用李群求导？

李群与李代数的关系？

李代数左乘扰动的理解？

0 为什么引入李群与李代数？

slam中后端优化需要用到求解最小二乘问题，优化变量为旋转矩阵与平移矩阵。但是旋转矩阵自身是带有约束的，正交且行列式为1，他们作为优化变量时，会引入额外的约束，使优化变的困难，不加约束会使每一步迭代得到的结果不是旋转矩阵。通过李群李代数的转换关系，把位姿估计变成无约束的优化问题。

1 李群与李代数在SLAM中的用途

视觉SLAM中用李群表示相机位姿T，用李代数求导数。

对于同一个特征点，如果上一时刻得到该点相机坐标系下的三维坐标为P，当前时刻观测到相机坐标系下的坐标为Z，则两时刻相机位姿变化矩阵为T，有如下公式：

                                                                                 

为误差。目的是求解矩阵T，得到相机的位姿变换，以此迭代下去得到相机的运动轨迹。

由于误差不可避免，并且两时刻之间可以观测到很多匹配好的特征点，因此一般用很多观测到的特征点构建优化问题，以减小求解误差：

                                                                                              (1-2)

变化矩阵的形式如下：

                                                                                 

变化矩阵T包含了旋转R与平移t信息，其重要性质：两个矩阵T相乘得到的同样是一个变化矩阵。比如矩阵T1和T2表示两个变化矩阵，则：

                                                    

因此变化矩阵T恰好是一个李群，即对乘法封闭的李群。我们的目的是求解问题（1-2），即优化变量为T，优化问题为最小二乘问题，可以利用李群的性质求解优化问题（1-2）。

2 不用李群求导数的原因

对于最小二乘问题（1-2）求解办法一般是梯度法，需要求解函数J对矩阵T的导数，并且每次迭代：。但是李群T对加法没有封闭性。比如T1与T2都是变化矩阵，但是T1+T2一定不是变化矩阵，即无法求导得到。因此需要进行转化，将该问题转化为对加法封闭，并且能够表示变化矩阵信息。

李代数对应李群，李代数满足加法，因此可以利用李群与李代数之间的关系求解导数。

3 李群与李代数的关系

李群 = 矩阵集合+运算，比如变化矩阵T对乘法封闭，即构成"实数+乘法"的群。

李代数描述了李群的导数，是向量。

旋转矩阵R构成的李群对应的李代数是旋转向量，变化矩阵T构成的李群对应的李代数是旋转向量与平移向量组成的六维向量，slam中只用到这两个群的概念，其余的不会。并且这两个李群与对应的李代数是一一对应的。（角度固定在之间。）

李群转化为李代数通过对数映射，李代数转化为通过指数映射完成。具体如下：

 

a表示单位方向，为模长，即旋转向量。

4 左乘扰动模型理解

旋转矩阵的左乘扰动模型求导：

假设对空间点P进行了旋转R操作，需要求RP关于R的导数，求导方法是对R进行一次扰动。设R的李代数为，的李代数为。对求导：

                                             

理解：我们的目的是迭代求解出旋转矩阵，只要能够求解出优化方程中的旋转矩阵，使目标函数逐渐减小即可。因此给旋转矩阵乘以一个旋转矩阵微小扰动，然后对该扰动求解导数，此时变量变成了微小扰动。如果能够求解出该微小扰动，并且该微小扰动可以使目标函数减小，说明该微小扰动作为更新量是可行的，迭代公式变为。

同理变化矩阵的左乘扰动模型求导如下：