视觉orb_slam中LM算法的分析

orb_slam中LM算法的分析

                                         刘康

                                                                            2017年 1月

在orb_slam的PoseOptimization()函数中的LM算法，可以分解成几个部分，首先是线性方程的建立和求解，然后是位姿矩阵的迭代更新；对于求解线性方程，就是求解方程 ，然后应用∆x迭代更新当前的位姿  ，其中 ， 分别表示第k次，k+1次的位姿；⨁符号表示的是加法意义的运算，当前的系统下表示矩阵的乘法。

在LM的算法中 ， , J是雅克比矩阵，e表示的是对应的误差项,增加了一个 正定因子；

该算法中的需要求解的方面如下：

1，J矩阵的求解方法；

2，信息矩阵 的求取方法；

3，正定因子 的初始化，更新方式；

4，向量b的求解方法；

5，线性方程H*∆x= b 的求解；

6，利用线性方程求解的值∆x更新位姿；

7，得到新的位姿之后在次进入步骤1到6的循环；

8，当位姿达到收敛，或者精度达到要求时，停止迭代。

计算步骤的大体的框图如下：

1，求解J矩阵：

在optimization()优化函数中，给出的误差项是对应的匹配点的重投影误差，也就是二维的像素的差值，要求的是当前的帧的位姿，位姿的变化矩阵可以由六个李代数参数表示，也就是一个六维的向量表示，所以J矩阵是一个 的矩阵，具体的公式如下：

J=

2，信息矩阵 的求取

在求解LM算法中信息矩阵反应的是提取特征点时所在的图像金字塔的层数的因子这一特性，金字塔一共八层，每一层的特征乘子为1.2，所以对应层的特征因子为 ，其中n为对应的层数，信息矩阵的求解公式如下：

3，正定因子的初始化

LM算法中正定因子的作用主要是保证H矩阵的正定性，线性方程有解。本次算法中正定因子初始化的过程如下：

首先由1,2步可以得到矩阵 ，求解正定因子 的公式如下：

其中 表示给定的一个参数值，这里给出  ； 表示的是矩阵A的所有的对角元素， 表示对角元素中的最大值。

 

4，向量b的求取

可以知道在LM算法中  , J是雅克比矩阵，e表示的是对应的误差项，也就是对应的像素值的差。

 

5，线性方程H*∆x= b 的求解

这里求解的应用的是eigen库中的ldlt方法求解方程：

∆x =H.ldlt().solve(b)

 

6，利用线性方程求解的值∆x更新位姿

我们更新的方法为：  ，其中 ， 分别表示第k次，k+1次的位姿；⨁符号表示的是加法意义的运算，当前的系统下表示矩阵的乘法。

现在详细的讲解一下具体的求解，首先我们的位姿 ， 是一个 的矩阵，所以李代数∆x向量需要转化为一个 的矩阵，转化的方法如下：

设转化后的矩阵为  ，所以有  其中exp()就是一种把李代数转化为矩阵的函数，下面就是直接的矩阵的乘法问题：

 

更新位姿时，要左乘以增量矩阵。

 

7，得到新的位姿之后的判断

7.1 判断条件然后决定是否把这次得到的位姿作为本次更新的位姿

在开始的时候首先计算得到位姿更新之前的所有误差项的二范数的和，这里用变量chi2表示,计算公式如下：

更新位姿以后现在的误差项 发生了变化，为 ；所以所有误差项的二范数的和为：

判断一个公式：

a， 当rh1的值大于0时，把当前的跟新的位姿作为本次更新的位姿，并且更新正定因子 的值：

更新后的值在下一次迭代的时候使用。

b，当rh1的值小于等于0时，增加 的值重新进行计算一次位姿，直到rh1的值大于0，或者计算10次以后仍然不满足时，停止迭代；

  增加 值的策略如下：

首先有初始的 的值为 ，同时定义一个更新因子nu_=2;

当rh1<=0时， ，同时增加为nu_=2* nu_；

下一次迭代之后继续有rh1<=0时，用新的 和nu_继续更新参数的值。

 

7.2判断条件然后决定是否进行下一次更新

判断不进行进行更新的条件主要有两项，第一项是设置的最多迭代次数，当迭代到达最大次数时自动结束，在本次算法中设置的是10次；另一项判断迭代结束的条件是当优化后的位姿达到了一定的精度之后，判断的条件如下：

if((iniChi-chi2)*1e3<iniChi)

                  _nBad++;

                  else

                  _nBad=0;                  

                  if(_nBad>=3)

                  break;     //若连续3次优化的误差都达到了精度要求，则结束优化       

其中iniChi表示迭代之前的总的误差项的二范数的和，chi2表示当前的误差项的二范数的和；上面的程序表示当连续优化三次误差的变化都不大的时候，停止优化。

8，核函数

本次算法中为了使得不受错误的匹配得到的大的误差的干扰，应用了核函数来调整误差很大的误差项的权重值，使得能够很大程度上降低错误匹配的影响。