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

前言

本篇是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中的梯度是相同的：
∂ p ′ ∂ P ′ ∂ P ′ ∂ Δ δ = − [ 1 Z ′ f x 0 − 1 Z ′ 2 f x X ′ 0 1 Z ′ f y − 1 Z ′ 2 f y Y ′ ] [ I − P ′ ∧ ] ∇ e = ∂ I 2 ∂ p ′ [ 1 Z ′ f x 0 − 1 Z ′ 2 f x X ′ 0 1 Z ′ f y − 1 Z ′ 2 f y Y ′ ] [ I − P ′ ∧ ] \frac {\partial p'}{\partial P'} \frac {\partial P'}{\partial \Delta \delta} = -

$$\begin{bmatrix} \frac{1}{Z'}f_x & 0 & -\frac{1}{Z'^2}f_xX' \\ 0 & \frac{1}{Z'}f_y & -\frac{1}{Z'^2}f_yY' \\ \end{bmatrix}$$ $$\begin{bmatrix} I & -P'^\land \end{bmatrix}$$ \\ \quad \\ \nabla e = \frac {\partial I_2}{\partial p'} $$\begin{bmatrix} \frac{1}{Z'}f_x & 0 & -\frac{1}{Z'^2}f_xX' \\ 0 & \frac{1}{Z'}f_y & -\frac{1}{Z'^2}f_yY' \\ \end{bmatrix}$$ $$\begin{bmatrix} I & -P'^\land \end{bmatrix}$$ \\ \quad \\ ∂P′∂p′​∂Δδ∂P′​=−[Z′1​fx​0​0Z′1​fy​​−Z′21​fx​X′−Z′21​fy​Y′​][I​−P′∧​]∇e=∂p′∂I2​​[Z′1​fx​0​0Z′1​fy​​−Z′21​fx​X′−Z′21​fy​Y′​][I​−P′∧​]

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

直接法优缺点

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

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

后记

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