DSO流程通读[还没写完]

文件结构 

 

.
├── calib
│   ├── EuRoc
│   └── KITTI
└── src
     ├── FullSystem                      
     ├── IOWrapper
     ├── main_dso_pangolin.cpp   /* main 函数所在地 runthread 包含主循环 addActiveFrame(mat, timestamp)进入系统*/
     ├── OptimizationBackend
     └── util

DSO的前端里程计全在文件夹src/FullSystem中，其中里程计的主流程管理在FullSystem.cpp中，管理主流程的函数是addActiveFrame。

├── src
│   ├── FullSystem
│   │   ├── CoarseInitializer.cpp
│   │   ├── CoarseInitializer.h
│   │   ├── CoarseTracker.cpp
│   │   ├── CoarseTracker.h
│   │   ├── FullSystem.cpp
│   │   ├── FullSystemDebugStuff.cpp
│   │   ├── FullSystem.h
│   │   ├── FullSystemMarginalize.cpp
│   │   ├── FullSystemOptimize.cpp
│   │   ├── FullSystemOptPoint.cpp
│   │   ├── HessianBlocks.cpp
│   │   ├── HessianBlocks.h
│   │   ├── ImmaturePoint.cpp
│   │   ├── ImmaturePoint.h
│   │   ├── PixelSelector2.cpp
│   │   ├── PixelSelector2.h
│   │   ├── PixelSelector.h
│   │   ├── ResidualProjections.h
│   │   ├── Residuals.cpp
│   │   └── Residuals.h
 

后端优化  src/OptimizationBackend 

├── src
│        ├── OptimizationBackend
│   │   ├── AccumulatedSCHessian.cpp
│   │   ├── AccumulatedSCHessian.h
│   │   ├── AccumulatedTopHessian.cpp
│   │   ├── AccumulatedTopHessian.h
│   │   ├── EnergyFunctional.cpp
│   │   ├── EnergyFunctional.h
│   │   ├── EnergyFunctionalStructs.cpp
│   │   ├── EnergyFunctionalStructs.h
│   │   ├── MatrixAccumulators.h
│   │   └── RawResidualJacobian.h

去畸变等一些零散功能 src/utils

├── src
│       └── util
│       ├── DatasetReader.h
│       ├── FrameShell.h
│       ├── globalCalib.cpp
│       ├── globalCalib.h
│       ├── globalFuncs.h
│       ├── ImageAndExposure.h
│       ├── IndexThreadReduce.h
│       ├── MinimalImage.h
│       ├── nanoflann.h
│       ├── NumType.h
│       ├── settings.cpp
│       ├── settings.h
│       ├── Undistort.cpp
│       └── Undistort.h

数据集读写和可视化UI代码   src/IOWrapper

├── src
│   │   ├── IOWrapper
│   │   ├── ImageDisplay_dummy.cpp
│   │   ├── ImageDisplay.h
│   │   ├── ImageRW_dummy.cpp
│   │   ├── ImageRW.h
│   │   ├── OpenCV
│   │   ├── Output3DWrapper.h
│   │   ├── OutputWrapper
│   │   └── Pangolin

关键数据结构

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

1）PointHessian：所有活跃点的信息。所谓活跃点，是指它们在相机的视野中，其残差项仍在参与优化部分的计算

2）pointHessiansMarginalized：已经边缘化的地图点

3）pointHessiansOut：判断为外点的地图点

4）immaturePoints：未成熟的地图点

整体工作流程

main 函数在main_dso_pangolin.cpp   其中runthread包含主循环      addActiveFrame(mat, timestamp)进入系统

addActiveFrame函数

1）先根据输入创建FrameHessian和FrameShell, 进行相应初始化, 存储所有帧, 工作过程中，系统会维护5到7个关键帧组成的滑动窗口，每个关键帧对应一个FrameHessian

2）新来一帧图像时，会产生新的地图点，但此时地图点深度是未知的，它就成为immaturePoints（未成熟的地图点），随着后面接收到的图像越来越多，地图点会不断被观测，如果最终收敛，它就会变成已知准确深度的地图点，即PointHessian（活跃点）

3）为了构建优化问题，每个关键帧中的PointHessian可以往其他帧投影，投影会形成残差项，放在PointHessian::residuals中，所有残差项相加即构成损失函数，可以进行优化求解

4）如果由于遮挡等导致PointHessian无法完成投影，那么他就属于外点，即PointHessiansOut（判断为外点的地图点）

5）随着系统的运行，滑动窗口需要不断添加新的帧，移除老的帧，在移除老的帧的时候需要进行边缘化，即把删掉的关键帧中的PointHessian转成先验的地图点，即pointHessiansMarginalized（已经边缘化的地图点）

