前言
直接法是视觉里程计另一主要分支，它与特征点法有很大不同。随着SVO、LSD-SLAM等直接法SLAM方案的流行，直接法本身也得到越来越多的关注。特征点法与直接究竟谁更好一些，是近年视觉里程计研究领域一个非常有趣的问题。本讲，我们将介绍直接法的原理，并利用g2o实现基于直接法的视觉里程计。
1. 直接法的引出
尽管特征点法在视觉里程计中占据主流地位，研究者们认识它至少有以下几个缺点：
- 关键点的提取与描述子的计算非常耗时。实践当中，SIFT目前在CPU上是无法实时计算的，而ORB也需要近20毫秒的计算。如果整个SLAM以30毫秒/帧的速度运行，那么一大半时间都花在计算特征点上。
- 使用特征点时，忽略了除特征点以外的所有信息。一张图像有几十万个像素，而特征点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
- 相机有时会运动到特征缺失的地方，往往这些地方都没有什么明显的纹理信息。例如，有时我们会面对一堵白墙，或者一个空荡荡的走廓。这些场景下特征点数量会明显减少，我们可能找不到足够的匹配点来计算相机运动。
我们看到使用特征点确实存在一些问题。针对这些缺点，也存在若干种可行的方法：
- 只计算关键点，不计算描述子。同时，使用光流法（Optical Flow）来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算需要一定时间；
- 只计算关键点，不计算描述子。同时，使用直接法来计算特征点在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加简单。
- 既不计算关键点、也不计算描述子——根据像素来直接计算相机运动。
第一种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时仍使用PnP或ICP算法。而在，后两个方法中，我们会根据图像的像素信息来计算相机运动，它们称为直接法。
使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差（Reprojection error）来优化相机运动。在这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我们为何要对特征进行匹配或跟踪的理由。而在直接法中，我们最小化的不再是重投影误差，而是测量误差（Phometric error）。
直接法是本讲介绍的重点。它的存在就是为了克服特征点法的上述缺点（虽然它会引入另一些问题）。直接法直接根据像素亮度信息，估计相机的运动，可以完全不用计算关键点和描述子。于是，直接法既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像特征），直接法就能工作。根据使用像素的数量，直接法分为稀疏、稠密和半稠密三种，具有恢复稠密结构的能力。相比于特征点法通常只能重构稀疏特征点，直接法和稠密重建有更紧密的联系。
历史上，虽然早期也有一些对直接法的使用，但直到RGB-D相机出现后，人们才发现直接法对RGB-D相机，进而对单目相机，都是行之有效的方法。随着一些使用直接法的开源项目的出现（如SVO、LSD-SLAM等），它们逐渐地走上主流舞台，成为视觉里程计算法中重要的一部分。
2. 直接法数学推导
直接法和光流非常相似，它们都是基于灰度不变假设的：
灰度不变假设：同一个空间点的像素灰度，在各个图像中是固定不变的。
灰度不变假设是一个很强的假设，实际当中很可能不成立。事实上，由于物体的材质不同，像素会出现高光和阴影部分；有时，相机会自动调整曝光参数，使得图像整体变亮或变暗。这些时候灰度不变假设都是不成立的，因此直接法/光流的结果也不一定可靠。不过，暂且让我们认为该假设成立，从而看看如何计算相机的运动。我们先介绍直接法的原理，然后使用g2o实现直接法。
考虑某个空间点 $P$ 和两个时刻的相机。$P$ 的世界坐标为 $[X,Y,Z]$，它在两个相机上成像，其非齐次像素坐标为 $\mathbf{p}_1, \mathbf{p}_2$。我们的目标是求第一个相机到第二个相机的相对位姿变换。设第一个相机旋转、平移为 $\mathbf{I}, \mathbf{0}$，第二个相机外参为 $\mathbf{R}, \mathbf{t}$（李代数为$\mathbf{\xi}$）。同时，两相机的内参相同，记为$\mathbf{K}$。为清楚起见，我们列写完整的投影方程：
\[{\mathbf{p}_1} = {\left[ \begin{array}{l}
u\\
v
\end{array} \right]_1} = \mathbf{D} \frac{1}{Z_1} \mathbf{KP} \]
\[{\mathbf{p}_2} = {\left[ \begin{array}{l}
u\\
v
\end{array} \right]_2} = \mathbf{D}\frac{1}{Z_2} \mathbf{K}\left( {\mathbf{RP} +\mathbf{t}} \right) = \mathbf{D}\frac{1}{Z_2} \mathbf{K} \exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}\]
其中 $Z_1,Z_2$ 是 $P$ 在两个相机坐标系下的深度坐标值。$\mathbf{D}$ 为齐次坐标到非齐次坐标的转换矩阵：
\[\begin{equation}
\mathbf{D} = \left[ {\begin{array}{*{20}{c}}
1&0&0\\
0&1&0
\end{array}} \right]
\end{equation}\]
在直接法中，我们同样是解一个优化问题，但这个优化最小化的不是重投影误差，而是测量误差（Photometric Error），也就是 $P$ 的两个像的亮度误差：
\[\begin{equation}
e = {\mathbf{I}_1}\left( {{\mathbf{p}_1}} \right) - {\mathbf{I}_2}\left( {{\mathbf{p}_2}} \right)
\end{equation}\]
我们优化该误差的二范数：
\[\begin{equation}
\mathop {\min }\limits_{\mathbf{\xi}} J\left( \mathbf{\xi} \right) = { e^T} e
\end{equation}\]
能够做这种优化的理由即灰度不变假设。在直接法中，我们假设一个空间点在各个视角下，成像的灰度是不变的。同样的，这依然是一个很强的假设，而且与光流法十分相似。实际当中，我们有许多个（比如$N$个）空间点$P_i$，那么，整个相机位姿估计问题变为：
\[\begin{equation}\label{key}
\mathop {\min }\limits_{\mathbf{\xi}} J\left( \mathbf{\xi} \right) = \sum\limits_{i = 1}^N {e_i^T{e_i}}, \quad {e_i} = { \mathbf{I}_1}\left( {{\mathbf{p}_{1,i}}} \right) - {\mathbf{I}_2}\left( {{ \mathbf{p}_{2,i}}} \right)
\end{equation}\]
为了求解这个优化问题，我们关心误差$e$是如何随着相机位姿$\mathbf{\xi}$变化的，需要分析它们的导数关系。因此，使用李代数上的扰动模型。我们给$\exp (\mathbf{\xi})$左乘一个小扰动$\exp( \delta \mathbf{\xi} )$，得：
\[\begin{equation}\begin{array}{ll}
e\left( { \mathbf{\xi} \oplus \delta \mathbf{\xi} } \right) &= { \mathbf{I} _1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP} } \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK}\exp \left( {\delta {\mathbf{\xi} ^ \wedge }} \right)\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}} \right)\\
& \approx {\mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK} \left( {1 + \delta {\mathbf{\xi} ^ \wedge }} \right)\exp \left( {{ \mathbf{\xi} ^ \wedge }} \right) \mathbf{P} } \right)\\
&= {\mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK}\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P} + \frac{1}{{{Z_2}}} \mathbf{DK} \delta { \mathbf{\xi} ^ \wedge }\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}} \right)
\end{array}\end{equation}\]
记
\[\begin{equation}\begin{array}{ll}
\mathbf{q} &= \delta \mathbf{\xi} ^\wedge \exp \left( {{ \mathbf{\xi} ^ \wedge }} \right) \mathbf{P} \\
\mathbf{u} &= \frac{1}{{{Z_2}}} \mathbf{DK} \mathbf{q}
\end{array}\end{equation}\]
$\mathbf{q}$ 即 $P$ 在第二个相机坐标系下的坐标，而$\mathbf{u}$为它的像素坐标。于是，利用一阶泰勒展开，有：
\[\begin{equation}\begin{array}{ll}
e \left( { \mathbf{\xi} \oplus \delta \mathbf{\xi} } \right) &= {\mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK} \exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P} + \mathbf{u}} \right)\\
& \approx { \mathbf{I}_1}\left( {\frac{1}{{{Z_1}}} \mathbf{DKP}} \right) - {\mathbf{I}_2}\left( {\frac{1}{{{Z_2}}} \mathbf{DK}\exp \left( {{\mathbf{\xi} ^ \wedge }} \right) \mathbf{P}} \right) - \frac{{\partial { \mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \mathbf{q}}}\frac{{\partial \mathbf{q}}}{{\partial \delta \mathbf{\xi} }}\delta \mathbf{\xi} \\
&= e\left( \mathbf{\xi} \right) - \frac{{\partial {\mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \mathbf{q}}}\frac{{\partial \mathbf{q}}}{{\partial \delta \mathbf{\xi} }}\delta \mathbf{\xi}
\end{array}\end{equation}\]
我们看到，一阶导数由于链式法则分成了三项，而这三项都是容易计算的：
- $ \partial \mathbf{I}_2 / \partial \mathbf{u} $ 为$\mathbf{u}$处的像素梯度；
- $ \partial \mathbf{u} / \partial \mathbf{q} $ 为投影方程关于相机坐标系下的三维点的导数。我们把投影方程展开，为： \[\begin{equation}
u = \frac{{{f_x}X + {c_x}}}{Z},v = \frac{{{f_y}Y + {c_y}}}{Z}
\end{equation}\]于是导数为：
\[\begin{equation}
\frac{{\partial \mathbf{u}}}{{\partial \mathbf{q}}} = \left[ {\begin{array}{*{20}{c}}
{\frac{{\partial u}}{{\partial X}}}&{\frac{{\partial u}}{{\partial Y}}}&{\frac{{\partial u}}{{\partial Z}}}\\
{\frac{{\partial v}}{{\partial X}}}&{\frac{{\partial v}}{{\partial Y}}}&{\frac{{\partial v}}{{\partial Z}}}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
{\frac{{{f_x}}}{{\rm{Z}}}}&0&{ - \frac{{{f_x}X}}{{{Z^2}}}}\\
0&{\frac{{{f_y}}}{Z}}&{ - \frac{{{f_y}Y}}{{{Z^2}}}}
\end{array}} \right]
\end{equation}\] - ${\partial \mathbf{q}}/{\partial \delta \mathbf{\xi} }$为变换后的三维点对变换的导数，这在李代数章节已经介绍过了：
\[\begin{equation}
\frac{{\partial \mathbf{q}}}{{\partial \delta \mathbf{\xi} }} = \left[ { \mathbf{I}, - {\mathbf{q}^ \wedge }} \right]
\end{equation}\]
在实践中，由于后两项只与三维点$\mathbf{q}$有关，而与图像无关，我们经常把它合并在一起：
\[\begin{equation}
\frac{{\partial \mathbf{u}}}{{\partial \delta \mathbf{\xi} }} = \left[ {\begin{array}{*{20}{c}}
{\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]
\end{equation}\]
于是，现在我们推导了误差相对于李代数的雅可比矩阵：
\[ \begin{equation}
\label{eq:jacobianofDirect}
\mathbf{J} = - \frac{{\partial { \mathbf{I}_2}}}{{\partial \mathbf{u}}}\frac{{\partial \mathbf{u}}}{{\partial \delta \mathbf{\xi} }}
\end{equation}\]
对于$N$个点的问题，我们可以用这种方法计算优化问题的雅可比，然后使用G-N或L-M计算增量，迭代求解。例如，在G-N方法下，增量$\delta {\mathbf{\xi} ^*} $的计算方式为：
\[\begin{equation}
\left( {\sum\limits_{i = 1}^N { \mathbf{J}_i^T \mathbf{J}_i} } \right)\delta {\mathbf{\xi} ^*} = - \sum\limits_{i = 1}^N {{\mathbf{J}_i^T}{e_i}}
\end{equation}\]
至此，我们推导了直接法估计相机位姿的整个流程，下面我们讲讲直接法是如何使用的。
3. 直接法的使用
在我们上面的推导中，$P$是一个已知位置的空间点，它是怎么来的呢？在RGB-D相机下，我们可以把任意像素反投影到三维空间，然后投影到下一个图像中。如果在单目相机中，我们也可以使用已经估计好位置的特征点（虽然是特征点，但直接法里是可以避免计算描述子的）。根据$P$的来源，我们可以把直接法进行分类：
- $P$来自于稀疏特征点，我们称之为稀疏直接法。通常我们使用数百个特征点，并且会像L-K光流那样，假设它周围像素也是不变的。这种稀疏直接法速度不必计算描述子，并且只使用数百个像素，因此速度最快，但只能计算稀疏的重构。
- $P$来自部分像素。我们看到式（\ref{eq:jacobianofDirect}）中，如果像素梯度为零，整一项雅可比就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。这称之为半稠密（Semi-Dense）的直接法，可以重构一个半稠密结构。
- $P$为所有像素，称为稠密直接法。稠密重构需要计算所有像素（一般几十万至几百万个），因此多数不能在现有的 CPU上实时计算，需要GPU的加速。
可以看到，从稀疏到稠密重构，都可以用直接法来计算。它们的计算量是逐渐增长的。稀疏方法可以快速地求解相机位姿，而稠密方法可以建立完整地图。具体使用哪种方法，需要视机器人的应用环境而定。
4. 使用g2o实现直接法
现在，我们来演示如何使用稀疏的直接法。由于我们不涉及GPU编程，稠密的直接法就省略掉了。同时，为了保持程序简单，我们使用RGB-D数据而非单目数据，这样可以省略掉单目的深度恢复部分（这个很麻烦，我们之后另讲）。
求解直接法最后等价于求解一个优化问题，因此我们可以使用g2o这样的通用优化库帮助我们求解。在使用g2o之前，需要把直接法抽象成一个图优化问题。显然，直接法是由以下顶点和边组成的：
- 优化变量为一个相机位姿，因此需要一个位姿顶点。由于我们在推导中使用了李代数，故程序中使用李代数表达的$SE(3)$位姿顶点，如g2o/types/types\_six\_dof\_expmap.h中的“VertexSE3Expmap”。
- 误差项为一个像素的测量误差。由于整个优化过程中$\mathbf{I}_1(\mathbf{p}_1)$保持不变，我们可以把它当成一个固定的预设值，从而调整相机位姿，使$\mathbf{I}_2(\mathbf{p}_2)$接近这个值。于是，这种边只连接一个顶点，为 一元边。由于g2o中本身没有计算测量误差的边，我们需要自己定义一种新的边。
整个直接法图优化问题是由一个相机位姿顶点与许多条一元边组成。如果使用稀疏的直接法，那我们大约会有几百至几千条这样的边；稠密直接法则会有几十万条边。优化问题对应的线性方程是计算李代数增量（6$\times$1），本身规模不大，所以主要的计算时间会花费在每条边的误差与雅可比的计算上。下面的实验中，我们先来定义一种专门用于计算直接法的边，然后，使用该边构建图优化问题并求解之。
实验工程位于：https://github.com/gaoxiang12/slambook 中的 ch8/directMethod中。
工程目录下的g2o\_types.h中给出了自定义边的声明，g2o\_types.cpp中给出实现，我们在CMakeLists.txt里将它们编译了成一个库。
1 #ifndef G2O_TYPES_DIRECT_H_ 2 #define G2O_TYPES_DIRECT_H_ 3 4 // 定义应用于直接法的g2o边 5 #include <Eigen/Geometry> 6 #include <g2o/core/base_unary_edge.h> 7 #include <g2o/types/sba/types_six_dof_expmap.h> 8 #include <opencv2/core/core.hpp> 9 namespace g2o 10 { 11 // project a 3d point into an image plane, the error is photometric error 12 // an unary edge with one vertex SE3Expmap (the pose of camera) 13 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 14 { 15 public: 16 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 18 EdgeSE3ProjectDirect() {} 19 20 EdgeSE3ProjectDirect( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image ) 21 : x_world_(point), fx_(fx), fy_(fy), cx_(cx), cy_(cy), image_(image) 22 {} 23 24 virtual void computeError(); 25 26 // plus in manifold 27 virtual void linearizeOplus( ); 28 29 // dummy read and write functions because we don't care... 30 virtual bool read( std::istream& in ); 31 virtual bool write( std::ostream& out ) const ; 32 33 protected: 34 float getPixelValue( float x, float y ); 35 public: 36 // 3d point in world frame 37 Eigen::Vector3d x_world_; 38 // Pinhole camera intrinsics 39 float cx_=0, cy_=0, fx_=0, fy_=0; 40 // the reference image 41 cv::Mat* image_=nullptr; 42 }; 43 } // namespace g2o 44 #endif // G@O_TYPES_DIRECT_H_