DSO 系列文章(2)——DSO点帧管理策略

DSO代码注释：https://github.com/Cc19245/DSO-CC_Comments

1.点所构成的残差Residual的管理

1.1.前端残差的状态

PointFrameResidual的状态有三种：

• IN——内点。表示这个残差状态正常，可以参与优化；

• OOB——Out Of Boundary，出界点。表示把host帧上的点通过线性化点或最新状态的相机位姿投影到target帧之后，这个点不在图像范围内，表示这个点出界了，那么这个残差不能参与优化。

  注意：一个残差只要被判断为出界，后面就再也不用他了，作者代码中也有一句注释叫做can never go back from OOB。这个确实也是有道理的，因为前端已经跟踪得到了一个初始位姿了，有投影匹配的点是不太可能出界的。

• OUTLIER——外点，表示这个残差能量超过了阈值，注意和OOB区分。外点类似于特征点法中的无匹配，也就是找到了匹配关系，但是误差太大，如果使用它优化会造成不好的影响，干脆就不用这个残差。

  注意：随着优化的进行，外点的残差能量有可能会慢慢降低到小于阈值，此时它就可以恢复成内点，然后继续参与优化。因此外点是可以恢复的。

为了让OOB起到一票否决的作用，在代码中使用了state_state变量表示这个残差上次的状态，一旦它上次的状态是OOB，那么函数都会直接跳过或返回，从而这个残差永远不可能再次参与到优化中。

1.2.后端点的残差的状态

EFResidual的状态就比较简单了，只有一个变量bool isActiveAndIsGoodNEW来表示这个点能量残差是否在后端优化中使用。这个变量是由前端残差的状态来设置的，只有前端残差的状态是IN，这个变量才是true，否则全都是false。

那么什么时候设定后端残差的状态呢？就是调用applyRes把前端雅克比传递给后端的时候，此时会一并把前端残差的状态传递给后端，让后端残差的状态得到更新。下面对几个函数进行说明：

• PointFrameResidual::linearize

  （1）前端残差进行线性化求雅克比。注意这个过程中有两部分雅克比， 一个是像素点关于相机位姿、内参、逆深度部分雅克比，它们使用线性化点的状态，保持不变；另一个是像素梯度和残差，它们使用最新的状态，因此是变化的。

  （2）在此过程中会判断前端残差的状态，存储到前端残差的类成员变量中。

