Direct Sparse Odometry：一种花里胡哨的解读 (2024)

首先需要声明的是，这个总结参考了很多优秀SLAM工作者、科研人员的博客、文章。

其中有自己的理解，但源于初学者的知识局限，还处于知识接受阶段，所以大多都不是原创性的，很多图片、描述也会复用他人的成果，可以视作一个个人笔记和总结，供学习备忘。

但是我一直以为自己搞明白一个东西并能够讲清楚对于自己理解问题本身是有很大的帮助的，相信很多相关从业人员也有相同的感受。

在下文中延续【自主机器人位姿估计数学基础https://zhuanlan.zhihu.com/p/524804452】中的【术语：核心作用】的模式来对论文、代码中的一些专业术语的作用、来源进行相关的描述。

Introduction

Reference

• 论文部分
• DSO论文原文
• 泡泡机器人公开课E57 DSO初探
• 深蓝学院觉罗雅威DSO论文带读
• 史上最全DSO学习资料
• DSO光度标定
• DSO总结
• 光度标定中vignette相关
  
• 代码部分
• DSO原始代码
• 泡泡机器人E64 DSO原理刘海伟
• DSO代码注释版 （帮助非常大，PPT也是从大佬这里截取的，感谢！）
• https://blog.csdn.net/gbz3300255/article/details/110236977
• https://www.cnblogs.com/yepeichu/p/13917061.html
• https://www.cnblogs.com/JingeTU/p/8329780.html
  
• 数学推导部分
• https://tongling916.github.io/tags/#DSO（非常详细、漂亮）
  
• SLAM相关基础知识
• 学习SLAM需要哪些预备知识？ - 知乎 https://www.zhihu.com/question/35186064

Funny

话不多说，我们进入主题。

Structure

文章的整体结构可以参考左侧的目录 ，但是本身markdown编写时好几级标题，知乎只支持二级标题，所以见谅。主要分为了两大部分：

• 整体思考(写在前面避免没有人看)
• 论文分析
• 论文引言部分
• Direct Sparse Model细节
• Windowed Optimization
• Visual Odometry Front-end
• 代码解读
• 代码整体结构
• 非核心部分：utils & IOWrapper
• 核心数据结构
• 前端处理
• 前后端接口
• 后端优化
• 算法整体pipeline

Overall Thinking

个人感觉的话，理解这种 SLAM 框架或者说是 Odometry 的核心，主要还是有以下几个重点:

• 理解前后端的作用;其实不管是 SLAM 系统，还是计算机软件，个人理解前后端分离的一部分目的在于模块化，把复杂繁琐的步骤打包处理是我们在解决问题时经常使用的方法。
• 视觉 SLAM 系统中的前端主要的任务在于：利用运动中的相机在不同时刻获取到的图像帧，通过特征匹配，求解出相邻域之间的相机位姿变换，并完成图像帧之间的融合，重建地图
• 但是，这时得到的还是一个局部优化的粗略结果，后续还需要通过后端优化，利用更多约束条件对其进行微调，以获得精确的位姿和地图
• 所以体现在 DSO 当中的话，它的前端主要完成了：帧和点的选择、帧和点之间的关系判别 (一个点在某个帧中是否 visible)、参数的初始化、帧和点的边缘化等
• 而在后端中，则主要是各种各样的参数批量求导优化;这里面其实又涉及到了很多细节以及比较巧妙的 trick

• 理解核心的数据结构；很多时候的编程、代码阅读等都是以基础的数据结构作为依托的， 我们先将一个问题的大致处理流程确定，然后就会涉及到各种细节问题；如果以面向对象 编程的思维去思考的话，就需要把处理的对象进行解构，把它数学化、原子化为计算机能够处理的一个个变量，体现在 DSO 中的话，则有以下几个关键的数据结构：
• 帧的处理：在视觉 SLAM 或者里程计中，基础的数据应该一张张的图像，或者说更基础一些是一个个的像素点;
• 点的处理：在 DSO 中一个点的状态包括了它对应的帧、投影的残差、逆深度等信息；也存在不同类型的点如未参与计算的未成熟点等

• 理解残差的组成及各分量的求导;个人理解我们本质上是在求解一个优化问题，也就是找到一组最优的变量使得误差函数/损失函数最小化或者能量函数最大化；所以残差是优化的基本量；为了批量求解我们采取了矩阵和向量的多变量形式；由此多维情况中一阶导数 Jabobian 矩阵和二阶矩阵 Hessian 矩阵，体现在 DSO 中的话，残差的组成包括了：

关键帧状态
六自由度运动位姿 Ti
两个描述光度的曝光参数 a,b

地图点状态:该点在主导帧的逆深度 dpi

• 理解算法的整体流程和数据流；在 DSO 中主要涉及了以下几个流程步骤
• 初始化
• 跟踪
• 优化

Paper Analysis

这篇论文最重要的一个贡献就是提出了一种【稀疏直接法的单目视觉里程计】也即【direct sparse monocular visual odometry】；我们逐个介绍

Introduction

Odometry：运动估计位置

https://www.bilibili.com/read/cv7732328/

所谓的里程计指的就是移动机器人利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。该方法被用在许多机器人系统来估计机器人相对于初始位置移动的距离 。

上面这篇文章提到了有几种里程计

• 轮式里程计、视觉里程计、视觉惯性里程计……

一个视觉里程计的示意图如下图所示：

输入：

多帧传感器运动情况下的序贯数据如相机的 n 帧图像： I_i,i=1,2,\cdots,n

传感器camera的初始位姿： c_0

传感器camera到载具vehicle的坐标变换 T_{cv}

载具到世界world的坐标变换 T_{vw}

中间表示：

由序贯数据恢复的多帧之间传感器的运动 T_i,i=1,2,\cdots,n

输出

具体的载具在时刻 m 下世界坐标系 w 的计算过程可以是：

p_{w,m} = T_{vw}T_{cv}\Pi_{i=1}^{m} T_i\cdot c_0

Monocular Visual：更易获得

