视觉SLAM学习打卡【8-1】-视觉里程计·直接法

• 本节直接法与上节特征点法，为视觉里程计估计位姿的两大主流方法。而在引出直接法前，先介绍光流法（二者均对灰度值 I 做文章）。
• 至此，前端VO总算结束了。学下来一个感受就是前几章的数学基础很重要，尤其是构建最小二乘的非线性优化（BA），几乎每种方法都有其一席之地。

一、光流法

• 特征点法需要同时提取两张图中的特征点，进而匹配描述子，最后进行位姿估计。而光流法只需要提取初始图像中的特征点，把匹配描述子换成光流跟踪，估计相机位姿仍需使用对极几何、PnP、ICP。
• 光流是一种描述像素随时间在图像之间运动的方法
• 代表有Lucas-Kanade光流（稀疏光流：只计算部分像素运动）& Horn-Schunck光流（稠密光流：计算全部像素运动）

（1）前提（实际中较难满足）

一个在 t 时刻，位于(x , y) 处的像素，它的灰度可以写成$I(x,y,t)$，设其在（t+dt）时刻，运动到（x+dx，y+dy）处，则其灰度变为$\mathbf{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)$

• 灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的.$$\mathbf{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)=\mathbf{I}(x,y,t)$$
• 一个w × w的窗口，一共有$w^2$个像素，这个窗口内所有的像素都具有同样的运动，且每个像素都满足灰度不变假设.

（2）理论推导

把（t+dt）时刻的像素值进行泰勒展开$$I\left(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t\right)\approx I\left(x,y,t\right)+\frac{\partial I}{\partial x}\mathrm{d}x+\frac{\partial I}{\partial y}\mathrm{d}y+\frac{\partial I}{\partial t}\mathrm{d}t$$由灰度不变假设可知，$$\frac{\partial \mathbf{I}}{\partial x}\mathrm{d}x+\frac{\partial \mathbf{I}}{\partial y}\mathrm{d}y+\frac{\partial \mathbf{I}}{\partial t}\mathrm{d}t=0$$移项、等式两边同除以dt得$$\frac{\partial \mathbf{I}}{\partial x}\frac{\mathrm{d}x}{\mathrm{d}t}+\frac{\partial \mathbf{I}}{\partial y}\frac{\mathrm{d}y}{\mathrm{d}t}=-\frac{\partial \mathbf{I}}{\partial t}$$其中，$\frac{\partial I}{\partial x}=I_x,\frac{\partial I}{\partial y}=I_y$，分别是图像灰度值在该点处x和y方向上的梯度；$\frac{\mathrm{d}x}{\mathrm{d}t}=u$，$\frac{\mathrm{d}y}{\mathrm{d}t}=v$，分别是像素在x轴和y轴上的运动速度；$\frac{\partial I}{\partial t}=I_t$为图像灰度值对时间变化量。
把上式写成矩阵形式 [ I x I y ] [ u v ] = − I t \begin{bmatrix}I_x&&I_y\end{bmatrix}\begin{bmatrix}u\\\\v\end{bmatrix}=-\boldsymbol I_t [Ix​​​Iy​​] ​uv​ ​=−It​该矩阵方程是一个欠定方程组，存在自由度。根据前提2，有$w^2$个方程，即变为超定方程。 [ I x I y ] k [ u v ] = − I t k , k = 1 , … , w 2 \begin{bmatrix}\boldsymbol{I}_x&\boldsymbol{I}_y\end{bmatrix}_k\begin{bmatrix}u\\\\v\end{bmatrix}=-\boldsymbol{I}_{tk},\quad k=1,\ldots,w^2 [Ix​​Iy​​]k​ ​uv​ ​=−Itk​,k=1,…,w2解为 A [ u v ] = − b . [ u v ] ∗ = − ( A T A ) − 1 A T b . \begin{aligned}&A\begin{bmatrix}u\\\\v\end{bmatrix}=-b.\\\\&\begin{bmatrix}u\\\\v\end{bmatrix}^*=-\left(A^\mathrm{T}A\right)^{-1}A^\mathrm{T}b.\end{aligned} ​A ​uv​ ​=−b. ​uv​ ​∗=−(ATA)−1ATb.​

