2．DSO模型

DSO采用最小化光度误差的优化算法，并且考虑了光度标定模型，其优化范围不是所有帧，而是由最近帧及其前几帧形成的滑动窗口。与现有的直接法不同，我们优化所有模型参数（包括相机内参，相机外参和逆深度值），但算法效率与间接法中常用的bundle adjustment算法相近。同时，我们采用和其他直接法相同的地图点表示方法，即三维点被表示为相应坐标下的逆深度值，因此只有一个自由度。

符号        在整篇论文中，粗体小写字母（x）表示向量，粗体大写字母（H）表示矩阵。 标量将由浅色的小写字母（t）表示，函数（包括图像）由浅色的大写字母（I）表示。 相机姿态被表示为变换矩阵，可将点从世界坐标系变换到相机坐标系中。线性化的姿态增量将表示为李代数元素，本文直接写为向量。我们进一步使用左乘法公式定义常用算子： ，即

 （1）

                                                   .                           

2.1标定

本文对图像形成过程进行全面建模。除了相机几何模型 ——其将3D点投影到2D图像上。还考虑了相机的光度模型——其将像素接收的真实世界能量映射为相应的强度值。需要注意的是，间接法由于提取的特征点和描述子通常对于光度变化具有不变性（高鲁棒性），因此在间接法中使用光度模型基本无益处从而被广泛忽略。

2.1.1相机几何标定

虽然本文使用了广角相机，但简单起见，我们采用针孔相机模型，只是在预处理步骤中去除径向失真，这也使得本文的方法与其他使用针孔相机模型的方法具有可比较性。在本文中，我们将用表示投影，用表示反投影，其中c表示相机的内参（对于针孔模型，内参为焦距和图像原点相对于光心成像点的纵横偏移量）。注意，类似于[1]，我们的方法可以扩展到其他相机模型，但会增加计算量。

2.1.2 相机光度标定

我们采用文献[2]中提及的光度模型，其考虑了非线性响应函数以及透镜衰减（晕影）。图3展示了一个TUM monoVO数据集中的光度模型参数。光度模型可由下式表示：   

（2）

其中和分别是帧i中的辐照度和观测到的像素强度，是曝光时间。测试视频的每个图像帧首先都会通过光度模型进行校正，然后再输入到视觉里程计模型中，即计算

（3）

在本文的其余部分，除非另有说明，否则始终采用经过光度校准的图像。

图3.光度标定  上图：图一中采用的相机的逆非线性响应函数和透镜衰减V.下图：一个包含室内和室外场景的视频序列的曝光时间（以毫秒为单位）。注意它涵盖了0.018到10.5ms，变化超过500倍。我们在光度标定模型中充分地考虑其影响，而不是将其视为未知噪声。

2.2 模型公式

参考帧中的点的光度误差是通过目标帧中相应点邻域上的光度值的加权SSD来计算。经过实验发现，按图4的模式（邻域含8个像素）进行计算时，可在计算效率、动态环境的鲁棒性和提供足够的信息之间给出了良好的折衷。注意，所谓的信息是指，在这样小的像素邻域上计算SSD，近似于计算中心像素点辐照度的一阶和二阶导数常数项（辐照度恒定初外）。计算公式如下，

  （4）

