话不多说,直接上流程图吧,一图胜千言(这张图是高翔博士博客里的,链接:zhuanlan.zhihu.com/p/29

有了这张图,我们就对前端里程计有了清晰的整体认识,下面我们看具体的流程。

DSO的前端和后端在文件结构上分的比较清楚,前端里程计全在文件夹src/FullSystem中,其中里程计的主流程管理在FullSystem.cpp中,管理主流程的函数是addActiveFrame。

(下面介绍流程的部分内容来自博客blog.csdn.net/gbz330025

1. 初始化

初始化需要有两帧。

1.1 第一帧。

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

1.2 第二帧

第二帧依然交由 CoarseInitializer。CoarseInitializer::trackFrame 处理完成之后,在 FullSystem::initializerFromInitializer 中为第一帧生成 pointHessians,一共2000个左右。随后将第二帧作为 KeyFrame 输入到 FullSystem::deliverTrackedFrame,最终流入 FullSystem::makeKeyFrame。

1)CoarseInitializer::trackFrame

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

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

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 相当于估计的真值,用于计算误差。

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

2. 计算新帧位姿

主要在函数FullSystem::trackNewCoarse中。

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

3. 判断是否需要新增为关键帧,并执行相应操作

判断结果无非两种,是或不是

3.1 普通帧操作

  • 非关键帧的作用是更新窗口中关键帧 immaturePoints 的逆深度,FullSystem::makeNonKeyFrame 调用 FullSystem::traceNewCoarse 做这件事情。对于每一个 ImmaturePoint 是在 ImmaturePoint::traceOn 中完成的。
  • ImmaturePoint::traceOn 这个函数是将关键帧上的点与非关键帧影像进行匹配,得到在非关键帧影像上的位置,知道这个信息就可以更新深度了。先进行极线搜索,搜索得到一个比较好的值,再使用高斯牛顿优化。
  • 最后更新了 ImmaturePoint::idepth_min 和 ImmaturePoint::idepth_max,对于 immaturePoint 仅仅只有一个 idepth 的范围,没有确切的 idepth。

3.2 关键帧操作

  • 一开始的套路和非关键帧一样,调用 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 加入到窗口优化过程中。