（3）附：超定方程求解

• 超定方程组的数学推导主要涉及到最小二乘法的原理。
• 假设有超定方程组（方程组的个数多于未知数的个数）：$A\mathbf{x}=\mathbf{b}$，其中，A 是一个 m×n 的矩阵（m>n），x 是一个 n 维向量，b 是一个 m 维向量。
• 由于方程组的个数多于未知数的个数，该方程组通常没有精确解。因此，我们寻找一个近似解$x_{\mathrm{approx}}$，使得所有方程的残差平方和最小。残差定义为每个方程的观测值 b 与计算值$A{\mathbf{x}}_{\mathrm{approx}}$之差，即$$r=b-A{x}_{approx}$$我们的目标是找到$x_{\mathrm{approx}}$，使得残差平方和 S 最小$$S={\mathbf{r}}^T\mathbf{r}=(\mathbf{b}-A{\mathbf{x}}_{\text{approx}}{)}^T(\mathbf{b}-A{\mathbf{x}}_{\text{approx}})$$为了找到 S 的最小值，我们对 S 关于$x_{\mathrm{approx}}$求导，并令导数等于零，先展开 S$$S={\mathbf{b}}^T\mathbf{b}-{\mathbf{b}}^{T}A{\mathbf{x}}_{\text{approx}}-{\mathbf{x}}_{\text{approx}}^T{A}^T\mathbf{b}+{\mathbf{x}}_{\text{approx}}^T{A}^{T}A{\mathbf{x}}_{\text{approx}}$$求导
  $$\frac{\partial S}{\partial {\mathbf{x}}_{\text{approx}}}=-2A^T\mathbf{b}+2A^{T}A{\mathbf{x}}_{\text{approx}}$$（可参考：矩阵求导公式大全）
  令导数等于零，得到$$-2A^T\mathbf{b}+2A^{T}A{\mathbf{x}}_{\text{ approx }}=0$$整理后，得到正规方程（Normal Equation）$$A^{T}A{\mathbf{x}}_{\mathrm{approx}}=A^T\mathbf{b}$$这个方程是一个 n×n 的线性方程组，其解 $x_{\mathrm{approx}}$就是超定方程组的最小二乘解，即$$x_{approx}=(A^{T}A{)}^{-1}{A}^{T}b$$

二、直接法

不需要特征点（关键点），也不用匹配，只要求有像素梯度即可。特征点法和光流法分两步：先匹配后估计位姿；直接法两步一块走，以位姿为优化变量，求最优解（目标函数为运动前后像素光度误差，也算间接匹配）。

（1）理论推导