式中，是误差计算模式(即计算SSD的像素集合）; 是图像，是曝光时间;是Huber函数。此外，点可通过p点坐标和逆深度值求得，如下，

（5）

其中，

（6）

图4.误差计算模式  模式用于计算光度误差。忽略右下像素以计算SSE。注意，由于每个点都有一个变量（逆深度值），并且未正则化。为了便于仅对两个帧进行优化时所有模型参数都具有良好约束，我们需要。图19评估了误差计算模式对跟踪精度的影响。

为了使得我们的方法在曝光时间未知的情况下能够正常工作，我们引入光度仿射变换函数，函数为。注意，与以前大多数的公式[3,4]不同，该函数具指数形式，这既能防止函数变为负值，并且避免指数增加引起的数值问题。

另外，除了使用Huber函数，我们还使用与强度梯度有关的加权系数来提高鲁棒性，

 （7）

该系数随着强度梯度值的增大而减小。从概率的角度，可以解释为在投影点位置上添加微小的独立几何噪声。综上，光度误差取决于以下变量：（1）点的逆深度值，（2）相机内参c，（3）所涉及的帧的位姿，以及（4）它们光度仿射变换参数。

总光度误差由下式给出：

（8）

式中i遍历所有集合中的图像帧，p遍历图像帧i的所有地图点，j遍历所有帧中能够观测到点p的对应点。图5是一个简单的示意图：与间接法中的重投影误差模型不同，每个残差项与该点所属帧的位姿有相关性，即每个残差项取决于两个帧而不是一个帧。虽然这将非对角项添加到Hessian矩阵中表征位姿相关性的矩阵块中，但在应用舒尔补边缘化相应的地图点参数后，这并不影响Hessian的稀疏结构，因此可通过类似于间接法的优化方法求解模型参数。需要注意的是，两个帧位姿的雅可比矩阵和两帧的相对位姿呈线性关系。实际上，当计算Hessian和舒尔补时，利用上述的性质可以大大减少了由更多变量依赖性引起的附加计算。

图5.DSO模型的因子图  该示例包含四个关键帧和四个地图点;一个点属于KF1，两个点属于KF2，一个点属于KF4。每个光度误差项（在等式（4）中定义）取决于该点的所属帧（蓝色），观察到的该点的其他帧（红色）和该点的逆深度值（黑色）。此外，所有项都与全局相机内参c有关，但未标出。

如果曝光时间是已知的，光度仿射变换函数恒为零： 

(9)

若无光度标定模型时，我们设置和，因为在这种情况下，应该将曝光时间作为未知的模型参数。应该注意的是，如果和都包含噪声测量[7]，则乘法因子的最大似然估计是有偏差的。导致在的情况下a的漂移。虽然这通常对位姿估计影响不大，但是如果场景仅包含很少的纹理特征，则有可能引入偏差。

地图点表示        在本文提出的直接法中，地图点仅由一个参数（参考帧中的逆深度值）表示，而在间接法中地图由三个未知量表示。为了理解这种差异的原因，我们首先注意到，在这两种情况下，3D点实际上是在连续的，真实世界的三维表面上的离散样本。不同之处是该表面上的点的2D坐标。在间接法中，特征点被定义为（地图点投影到图像中）角点检测函数中响应较大的值。这意味着特征点所在的表面以及特征点位置都是未知的，并且需要估计。而在直接法中，地图点被简单地定义为从像素点出发的射线击中3D表面，因此只有一个未知数。这不仅减少参数数目，而且便于进行逆深度参数化，使得其在高斯框架中能更好地表示立体视觉中深度估计的不确定性，特别是对于远距离点[5]。

一致性        严格来说，所提出的直接法允许多次使用一些观测值（像素值），而有些观测值却不被使用。这是因为尽管我们试图在空间中均匀采样点来避免这种情况（参见第3.2节），但我们允许观测进行重叠，从而依赖于相同的像素值。尤其在纹理特征少的场景中，地图点必须从具有纹理的小区域中选择。然而，我们认为这种影响在实践中是可以忽略的，并且如果愿意的话，可以通过禁止（或降低权重）使用相同观测值来避免其影响。

2.3窗口优化算法

我们遵循Leutenegger等人的方法 [6]，使用滑动窗口优化算法最小化总光度误差（8），这在计算速度和灵活性之间给出了良好的折衷。

为了便于表示，我们将（1）中定义的运算符扩展到除SE（3）之外的所有待优化参数。我们将使用来表示所有优化变量，包括相机位姿，光度仿射变换参数，逆深度值和相机内参。在[6]中，边缘化关键帧和地图点时的参数ζ会固定其切线空间，任何关于该参数的更新都会在该空间累积。该参数命名为，更新值表示为。因此，当前状态估计由给出（图6）。

图6.滑动窗口优化算法        红色曲线表示由SE（3）中的非欧几里德相机位姿以及其他的欧几里德参数组成的参数空间。蓝线对应于的切线空间，在该空间上（1）累积X边缘化的先验信息，（2）计算高斯 - 牛顿步长δ。对于每个参数，一旦该参数边缘化，则固定其切线空间。注意，我们在符号中未区分所有参数，因为对于欧几里德参数切线空间和非欧几里得参数空间的操作是等效的。

高斯- 牛顿法        在高斯牛顿方法中，我们计算Hessian矩阵和常数项公式为     （10）

其中是包含权重的对角矩阵，是残差向量，
是r的雅可比矩阵。

注意，每个点需计算次残差。为了表达简单，下文中我们将仅考虑单个残差和其在雅可比矩阵的相关行。在优化和边缘化时，残差总是在当前状态估计值进行计算，即   

（11）

其中是计算残差时的状态变量。计算雅可比矩阵公式为

 （12）

它可以分解为

（13）

其中表示几何参数，表示光度参数。同时我们采用两个假设近似，以减少计算量。

首先，在计算和时，令，该方法被称为“First Estimate Jacobians”[6，7]，但需保证系统的一致性和防止线性化的误差累计。特别地，当目标函数存在非线性零空间（比如绝对位姿和尺度系数），线性化会累计误差，并最终破坏系统。然而在实验中，这种近似效果非常好，因为随着x增大，，变化很平滑。虽然变化不太平滑，但不影响零空间。同时我们使用中心差来计算整数位置处的图像导数，然后对其进行双线性插值。

其次，假设对于同一点的残差是定值，并且仅针对中心像素进行评估。这种近似在实践中效果非常好的，它显著减少了计算量，而且对精度基本没影响。

经过近似得到线性系统，计算更新值，并更新当前状态：

（14）

注意，由于采用First Estimate Jacobian 近似，乘法公式（在（12）中用代替）会产生完全相同的雅可比矩阵，因此是等效的。

在每个更新步骤之后，我们使用和来更新不边缘化的变量的。这包括所有逆深度值以及最新关键帧的位姿。 每次添加新的关键帧时，我们执行最多6次迭代，如果δ足够小就提前退出迭代过程。 我们发现由于初值与最优点相近，因此不需要采用LM算法加快收敛。

边缘化    当优化的变量太多时，将使用舒尔补来边缘化旧的变量。与[6]类似，我们删除任何影响H稀疏结构的残留项：当边缘化图像帧i时，我们首先边缘化中的所有点，以及在最后两个关键帧中没有观察到的点。但帧i中不属于的点将从系统中丢弃。

边缘化的过程如下：表示要边缘化的状态变量的光度误差。我们首先在当前状态下附近计算的高斯 - 牛顿近似， 

（15）

其中表示的当前值。丢弃常数c，，并且H，b如（10-13）中定义。这是关于x的二次函数，我们可以使用舒尔补来边缘化一个变量的子集。一个线性系统可写成

（16）

其中β表示我们要边缘化的变量，α表示我们保留的变量。使用舒尔补得到，

（17）

（18）

的光度误差可写为

（19）

这是关于x的二次函数，可以代替相应的非线性项，添加到总光度误差中，进行随后的优化和边缘化过程。注意，这需要的切线空间对于所有后续优化和边缘化中出现在中的所有变量保持相同。

参考文献：

【原文】Engel J, Koltun V, Cremers D. Direct Sparse Odometry[J]. 2016.

【1】 D.Caruso, J. Engel, and D. Cremers. Large-scale direct SLAM for omnidirectionalcameras. In International Conference onIntelligent Robot Systems (IROS), 2015. 4

【2】J.Engel, V. Usenko, and D. Cremers. A photometrically calibrated benchmark formonocular visual odometry. In arXivpreprint arXiv, 2016. 4, 10, 11,12, 16

【3】H.Jin, P. Favaro, and S. Soatto. A semi-direct approach to structure from motion.The Visual Computer, 19(6):377– 394,2003. 2, 5

【4】 J.Engel, J. Stueckler, and D. Cremers. Large-scale direct slam with stereocameras. In International Conference onIntelligent Robot Systems (IROS), 2015. 5

【5】 J.Civera, A. Davison, and J. Montiel. Inverse depth parametrization for monocularSLAM. Transactions on Robotics,24(5):932–945, 2008. 6

【6】 S.Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale. Keyframe-basedvisual–inertial odometry using nonlinear optimization. International Journal of Robotics Research, 34(3):314–334, 2015. 3, 6, 7, 13

【7】G.P. Huang, A. I. Mourikis, and S. I. Roumeliotis. A first-estimates Jacobian EKFfor improving SLAM consistency. In InternationalSymposium on Experimental Robotics, 2008. 6

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

【注】商业转载请联系刘富强（liufuqiang_robot@hotmail.com）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。