SLAM中的优化、李群李代数和BA

Table of Contents

1.SLAM中优化目标函数的表示

2.李群李代数的引入

3.BA

---

本文为高博《视觉SLAM十四讲》李群李代数部分的学习笔记。

1.SLAM中优化目标函数的表示

SLAM的核心问题就是通过观测数据来得到其对应的位姿，通用的方法就是构建为一个优化问题，然后求解最优的旋转矩阵和平移向量，使得误差最小化。

构建优化问题来求解相机位姿是问题求解的总体思路，但是在构建优化问题时，发现作为要求解的优化变量，必须是一个正交矩阵并且行列式为1。如果直接将作为优化变量的话，会引入额外的约束，这样优化问题就变得困难了。

因为李群和李代数之间可以进行相互转换，如果将（是特殊正交群）转换为其对应的李代数变量，用这个李代数变量作为优化变量就可以构造无约束的优化。求解出的将是旋转矩阵对应的李代数，再将李代数转换为对应的李群即是要求解的。

假设一种现实场景：在三维世界中，相机在某个时刻的位姿为，此时观察到远处的一个点，在相机中产生了观测数据。

此时，由相机的成像原理可得：

                                                                                                 

为误差项，所以误差项可表示为：

                                                                                                  

假设相机从地点运动到了地点，在这个过程中有次观测，则整个过程中的每个误差取平方再进行累加，就得到了总体误差。对其取最小化可记为：

                                                                                 

要求解该式对于的最小化，就需要计算关于变换矩阵的导数。SLAM中就是通过构建与位姿有关的函数（也就是最小误差的表达式），然后讨论该函数关于位姿的导数，以调整当前的估计值。

而上并没有良好定义的加法，它们都是群。在上式中，如果要对进行求导，就需要考虑到其中的约束条件（其实就是变换矩阵中旋转矩阵的约束）。

此时，就引入的李群李代数来帮助求解。

2.李群李代数的引入

用于描述相机位姿的旋转矩阵构成了特殊正交群，而变换矩阵构成了特殊欧式群：

                                                          

                                                          

由于群的定义中要求群对于一种运算要满足封闭性、结合律、幺元和逆四个条件。

但是，旋转矩阵和变换矩阵对加法是不封闭的。也就是说两个旋转矩阵相加，和不再是一个旋转矩阵：

                                                          

这样的话，就会遇到一个问题：在SLAM中用旋转矩阵或者变换矩阵表示相机的位姿，但是在优化方程中由于它们不满足封闭性，所以都不便于求导。

这时候引入了李群的概念，具有连续光滑性质的群叫做李群，所以和都属于李群，每个李群都有其对应的李代数。

这里的思路就变化为：对李群不便于求导，发现其李代数比较方便求导，所以就先用李代数进行求导，然后将结果再转为李群的表示。

此时，高博书中那张很有名的旋转矩阵和变换矩阵的李群和李代数之间的转换图就出现了：

上边图中的李群李代数变换，进行下面两点说明：

1）图中的指数映射，其实就是罗德里格斯公式；

2）图中的对数映射，其实就是利用迹的性质分别求解转角和转轴。

三维旋转中，用三维的向量表示三维的旋转对应的李代数，表示将李代数向量写成对应的反对称矩阵。

三维变换中，用六维的向量表示变换矩阵的李代数，表示将李代数向量拓展成对应的反对称矩阵。

---

旋转矩阵变换成李代数形式后，怎么进行求导呢？这是个重要话题，因为引入李代数就是为了方便优化函数对旋转矩阵的求导。

存在两种求解方法：

1）李代数进行求导

假设空间点进行了旋转，得到了。现在要计算旋转之后点的坐标相对于旋转的导数，则要求的导数为：，此时，由于对加法不封闭，所以该导数无法按照导数的定义来进行求解。

设的李代数为，按照指数映射将导数转换为对李代数，再按照导数的定义展开：

                                   (由BCH线性近似所得)

                               （泰勒展开舍去高阶项取近似）

                             

                                          （反对称符号看做叉积，交叉变号）

                              

                               