我们的目标是求第一个相机到第二个相机的相对位姿变换。我们以第一个相机为参照系，设第二个相机的旋转和平移为R , t（对应李群为T)。同时，两相机的内参相同，记为K。 p 1 = [ u v 1 ] 1 = 1 Z 1 K P , \boldsymbol{p}_1=\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}_1=\frac1{Z_1}KP, p1​= ​uv1​ ​1​=Z1​1​KP, p 2 = [ u v 1 ] 2 = 1 Z 2 K ( R P + t ) = 1 Z 2 K ( T P ) 1 : 3 \boldsymbol{p}_2=\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}_2=\frac{1}{Z_2}\boldsymbol{K}\left(\boldsymbol{R}P+t\right)=\frac{1}{Z_2}\boldsymbol{K}\left(\boldsymbol{T}P\right)_{1:3} p2​= ​uv1​ ​2​=Z2​1​K(RP+t)=Z2​1​K(TP)1:3​构建优化函数，直接法中我们要求的是最小化光度误差，优化的参数是相机位姿T；特征点法中求的是最小化重投影误差，优化的参数也是相机位姿T.$$e={\mathbf{I}}_1\left({\mathbf{p}}_1\right)-{\mathbf{I}}_2\left({\mathbf{p}}_2\right).$$$$\underset{T}{\min }J\left(\mathbf{T}\right)=\Vert e{\Vert }^2.$$$$\underset{\mathbf{T}}{\min }J\left(\mathbf{T}\right)=\sum _{i=1}^N{e}_i^{\mathrm{T}}{e}_i,e_i={\mathbf{I}}_1\left({\mathbf{p}}_{1,i}\right)-{\mathbf{I}}_2\left({\mathbf{p}}_{2,i}\right).$$（注：此处得J不代表雅可比矩阵，仅为误差2范数的一个表示符号）
定义两个中间变量，q是P在第二个相机坐标系下的坐标，而u是在第二个相机像素坐标系下的像素坐标。$$\begin{array}{c}q=TP,\\ u=\frac{1}{Z_2}Kq.\end{array}（此处u为上面的p2即像素坐标，q为其相机坐标）$$ 由于优化的参数是位姿，所以要对位姿求导。P1的像素点坐标是固定的常数（不随位姿变化而改变），所以上式对位姿求导后变为$$\frac{\partial e}{\partial T}=\frac{\partial {\mathbf{I}}_1\left({\mathbf{p}}_1\right)-\partial {\mathbf{I}}_2\left({\mathbf{p}}_2\right)}{\partial T}=\frac{\partial {\mathbf{I}}_1\left({\mathbf{p}}_1\right)-\partial {\mathbf{I}}_2\left(\mathbf{u}\right)}{\partial T}=-\frac{\partial {\mathbf{I}}_2\left(\mathbf{u}\right)}{\partial T}$$根据李代数的求导，我们选择左扰动模型（李群对加法不封闭，但李代数封闭），利用上面定义的中间变量，把u置换为位姿变化形式.（$\frac{\partial e}{\partial T}=\frac{\partial e}{\partial \delta \xi }$）$$\begin{array}{rl}\frac{\partial e\left(\mathbf{\xi }\oplus \delta \mathbf{\xi }\right)}{\partial \delta \mathbf{\xi }}& =-\frac{\partial {\mathbf{I}}_2\left(\frac{1}{Z_2}\mathbf{K}\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)}{\partial \mathbf{\delta }\mathbf{\xi }}\\ & \approx -\frac{\partial I_2\left(\frac{1}{Z_2}K\left(1+\delta {\xi }^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)P\right)}{\partial \mathbf{\delta }\mathbf{\xi }}\\ & =-\frac{\partial I_2\left(\frac{1}{Z_2}K\exp \left({\xi }^{\wedge }\right)P+\frac{1}{Z_2}K\delta {\xi }^{\wedge }\exp \left({\xi }^{\wedge }\right)P\right)}{\partial \delta \xi }\\ & =-\frac{\partial I_2\left(u+\frac{1}{Z_2}K\delta {\xi }^{\wedge }q\right)}{\partial \mathbf{\delta \xi }}\end{array}$$其中，≈处运用了指数的泰勒一阶展示，$\frac{1}{Z_2}K\delta {\xi }^{\wedge }q$是像素点位置变化量，$\delta \xi$/$\delta {\xi }^{\wedge }$均为扰动量的李代数形式（实际中不做详细区分）。
把$I_2\left(p_2\right)$在$p_2=u$处泰勒一阶展开$$I_2\left(u+\frac{1}{Z_2}K\delta {\xi }^{\wedge }q\right)\approx I_2\left(u\right)+\frac{\partial I_2}{\partial \delta \xi }\delta \xi$$接着上面的推导，可得$$\begin{array}{c}\frac{\partial e}{\partial \mathbf{\delta \xi }}=-\frac{\partial I_2\left(u+\frac{1}{Z_2}K\delta {\xi }^{\wedge }q\right)}{\partial \mathbf{\delta \xi }}\\ =-\frac{\partial (I_2\left(u\right)+\frac{\partial I_2}{\partial \delta \xi }\delta \xi )}{\partial \mathbf{\delta \xi }}\\ =0-\frac{\partial I_2}{\partial \delta \xi }\end{array}$$根据求导的链式法则$$\frac{\partial e}{\partial \mathbf{T}}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}$$关于$\frac{\partial e}{\partial T}$直接化为$I_2$的偏导，不用上述推导也可以想明白（因为以p1为参考，扰动在p2中）