所谓的视觉里程计指的是：通过移动机器人上搭载的单个或多个相机的连续拍摄图像作为输入，从而增量式地估算移动机器人的运动状态。而目前对于视觉里程计或者视觉SLAM的研究中单目也占了很大一部分。

个人理解单目里程计/SLAM算法的研究研究的意义主要在于：

• 单目相机更容易获得且成本更低；目前很多移动设备端都配有单目相机，而像iPad 2021 Pro这种配备LiDAR的极少，搭载深度相机、双目相机的方案也在移动厂商consumption市场的成本中被折衷掉了
• 单目相机本身不提供深度信息，所以相当于是一个很苛刻的环境；简单而言就是从降低成本的角度来说，我们尽可能的压缩成本，把更多的精力放到提升算法上面，而算法仅有单次或多次的成本，而不作为生产资料本身计入成本；有点类似负重沙袋或者是带着隔氧面罩练习跑步。

但是也需要知道的是，在2016年DSO只是作为odometry而非完整的SLAM框架被提出。完整的SLAM还包含了很多其他模块。

Direct：无中间表示，最小化光度误差

在本文提出之前，以特征点法为代表的间接法占主导；作者主要比较了直接法、间接法的共同点和区别，共同点在于：

都可以形式化的表示为以含噪声的测量 \boldsymbol{Y} 为输入，从而估计未知的位姿 \boldsymbol{X} ；通常用最大似然估计如 \boldsymbol{X}^* :=argmax_{\boldsymbol{X}}P(\boldsymbol{Y}|\boldsymbol{X})

而两者的处理流程区别在于：

• 间接法(分步进行)
• 首先将raw sensor measurements预处理后生成intermediate representation从而先解决一部分问题(先找几何对应关系)
• 该步通常以提取特征点为主但是也有其他的方法
• 如以dense,regularized optical flow来建立raw sensor measurements与intermediate representation的correspondences
• 如提取并匹配如line-segment,curve-segment等geometric primitives
• 然后intermediate values作为 \boldsymbol{Y} 来通过概率模型估计geometry和camera motion(再用几何对应关系求解问题)
• 直接法
• 不进行预处理，直接使用raw sensor measurements；在相机中即为在特定time period和特定direction接收到的光

处理方法的不同导致了它们需要优化的误差也不同

• 间接法
• geometric error几何误差
• 直接法
• photometric error光度误差
• 也可能处理geometric error，比如说深度相机、雷达的情况下

具体的区别可由图所示：

Sparse：去除几何先验

而后作者比较了稠密法和稀疏法的区别

• 稀疏法：use and reconstruct only a selected set of independent poins，如角点corners
• 稠密法：use and reconstruct all pixels in the 2D image domain
• 半稠密：use and reconstruct a(largely connected and well-constrained) subset

稠密和稀疏在geometry prior方面也有所区别

• 稀疏法
• no notion of neighborhood 没有邻域这个概念
• geometry parameters(keypoint positions) are conditionally independent given the camera poses & intrinsics几何参数独立于相机位姿、内参
• 稠密法
• exploit the connectedness of the used image region to formulate a geometry prior利用区域中的连接性来形成几何先验

Direct+Sparse：特定的适用范围

所以根据直接/间接、稠密/稀疏的组合，有以下四种，它们的特点和例子分别如表所示

作者也在introduction/motivation部分提及了选取从直接和稀疏两方面的思想来源。

Direct motivation

其中direct部分的主要来源：

• 特征点法对于现成的商品级相机更加robust
• 如提供的automatic exposure changes,non-linear response functions(gamma correction /white-balancing), lens attenuation (vignetting), debayering artefacts, or even strong geometric distortions caused by a rolling shutter
• 然而很多相机是为CV算法提供数据，而不是作为消费品
• 提供了complete sensor model完整的模型，这意味着我们对相机本身更加了解，标定参数也非黑箱模型
• capture data in a way that best serves the processing algorithms
• 这个情况下很多参数就不是未知的了，也就是说为CV算法开发的相机提供的精确模型使得直接根据像素点的信息来估计位姿变得更加精确
• One of the main benefits of a direct formulation is that it does not require a point to be recognizable by itself, thereby allowing for a more finely grained geometry representation (pixelwise inverse depth)
• Furthermore, we can sample from across all available data – including edges and weak intensity variations – generating a more complete mode and lending more robustness in sparsely textured environments

Sparse motivation

而Sparse部分的主要来源：

• 几何先验构建稠密地图困难
• 几何先验有时候会引入bias

Direct Sparse Model

Calibration：图像的标定包含几何和光度两部分

The direct approach comprehensively models the image formation process

也就是说直接法对图像形成过程完全建模，而相机模型中考虑两方面

• geometric camera model：projects a 3D point onto the 2D image
• 几何模型描述了三维点到二维图像的投影
• 涉及相机内参 f_x,f_y,c_x,c_y
• photometric camera model：maps real-world energy received by a pixel on the sensor to the respective intensity value
• 光度模型描述了真实世界中能量到图像像素强度的转换
• 涉及响应函数 a,b

因此涉及到了Geometric Camera Calibration和Photometric Camera Calibration两方面

Geometric Calibration：针孔相机模型去畸变

对于几何模型校正

• 针孔模型pinhole camera model
• 径向畸变在预处理阶段除去radial distortion

但是实际上在代码中，体现了几种相机模型，可以参考https://github.com/JakobEngel/dso中的README.md；而在代码中则体现在Undistort.cpp中

Photometric Calibration：真实世界到像素强度畸变的修正

https://www.zhihu.com/question/53080536

人眼看到的亮度/传感器接收的值和现实中的值是不一样的；光的本质是电磁波。

对于光度模型校正，参考【J. Engel, V. Usenko, and D. Cremers. A photometrically calibrated benchmark for monocular visual odometry. In arXiv preprint arXiv, 2016. 4, 10, 11, 12, 16】中的模型，主要包括

