ORB-SLAM-Mono论文要点总结（六）——— 附录-非线性优化 Nonlinear Optimizations

非线性优化

1. BA优化[1]
   地图点的3-D位置坐标为$X_{w,j}\in R^3$，关键帧位姿变换$T_{iw}\in SE(3)$(变换空间)。$w$表示世界参考系，优化使得地图点和与之匹配的特征点$x_{i,j}\in R^2$间的重投影误差最小。在关键帧$i$中一个地图点$j$的观测误差为：
   $$e_{i，j}=x_{i,j}-{\pi }_i(T_{iw}，X_{w,j})$$
   这里$\pi$为投影函数：
   
   图中世界系到$i$关键帧的旋转矩阵$R_{iw}\in SO(3)$旋转矩阵李群空间，位移变换$t_{iw}\in R^3$，分为为变换矩阵$T_{iw}$的一部分。$（f_{i,u}，f_{i,v}）$和$(c_{i,u}，c_{i,v})$分别表示与相机$i$对应的焦距长度和主点坐标。最小化代价函数为：
   $$C=\sum _{i,j}{\rho }_h(e_i^T{\Omega }_{i,j}^{-1}{e}_{i,j})$$上式中${\rho }_h$是huber鲁棒损失函数，${\Omega }_{i,j}={\sigma }_{i,j}^2{I}_{2\ast 2}$为检测到的特征点与尺度相关的协方差矩阵。
   下面是基于以上重投影误差原理和建立的代价函数在不同工作模块工作时，对应的优化方式，固定不变的状态，以及每种优化方式待优化的状态：

工作模块	优化方式	固定项	待优化项
地图初始化	full BA	第一个关键帧	所有地图点和关键帧
局部建图	local BA	关键帧	局部地图里所有的地图点
跟踪	位姿优化/motion-only BA	所有地图点	相机位姿

2. 基于相似变换空间Sim3约束的位姿图优化[6]
   基于两个边的位姿图（见VII-D）, 可以定义一边的误差为：
   $$e_{i,j}=lo{g}_{Sim(3)}(S_{ij}{S}_{jw}{S}_{iw}^{-1})$$上式中$S_{ij}$是图优化之前基于SE(3)位姿计算的两个关键帧的相对显示变换，令尺度系数为1？。在回环边，基于Horn[42]方法计算相对变换。$lo{g}_{Sim(3)}$[48]转换到切空间为了将误差转换为7维向量？。优化相似变换空间的的关键帧位姿时最小化的代价函数为：
   $$C=\sum _{i,j}(e_{i,j}^T{\Lambda }_{i,j}{e}_{i,j})$$上式中${\Lambda }_{i,j}$如[48]中所述的信息矩阵，作者将其设为单位阵。为了固定规范7自由度，固定回环关键帧。尽管此方法是一个大致的全局BA（full BA），在实验VIII-E中证明比BA能够更快更好的收敛。
3. 关键帧间的相似变换优化
   基于关键帧1和关键帧2间给定的一组n个匹配 $i\Rightarrow j$(特征点和它们对应的3D地图点)，为了优化相对位姿$S_{12}$(见VII-B)，可以最小化两帧图像间的重投影误差为：
   
   代价函数为：
   
   上式中${\Omega }_{1,i}$和${\Omega }_{2,j}$为与图像1和图像2中特征点尺度相关的协方差度量。在此优化过程中，地图点时固定的。