DSO 代码框架

从数据流的角度讲一遍 DSO 代码框架。

DSO 的入口是 FullSystem::addActiveFrame，输入的影像生成 FrameHessian 和 FrameShell 的 Object，FrameShell 是 FrameHessian 的成员变量，FrameHessian 保存影像信息，FrameShell 保存帧的位置姿态信息。代码中一般用 fh 指针变量指向当前帧的 FrameHessian。在处理完成当前帧之后，会删除 FrameHessian，而保存 FrameShell 在变量 allFrameHistory 中，作为最后整条轨迹的输出。

其实，输入影像在 main_dso_pangolin.cpp 中已经处理，影像值不是原始的灰度值，而是辐射值，这些辐射值的范围依旧是 [0, 255]，float 型。（如果有进行辐射标定。）

数据预处理部分是在 FullSystem::addActiveFrame 中调用的 FrameHessian::makeImages，这个函数为当前帧的影像建立影像金字塔，并且计算每一层影像的梯度。这些计算结果都存储在 FrameHessian 的成员变量中，1. dIp 每一层影像的辐射值、x 方向梯度、y 方向梯度；2. dI 指向 dIp[0] 也就是原始影像的信息；3. absSquaredGrad 存储 xy 方向梯度值的平方和。

1. 第一帧

进入 FullSystem::addActiveFrame，首先判断是否完成了初始化，如果没有完成初始化，就将当前帧 fh 输入 CoarseInitializer::setFirst 中。完成改函数之后，退出，接着处理下一帧。

CoarseInitializer::setFirst，在影像的每一层选取点，作为后续第二帧匹配生成 pointHessians 和 immaturePoints 的候选点，这些点存储在 CoarseInitializer::points 中。每一层点之间都有联系，在 CoarseInitializer::makeNN 中计算每个点最邻近的10个点 neighbours，在上一层的最邻近点 parent。

pointHessians 是成熟点，具有逆深度信息的点，能够在其他影像追踪到的点。immaturePoints 是未成熟点，需要使用非关键帧的影像对它的逆深度进行优化，在使用关键帧将它转换成 pointHessians，并且加入到窗口优化。

2. 第二帧

初始化需要有两帧，所以第二帧依然交由 CoarseInitializer。CoarseInitializer::trackFrame 处理完成之后，在 FullSystem::initializerFromInitializer 中为第一帧生成 pointHessians，一共2000个左右。随后将第二帧作为 KeyFrame 输入到 FullSystem::deliverTrackedFrame，最终流入 FullSystem::makeKeyFrame。（FullSystem::deliverTrackedFrame 的作用就是实现多线程的数据输入。）

2.1 CoarseInitializer::trackFrame

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

一些加速优化过程的操作：1. 每一层匹配开始的时候，调用一次 CoarseInitializer::propagateDown，将当前层所有点的逆深度设置为的它们 parent （上一层）的逆深度；2. 在每次接受优化结果，更新每个点的逆深度，调用一次 CoarseInitializer::optReg 将所有点的 iR 设置为其 neighbour 逆深度的中位数，其实这个函数在 CoarseInitializer::propagateDown 和 CoarseInitializer::propagateUp 中都有调用，iR 变量相当于是逆深度的真值，在优化的过程中，使用这个值计算逆深度误差，效果是幅面中的逆深度平滑。

优化过程中的 lambda 和点的逆深度有关系，起一个加权的作用，也不是很明白对 lambda 增减的操作。在完成所有层的优化之后，进行 CoarseInitializer::propagateUp 操作，使用低一层点的逆深度更新其高一层点 parent 的逆深度，这个更新是基于 iR 的，使得逆深度平滑。高层的点逆深度，在后续的操作中，没有使用到，所以这一步操作我认为是无用的。

2.2 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 相当于估计的真值，用于计算误差。（这里和 CoarseInitializer 对 idepth 和 iR 的定义是相反的，我再想想。）

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

3. 第 3 4 5 6 7 8 ... 帧

后面帧的流程就是先使用 FullSystem::trackNewCoarse 将当前帧与上一个关键帧进行匹配，得到初始位姿，随后判断当前帧是否需要成为关键帧，并输入到 FullSystem::deliverTrackedFrame，最终输入到 FullSystem::makeKeyFrame 或 FullSystem::makeNonKeyFrame 中。

3.1 FullSystem::trackNewCoarse

coarseTracker->lastRef 中存储了最新关键帧，allFrameHistory 存储了所有帧的位姿。按照 1 倍，2倍，0.5倍，0 倍速度的假设，构造当前帧的位姿假设。这些位姿的假设都来源于前面两帧与关键帧两两之间的相对位姿和关键帧的绝对位姿。而这些假设中最重要的是，当前帧到前一帧的相对位姿等于前一帧到前前一帧的相对位姿，之所以说这个重要，是因为这样后面在这样计算出来的当前帧位姿上，进行了 a TON of 旋转作为假设，加入到总的假设（lastF_2_fh_tries）中。

进行好这些假设之后，就从第一个假设开始，用 CoarseTracker::trackNewestCoarse 与最新关键帧匹配。依旧是高斯牛顿优化，而这个优化只优化相两帧的相对状态（相对位姿 6 + 光度仿射变换 2）。当然也不是所有的假设都需要优化一遍，当前假设得到的结果与前面假设得到的结果比较，当前结果与前面一帧匹配的结果比较（这个跨度有点大，是上一帧），满足条件就可以跳出了。

3.2 FullSystem::makeNonKeyFrame

非关键帧的作用是更新窗口中关键帧 immaturePoints 的逆深度，FullSystem::makeNonKeyFrame 调用 FullSystem::traceNewCoarse 做这件事情。对于每一个 ImmaturePoint 是在 ImmaturePoint::traceOn 中完成的。

3.2.1 ImmaturePoint::traceOn

ImmaturePoint::traceOn 这个函数是将关键帧上的点与非关键帧影像进行匹配，得到在非关键帧影像上的位置，知道这个信息就可以更新深度了。先进行极线搜索，搜索得到一个比较好的值，再使用高斯牛顿优化。

最后更新了 ImmaturePoint::idepth_min 和 ImmaturePoint::idepth_max，对于 immaturePoint 仅仅只有一个 idepth 的范围，没有确切的 idepth。

3.3 FullSystem::makeKeyFrame

一开始的套路和非关键帧一样，调用 FullSystem::traceNewCoarse。

调用 FullSystem::flagFramesForMarginalization 标记需要被 marg 掉的帧，这些帧在 frameHessians 这个 std::vector 中存储方式是越老的帧越前。

遍历窗口中所有帧的 pointHessians，建立它们链接自己 hostframe 与当前帧的 PointFrameResidual，加入到 EnergyFunctional 中。

调用 FullSystem::activatePointsMT 遍历窗口中所有帧的 immaturePoints，如果可以投影到的当前帧上，那么再试一试这些帧能不能投影到窗口中其他帧上去，找到所有能够投影的链接，形成 PointFrameresisual。（FullSystem::optimizeImmaturePoint）为了将这些点投影到尽可能多的帧上去，也是进行了高斯牛顿优化它的逆深度。这里的逆深度就是 immaturePoint 的 idepth_max 和 idepth_min，在 ImmaturePoint::traceOn 中计算得到的。

接下来就是调用 FullSystem::optimize 窗口优化。窗口优化完，就是 marg 掉不需要的帧和点。

做完这些操作之后，有一个重要的操作是 FullSystem::makeNewTraces 在当前关键帧提取 immaturePoints，这样下一个关键帧处理的时候可以生成 pointHessians 加入到窗口优化过程中。