• non-linear response function： G:\mathbb{R}\to[0,255] ， G 将现实中的值映射到传感器的值
• lens attenuation(vignetting)： V:\Omega\to[0,1]

将它们结合后得到 I_i(x) = G(t_iV(\boldsymbol{x})B_i(\boldsymbol{x}))

于是得到模型后我们就可以对像素的intensity进行光度校正 I_i'(x) :=t_iB_i(\boldsymbol{x}) = \frac{G^{-1}(I_i(\boldsymbol{x}))}{V(\boldsymbol{x})}

Photometric Error：待优化(最小化)的误差项

其中整体待优化的误差项为： E_{\mathbf{p} j}:=\sum_{\mathbf{p} \in \mathcal{N}_{\mathbf{p}}} w_{\mathbf{p}}\left\|\left(I_{j}\left[\mathbf{p}^{\prime}\right]-b_{j}\right)-\frac{t_{j} e^{a_{j}}}{t_{i} e^{a_{i}}}\left(I_{i}[\mathbf{p}]-b_{i}\right)\right\|_{\gamma} 而这个式子中值得注意的有以下几点：

• \mathbf{p} \in \mathcal{N}_{\mathbf{p}} 表示了target帧中邻域中的一些点，也就是说并不是优化单个点的误差，而是该邻域内多个点的加权平均
• 上述的权重由 w_{p} 标识，是gradient- depend weighting，图解释了这样选择权重的原因
• 而相应的residual pattern如图所示，尝试了多种pattern后选择了pattern8
• \boldsymbol{p}'= \Pi_{\boldsymbol{c}}(\boldsymbol{R}\Pi_{\boldsymbol{c}}^{-1}(\boldsymbol{p},d_{\boldsymbol{p}})+\boldsymbol{t})
• \mathbf{p'} 标识了投影后的点
• 相机坐标系中的点 \mathbf{p} 通过 \Pi _{\mathbf{c}}^{-1} 逆投影到世界坐标系
• 然后施加旋转平移 \mathbf{R,t} 后到达世界坐标系的另一个位置
• 然后再通过 \Pi _{\mathbf{c}} 投影到相机坐标系
• d_{\boldsymbol{p}} 逆深度表示了相对于该帧的值，更有规律可循
• 范数用Huber Norm而非L2 Norm表示，并非简单的平方相加开根号，增加了核函数
• 式子中的 b,t,a 都属于光度修正部分的参数，而指数运算的主要作用在于
• e 指数模式规避了乘除以及非负性

更加形象化一点表达如图所示：

Full Photometric Error：所有的误差放到一起后优化

全部的误差求和后得到如图？所示的模型，可以看到我们整个优化的变量包含了

• 关键帧状态
• 六自由度运动位姿 \mathbf{T}_i
• 该点在主导帧的逆深度 d_{\mathbf{p}_i}
• 地图点状态
• 该点在主导帧的逆深度 d_{\mathbf{p}_i}

Factor Graph：误差来源的图示

其中对右下角的图进行的解释：

• 关键帧：KF1、KF2、KF3、KF4
• 空间点：Pt1、Pt2、Pt3、Pt4
• 蓝色的线代表：帧中包含了哪些点
• 红色的线代表：点在哪些帧中被观测到了

Windowed Optimization

Optimization on active key frames window：保留关键帧，避免过大计算量

滑动窗口的思想主要在于：

• 只要求解出一部分，就将其抛掉，避免矩阵维数过高
• 但是会存在问题consistency
• 零空间的问题：非齐次方程的解=齐次方程通解+特解；坍缩成一个点，自由度被干掉，零空间消失
• 因此使用了FEJ手段，只在第一次使用！！

Gauss-Newton Optimization：用$J^TJ$近似$H$

First-Estimates Jacobian：维护零空间的能观性

Marginalization：去除不需要的点/帧

Visual Odometry Front-End

在DSO的前端中，主要做了以下几件事情

简言之就是

• 在不断到来的由帧组成的视频流中，我们需要根据这些数据来组成整体的误差项 E_{photo} ，从而提供给后端优化
• 哪些帧和帧中的哪些点是能够被我们利用的
• 某个点在哪些帧中是visible的
• 对于整体误差项 E_{photo} 中的新参数的初始化
• 为了维持整体的稀疏性(原因在前文提到过，稀疏导致了几何先验少，优化方便)，我们需要对不需要使用的帧和点进行边缘化marginalization

而主要涉及的就是两个数据结构帧和点的管理，在代码中则体现为struct FrameHessian和struct PointHessian两个结构体，这一部分我们留到代码部分讲解。

Frame Management：对于帧的处理

1. Initial frame tracking
2. Keyframe creation
3. Keyframe marginalization
   

在帧管理中，主要的观点在于：

• 维护一个包含 N_f=7 个关键帧Keyframe的滑动窗口
• 当新的一帧到来时对其进行初始化
• 因为为了减少数据量，并非所有的帧都需要追踪，所以我们决定哪些帧是关键帧，也即Keyframe Creation，主要考虑了三种情况
• 视角变化的情况field of view changes
• 相机平移导致的遮挡和非遮挡Camera translation causes occlusions and disocclusions
• 相机的曝光时间显著改变camera exposure time changes significantly
• 有一些不需要的帧需要对其进行边缘化
  
• 总是保持最新两帧always keep the latest two keyframes ( I_1 and I_2 ).
  
• 在 I_1 中能够观测到的点占该帧所有点不超过5%的话，这一帧就被边缘化Frames with less than 5% of their points visible in I_1 are marginalized
  
• 如果超出7个帧活跃的话，我们首先计算各个帧的一个score也即 s(I_i) ，得分最高的帧被边缘化
• 边缘化的时候
• 先移除关键帧中的点
• 再移除关键帧本身

图像金字塔
在第一帧到达后，需要用图像金字塔进行相应的alignment。具体可以参考：https://blog.csdn.net/zaishuiyifangxym/article/details/90167880中的讲解。
而在DSO中图像金字塔的使用https://blog.csdn.net/weixin_43424002/article/details/108308149

Point Management：对于帧中的点、空间点的处理

在点管理中，主要的观点在于

• 首先在 N_f=7 个关键帧中，每帧都要选取 N_p=2000 个点
• 在选取完点后，并不会马上进行优化，而是先生成coarse depth value
• 在选取的 N_f\cdot N_p = 7 \cdot2000 个点中进一步筛选得到 N_p=2000 个点，这这些点在空间中和active frames中均匀分布

整体的步骤则为：

1. Candidate point selection

选取图像上的点，需要满足

图像中均匀分布well-distributed in the image

相对于周围的像素而言梯度较大have sufficiently high image gradient magnitude with respect to their immediate surroundings.。处理的方法参见【region- adaptive gradient threshold】

2.candidate point tracking：利用【discrete search along the epipolar line】方法

3.cadidate point activation

• 为了保持点的数量，真正优化的点要再选择一下
• 把以前的点投影的新一帧，选择那些距离投影过来的点远的点

Code Analysis

Overview

DSO代码整体结构如下图所示，其中./CMakeLists.txt、./cmake、./thirdparty主要是工程编译规则、相关用到的本地库的寻址、作者用到的对应版本第三方库的提供等；核心部分在./src文件夹下。

各个模块的作用如表所示，核心代码在于FullSystem和OptimizationBackend两部分。我们先介绍非核心的两部分，即util和IOWrapper两部分。

Util

IOWrapper

IOWrapper部分主要调用了pangolin来实现DSO代码流程的可视化UI

SampleOutputWrapper.h中继承了Output3DWrapper.h中Output3DWrapper类，其中成员函数如关键帧的发布：

virtual void publishKeyframes( std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) override { for(FrameHessian* f : frames) { printf("OUT: KF %d (%s) (id %d, tme %f): %d active, %d marginalized, %d immature points. CameraToWorld:\n", f->frameID, final ? "final" : "non-final", f->shell->incoming_id, f->shell->timestamp, (int)f->pointHessians.size(), (int)f->pointHessiansMarginalized.size(), (int)f->immaturePoints.size()); std::cout << f->shell->camToWorld.matrix3x4() << "\n"; int maxWrite = 5; for(PointHessian* p : f->pointHessians) { printf("OUT: Example Point x=%.1f, y=%.1f, idepth=%f, idepth std.dev. %f, %d inlier-residuals\n", p->u, p->v, p->idepth_scaled, sqrt(1.0f / p->idepth_hessian), p->numGoodResiduals ); maxWrite--; if(maxWrite==0) break; } } }

Core Data Structure

• 高翔博士对数据结构的总结
• 刘海伟对于数据结构的总结
• 前后端分离，互相含有指针可以映射和复用，类似于传参的效果
• 多个对象在同步时不至于相互冲突、加锁，以至于前后端同步性很强
• 后端中EFXX相关的是EnergyFunction的缩写
• 比如说在前端HessianBlocks.h中在点管理中的基础数据结构struct PointHessian就通过一个指针EFPoint* efPoint;链接到了后端的EnergyFunctionalStructs.h模块中的class EFPoint

struct FrameHessian：帧

最核心的数据结构就是 FrameHessian 以及它所包含的各类 Point，FrameHessian 具体包含的 内容如下:

1. PointHessian:所有活跃点的信息。所谓活跃点，是指它们在相机的视野中，其残差项仍在参与优化部分的计算
2. pointHessiansMarginalized:已经边缘化的地图点
3. pointHessiansOut:判断为外点的地图点
4. immaturePoints:未成熟的地图点

这些数据结构，贯穿于系统的整个工作流程中，具体流程是:

1. 工作过程中，系统会维护 5 到 7 个关键帧组成的滑动窗口，每个关键帧对应一个 FrameHessian
2. 新来一帧图像时，会产生新的地图点，但此时地图点深度是未知的，它就成为 immaturePoints(未成熟的地图点)，随着后面接收到的图像越来越多，地图点会不断被观测，如果最终 收敛，它就会变成已知准确深度的地图点，即 PointHessian(活跃点)
3. 为了构建优化问题，每个关键帧中的 PointHessian 可以往其他帧投影，投影会形成残差 项，放在 PointHessian::residuals 中，所有残差项相加即构成损失函数，可以进行优化求解
4. 如果由于遮挡等导致 PointHessian 无法完成投影，那么他就属于外点，即 PointHessiansOut (判断为外点的地图点)
5. 随着系统的运行，滑动窗口需要不断添加新的帧，移除老的帧，在移除老的帧的时候需要 进行边缘化，即把删掉的关键帧中的 PointHessian 转成先验的地图点，即 pointHessiansMarginal- ized(已经边缘化的地图点)
6. 后端优化部分和前段具有相同的数据结构，通过互相持有对方指针和前端通信

帧的数据结构中：主要的成员变量构成了描述该帧的primitive：

主要的成员函数则设置了一些对成员变量的相应操作：

而且需要注意的是，帧和帧之间的相对位姿变换需要有一些预计算的步骤，定义在了struct FrameFramePrecalc中。

struct CalibHessian：标定

主要包括了两部分：

几何标定：相机内参 f_x,f_y,c_x,c_y

光度标定：伽马响应函数中涉及到的 B

这一部分的程序变量较少，也比较简单，值得注意的有：

I_{i}^{\prime}(\mathbf{x}):=t_{i} B_{i}(\mathbf{x})=\frac{G^{-1}\left(I_{i}(\mathbf{x})\right)}{V(\mathbf{x})} ​​

而对于内参参数的设置，有两种方式

• inline void setValue(const VecC &value)：通过直接的内参值来设置
• inline void setValueScaled(const VecC &value_scaled)：内参有一定的缩放

而响应函数也有相应的求导

• EIGEN_STRONG_INLINE float getBGradOnly(float color)：响应函数的导数
• EIGEN_STRONG_INLINE float getBInvGradOnly(float color)：响应函数逆的导数

其中getBGradOnly()定义为：

//* 响应函数的导数 EIGEN_STRONG_INLINE float getBGradOnly(float color) { int c = color+0.5f; if(c<5) c=5; if(c>250) c=250; return B[c+1]-B[c]; }

getBInvGradOnly()定义为：

//* 响应函数逆的导数 EIGEN_STRONG_INLINE float getBInvGradOnly(float color) { int c = color+0.5f; if(c<5) c=5; if(c>250) c=250; return Binv[c+1]-Binv[c]; }

struct PointHessian：图像点

struct Pnt：三维空间点

具体定义如下，参考龚益群注释版本

struct Pnt{public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // index in jacobian. never changes (actually, there is no reason why). float u,v; // idepth / isgood / energy during optimization. float idepth; //!< 该点对应参考帧的逆深度 bool isGood; //!< 点在新图像内, 相机前, 像素值有穷则好 Vec2f energy; //!< [0]残差的平方, [1]正则化项(逆深度减一的平方) // (UenergyPhotometric, energyRegularizer)  bool isGood_new; float idepth_new; //!< 该点在新的一帧(当前帧)上的逆深度 Vec2f energy_new; //!< 迭代计算的新的能量 float iR; //!< 逆深度的期望值 float iRSumNum; //!< 子点逆深度信息矩阵之和 float lastHessian; //!< 逆深度的Hessian, 即协方差, dd*dd float lastHessian_new; //!< 新一次迭代的协方差 // max stepsize for idepth (corresponding to max. movement in pixel-space). float maxstep; //!< 逆深度增加的最大步长 // idx (x+y*w) of closest point one pyramid level above. int parent; //!< 上一层中该点的父节点 (距离最近的)的id float parentDist; //!< 上一层中与父节点的距离 // idx (x+y*w) of up to 10 nearest points in pixel space. int neighbours[10]; //!< 图像中离该点最近的10个点 float neighboursDist[10]; //!< 最近10个点的距离 float my_type; //!< 第0层提取是1, 2, 4, 对应d, 2d, 4d, 其它层是1 float outlierTH; //!< 外点阈值};

class ImmaturePoint：未成熟点

所谓的未成熟点是什么意思呢？在单目SLAM中地图点开始时是深度未知的，所以我们要沿着极线搜索，从而确定每个未成熟点的逆深度及其变化范围，在未成熟点的逆深度收敛则可以确定其三维坐标，形成正常的地图点，也就是struct PointHessian

未成熟点有几种状态，定义在了enum ImmaturePointStatus中

enum ImmaturePointStatus { IPS_GOOD=0, // traced well and good // 搜索区间超出图像, 尺度变化太大, 两次残差都大于阈值, 不再搜索 IPS_OOB, // OOB: end tracking & marginalize! // 第一次残差大于阈值, 外点 IPS_OUTLIER, // energy too high: if happens again: outlier! // 搜索区间太短，但是没激活 IPS_SKIPPED, // traced well and good (but not actually traced). // 梯度和极线夹角太大 IPS_BADCONDITION, // not traced because of bad condition. IPS_UNINITIALIZED}; // not even traced once.

而未成熟点则包含了类似于struct PointHessian中的一些成员变量，有所不同的是，增加了一些变量

在搜索中，主要用到了以下几个函数

• ImmaturePointStatus traceOn()
• double linearizeResidual()
• float getdPixdd()
• float calcResidual()

class ImageAndExposure：辐照度和曝光时间

辐照度值 B 和曝光时间 t 的定义，主要实现如下所示。可见其将曝光时间默认设置为了1

float* image; // irradiance. between 0 and 256 int w,h; // width and height; double timestamp; float exposure_time; // exposure time in ms. //! 论文中曝光时间t * 辐照度B inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_) { image = new float[w*h]; //这表示图像,666, 标定后的辐照度 exposure_time=1; }

而DSO还支持了将曝光时间赋给other和深度拷贝两种方式：

// 曝光时间赋值给other inline void copyMetaTo(ImageAndExposure &other) { other.exposure_time = exposure_time; } // 深度拷贝 inline ImageAndExposure* getDeepCopy() { ImageAndExposure* img = new ImageAndExposure(w,h,timestamp); img->exposure_time = exposure_time; memcpy(img->image, image, w*h*sizeof(float)); return img; }

class PointFrameResidual

Front-End Processing

FrameHessian::makeImages()：构建图像金字塔

该函数计算了各层金字塔图像的像素值和梯度；首先为每一层创建图像值, 和图像梯度的存储空间，且dI指向了原始图像也即第0层

// 每一层创建图像值, 和图像梯度的存储空间 for(int i=0;i<pyrLevelsUsed;i++) { dIp[i] = new Eigen::Vector3f[wG[i]*hG[i]]; absSquaredGrad[i] = new float[wG[i]*hG[i]]; } dI = dIp[0]; // 原始图像

用二维数组存储了像素的color字段，用wl和hl指示该层图像的大小；遍历每一层金字塔，其中的关键步骤就在于

• 各层金字塔图像像素值的计算：像素4合1, 生成金字塔
• 各层金字塔图像梯度的计算

分别如下，图像像素值计算：

// 像素4合1, 生成金字塔 for(int y=0;y<hl;y++) for(int x=0;x<wl;x++) { dI_l[x + y*wl][0] = 0.25f * (dI_lm[2*x + 2*y*wlm1][0] + dI_lm[2*x+1 + 2*y*wlm1][0] + dI_lm[2*x + 2*y*wlm1+wlm1][0] + dI_lm[2*x+1 + 2*y*wlm1+wlm1][0]); }

图像梯度计算：

for(int idx=wl;idx < wl*(hl-1);idx++) // 第二行开始 { float dx = 0.5f*(dI_l[idx+1][0] - dI_l[idx-1][0]); float dy = 0.5f*(dI_l[idx+wl][0] - dI_l[idx-wl][0]); if(!std::isfinite(dx)) dx=0; if(!std::isfinite(dy)) dy=0; dI_l[idx][1] = dx; // 梯度 dI_l[idx][2] = dy; dabs_l[idx] = dx*dx+dy*dy; // 梯度平方 if(setting_gammaWeightsPixelSelect==1 && HCalib!=0) { //! 乘上响应函数, 变换回正常的颜色, 因为光度矫正时 I = G^-1(I) / V(x) float gw = HCalib->getBGradOnly((float)(dI_l[idx][0])); dabs_l[idx] *= gw*gw; // convert to gradient of original color space (before removing response). } }

PixelSelector::PixelSelector()：像素点的选取

这一部分主要涉及到了论文中提到的像素点的选取，也即论文中涉及到的8个点的pattern；刘海伟对其的解释在于：pattern中扣掉一个点主要在于加速，8位对齐，操作方便。

这一部分主要在Pixselector.h、Pixselector2.h、Pixselector2.cpp中；而涉及到的关键函数有：

FullSystem::trackNewCoarse

使用确定的运动模型对新来的一帧进行跟踪, 得到位姿和光度参；假设了不同的运动，具体来说就是

• 5种运动模型：匀速、倍速、半速、零速以及没有运动
• 26×N种旋转情况：四元数的26种旋转情况，N个比较小的角度

具体可以参考博客https://blog.csdn.net/wubaobao1993/article/details/104022702，讲解的很详细了。

FullSystem::traceNewCoarse

利用新的帧 fh 对关键帧中的ImmaturePoint进行更新

1. 根据逆深度范围得到极线搜索的范围
2. 计算图像梯度和极线夹角的大小，如果太大则误差会很大
3. 在极线上按照一定步长进行搜索能量最小的位置，和大于设置半径(2)的第二小的位置，后/前作为质量，越大越好
4. 沿着极线进行GN优化，直到增量足够小
5. 根据搜索得到的投影位置计算新的逆深度范围

Interface between Front and Back

前后端的接口是void deliverTrackedFrame(FrameHessian* fh, bool needKF)

Back-end Processing

Data Structure

class EFResidual

主要的变量如下，可以看出包括了相应的数据、索引信息、帧的host与target之分、Jacobian值、状态值等

PointFrameResidual* data; int hostIDX, targetIDX; //!< 残差对应的 host 和 Target ID号 EFPoint* point; //!< 残差点 EFFrame* host; //!< 主 EFFrame* target; //!< 目标 int idxInAll; //!< 所有残差中的id RawResidualJacobian* J; //!< 用来计算jacob, res值 VecNRf res_toZeroF; //!< 更新delta后的线性残差 Vec8f JpJdF; //!< 逆深度Jaco和位姿+光度Jaco的Hessian // status. bool isLinearized; //!< 计算完成res_toZeroF // if residual is not OOB & not OUTLIER & should be used during accumulations bool isActiveAndIsGoodNEW; //!< 激活的还可以参与优化

class EFPoint

主要的变量如下，可以看出包括了原始数据、先验信息矩阵、索引ID、该点的所有残差、各种Hessian、点的状态等

PointHessian* data; //!< PointHessian数据 float priorF; //!< 逆深度先验信息矩阵, 初始化之后的有 float deltaF; //!< 当前逆深度和线性化处的差, 没有使用FEJ, 就是0 // constant info (never changes in-between). int idxInPoints; //!< 当前点在EFFrame中id EFFrame* host; // contains all residuals. std::vector<EFResidual*> residualsAll; //!< 该点的所有残差 float bdSumF; //!< 当前残差 + 边缘化先验残差 float HdiF; //!< 逆深度hessian的逆, 协方差 float Hdd_accLF; //!< 边缘化, 逆深度的hessian VecCf Hcd_accLF; //!< 边缘化, 逆深度和内参的hessian float bd_accLF; //!< 边缘化, J逆深度*残差 float Hdd_accAF; //!< 正常逆深度的hessian VecCf Hcd_accAF; //!< 正常逆深度和内参的hessian float bd_accAF; //!< 正常 J逆深度*残差 EFPointStatus stateFlag; //!< 点的状态

class EFFrame

//! 位姿 0-5, 光度ab 6-7 Vec8 prior; //!< 位姿只有第一帧有先验 prior hessian (diagonal) Vec8 delta_prior; //!< 相对于先验的增量 // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) Vec8 delta; //!< 相对于线性化点位姿, 光度的增量 // state - state_zero. std::vector<EFPoint*> points; //!< 帧上所有点 FrameHessian* data; //!< 对应FrameHessian数据 int idx; //!< 在能量函数中帧id // idx in frames. int frameID; //!< 所有历史帧ID

class EnergyFunctional

在残差的计算中，class EnergyFunctional总领全局、协调各方。

struct RawResidualJacobian：各种Jacobian

\left[\begin{array}{cccc} \text { pattern res } & \operatorname{Vec}(8,1) & \text { resF } & \\ \text { posese }(3) & \operatorname{Vec}(6,1) & J p d x i[2] & \left(\frac{\partial p_{x}}{\partial \xi}, \frac{\partial p_{y}}{\partial \xi}\right) \\ \text { camera para } & \operatorname{Vec}(4,1) & J p d c[2] & \left(\frac{\partial p_{x}}{\partial c}, \frac{\partial p_{y}}{\partial c}\right) \\ \text { point depth } & \operatorname{Vec}(2,1) & J p d d & \left(\frac{\partial p_{x}}{\partial d_{p}}\right) \\ \text { bright gradient } & \operatorname{Vec}(8,1) & J I d x[2] & \frac{\partial I}{\partial x y} \\ \text { bright a,b } & \operatorname{Vec}(8,1) & J a b F[2] & \left(\frac{\partial r}{\partial a}, \frac{\partial r}{\partial b}\right) \\ \text { aux } & \operatorname{Mat}(2,2) & J I d x 2 & \frac{\partial I}{\partial p} \frac{\partial I}{\partial p} \\ \text { aux } & \operatorname{Mat}(2,2) & J a b J I d x & \frac{\partial r}{\partial a b} \frac{\partial I}{\partial p} \\ \text { aux } & \operatorname{Mat}(2,2) & J a b 2 & \frac{\partial r}{\partial a b} \frac{\partial r}{\partial a b} \end{array}\right]

struct RawResidualJacobian{ EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // ================== new structure: save independently =============. VecNRf resF; //!< 每个patch的8个残差 // the two rows of d[x,y]/d[xi]. Vec6f Jpdxi[2]; // 2x6 //!< 点对位姿：旋转3+平移3 // the two rows of d[x,y]/d[C]. VecCf Jpdc[2]; // 2x4 //!< 点对相机参数：fx fy cx cy // the two rows of d[x,y]/d[idepth]. Vec2f Jpdd; // 2x1 //!< 点对逆深度：idepth // the two columns of d[r]/d[x,y]. VecNRf JIdx[2]; // 9x2 //!< patch光度误差对点, 8×2：x,y // = the two columns of d[r] / d[ab] VecNRf JabF[2]; // 9x2 //!< patch光度误差对光度仿射， 8x2：a,b //!< 对应的小的hessian // = JIdx^T * JIdx (inner product). Only as a shorthand. Mat22f JIdx2; // 2x2 // = Jab^T * JIdx (inner product). Only as a shorthand. Mat22f JabJIdx; // 2x2 // = Jab^T * Jab (inner product). Only as a shorthand. Mat22f Jab2; // 2x2};

Residual Computation

residual的分类可以从几个维度进行，划分后提取公因子变得方便；而在residual的计算中，作者在代码中也采用了一些中间变量或者说shorthand便于复用。

• 按照Jacobian的类型划分，把Jacobian分成两部分分别求解
• 几何参数geo
• 光度参数photo
• 按照层级
• 全局属性： c 也即相机的内参，标定的比较好的话，不作为优化变量
• 帧属性： \xi_i,\xi_j,a_i,b_i,a_j,b_j ，
• 点属性： d_{k_i} ，pattern中的8个点其实用的都是一个 d
• 按照参数的来源
• I_i
• I_j

Main Pipeline

main_dso_pangolin.cpp

程序入口在main_dso_pangolin.cpp处，在实际运行程序时，我们在命令行输入了以下的命令行参数：

bin/dso_dataset \ files=XXXXX/sequence_XX/images.zip \ calib=XXXXX/sequence_XX/camera.txt \ gamma=XXXXX/sequence_XX/pcalib.txt \ vignette=XXXXX/sequence_XX/vignette.png \ preset=0 \ mode=0

进入主函数后，主要的处理步骤包括了

1. 参数解析void parseArgument(char* arg)
2. 调用settings.cpp进行一些参数的设置(参数含义可以参考DSO的GitHub项目中的README.md)
3. 调用util/DatasetReader.h下的class ImageFolderReader读取图像、校正文件等
4. 新建一个Fullsystem的object后设置光度校正fullSystem->setGammaFunction()
5. 新建一个以pangonlin库为基本开发组件的IOWrap::PangolinDSOViewer* viewer
6. 对于fullSystem可能存在的一些异常情况进行处理
7. 对于处理的帧数、时间进行格式化输出
8. 进入到FullSystem.cpp，入口是void FullSystem:: addActiveFrame( ImageAndExposure* image, int id )

FullSystem Explaination

FullSystem主要包括了几大部分，其中FullSystem.h包括了这几部所有成员变量、成员函数的声明；

其中主pipeline中几个函数的定义和作用如图和表所示：

addActiveFrame

我们的最主要的输入是在dataset/image.zip中的一系列图像。addActiveFrame则是对图像处理的入口。整体而言，该函数分为以下几个步骤：

1. 加线程锁boost::unique_lock<boost::mutex> lock(trackMutex)
2. 创建FrameHessian和FrameShell的指针fh和shell, 并进行相应初始化, 并存储所有帧allFrameHistory.push_back(shell)
3. 得到曝光时间fh->exposure_time, 生成金字塔,、计算整个图像梯度makeImages
4. 进行初始化
1. 加入第一帧后如果未初始化则调用coarseInitializer->setFirst(&Hcalib, fh)
2. 已初始化则跟踪成功, 完成初始化，initializeFromInitializer(fh)；解锁lock.unlock()；将该帧通过deliverTrackedFrame(fh, true)传到后端
5. 初始化完成后进行前端操作，首先对新来的帧进行跟踪, 得到位姿光度, 判断跟踪状态
1. 交换参考帧和当前帧的coarseTracker即CoarseTracker* tmp = coarseTracker; coarseTracker=coarseTracker_forNewKF; coarseTracker_forNewKF=tmp;
2. 使用旋转和平移对像素移动的作用比来判断运动状态Vec4 tres = trackNewCoarse(fh);
6. 判断是否插入关键帧，通过bool needToMakeKF来标识
7. 将needToMakeKF = true的关键帧通过deliverTrackedFrame发布出去

DSO的主程序大致就是如此，然而其中涉及到了一些小的细节，我们分别顺序讲解。

CoarseIntializer

setFirst()

在初始化部分，首先setFirst函数对第一帧进行了相应处理，包括了

1. 计算图像金字塔每层的内参，通过makeK()函数实现
2. 新建一个PixelSelector的object
3. 针对不同层选择梯度较大的像素，这个过程中需要
4. 设置网格的大小
5. 设置选择像素点的个数
6. 第0层通过makeMaps()来提取特征像素
7. 其他层通过makePixelStatus来选出goodpoints
8. 在选出的像素点中，添加点的信息、计算pattern内像素梯度和
9. 通过makeNN()生成每一层点的KDTree, 并用其找到邻近点集和父点

trackFrame

跟踪新的一帧，

1. 先显示新来的帧pushLiveFrame
2. 初始化每个点逆深度为1, 初始化光度参数、位姿SE3；如果有仿射系数则估计初值
3. 使用计算过的上一层来初始化下一层
4. 迭代之前计算能量, Hessian等
5. 迭代求解
1. 计算边缘化后的Hessian矩阵
2. 求解增量
3. 更新状态, doStep中更新逆深度
4. 计算更新后的能量并且与旧的对比判断是否accept
5. 接受的话, 更新状态,; 不接受则增大lambda
6. 优化后赋值位姿, 从底层计算上层点的深度

CoarseTracker

trackNewCoarse

使用确定的运动模型对新来的一帧进行跟踪, 得到位姿和光度参；假设了不同的运动，具体来说就是

• 5种运动模型：匀速、倍速、半速、零速以及没有运动
• 26×N种旋转情况：四元数的26种旋转情况，N个比较小的角度

具体可以参考博客https://blog.csdn.net/wubaobao1993/article/details/104022702，讲解的很详细了。

trackNewestCoarse

对新来的帧进行跟踪, 优化得到位姿, 光度参数；

1. 由粗到精迭代优化，如果大于能量阈值的 点超过60%，则放大阈值，且该层多优化一遍。
2. 如果某一层的能量值大于1.5倍最小值，直接判断为失败，结束节省时间。
3. 如果跟踪成功且第0层好于当前，则保留 结果，并更新每一层的最小能量值。
4. 第0层最小值小于阈值则停止，把目前最好的值作为下次跟踪的阈值。

addActiveFrame

关键帧的筛选；通过像素移动的大小判断，位移较大、平 移+旋转较大、曝光变化大、跟踪得到的 能量变化大，则插入(论文中说只考虑位 移，有效处理遮挡/去遮挡)。

Trace

makeNonKeyFrame

更新未成熟点ImmaturePoint的逆深度 范围后，删除fh，只保留FrameShell

traceNewCoarse

利用新的帧 fh 对关键帧中的ImmaturePoint进行更新

1. 根据逆深度范围得到极线搜索的范围
2. 计算图像梯度和极线夹角的大小，如果太大则误差会很大
3. 在极线上按照一定步长进行搜索能量最小的位置，和大于设置半径(2)的第二小的位置，后/前作为质量，越大越好
4. 沿着极线进行GN优化，直到增量足够小
5. 根据搜索得到的投影位置计算新的逆深度范围

Marginalize

flagFramesForMarginalization()

对于关键帧的边缘化策略

• 活跃点只剩下5%的
• 和最新关键帧曝光变化大于0.7
• 距离最远的关键帧

makeKeyFrame

生成关键帧, 优化, 激活点, 提取点, 边缘化关键帧，主要步骤包括了

1. 设置当前估计的fh的位姿, 光度参数
2. 把这一帧来更新之前帧的未成熟点
3. 选择要边缘化掉的帧
4. 加入到关键帧序列
5. 构建之前关键帧与当前帧fh的残差(旧的)
6. 激活所有关键帧上的部分未成熟点(构造新的残差)
7. 对滑窗内的关键帧进行优化
8. 去除外点, 把最新帧设置为参考帧
9. 标记删除和边缘化的点, 并删除&边缘化
10. 生成新的点
11. 边缘化掉关键帧

activatePointsMT

1. 阈值计算, 通过距离地图来控制数目
2. 处理未成熟点, 激活/删除/跳过
3. 优化上一步挑出来的未成熟点, 进行逆深度优化, 并得到pointhessian
4. 把PointHessian加入到能量函数, 删除收敛的未成熟点, 或不好的点
5. 把删除的点丢掉

optimize

1. 使用GN法对位姿、光度参数、逆深度、相机内参 进行优化，由于边缘化需要维护两个H矩阵和b向量
2. 其中位姿和相机内参使用FEJ，除了最新一帧，相 关H矩阵固定在上一次优化，残差仍然使用更新后的状态求
3. 被边缘化部分残差更新: b=b-H\cdot\Delta
4. 其中第一帧位姿和其上点逆深度，由于初始化具有先验，光度参数有先验
5. 使用伴随性质将相对位姿变为世界系下的(local ->global)
6. 减去求解出的增量零空间部分，防止在零空间乱飘

setCoarseTrackingRef

//@ 把优化完的最新帧设为参考帧void CoarseTracker::setCoarseTrackingRef( std::vector<FrameHessian*> frameHessians){ assert(frameHessians.size()>0); lastRef = frameHessians.back(); makeCoarseDepthL0(frameHessians); // 生成逆深度估值 refFrameID = lastRef->shell->id; lastRef_aff_g2l = lastRef->aff_g2l(); firstCoarseRMSE=-1;}

makeNewTraces

//@ 提取新的像素点用来跟踪void FullSystem::makeNewTraces(FrameHessian* newFrame, float* gtDepth){ pixelSelector->allowFast = true; //bug 没卵用 //int numPointsTotal = makePixelStatus(newFrame->dI, selectionMap, wG[0], hG[0], setting_desiredDensity); int numPointsTotal = pixelSelector->makeMaps(newFrame, selectionMap,setting_desiredImmatureDensity); newFrame->pointHessians.reserve(numPointsTotal*1.2f); //fh->pointHessiansInactive.reserve(numPointsTotal*1.2f); newFrame->pointHessiansMarginalized.reserve(numPointsTotal*1.2f); newFrame->pointHessiansOut.reserve(numPointsTotal*1.2f); for(int y=patternPadding+1;y<hG[0]-patternPadding-2;y++) for(int x=patternPadding+1;x<wG[0]-patternPadding-2;x++) { int i = x+y*wG[0]; if(selectionMap[i]==0) continue; ImmaturePoint* impt = new ImmaturePoint(x,y,newFrame, selectionMap[i], &Hcalib); if(!std::isfinite(impt->energyTH)) delete impt; // 投影得到的不是有穷数 else newFrame->immaturePoints.push_back(impt); } //printf("MADE %d IMMATURE POINTS!\n", (int)newFrame->immaturePoints.size());}

marginalizeFrame

• 将被边缘化的帧的8个状态挪到右下角，然后计算Schur Complement， 将其消掉
• 删除在被边缘化帧上的残差
• DSO里面操作的都是PoseGraph