6）后端优化部分和前段具有相同的数据结构，通过互相持有对方指针和前端通信

代码流程

初始化过程

CoarseInitializer::setFirst  第一帧处理, 通过PixelSelector提取特征像素, 提点存储在CoarseInitializer::points, 每层一个points指针,  按照选择的像素数目初始化空间, 并且初始化像素点, 再通过该点搜索周边点的匹配关系, CoarseInitializer::makeNN,包含临近点和父点

CoarseInitializer::trackFrame 第二帧处理   在 FullSystem::initializerFromInitializer 中生成 pointHessians  将第二帧作为 KeyFrame 输入到 FullSystem::deliverTrackedFrame  最终流入 FullSystem::makeKeyFrame。

CoarseInitializer::trackFrame

CoarseInitializer::trackFrame 将所有 points （第一帧提到的点）的逆深度初始化为1。

从金字塔最高层到最底层依次匹配，每一层的匹配都是高斯牛顿优化过程，在 CoarseIntializer::calcResAndGS 中计算Hessian矩阵等信息，计算出来的结果在 CoarseInitializer::trackFrame 中更新相对位姿到临时变量中（存储在局部变量中，现在还没有确定要不要接受这一步优化）， CoarseInitializer::trackFrame 调用 CoarseInitializer::doStep 中更新点的逆深度信息。随后再调用 CoarseIntializer::calcResAndGS，计算新的能量，如果新能量更低，那么就接受这一步优化，在 CoarseInitializer::applyStep 中生效前面保存的优化结果。

优化过程中的 lambda 和点的逆深度有关系，起一个加权的作用。在完成所有层的优化之后，CoarseInitializer::propagateUp 使用低一层点的逆深度更新其高一层点 parent 的逆深度，这个更新是基于 iR 的，使得逆深度平滑。高层的点逆深度，在后续的操作中，没有使用到。

在CoarseIntializer::calcResAndGS 中计算Hessian矩阵等信息, 输入参数 : lvl 金字塔层级refToNew_current 位姿信息 , refToNew_aff_current光度放射变换参数,  输出参数 : H b Hsc bsc 。   对应增量方程schur消元后的几个矩阵有了他们就能直接计算出增量方程的解了 , 解就是位姿增量 ,仿射系数增量和逆深度增量, CoarseIntializer::calcResAndGS这里只是算出了增量方程schur消元系数 还没解增量方程.

每一个关键帧的状态为八维：六自由度的运动位姿加上两个描述光度的参数；

每个地图点的状态变量为一维，即该点在主导帧（Host）里的逆深度。于是，每个残差项（或能量项E），将关联两个关键帧与一个逆深度。事实上，还有一个全局的相机内参数亦参与了优化，但未在此图中表示。

 

FullSystem::initializeFromInitializer

FullSystem::initializeFromInitializer，第一帧是 firstFrame，第二帧是 newFrame，从 CoarseInitializer 中抽取出 2000 个点作为 firstFrame 的 pointHessians。设置的逆深度有 CoarseIntiailzier::trackFrame 中计算出来的 iR 和 idepth，而这里使用了 rescaleFactor 这个局部变量，保证所有 iR 的均值为 1。iR 设置的是 PointHessian 的 idepth，而 idepth 设置的是 PointHessian 的 idepth_zero，idepth_zero 相当于估计的真值，用于计算误差。

注意这里已经将第一帧加入到 EnergyFunctional 的帧中，为后面的优化做准备。搜索 ef 变量就能看到这个操作。

 Track过程

• coarseTracker->lastRef 中存储了最新关键帧，allFrameHistory 存储了所有帧的位姿。按照 1 倍，2倍，0.5倍，0 倍速度的假设，构造当前帧的位姿假设。这些位姿的假设都来源于前面两帧与关键帧两两之间的相对位姿和关键帧的绝对位姿。假设当前帧到前一帧的相对位姿 等于 前一帧到前前一帧的相对位姿，之所以说这个重要，是因为这样后面在这样计算出来的当前帧位姿上，进行了 a TON of 旋转作为假设，加入到总的假设（lastF_2_fh_tries）中。
• 进行好这些假设之后，就从第一个假设开始，用 CoarseTracker::trackNewestCoarse 与最新关键帧匹配。依旧是高斯牛顿优化，而这个优化只优化相两帧的相对状态（相对位姿 6 + 光度仿射变换 2）。当然也不是所有的假设都需要优化一遍，当前假设得到的结果与前面假设得到的结果比较，当前结果与前面一帧匹配的结果比较（这个跨度有点大，是上一帧），满足条件就可以跳出了。

[1]. DSO详解, 高翔

[2]. DSO代码解读, 龚益群

[3]. DSO源码解读, 任乾