DSO代码阅读

最近再看DSO的代码，这里总结一下。这里首先介绍一些变量的名称便于之后的理解。

变量及函数名称简介

FrameHessian

EFFrame其中的EF表示的是Energy Function，类似的是EFPoint。这两个类分别被FrameHessian和PointHessian调用。
AffLight表示光度模型和曝光量的两个参数a，曝光量。b光度学模型的偏置。
trackNewCoarse 中关心的内容是三帧分别是slast、sprelast、lastF。lastF表示最近的参考帧coarseTracker->lastRef。slast、sprelast表示allFrameHistory中的最新的两帧图像。
allFrameHistory的构造，他是在addActiveFrame中添加的，先构造空的结构，然后把图像数据丢进去，使用makeImage函数。
makeImages 构造dI，梯度图像。
coarseTracker->lastRef在coarseTracker类的成员函数setCoarseTrackingRef中设置，frameHessians.back出一帧。frameHessians在makeKeyFrame中pushback。
Hessian矩阵是8X8的，其中SE3为6个，光度为2个。
CPARS = 4, PARS表示intrinsic parameters。VecCf表示 CPARS 数量的float vector。
CalibHessian 内参数校正和处理。

pixelSelector and pixelSelector2

两者被被CoarseInitializer中的setFirst调用。
另外后者还被makemaps调用，出现在trackNewCoarse中。

系统的流程下图所示：流程图

前端

DSO的前端比较容易理解，首先从数据集合或者订阅ROS topic，读入图像，然后对图像简单的处理。系统首先构造一个类FullSystem，在这个类中定义了系统的顶层函数，如下图所示。黑色字体是前端使用的一些函数，后端的优化函数是红色字体。FullSystem首先执行active frame，在其中跟踪第一帧，使用CoarseInitializer完成系统的初始化，然后跟踪新到帧。对于新到帧处理为trackNewCoarse函数。进入trackNewCoarse函数可以看到内部，使用calcRes函数计算残差，然后SSE加速计算jacobian，最后使用Eigen函数idlt求解线性方程。

trackNewCoarse函数的主要内容是

push many tries，这些tries是根据上一次的运动量做的估计，分别在上一帧的运动上上增加了许多微调，单倍上次运动，双倍运动，上次运动的一半。还有xyz三个轴分别扰动0.05。
for every tries, do trackNewestCoarse。其中trackNewestCoarse做的事有
- 根据tries计算对应残差
- 根据根据残差和梯度计算雅克比矩阵
- 迭代优化位姿

Initializer

CoarseInitializer中使用了类似CoarseTracker中的跟踪方法，使用trackerframe完成点的匹配和点的建立。如图所示.

在trackframe中calcResAndGS计算残差，雅克比和Hessian，函数applystep中如果点是好的，那么将点保存下来。dostep函数中完成了b, step 等变量的更新等。calcEC中使用AccumulatorX累计残差。

关键帧

FullSystem在执行active frame完之后，进入dilver tracker frame，这里首先进行display image，然后进行make key frame。其中makeKeyFramem包括：

trace new coarse
flag frame for marginalization
add frame to Struct
add new residuals for old points
active point in marginalization
optimization
remove outliers
get nulll space
marginalize points
add new Immature points & new residuals
marginalize frames

traceNewCoarse 的主要作用是给被跟踪的点分类。该函数被makeKeyFrame和makeNonKeyFrame被调用。两者在mappingLoop和makeNonKeyFrame中分情况对立出现。

关于optimization

后端的优化主要包括计算残差、线性化点、计算能量L以及能量M、Apply Res Reductor、系统迭代求解。

DSO的后端很大。这里主要包含四个类分别是EFResidual、EFFrame、EFPoint以及EFFunctional。EFResidual主要负责计算残差，成员函数很简单，仅有takeDataF()。该函数用来计算JI_JI_Jd，EFFrame、EFPoint都有这个函数。

另外三个类主要提供了成员变量，这些数据的操作在EFFunctional中，四个类的成员变量如表所示他们之间的关系如下图所示

计算残差

在ImmaturePoint类中定义了函数calcResidual和linearizeResidual，分别用来计算残差以及计算残差的线性化。这些残差在优化中中被再次使用，累计优化。

residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]);

线性化

线性化是优化的前提，在程序中，对于每个点Hessian的计算有三种方式，分别是激活点，线性化点和边缘化点，但是在类EFPoint张并没有设置标志这些类的标志，那么为什么要使用不同的模式来计算Hessian呢？

template <int mode>
void AccumulatedTopHessianSSE::addPoint(
    EFPoint *p, EnergyFunctional const *const ef,
    int tid) // 0 = active, 1 = linearized, 2=marginalize

AccumulatedTopHessianSSE中的addpoint是一个模板函数，参数化模式。在程序中给出了三种累加器，见下图，分别是AccumulatorXX，AccumulatorX，AccumulatorApprox。前面两种用于accSSE_bot，最后一个用于accSSE_top_L以及accSSE_top_A。三者在solvesystem中被先后调用，三个函数对点不做区分的计算，因此每个点都参与这三种方式的Hessian计算，最后累加在一起：

MatXX HT_act = HL_top + HA_top - H_sc;
VecX bT_act = bL_top + bA_top - b_sc;
HFinal_top = HT_act + HM;
bFinal_top = bT_act + bM_top;

