解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 8 讲 视觉里程计2

在第七讲中，作者介绍了基于特征点估计相机运动的方法。特征点法存在以下几个缺点：

• 1.关键点的提取和描述子的计算非常耗时间；
• 2.特征点是提取出来的一些代表性的点，但是忽略了特征以外的所有点；
• 3.对于纹理信息很弱的地方，特征点法往往会失败。

于是乎一些改进的思路就出现了：

• 1.保留特征点，但是只计算关键点，去掉耗时的描述子计算过程，然后使用光流法跟踪特征点的运动；
• 2.只计算关键点，不计算描述子，使用直接发来计算特征点在下一时刻图像中的位置；
• 3.不计算关键点，也不计算描述子，只根据像素灰度差异直接计算出相机运动。

解读

下面我们来看看光流法：

光流法

光流法描述了像素在图像中的运动。计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。

先看一下稀疏光流法的代表：Lucas-Kanade光流。

在LK光流中，认为来自相机的图像是随时间变化的，图像可以看作是时间的函数：$I(t)$，那么在$t$时刻，位于$(x,y)$处的像素，它的灰度值可以写成
$$I(x,y,t)\text{\hspace{1em}(0)}$$假设空间中有一个固定的点，它在相机中投影的坐标就是$(x,y)$，当在$t+dt$时刻的时候，它的投影坐标变成了$(x+dx,y+dy)$，那么依然采用（0）式子来表示灰度值，那么此时
$$I(x+dx,y+dy,t+dt)\text{\hspace{1em}(1)}$$理论上来说，空间中的点的像素灰度值，在各个图像中是固定不变的（灰度不变假设）。而实际情况下这是很难成立的，但是当我们运动很微小，时间也很短的情况下，空间中的同一个点，在前后两帧中投影的灰度值可能变化的很小。所以在灰度不变假设下，可以得到（0）式和（1）式相等，即就是：
$$I(x,y,t)=I(x+dx,y+dy,t+dt)\text{\hspace{1em}(2)}$$我觉得（2）式应该挺好理解的吧，你可以自己细致的思考一下！

现在将（2）式右边进行泰勒展开，保留一阶项，得：
$$I(x+dx,y+dy,t+dt)\approx I(x,y,z)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt\text{\hspace{1em}(3)}$$上式是多元函数的泰勒展开，网上公式一大堆，你要是忘记了，再去搜索看一下。

因为有灰度不变假设，所以
$$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0\text{\hspace{1em}(4)}$$将（4）式两边都除以$dt$，得到：
$$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}\text{\hspace{1em}(5)}$$其中：
　　$\frac{dx}{dt},\frac{dy}{dt}$为像素点在$x,y$轴的运动速度，把它们记作$u,v$
　　$\frac{\partial I}{\partial x},\frac{\partial I}{\partial y}$为像素点在$x,y$方向的像素变化梯度，记作$I_x,I_y$
　　$\frac{\partial I}{\partial t}$为图像点像素随时间的变化量，记作$I_t$.

将（５）式写成向量的形式：
$$[I_x,I_y]\left[\begin{array}{c}u\\ v\end{array}\right]=-I_t\text{\hspace{1em}(6)}$$上式有两个未知量，但是只有一个方程，显然方程数量还不够。