• ``PointFrameResidual::applyRes`

  把前端残差更新到后端，同时把前端残差的状态也更新到后端（这个很正常，因为残差都给后端了，自然也要告诉后端这个残差是否有效）

• FullSystem::linearizeAll_Reductor(true/false)

  （1）一定会调用PointFrameResidual::linearize计算最新的雅克比，并且判断前端残差状态。

  （2）如果传入true，会把前端雅克比和残差状态更新到后端，并且把前端非IN状态的残差放到toRemove数组中。

• FullSystem::linearizeAll(true/false)

  （1）一定会调用FullSystem::linearizeAll_Reductor(true/false)。

  （2）如果传入true，那么会把linearizeAll_Reductor函数里统计出来的非IN状态的残差，从PointHessian和后端优化的大bossEnergyFunctional中丢弃，也就是这个残差不再存在了。

  注意：什么时候这个函数会传入true呢？答案是只有在完成迭代优化的循环之后才会传入true，因为此时已经得到了本次滑窗优化的最终结果，可以把使用这个最终结果判断的非IN状态的残差从前端和后端删掉了。但是其他任何时候，比如迭代优化之前或迭代优化过程中，我们只能对残差求雅克比、判断它们状态，如果不是IN那么后端优化就不使用，但是我们不能把这些残差删掉，因为优化还没开始或者还没完成，可能这次是OUTLIER的残差，优化几次就变成了IN，所以我们不能删掉这些残差。

1.3.点的某个残差的删除

如上所述，在FullSystem::optimize中后端迭代优化完成后，会调用FullSystem::linearizeAll(true)根据最新的状态把所有残差重新线性化一次，由于传入了true所以会筛选出非IN状态的残差，并把他们删除掉，该函数中实现的删除残差的步骤如下：

1. EnergyFunctional::dropResidual删除后端残差：先把这个残差从后端能量点的残差数组中弹出，然后把前端对应的残差持有的这个后端残差指针置零，最后delete释放内存。
2. FullSystem::deleteOut删除前端残差：先把前端点的残差数组中要删除位置的残差的指针delete释放到，然后把数组最后一个残差赋值到这个位置。其实本质上和删除后端残差的操作一致，只不过释放指针和弹出数组的先后顺序不同。

2.点Point的管理

2.1.如何删除点——点Point的删除

如上所述，在FullSystem::optimize中后端迭代优化完成后，会根据最新的状态把所有残差重新线性化一次，同时筛选出非IN状态的残差，并把他们删除掉。出了FullSystem::optimize函数之后，执行FullSystem::removeOutliers函数，把Point的外点删除。

此函数中就是判断所有帧上的所有点，如果这个点的残差个数为0，那么他就要被删除掉了。所以从这里删除的判断条件来看，叫removeOutliers可能不是太恰当，可能叫removePointsNonRes，即删除没有残差的点更好一点。

下面细看这个函数中的实现步骤：

1. 先把这个PointHessian加入到帧的前端点删除数组pointHessiansOut中，等待后面统一删除。设置这个前端PointHessian对应的后端EFPoint状态为PS_DROP，给后面后端删除EFPoint使用。然后把这个点从帧的点数组中删掉，注意这里还没有delete释放这个PointHessian的内存，只是把它加入到了外点数组中。
2. 调用EnergyFunctional::dropPointsF把后端的点删掉。此函数中调用removePoint，该函数中会先把这个EFPoint对应的所有EFResidual删掉，然后把这个点从能量帧的点能量数组中删掉，最后把这个delete这个能量点释放内存。

注意：从上面两步可以看出来，这里的代码设计还是有点瑕疵的，因为删除前端的点之后没有释放内存，而是等待后面统一释放。而删除后端的点则释放了内存，前后端的操作不统一了。

7.28最新想法：可能作者是故意这样设计的，因为看代码中显示部分好像有删除点的数组相关的内容，所以就是把这些点存到数组中来显示历史上的所有点？

7.29最新想法：这个其实是没有问题的，确实是作者有意为之，而且原因也就是删除的点还要被使用。因为上面也说了，要删除的PointHessian指针被放到了前端点的删除数组pointHessiansOut中，后面要给显示线程使用。即然被放到了这个数组中，自然这个指针就不能释放了，否则这个数组中存的就都是野指针了。而把这些点从正常使用的前端点数组pointHessians中弹出，就意味着以后肯定无法再使用这个点了，因此从系统功能上已经实现了删除这个点的目的。而对于后端来说，既然这个点被删除了，那么我后端优化肯定就不会使用它构造H/b了，因此完全可以把这个点能量EFPoint完全从内存中抹去。

2.2.边缘化时删除哪些点？

如上述所说，在FullSystem::optimize中后端迭代优化完成后，会根据最新的状态把所有残差重新线性化一次，同时筛选出非IN状态的残差，并把他们删除掉。出了FullSystem::optimize函数之后，执行FullSystem::removeOutliers函数，把哪些没有残差的Point删除掉。

接下来系统就进入了边缘化的阶段，首先是判断哪些点要边缘化掉，这样就会利用这些点构成的残差计算一个H/b，然后把这些点的逆深度Schur消元掉，只保留68x68的相机状态。然后下面对关键帧进行边缘化，再把这个H/b消掉一个相机帧得到维度缩减的HM/bM，从而给下一帧使用。

那么如何判断哪些点要被边缘化掉或者直接丢掉呢？在函数FullSystem::flagPointsForRemoval中实现这些功能。这个函数中选择要边缘化或者删除的点，只有两个根据：

（1）即将被边缘化的帧上的点：显然帧都没了，帧上的点肯定也不能再存在了。所以如果这个点性质比较好（比如多次构成的残差足够多、优化的逆深度协方差足够小），那么就把它边缘化掉，从而给后面的帧形成约束；如果这个点的性质不好，那么就直接把它丢掉，而不能边缘化，因为这样可能会引入错误的约束；

（2）其他帧上的点根据性质判断：即代码中的PointHessian::isOOB函数，内部会判断非边缘化帧上的点是否要被选择边缘化或者丢弃，这部分判断准则还不是很明白。

筛选出来这些点之后，在代码中就是对这些点进行处理，要么边缘化，要么直接丢掉。而如果一个点既不是外点，也不是要边缘化/丢掉的点，那么它就是正常的点，会继续存在于滑窗中，因此对它不进行任何处理，这就是为什么 代码 中elseif后面没有else了，也就相当于一个空的else，即如果是else则什么也不干。

最后注意：跟优化之后接着删除点一样，也上面的函数里面也是只把要丢掉的前端点PointHessian放到了删除数组中，并且从使用数组中弹出，这样就是完成了前端点的删除。而后端的对应的能量点在这个函数中没有释放，所以还要再释放后端的能量点，但是这个释放放在了FullSystem::flagPointsForRemoval函数外面去调用，这是我觉得不太好的地方，放在该函数里面调用更好。而且这个函数命名也不准确，我觉得命名为MarginOrDropPoints更好，也就是边缘化或者丢掉点。同理应该把后面的ef->marginalizePointsF();函数也一并放进里面调用，实际就完成了整个点的边缘化。这样拆开反而逻辑没有那么清晰了。

3.帧FrameHessian的管理

1. 在上一步边缘化掉点之后，得到了这些点的残差构成的HM/bM矩阵，但是这个矩阵中仍然是含有要被边缘化掉的帧的状态的，因此还要对这个HM/bM进行Schur消元得到消元之后维度缩减的HM/bM，给下次滑窗优化使用。

2. 在后端对边缘化的帧进行Schur消元之后，还有一个步骤需要做，那就是把其他帧上以被边缘化掉的帧为target帧的那些点的残差删掉，这个也是很正常的，因为这帧没有了，自然其他帧上的点和这帧关联的残差也就没有了。注意这个不是说其他帧上的点也不用了，这些点还是和别的帧可以构成残差的，所以这个和边缘化/丢掉点是不同的，这个是对仍然存在于滑窗中的点的残差进行处理。

3. 而删除点的某个残差，则跟上面1.3节讲述的一样了，那就是先dropResidual删掉后端残差，再deleteOut删除前端残差。

4. 把残差处理完了，就要删除这个帧了，调用deleteOutOrder函数把这个帧从FullSystem的帧数组中弹出，这就保证了它不会在FullSystem中再被调用参与滑窗优化了，但是不会释放它的内存。这个跟PointHessian的删除逻辑是一样的，本质上是因为这个FrameHessian的指针在优化之前已经被加入到allFrameHistory里面了，如果在这里把指针释放了，那么allFrameHistory里面存的就是野指针了。

5. 由于删除了滑窗中的一个关键帧，所以需要调用setPrecalcValues函数重新计算各个帧的线性化点状态、最新状态、状态增量等变量，然后还要调用ef->setAdjointsF重新计算各个帧之间的伴随矩阵。至此，帧边缘化的操作全部完成。