研究SLAM过程中,专门看的算法是DSO,其前端是一个直接法结合滑窗优化的产物。在实现滑窗代码时对其理解不够深刻,当翻起书再看时才恍然大悟。总结如下。

  • 前端滑窗优化的残差项和BA的完全不同。在DSO中,点的参数只有深度,而且是在host帧上的深度,残差项的构成原理时像素点根据深度进行重投影的亮度误差。而亮度的点是根据特征点的深度进行重投影得到的。可见一个参数和两个关键帧以及一个地图点相关。而下面公式中的$\xi$是target和host的相对位姿,在求导时首先求得残差对相对位姿的倒数$J_{th_\xi}$,之后还要根据adjoint转化为对target和host各自位姿的倒数$J_{t_\xi}$,$J_{h_\xi}$。具体可参考Adjoint of SE(3)
    $$e(d,p,\xi_{th}) = I_{host}(p) - I_{target}(p'(p,d,\xi_{th}))$$

$$p'(p,d,\xi_{th}) = \frac{dK^{-1}p\exp{(\xi_{th})}K}{P_3}$$
$$P = d\exp{(\xi_{th})}K^{-1}p$$

  • 而在BA中,残差项描述的时某个相机对地图点的观测值(像素点位置)和系统预测出的地图点参数以及相机参数推导出来的投影位置的距离。在这种情况下,一个地图点的参数应该是绝对的三维世界坐标系$p(x,y,z)$。所以来说BA的残差求导相对于前端滑窗优化来说要来的简单点,但是BA难点在于其规模会很大,在看ceres时找到BA的数据集[BAL dataset]。随便拿到一个例子就有1900多个相机和六万多个路标点。就算使用舒尔补,相机的Hession矩阵也能够达到20000x20000的级别(相机参数包含位姿、畸变、焦距9个变元),不过这也是有可能解的,因为在BA中相机的Hession部分也是稀疏的。当然了,当进行舒尔布后,左上角的相机Hession块还是会变稠密。这就需要更深层次的方程组求解优化了。
    $$e(P,\xi) = ||p_{obs} - p_{pred}(P,\xi)||$$

$$p_{pred}(P,\xi) = \frac{Kexp(\xi)P}{P_3}$$

  • 总的来说雅克比的不同导致了BA和滑窗的Hession矩阵呈现不同的形式,BA的Hession阵部分的相机、地图(左上、右下都是对角块矩阵),而滑窗Hession的相机部分非稀疏。如下图,为滑窗和BA优化前的初始情况。
    Screenshot from 2019-04-18 15-20-13.png

标签: 非线性优化, SLAM

添加新评论