优化知多少之简述SLAM中的优化

在学习SLAM的过程中,我们会遇到各种BA问题,关于优化问题,有局部优化、全局优化、非线性优化、图优化、位姿图优化、BA优化等,那这些东西到底是什么意思?

BA

BA全称Bundle Adjustment,译为光束法平差,或者束调整、捆集调整.

光束法平差意思是每个特征反射的光束,通过调整它们的空间位置和相机姿态,使它们都汇聚到相机光心,这个过程叫BA.

说了这么多,其本质是一个优化模型,其目的是最小化重投影/光度误差,用于优化相机位姿和世界点(空间位置).

局部BA与全局BA

局部BA用于优化局部的相机位姿,提高跟踪的精确度.

全局BA用于全局过程中的相机位姿, 使相机经过长时间、长距离的移动之后,相机位姿还比较准确.

在各个SLAM开源系统中,都有局部BA与全局BA,还是很重要的.

非线性优化

首先说啥非线性优化在SLAM中的一些历史,大家都知道我们都用BA来做非线性优化,但只是优化相邻两张图片间的位姿和路标点.而现在,对于后端优化来说,我们需要优化整个地图的全部位姿和全部路标点,数据量比之前大了不知多少倍.

虽然理论上来说,数据量大并不影响BA方法.但唯一的障碍是数据量大会导致计算时间急剧增大.因为在用梯度下降法求解时,每一轮迭代至少要解一个线性方程组,这就等同于求一个矩阵的逆.矩阵求逆的时间复杂度是O(n3),于是巨大的数据量导致这个矩阵维度极高,从而使求解用时大的离谱.这也就解释了为什么EKF曾经是后端优化的主流,因为它计算量小呀.

那为什么非线性优化又后来居上了呢？

21世纪以来,人们逐渐意识到如果矩阵具有一定形式的稀疏性,可以加速求逆的过程.而SLAM后端的非线性优化恰恰可以利用这一性质！

SLAM的相机位姿和路标点其实具有非常特殊的结构,并非随机产生.相机位姿和路标点之间是多对多的关系,一个相机位姿可以观测到多个路标点,一个路标点也可以被多个相机位姿观测到.由于相机的大范围运动, 局部区域的路标点只会被局部的几个相机位姿观测到 , 而其它大部分相机位姿都观测不到这些点,这是产生稀疏性的根源,当我们构建了非线性优化的代价函数后,需要求代价函数对所有优化变量的偏导数, 稀疏性意味着这些偏导数大部分为0,只有小部分不为0,这些不为0的项对应着相机 位姿与其能够观测到的路标点的组合. Schur消元法利用矩阵的稀疏性求逆,是BA中求解增量方程的常用手段.

由于上述各种原因,非线性优化成了SLAM的一种"主流方式",谈到非线性优化也就可以近似直接挂钩BA.

图优化

所谓的图优化,就是把一个常规的优化问题,以图(Graph)的形式来表述.

图是由顶点(Vertex)和边(Edge)组成的结构.

在SLAM里，图优化一般分解为两个任务：

• 构建图. 机器人位姿作为顶点,位姿间关系作为边.
• 优化图. 调整机器人的位姿(顶点)来尽量满足边的约束,使得误差最小.

下面附上十四讲的图片做个说明,在现实中,由于误差的存在,实际上机器人建立的地图是不准的(左图),于是我们通过设置边的约束,使得图优化向着满足边约束的方向优化,最后得到了一个优化后的地图(图中),通过和图右的真实地图相比,我们发现他们其实还是很接近的.

位姿图优化

对于路标的优化,可能进行几次之后就已经收敛了,这时候每次插入一个帧都再次进行一次BA仿佛有点用力过猛. 而且实际中,路标的数量远远大于位姿数量,因此BA在大规模建图时,它的计算量可能会越来越大,使得实时计算变得困难.

位姿图优化:仍然是用顶点表示优化变量,用边表示误差项.

BA优化，是把相机位姿和路标点当成优化变量,把路标点的重投影误差当成误差项,只在相机位姿顶点和路标点之间建立边的连接. 现在,为了减小运算量,我们只把相机位姿当成优化变量,让相邻的相机位姿之间用边连接起来.此时,边表达的就不是重投影误差了, 而是两个相机位姿之间相对运动估计的误差. 这个误差是用相机位姿顶点的值计算出的相对运动与用特征点法估计的相机运动之间的偏差,本质上与重投影误差没有区别,都是调整相机位姿使路标点在相机平面上的投影最准确.

总的来说,位姿图优化与普通图优化不同的是位姿图优化只有帧位姿作为顶点,没有3D地图点参与优化.

位姿图优化的意义在于：在优化几次以后把特征点固定住不再优化,只当做位姿估计的约束,之后主要优化位姿.

关于这点，有一个理解非常重要：三角形的位姿,是通过和世界坐标系的比较而得到的,其实世界坐标系在大部分情况下也就是一开机的时候的相机坐标系,把第一帧检测到的特征点的相机坐标,当成是世界坐标,以此为参照,逐步直接递推下去.而上图蓝色的线,是根据中途的两张图,单独拿它俩出来估计一个相对的位姿变换. 也就是说,不要红色的路标点了,只要位姿.位姿里的三角形是位姿,蓝色的线是两个位姿之间的变换.

换言之：在前端的匹配中,基本都是世界坐标系的点有了,然后根据图像坐标系下的点,估计世界坐标系与图像坐标系下的变换关系作为位姿(这里的世界坐标系下的坐标实际是根据第一帧一路递推下来的,可能错误会一路累积)我们假设算出来两个,分别是T1和T2.而蓝色的线,作为两个位姿之间的相对变换,也就是T1和T2之间的变换.但它并不是直接根据T1的逆乘以T2这种数学方式算出来的(不然还优化什么？肯定相等啊).而是通过T1和T2所在的图像进行匹配,比如单独算一个2D-2D之间的位姿变换或者光流法等,这样根据图像算出的位姿很有可能是和T1与T2用数学方式计算得到的位姿是有差异的(因为没有涉及到第一帧图像,就是单纯这两张图像的像素坐标).

以ORB-SLAM中的位姿图优化为例: 首先位姿图优化发生在回环的时候.这个时候,受到回环影响的关键帧就两个位姿：一个是回环前带有漂移的位姿, 一个是回环后将这个漂移消去的位姿. 抛开它构建essential graph这些过程,ORB-SLAM在位姿图优化的时候, 把回环前带有漂移的位姿作为了初值, 但利用回环后的位姿计算了帧间的相对位姿作为约束,也就是测量值,来进行优化.

总的来说:

• 可以把非线性优化直接理解成BA(其实不严谨,哈哈哈)
• 图优化和位姿图优化算是亲戚,反正字眼里都有"图优化",位姿图优化可以理解成图优化的进阶版,具有实际意义.
• 局部BA与全局BA是BA的两个儿子,各个SLAM开源系统都有他们的影子,一会儿局部一会儿全局,特恶心人.

参考

• https://www.jianshu.com/p/e4a45ccd128f
• https://blog.csdn.net/zkk9527/article/details/89068410