EFPoint中包含了很多Hcd以及bd，这些被由AccumulatorXX 或者AccumulatorX构造的累计器累计。RawResidualJacobian构造了许多雅克比项，这些由linearize计算，得到Hessian，然后在accSSE_top_L和accSSE_top_A累计。线性化完毕之后计算L Energy和M energy，这两者之间的不同在于前者使用了激活点和线性化点的Hessian，后者计算了边缘化（schur complement操作）之后的Hessian。如下图所示。其中对目标函数线性化得到Hessian。在计算JI^T * r 和 Jab^T * r中，激活点的r只是残差，而线性化能量是通过雅克比矩阵计算，这里体现relinearize。论文中提到多次线性化能够降低线性化误差。

边缘化

边缘化的对象主要有2种，分别是pose和point。

边缘化关键帧位于函数ef-marginalizeFrame()
边缘化路标点位于函数ef-marginalizePointF()。

边缘化帧的时候直接修改Hessian矩阵，而边缘化点的时候需要首先操作点。边缘化帧的中ef->marginalizeframe首先添加先验，然后乘以scale，invert bottom part，然后使用schur操作。在边缘化点中，对于buffer中每一帧，对于帧中的每个点，首先获取残差，使用accSSE_top_A和accSSE_bot累计点，然后accSSE_top_A以及accSSE_bot先后stitchDouble，重新计算H，b。最后丢弃点ef->dropResidual(r->efResidual)。

一些区分

stitchDouble、getStitchedDeltaF、setDeltaF

stitchDouble是accumulateTop的成员函数，在addpoint中计算的H的基础上叠加了host、target的位姿，最后加上先验信息。

H.block<8, 8>(hIdx, hIdx).noalias() += EF->adHost[aidx] *
                                             accH.block<8, 8>(CPARS, CPARS) *
                                             EF->adHost[aidx].transpose();

stitch就是拼接的意思，拼接两个位姿以及两者之间的变换成为一个闭环。该函数被调用的时候紧跟addpoint函数。

The only difference to the classical reprojection error is the additional dependency of
each residual on the pose of the host frame, i.e., each term depends on two frames
instead of only one. While this adds off-diagonal entries to the pose-pose block of the
Hessian, it does not affect the sparsity pattern after application of the Schur
complement to marginalize point parameters. The resulting system can thus be solved
analogously to the indirect formulation. Note that the Jacobians with respect to the two
frames’ poses are linearly related by the adjoint of their relative pose. In practice,
this factor can then be pulled out of the sum when computing the Hessian or its Schur
complement, greatly reducing the additional computations caused by more variable
dependencies.

原文中有这样一段话，大意是这样的。传统的重投影模型依赖于当前帧的位姿。而本文中使用的重投影模型依赖于2帧，分别成为host和target，我们会以利用两者之间为的位姿差。这样做虽然会在Hessian矩阵中pose-pose块中增加一些非对角元素，然而这不影响schur补边缘化点之后的稀疏模式。这样使得系统的求解类似与indirect方法的形式。其中雅克比相对于帧间位姿是线性的，这句话不太明白（ the Jacobians with respect to the two frames’ poses are linearly related by the adjoint of their relative pose.）。当然最终结果是简化计算量。

getStitchedDeltaF的内容非常简单，全部如下

VecX EnergyFunctional::getStitchedDeltaF() const {
  VecX d = VecX(CPARS + nFrames * 8);
  d.head<CPARS>() = cDeltaF.cast<double>();
  for (int h = 0; h < nFrames; h++)
    d.segment<8>(CPARS + 8 * h) = frames[h]->delta;
  return d;

就是计算cDeltaF和frame中的变量delta之间乘积。问题是这里的delta是什么意思，乘积之后的结果是为了计算M Energy，公式是delta.dot(2 * bM + HM * delta)。

setDeltaF函数是为了计算frame->delta，f->delta_prior，adHTdeltaF。delta_prior用于计算Hessian及能量的先验部分，先验根据该值的大小设定一个阈值，也就是目标函数中的huber 估计。

地图在哪里？

我们想把产生的点取出来，怎样找到那些点呢？很简单，因为所有的点都在pangolin中显示，我们去哪里找就行。我们可以看到在setFromKF中把当前帧中的immaturePoints、pointHessians、pointHessiansMarginalized、pointHessiansOut中所有的点加入一个数组，然后显示出来。数据结构详见下图这里首先从u、v、depth计算，然后转化到相机坐标系(x,y,z)，然后就可以加入这个数组中了，问题是这些点是相机坐标系的表示，需要转化到世界坐标系中，我们没有看到这个转化，但是显示却是对的，这时为什么？

后来仔细查找才会发现，refreshPC中有一个函数为

Sophus::Matrix4f m = camToWorld.matrix().cast<float>();
  glMultMatrixf((GLfloat *)m.data());

因此在pangolin gl中的点先经过这样的的一个矩阵乘法才显示出来，真的是大大节约了计算量。这里针对每隔一阵完成一个refreshPC

for (KeyFrameDisplay *fh : keyframes) {
        float blue[3] = {0, 0, 1};
        if (this->settings_showKFCameras)
          fh->drawCam(1, blue, 0.1);

        refreshed = +(int)(fh->refreshPC(
            refreshed < 10, this->settings_scaledVarTH, this->settings_absVarTH,
            this->settings_pointCloudMode, this->settings_minRelBS,
            this->settings_sparsity));
        fh->drawPC(1);
      }

点从哪里来

我们回溯点的形成中，终于发现这些点[u,v]在immaturePoints创建时，被添加，depth是之后逐渐更新的收敛的。添加的点受 if (selectionMap[i] == 0)控制，这里体现点的选择策略。

"博学之审问之慎思之明辨之笃行之。"