所以，可以得出旋转后的点相对于李代数的导数：

该方式求导的缺点：含有形式比较复杂的，一般不太希望有这么复杂的计算。所以就有了另一种更简单的导数计算方式：扰动模型。

---

这里的扰动模型还是需要花心思来理解的，高博《视觉SLAM十四讲》第四讲中有主要步骤的介绍。

两个李代数指数映射乘积完整形式由BCH公式给出，当其中的存在小量的时候，小量的二次以上的项忽略掉后可得到如下线性近似表达：

                                                                    

1）第一个近似公式中为小量。当对旋转矩阵(李代数为)左乘一个微小旋转矩阵(李代数为)时，可以近似地看做，在原有李代数上加上一项。该式为左乘近似公式。

2）同理第二个近似公式中中为小量。该式为右乘近似公式。

---

2）扰动模型（左乘）

对进行一次扰动来求解导数，这个扰动可以乘在左边也可以乘在右边。这里看一下左乘的求导。

设左扰动对应的李代数为，然后对求导：

                                                         

                                                                          

                                                                          

                                                                          

                                                                          

从求解结果可以看出，相比于直接对李代数求导，扰动模型省去了一个雅克比的计算，因此，扰动模型更为实用。

上用扰动模型对李代数进行求导：

假设某空间点经过一次变换（对应李代数为），得到。

给左乘一个扰动，设扰动项的李代数为。那么：

               

              

              

              

              

              

下面将利用这里的求解结果来计算重投影误差。

3.BA

求解3D-2D点对运动的方法叫做PnP(Perspective-n-Point)。它描述了当知道n个3D空间点及其在像素平面中投影位置的时候，如何估计相机的位姿。也就是求解世界坐标系中的3D点到像素坐标中点的旋转和平移。BA(Bundle Adjustment)就是PnP的一种求解方法，通过构造最小二乘问题来进行迭代优化求解。是一个最小化重投影误差问题。

n个空间点及其投影，要计算的相机位姿，它的李代数表示为。

对于空间点，其在相机中的像素平面投影坐标为。

则根据相机模型可得到下面公式：

                                                        

写成矩阵形式：

                                                                 （这里隐含存在齐次到非齐次的转换。）

由于相机位姿未知和观测点的噪声，该等式也存在误差。

而Slam中的优化就是把所有观测点的误差求和，构造最小二乘问题，然后寻找最好的相机位姿使最小二乘函数最小化。其实也就是求误差最小的情况下的相机位姿。

                                                         

可以看出，该式要表达的其实就是将像素坐标与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称为重投影误差。

如上图所示，通过特征点匹配知道了和是同一个空间点的投影，但此时相机的位姿是未知的。在初始值中，的投影与实际的之间有一定的距离。于是调整相机的位姿，使得这个距离变小(迭代优化其实就是做了这么一件事)。

误差函数记为：

                                                         

因为为像素坐标误差，所以是2维的。为相机位姿，是6维的。将是一个2*6的矩阵。

空间点投影到相机空间坐标系下为：

                                                        

根据相机坐标到像素归一化平面坐标变换可得：

                                                             

消去其中的（实际上就是的距离），得：

                                                                         

此时对误差利用链式法则求导：

                                                                  

这里的指李代数上的左乘扰动。

第一项是误差关于投影点的导数，则可得：

                                                      

第二项为变换后的点关于李代数的导数：

                                                       

定义中取前三维，得到：

                                                                       

此时，可得到2*6的雅克比矩阵为：

                                                  

这个雅克比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系，指导着位姿的优化。

除了优化位姿，还有特征点的空间位置。此时需要求解误差关于空间点的导数，这里仍然需要用到链式法则：

                                                                                         

由于可得对求导后将只剩下。此时可得：

                                                                               

该矩阵为优化特征点空间位置时的雅克比矩阵，指导着迭代的方向。

好在优化部分已经有第三方库提供了相应计算，所以写代码的时候调用就可以了，简化了实际的代码编写。