• $\frac{\partial I_2}{\partial u}$是图像像素的梯度，同光流法中计算一致（u是像素坐标2），即$\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}=[I_x{I}_y{]}^T.$
• $\frac{\partial u}{\partial q}$即像素坐标系下坐标2对相机坐标系下三维坐标2的导数.$$su=kq$$ [ s u s v s ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] \begin{bmatrix}su\\sv\\s\end{bmatrix}=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}X\\Y\\Z\end{bmatrix} ​susvs​ ​= ​fx​00​0fy​0​cx​cy​1​ ​ ​XYZ​ ​利用第3行消去s，得$$u=f_x\frac{X}{Z}+c_{x}v=f_y\frac{Y}{Z}+c_y$$所以$\frac{\partial u}{\partial q}$= ∂ u ∂ q = [ ∂ u ∂ X ∂ u ∂ Y ∂ u ∂ Z ∂ v ∂ X ∂ v ∂ Y ∂ v ∂ Z ] = [ f x Z 0 − f x X Z 2 0 f y Z − f y Y Z 2 ] 2 × 3 \frac{\partial\boldsymbol{u}}{\partial\boldsymbol{q}}=\begin{bmatrix}\dfrac{\partial u}{\partial X}&\dfrac{\partial u}{\partial Y}&\dfrac{\partial u}{\partial Z}\\\dfrac{\partial v}{\partial X}&\dfrac{\partial v}{\partial Y}&\dfrac{\partial v}{\partial Z}\end{bmatrix}=\begin{bmatrix}\dfrac{f_x}Z&0&-\dfrac{f_xX}{Z^2}\\\\0&\dfrac{f_y}Z&-\dfrac{f_yY}{Z^2}\end{bmatrix}_{2\times3} ∂q∂u​= ​∂X∂u​∂X∂v​​∂Y∂u​∂Y∂v​​∂Z∂u​∂Z∂v​​ ​= ​Zfx​​0​0Zfy​​​−Z2fx​X​−Z2fy​Y​​ ​2×3​
• $\frac{\partial q}{\partial \delta \mathbf{\xi }}$即变换后的三维点q = Tp对位姿变换δξ的导数，这里具体过程同第四讲扰动模型求导。$$\frac{\partial \mathbf{T}\mathbf{p}}{\partial \delta \mathbf{\xi }}=\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}=[\mathbf{I},-{\mathbf{q}}^{\wedge }{]}_{3\times 6}$$由于后两项只与三维点q 有关，而与图像无关，我们经常把它合并在一起$$\frac{\partial u}{\partial \delta \mathbf{\xi }}={\left[\begin{array}{cccccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}& -\frac{f_{x}XY}{Z^2}& f_x+\frac{f_x{X}^2}{Z^2}& -\frac{f_{x}Y}{Z}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}& -f_y-\frac{f_y{Y}^2}{Z^2}& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z}\end{array}\right]}_{2\times 6}$$终于，得到了残差函数e关于参数位姿T的导数，也就是我们需要的雅可比矩阵：$$\mathbf{J}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}.$$

（2）直接法的优缺点

优点如下：

• 不需要特征点，只要有图像梯度即可.
• 省区计算特征点、描述子时间.
• 可以构建半稠密、稠密地图（特征点无法做到）.
• 一般而言，直接法省去了光流计算时间（光流计算时间>计算并匹配描述子的时间）.

缺点如下：

• 直接发完全依靠梯度搜索，若相机运动过快 / 初始值选取不当 / 图像具有非凸性，进入局部最小值（实际中可以引入金字塔）.
• 单个像素误差太大，计算图像块，少数服从多数。因此，在选点较少时，结果变差.
• 比较前后光度值是建立在灰度不变假设的前提下，但现实中很明显做不到.