视觉SLAM十四讲学习5 位姿估计（8）直接法

前言

本篇是SLAM前端的最后一节，直接法。

直接法原理

上篇提到的光流法，用于特征点的快速匹配，然后再通过重投影误差优化估计位姿。这是一种“两步走”的位姿估计法，即先匹配，再优化

直接法基于光度不变假设，将匹配步骤省略，直接构建重投影误差优化位姿。首先给出相机模型，假设空间中某点$P$：
$$\begin{gathered} P^{\prime }=(TP{)}_{[1:3]} \\ p=\frac{1}{Z^{\prime }}K{P}^{\prime } \end{gathered}$$

对于相邻两帧图像，如果上一帧$P$与关键点$p^{\prime }$对应，与当前帧上的关键点$p$对应，直接法认为两点理论上的灰度值应当相同：
$$I_1(p^{\prime })=I_2(p)$$
因此，就可以构建光度误差进行优化：
$$\begin{gathered} e=I_1(p)-I_2(p^{\prime }) \\ \underset{T}{\min }\sum _{i=1}^n=\underset{T}{\min }\sum _{i=1}^n||I_1(p_i)-I_2(p_i^{\prime })|{|}_2^2 \end{gathered}$$

那么问题就变成了如何优化上面的非线性最小二乘问题。首先还是求优化函数的梯度：
$$\begin{gathered} e=I_1(p)-I_2(\frac{1}{s}K(e^{{\delta }^{\wedge }}P{)}_{[1:3]})=I_1(p)-I_2(\frac{1}{s}K{P}^{\prime }) \\ \nabla e=-\frac{\partial I_2}{\partial p^{\prime }}\frac{\partial p^{\prime }}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \Delta \delta } \end{gathered}$$
梯度的后两项，与BA中的梯度是相同的：
$$\begin{gathered} \frac{\partial p^{\prime }}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \Delta \delta }=-\left[\begin{array}{ccc}\frac{1}{Z^{\prime }}{f}_x& 0& -\frac{1}{Z^{\prime 2}}{f}_x{X}^{\prime }\\ 0& \frac{1}{Z^{\prime }}{f}_y& -\frac{1}{Z^{\prime 2}}{f}_y{Y}^{\prime }\end{array}\right]\left[\begin{array}{cc}I& -P^{\prime \wedge }\end{array}\right] \\ \nabla e=\frac{\partial I_2}{\partial p^{\prime }}\left[\begin{array}{ccc}\frac{1}{Z^{\prime }}{f}_x& 0& -\frac{1}{Z^{\prime 2}}{f}_x{X}^{\prime }\\ 0& \frac{1}{Z^{\prime }}{f}_y& -\frac{1}{Z^{\prime 2}}{f}_y{Y}^{\prime }\end{array}\right]\left[\begin{array}{cc}I& -P^{\prime \wedge }\end{array}\right] \end{gathered}$$

最后，使用高斯牛顿法或者LM法做非线性优化，就能得到位姿结果。

直接法优缺点

优点：从以上步骤可以看出，实际上直接法并不依赖特征点，甚至可以随机取点做优化，非常省时；可以实现半稠密，稠密建图。

缺点：只适合小运动的位姿估计；光度不变假设实际很难满足；图像非凸，优化容易陷入局部最小。使用时通常采用更多的像素点进行优化。

后记

下篇开始，进入代码学习，Ceres+g2o