从零啃DSO源码：初始化阶段核心模块与实现细节解析

1. 从零啃DSO源码初始化阶段到底在做什么第一次打开DSODirect Sparse Odometry的源码时我的反应和大多数人一样——文件不多但每个文件都像天书。尤其是初始化部分代码量不算大但涉及的概念密度极高光度标定、稀疏直接法、滑动窗口、边缘化先验这些东西如果不在脑子里建立起清晰的图景后面根本读不下去。DSO是直接法视觉里程计里绕不开的一个经典系统。它和特征点法最大的区别在于不提取关键点、不算描述子直接拿像素灰度值来优化相机位姿和地图点。这个思路听起来简单但真正落地时会遇到一堆问题——光度不一致怎么办、初始化怎么收敛、滑窗里的信息怎么保留。而初始化阶段恰恰是这些问题最集中的地方。这篇笔记面向的是已经了解SLAM基本概念、想深入读DSO源码的人。我会把初始化阶段拆成几个核心模块讲清楚每一步在干什么、为什么这么设计、代码里对应哪些函数。不是逐行翻译代码而是把设计意图和实现细节串起来。读完之后你应该能自己顺着代码走一遍初始化流程而不是对着屏幕发呆。2. 初始化在整个DSO系统中的位置与设计考量2.1 为什么初始化阶段值得单独拿出来研究DSO的初始化不是简单的“给个初值然后开始优化”。它要解决一个鸡生蛋蛋生鸡的问题要优化位姿和深度需要光度误差要算光度误差需要知道曝光时间和光度响应而这些东西在系统刚启动时全是未知的。我的理解是DSO的初始化本质上在做三件事第一选定一个参考帧建立初始的窗口结构第二在参考帧上撒点给每个点一个粗糙的深度初值第三通过几帧的观测把光度参数和深度值一起优化到一个可用的状态。这三件事做完系统才算真正“跑起来”。从代码结构上看初始化相关的逻辑主要集中在FullSystem的addActiveFrame函数里配合CoarseInitializer类完成。CoarseInitializer是一个独立的类专门负责前两帧的初始化做完之后就把结果交给正式的优化流程。2.2 直接法初始化与特征点法初始化的本质差异特征点法的初始化通常靠对极几何或者三角化流程是提取特征点、匹配、算基础矩阵或单应矩阵、分解出位姿、三角化地图点。这套流程的好处是几何关系明确坏处是依赖匹配质量纹理弱或者重复纹理多的时候容易崩。DSO走的是完全不同的路。它不匹配特征点而是直接在参考帧上选一批像素点然后通过最小化光度误差来同时优化位姿和深度。这意味着初始化阶段就要处理光度问题——不同帧的曝光可能不同相机响应曲线也不是线性的。所以DSO在初始化时就要估计曝光时间甚至光度响应函数。这个差异带来的直接影响是DSO的初始化对光照变化更敏感但一旦初始化成功在纹理弱的环境下反而比特征点法更稳。因为像素点多信息量大不像特征点法那样依赖少数几个关键点。2.3 初始化阶段的核心模块划分把初始化阶段的代码拆开看大致可以分成这么几块帧的预处理包括图像金字塔构建、梯度计算、光度标定参数的读取。这部分在FullSystem::addActiveFrame的前半段。参考帧选择与窗口管理决定当前帧是作为新的参考帧还是跟踪帧维护滑动窗口的结构。粗初始化CoarseInitializer前两帧之间的位姿和深度初始化这是最核心的部分。点的选取与深度初值在参考帧上选点给每个点一个初始深度范围。光度参数初始化曝光时间的初始估计和后续优化。这几块不是严格线性的有些步骤会交叉。比如点的选取和深度初值在粗初始化阶段就会做而光度参数的优化会贯穿整个初始化过程。3. 粗初始化模块的代码结构与关键实现3.1 CoarseInitializer类的整体设计CoarseInitializer这个类在DSO源码里算是比较独立的一块它的职责很明确给定两帧图像估计它们之间的相对位姿和每个点的深度。这个类只在初始化阶段用初始化完成后就不再参与后续优化。类的核心成员包括int w, h图像宽高。MatXX JbBuffer系列用于累积雅可比矩阵的缓冲区。Vec10f* JbBuffer_new每个点的雅可比缓冲区。Frame* frame参考帧指针。CalibHessian* Hcalib光度标定对象。核心函数有setFirst、setSecond、trackFrame、calcResAndGS、calcEC、optE、optD等。这些函数的分工是setFirst和setSecond分别设置参考帧和当前帧trackFrame做一次跟踪calcResAndGS计算残差和高斯-牛顿系统的增量optE优化曝光optD优化深度。我的理解是这个类的设计思路是“先粗后精”先用少量点、低分辨率做一次粗略的位姿估计然后逐步增加点数和分辨率最后把结果交给正式优化。这种策略在直接法里很常见因为直接法的优化问题是非凸的初值不好很容易陷入局部极小。3.2 setFirst与setSecond两帧的初始化配置setFirst做的事情比较直接把传入的帧设为参考帧然后初始化一些内部状态。具体来说它会保存帧指针和标定对象。根据图像尺寸分配缓冲区。初始化一些计数器比如numOpts、numRefs等。设置初始的曝光参数。setSecond稍微复杂一点。它除了设置当前帧之外还要做几件关键的事计算两帧之间的初始位姿猜测。这个猜测通常来自常速模型或者单位矩阵。在参考帧上选点并给每个点一个初始深度范围。初始化每个点的雅可比缓冲区。选点的逻辑在setSecond里是通过makePoints或者类似的函数完成的。选点的策略是在参考帧上均匀撒点但会避开梯度太小的区域因为梯度小的地方光度误差对深度不敏感优化起来没意义。深度初值的给法也很有讲究。DSO不是随便给一个固定值而是根据场景的典型深度范围给一个区间然后在这个区间里取一个初始值。这个区间通常是根据相机的基线或者场景的先验知识来定的。3.3 trackFrame粗初始化中的跟踪流程trackFrame是粗初始化的核心函数它做的是给定当前帧通过优化位姿和深度来最小化光度误差。这个函数的流程大致是构建图像金字塔。DSO在粗初始化阶段会用多层金字塔从粗到精逐层优化。对每一层迭代执行以下步骤计算残差和雅可比。构建高斯-牛顿方程。求解增量更新位姿和深度。判断收敛如果收敛就进入下一层。把优化结果保存下来供后续使用。这里有个细节值得注意在粗初始化阶段DSO并不是同时优化所有点的深度而是先优化位姿再优化深度交替进行。这种做法在直接法里叫“交替优化”好处是每次优化的变量少计算量小而且不容易陷入局部极小。坏处是收敛速度可能慢一些但在初始化阶段稳定性比速度更重要。3.4 calcResAndGS残差与高斯-牛顿系统的构建calcResAndGS这个函数是计算量的主要来源。它要做的事情是对每个选中的点计算光度残差然后累积雅可比矩阵和残差向量最终形成高斯-牛顿系统。光度残差的定义是r I_ref(p) - I_cur(p) * (t_cur / t_ref) - b其中I_ref和I_cur分别是参考帧和当前帧的灰度值t_ref和t_cur是曝光时间b是亮度偏移。这个残差对位姿和深度的雅可比需要分别计算。对位姿的雅可比涉及到图像梯度、相机内参和点的深度。对深度的雅可比则涉及到图像梯度和位姿。这些雅可比的计算在代码里是通过链式法则一步步展开的看起来复杂但逻辑是清晰的。我的经验是读这部分代码的时候不要一上来就盯着公式看。先把每个变量的物理意义搞清楚然后看代码里是怎么一步步算出来的。比如JbBuffer这个缓冲区它存的是每个点的雅可比和残差理解了这个后面的累积过程就顺了。3.5 optE与optD曝光与深度的交替优化optE和optD是两个相对独立的优化函数。optE优化的是曝光时间optD优化的是深度。optE的逻辑比较简单固定位姿和深度只优化曝光时间。因为曝光时间是一个标量所以这个优化是一维的可以用解析解直接算出来。代码里就是累加一些量然后做一次除法。optD优化的是每个点的深度。这里DSO用了一个技巧不是直接优化深度值而是优化深度的倒数也叫逆深度。逆深度的好处是在深度很大的时候逆深度接近零数值上更稳定。而且逆深度的分布通常更接近高斯分布适合高斯-牛顿优化。optD的流程是对每个点计算残差对逆深度的雅可比然后更新逆深度。这里有个细节DSO会限制逆深度的范围避免出现负深度或者无穷大深度。这个限制在代码里是通过setting_depthMin和setting_depthMax来控制的。4. 初始化中的光度标定与参数估计4.1 光度模型的基本假设与参数化DSO的光度模型基于一个假设不同帧之间的灰度差异可以用一个仿射变换来近似。具体来说I_cur(p) a * I_ref(p) b其中a和b是光度参数a主要跟曝光时间有关b是亮度偏移。在实际实现中DSO把a参数化为曝光时间的比值即a t_cur / t_ref。这个模型看起来简单但它解决了一个大问题不同帧的曝光不同导致的灰度差异。如果没有这个模型直接法优化会被光度差异带偏位姿估计会不准。我的理解是这个模型的假设在大多数情况下是成立的因为相机的响应曲线在中间调区域近似线性。但在高动态范围场景下这个假设可能不成立这时候就需要更复杂的光度模型比如考虑相机的响应函数。DSO的完整版本里确实有响应函数的估计但在初始化阶段通常只用仿射模型就够了。4.2 曝光时间的初始估计与优化策略曝光时间的初始估计在setSecond里做。DSO的做法是假设两帧的曝光时间相同即t_cur t_ref。这个假设在大多数情况下是合理的因为相机自动曝光通常不会在两帧之间剧烈变化。然后在trackFrame的迭代过程中曝光时间会通过optE不断优化。optE的优化目标是固定位姿和深度找到使光度误差最小的曝光时间。因为曝光时间是一维的这个优化可以解析求解。具体来说optE会累加每个点的I_ref * I_cur和I_cur * I_cur然后做一次除法得到最优的曝光时间比值。这个计算在代码里就是几行但背后的推导需要理解光度误差对曝光时间的导数。4.3 光度参数对初始化收敛的影响光度参数估计不准初始化很容易失败。我踩过的一个坑是在光照变化剧烈的场景下如果曝光时间的初始估计偏差太大粗初始化会收敛到一个错误的位姿然后整个系统就崩了。DSO的处理方式是在粗初始化阶段曝光时间的优化和位姿的优化是交替进行的。这样即使初始曝光估计不准经过几轮迭代也能收敛到合理值。但前提是场景的纹理要足够丰富否则光度误差的梯度太小优化会停滞。另一个经验是如果场景的亮度整体偏暗或者偏亮光度模型的线性假设可能不成立这时候初始化容易失败。解决办法是在预处理阶段做一次直方图均衡化或者调整相机的曝光设置让图像亮度落在中间调区域。5. 点的选取、深度初始化与窗口管理5.1 参考帧上选点的策略与实现细节选点的逻辑在CoarseInitializer里是通过一个叫makePoints的函数完成的不同版本的DSO可能函数名略有差异。选点的策略是在参考帧上均匀撒点但会避开图像边界。对每个候选点计算其局部梯度。如果梯度太小就丢弃这个点。对保留下来的点再做一个非极大值抑制避免点太集中。这个策略的目的是选出来的点要有足够的光度变化这样光度误差对深度和位姿才敏感。梯度小的点比如白墙上的点光度误差几乎不随深度变化优化起来没意义。选点的数量在粗初始化阶段通常比较少比如几百个。这是因为粗初始化只需要一个粗略的位姿和深度不需要太多点。点太多反而会增加计算量而且容易引入噪声。5.2 深度初值的给定方式与范围选择深度初值的给定方式直接影响初始化的收敛性。DSO的做法是给每个点一个深度范围然后在这个范围内取一个初始值。这个范围通常是根据场景的典型深度来定的。具体来说代码里会有一个setting_depthMin和setting_depthMax分别对应深度的最小值和最大值。初始深度通常取这两个值的几何平均或者算术平均。然后在优化过程中深度会被限制在这个范围内。这个范围的选择很关键。如果范围太窄可能把真实深度排除在外如果范围太宽优化容易陷入局部极小。我的经验是对于室内场景深度范围可以设为0.5米到10米对于室外场景可以设为1米到100米。当然这只是一个粗略的参考具体还要看相机的基线和场景的尺度。5.3 滑动窗口的初始化与关键帧管理DSO的滑动窗口在初始化阶段就开始建立了。窗口的大小由setting_windowSize控制通常是5到10帧。初始化的前两帧会作为窗口的起始帧后续帧通过跟踪加入窗口。窗口管理的逻辑在FullSystem::addActiveFrame里。当新的一帧到来时系统会判断它是作为关键帧还是普通帧。判断的依据通常是当前帧和参考帧之间的视差是否足够大或者跟踪质量是否下降。如果当前帧被选为关键帧它会加入窗口同时窗口中最老的一帧会被边缘化。边缘化的过程会把老帧的信息转化为先验保留在优化问题中。这个先验在初始化阶段尤其重要因为初始化时窗口里的帧少信息量不足先验可以帮助稳定优化。我的体会是初始化阶段的窗口管理要特别小心。如果窗口太小信息量不够优化容易发散如果窗口太大计算量增加而且老帧的信息可能已经过时。DSO的默认窗口大小是7这个值在大多数场景下都能工作但在快速运动的场景下可能需要调小。6. 初始化过程中的常见问题与排查经验6.1 初始化失败的典型表现与原因分析初始化失败的表现通常有几种位姿估计明显错误、深度图一片模糊、系统直接崩溃或者卡死。这些表现背后的原因各不相同。位姿估计错误通常是因为光度误差的优化陷入了局部极小。可能的原因包括初始位姿猜测太离谱、场景纹理太弱、光照变化太剧烈。排查的方法是检查初始位姿的猜测是否合理看看图像有没有明显的纹理对比两帧的亮度直方图。深度图模糊通常是因为深度初值给得不好或者优化没有收敛。可能的原因包括深度范围设得太宽、选的点太少、光度参数估计不准。排查的方法是检查深度范围是否合理看看选点的数量和分布检查曝光时间的优化结果。系统崩溃或者卡死通常是因为数值问题比如出现了NaN或者无穷大。可能的原因包括逆深度变成了负数、雅可比矩阵奇异、曝光时间变成了零。排查的方法是在关键步骤加打印看看哪个变量出了问题。6.2 光度变化剧烈时的应对方法光度变化剧烈是直接法初始化的一大挑战。DSO虽然有能力估计曝光时间但如果变化太剧烈仿射模型就不够用了。我的经验是在这种情况下可以尝试以下几个方法在预处理阶段做一次直方图匹配把当前帧的亮度分布对齐到参考帧。如果相机支持关闭自动曝光手动设置一个固定的曝光时间。在初始化阶段增加曝光时间的优化迭代次数让它有足够的时间收敛。如果还是不行可以考虑换一个光照更稳定的场景做初始化等系统跑起来之后再进入复杂场景。6.3 初始化耗时过长的优化思路初始化耗时过长通常是因为迭代次数太多或者每轮迭代的计算量太大。DSO的粗初始化在默认设置下通常能在几十毫秒内完成但如果场景复杂或者参数设置不当可能会慢很多。优化思路包括减少选点的数量。粗初始化不需要太多点几百个就够了。降低图像金字塔的层数。层数越多计算量越大。放宽收敛条件。初始化不需要太精确差不多就行。如果是在嵌入式平台上跑可以考虑用定点数代替浮点数或者用SIMD指令加速。下面这个表格整理了我遇到过的典型问题、可能原因和排查方法方便快速查阅问题表现可能原因排查方法位姿估计错误初始位姿猜测太离谱检查常速模型的输出位姿估计错误场景纹理太弱查看图像梯度分布位姿估计错误光照变化太剧烈对比两帧亮度直方图深度图模糊深度范围设得太宽检查depthMin和depthMax深度图模糊选点太少统计选点数量深度图模糊光度参数估计不准打印曝光时间优化结果系统崩溃逆深度变成负数检查深度更新公式系统崩溃雅可比矩阵奇异检查矩阵条件数系统崩溃曝光时间变成零检查optE的输出初始化耗时过长选点太多减少选点数量初始化耗时过长金字塔层数太多减少金字塔层数初始化耗时过长收敛条件太严放宽收敛阈值6.4 从初始化阶段过渡到正式优化的注意事项初始化完成后系统会从粗初始化切换到正式优化。这个切换不是瞬间完成的中间有一个过渡过程。过渡期间粗初始化得到的位姿和深度会作为正式优化的初值同时窗口里的帧会重新组织。我的经验是过渡期间最容易出问题的地方是粗初始化得到的深度可能不够精确正式优化需要几帧才能把它调整过来。如果在这几帧里出现了快速运动或者光照变化系统可能会跟丢。为了避免这个问题可以在过渡期间降低优化的学习率让系统慢慢调整。另外可以在过渡期间增加关键帧的选取频率让窗口里尽快积累足够的观测。7. 读DSO初始化源码的方法论与工具建议7.1 如何高效阅读直接法SLAM的源码读直接法SLAM的源码我的建议是不要一上来就逐行读代码先搞清楚系统的整体流程然后再深入细节。具体来说可以分三步走画流程图把初始化的流程画成一张图标出每个步骤的输入输出。不用太详细能看出数据流向就行。找关键函数在流程图上标出每个步骤对应的函数然后重点读这些函数。其他辅助函数可以暂时跳过。加打印调试在关键函数里加打印输出中间结果。然后跑一个简单的数据集看看输出是否符合预期。这个方法的好处是你能快速建立起对系统的整体认识然后再深入细节。如果一上来就逐行读很容易迷失在代码的海洋里。7.2 调试工具与可视化手段推荐调试直接法SLAM可视化非常重要。我常用的工具包括图像显示把参考帧、当前帧、深度图、残差图都显示出来直观地看到问题。轨迹显示把估计的轨迹和真值轨迹画在一起看看偏差有多大。参数曲线把曝光时间、深度值、残差等随时间的变化画出来看看有没有异常。这些工具在DSO的源码里有一部分已经提供了比如pangolin的显示。如果没有可以自己用OpenCV或者Matplotlib写一个简单的显示程序。7.3 从初始化扩展到整个系统的学习路径初始化只是DSO的一部分读懂了初始化接下来可以顺着数据流往下读跟踪初始化完成后系统进入跟踪模式逐帧估计位姿。优化跟踪过程中系统会定期做一次全局优化调整窗口里的所有变量。边缘化当窗口满了老帧会被边缘化信息转化为先验。回环DSO本身没有回环检测但可以结合其他模块实现。我的建议是先把初始化读透然后读跟踪最后读优化和边缘化。这样循序渐进每一步都有前面的基础不会太吃力。7.4 实操心得从编译到跑通第一个数据集最后分享一点实操心得。DSO的编译不算复杂但依赖比较多尤其是Pangolin和OpenCV的版本要匹配。我踩过的坑是Pangolin的版本太新和DSO的代码不兼容编译报了一堆错。后来换了一个老版本才编译通过。跑第一个数据集的时候建议用TUM的单目数据集因为它的场景比较简单光照也比较稳定。跑之前先检查一下配置文件里的参数尤其是相机的内参和光度标定参数。这些参数不对初始化肯定失败。跑通之后可以试着改一些参数比如窗口大小、选点数量、深度范围看看对初始化的影响。这种实验性的学习方式比单纯读代码要有效得多。提示DSO的初始化对参数比较敏感建议先用默认参数跑通再逐步调整。每次只改一个参数观察效果避免同时改多个参数导致问题难以定位。注意如果初始化一直失败先检查图像质量。图像太暗、太亮、太模糊都会导致初始化失败。确保图像清晰、亮度适中再检查参数。