- 前言
- 直接法原理
- 直接法优缺点
- 后记
本篇是SLAM前端的最后一节，直接法。
直接法原理上篇提到的光流法，用于特征点的快速匹配，然后再通过重投影误差优化估计位姿。这是一种“两步走”的位姿估计法，即先匹配，再优化
直接法基于光度不变假设，将匹配步骤省略，直接构建重投影误差优化位姿。首先给出相机模型，假设空间中某点 P P P： P ′ = ( T P ) [ 1 : 3 ] p = 1 Z ′ K P ′ P'=(TP)_{[1:3]} \\ p= \frac{1}{Z'} KP' P′=(TP)[1:3]p=Z′1KP′
对于相邻两帧图像，如果上一帧 P P P与关键点 p ′ p' p′对应，与当前帧上的关键点 p p p对应，直接法认为两点理论上的灰度值应当相同： I 1 ( p ′ ) = I 2 ( p ) I_1(p')=I_2(p) I1(p′)=I2(p) 因此，就可以构建光度误差进行优化： e = I 1 ( p ) − I 2 ( p ′ ) min T ∑ i = 1 n = min T ∑ i = 1 n ∣ ∣ I 1 ( p i ) − I 2 ( p i ′ ) ∣ ∣ 2 2 e=I_1(p)-I_2(p') \\ \quad \\ \min_T \sum_{i=1}^n = \min_T \sum_{i=1}^n ||I_1(p_i) - I_2(p_i')||^2_2 \\ e=I1(p)−I2(p′)Tmini=1∑n=Tmini=1∑n∣∣I1(pi)−I2(pi′)∣∣22
那么问题就变成了如何优化上面的非线性最小二乘问题。首先还是求优化函数的梯度： e = I 1 ( p ) − I 2 ( 1 s K ( e δ ∧ P ) [ 1 : 3 ] ) = I 1 ( p ) − I 2 ( 1 s K P ′ ) ∇ e = − ∂ I 2 ∂ p ′ ∂ p ′ ∂ P ′ ∂ P ′ ∂ Δ δ e= I_1(p) - I_2(\frac {1}{s} K(e^{\delta^\land}P)_{[1:3]}) = I_1(p) - I_2(\frac {1}{s} KP') \\ \quad \\ \nabla e = -\frac {\partial I_2}{\partial p'} \frac {\partial p'}{\partial P'} \frac {\partial P'}{\partial \Delta \delta} \\ e=I1(p)−I2(s1K(eδ∧P)[1:3])=I1(p)−I2(s1KP′)∇e=−∂p′∂I2∂P′∂p′∂Δδ∂P′ 梯度的后两项，与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′1fx00Z′1fy−Z′21fxX′−Z′21fyY′][I−P′∧]∇e=∂p′∂I2[Z′1fx00Z′1fy−Z′21fxX′−Z′21fyY′][I−P′∧]
最后，使用高斯牛顿法或者LM法做非线性优化，就能得到位姿结果。
直接法优缺点优点：从以上步骤可以看出，实际上直接法并不依赖特征点，甚至可以随机取点做优化，非常省时；可以实现半稠密，稠密建图。
缺点：只适合小运动的位姿估计；光度不变假设实际很难满足；图像非凸，优化容易陷入局部最小。使用时通常采用更多的像素点进行优化。
后记下篇开始，进入代码学习，Ceres+g2o