在LK光流法中，假设某一个窗口内的像素都具有相同的运动（这又是一个很强的假设，是一个很理论的情况）。对于大小为$w\ast w$大小的窗口，可以列出$w^2$个（6）式这样的方程，组成的超定线性方程如下：
$$\left[\begin{array}{c}[I_x,I_y{]}_1\\ ⋮\\ [I_x,I_y{]}_k\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-\left[\begin{array}{c}{I}_{t1}\\ ⋮\\ I_{tk}\end{array}\right]\text{\hspace{1em}(7)}$$对于（7）式这个超定线性方程，传统的解法就可以求出最小二乘解：
$${\left[\begin{array}{c}u\\ v\end{array}\right]}^{\ast }=-({\mathbf{A}}^T\mathbf{A}{)}^{-1}{\mathbf{A}}^T\mathbf{b}$$下面我对（7）式中的变量求法，解释一下：

对于像素点$I_1$当它运动到$I_2$位置的时候：
$$I_x=\frac{I_3-I_1}{x_3-x_1},I_y=\frac{I_4-I_1}{x_4-x_1}$$$$I_t=\frac{I_2-I_1}{\Delta t}$$我感觉你可能会有一些疑惑，明明不是说假设灰度不变吗？！那么为啥还要求解$I_2-I_1$呢？这里我们推导出最后的结论是站在灰度不变假设下的，但是实际上灰度是发生变化了的，这其中是不是让你觉得很玄幻，竟然还能这样操作！但是人家设计的算法就是可以工作！

跟踪到特征点之后，就可以用第七讲的内容求取相机的运动了！

直接法

直接法其实也是一个基于优化的方法，我觉得基于优化的方法，应该是比较好理解的。

我先直观的给你介绍一下直接法，对于上图有一个空间点$P$（参考坐标系是图像$I_1$），理论上它在$I_1$中的投影是$p_1$，它在$I_2$中的投影是$p_2$。这里依然沿用灰度不变假设，那么$p_1,p_2$的灰度值应该是一样的。现在有一个初始的$R,t$，那么将$P$按照投影关系和初始的$R,t$投影到$I_2$图像中，按说如果$R,t$是真实的正确值，那么投影之后的图像位置就是$p_2$，但是$R,t$如果给定初始是错误的，那么投影点和$p_2$之间就会有偏差，那么也会产生灰度值偏差，然后继续调整$R,t$直到灰度误差最小。这个过程就是直接法的思想，当然不只选择一个点，肯定是很多个点一起求灰度误差最小。

知道了核心思想，在推导过程就会明白很多！推导过程就都是前面已经学过的内容，我在这里就不再演示了，大家直接看书理解就行了。

直接法的讨论

对于已知位置的空间点$P$：

• 1.如果$P$来源于稀疏的关键点，那么就称为稀疏直接法；
• 2.如果$P$来源于部分关像素，那么就是半稠密法。
• 3.如果$P$来源于所有像素，那么就是稠密法。

实践

稀疏直接法

对于稀疏直接法，大家看代码学习，感觉没什么难理解的地方。如果编译源码时，你碰到了关于g2o相关内容的报错，请将代码修改如下：

    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的

    //DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    std::unique_ptr<DirectBlock::LinearSolverType> linearSolver(new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ());

    //DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    std::unique_ptr<DirectBlock> solver_ptr( new DirectBlock(std::move(linearSolver)));

    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );

半稠密直接法

对于半稠密直接法，其实代码和稀疏直接法区别不大，主要的区别就是提取关键点办成了，搜索像素点灰度值梯度变化比较大的点，然后将其设置为跟踪点。以下就是半稠密直接法提取灰度值梯度变化明显的点的代码。

if ( index ==0 )
{
    // select the pixels with high gradiants
    for ( int x=10; x<gray.cols-10; x++ )
        for ( int y=10; y<gray.rows-10; y++ )
        {
            Eigen::Vector2d delta (
                    gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1],
                    gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
            );
            if ( delta.norm() < 50 ) 
                continue;
            ushort d = depth.ptr<ushort> (y)[x];
            if ( d==0 )
                continue;
            Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
            float grayscale = float ( gray.ptr<uchar> (y) [x] );
            measurements.push_back ( Measurement ( p3d, grayscale ) );
        }
    prev_color = color.clone();
    cout<<"add total "<<measurements.size()<<" measurements."<<endl;
    continue;
}

直接法优缺点总结

优点：

• 1.可以省去计算特征点、描述子的时间；
• 2.只要求有像素梯度即可，不需要特征点。可以在特征缺失的场景下使用；
• 3.可以构建半稠密甚至是稠密的地图。
  缺点：
• 1.非凸性。当图像是强烈的非凸函数时，优化算法容易进入极小值；
• 2.单个像素没有区分度。
• 3.灰度值不变是很强的假设。直接法对光照的敏感度很高，整体灰度变化会破坏灰度不变假设，使得算法失败。

伟人之所以伟大，是因为他与别人共处逆境时，别人失去了信心，他却下决心实现自己的目标。