DSO代码解析--优化

1.关键帧

如果当前帧被认为是关键帧，则进入函数FullSystem::makeKeyFrame：

1. 和非关键帧一样，利用当前帧对前面关键帧中的未成熟点进行逆深度更新FullSystem::traceNewCoarse；
2. 标记后面需要边缘化（从活动窗口踢出）的帧FullSystem::flagFramesForMarginalization；
3. 将当前帧加入到滑动窗口中frameHessians.push_back(fh)，并计算一下该窗口中其他帧与当前帧之间的一些参数比如相对光度、距离等FullSystem::setPrecalcValues；
4. 将当前帧加入到总的能量函数中EnergyFunctional（ef）；
5. 遍历窗口中之前所有帧的成熟点pointHessians，构建它们和新的关键帧的点帧误差PointFrameResidual，加入到ef中；
6. 激活窗口中之前所有帧中符合条件的未成熟点，将其加入到ef中FullSystem::activatePointsMT；
7. 利用高斯牛顿法对活动窗口中的所有变量进行优化FullSystem::optimize；
8. 去除外点FullSystem::removeOutliers；
9. 边缘化不需要的点EnergyFunctional::marginalizePointsF；
10. 在当前帧中提取未成熟点FullSystem::makeNewTraces；
11. 边缘化不需要的帧FullSystem::marginalizeFrame。

1.1 边缘化决策

主要两点（FullSystem::flagFramesForMarginalization）：

1. 当活跃的帧的数量大于最低要求（５个）时，边缘化一帧中活跃的点少于５%或者和最新的帧相比光度参数变化剧烈( |a1−a2|>0.7)的帧（从最早的帧开始遍历）；
2. 如果过程１没有找到需要边缘化的帧，则从全部帧中找到除最近的两帧外离当前帧最远的一帧。

1.2 点的激活

DSO代码中PointHessian表示可用于追踪和参与局部优化的点，除了初始化的第一帧外，它来源于每次生成关键帧时对未成熟点的提取FullSystem::makeNewTraces，并在后续关键帧生成时进行激活FullSystem::activatePointsMT，成功激活的点就由ImmaturePoint变为PointHessian，激活的基本步骤如下：

1. 根据当前窗口中已有的成熟点的数量ef->nPoints，设置激活阈值currentMinActDist；
2. 将所有的成熟点投影到当前帧，生成距离地图CoarseDistanceMap::makeDistanceMap（比如位置p有一个投影点了，那么位置p的值设为0，周围一圈像素设为１，再外面一圈设为２，以此类推，迭代进行）；
3. 遍历所有的未成熟点，如果满足一些条件比如上一次的投影轨迹长度（极线）小于8，quality（次最小误差比最小误差）大于3等，就将逆深度设为其最大值和最小值的平均，将其投影到当前帧，然后考虑其在距离地图上的值，如果该值足够大（用到了第一步中的激活阈值），可以认为该点附近没有成熟点，所以将其加入待优化序列里，反之，则删除该点；
4. 对待优化序列里的未成熟点进行优化FullSystem::activatePointsMT_Reductor，然后激活；

未成熟点的优化是对其逆深度迭代优化。逆深度求导的过程和DSO初始化中的类似，额外加入了一个和点的梯度有关的系数$w_p$，梯度越大，权重系数越小：

$J_{\rho_{1}}=\frac{\partial f(\mathbf{x})}{\partial \rho_{1}}=w_{p} \sqrt{w_{h}} \rho_{1}^{-1} \rho_{2}\left(\nabla I_{x} f_{x}\left(t_{x}-u_{2}^{\prime} t_{z}\right)+\nabla I_{y} f_{y}\left(t_{y}-v_{2}^{\prime} t_{z}\right)\right)$

2 滑动窗口法

求导的参数：相对的光度参数，相对位姿，主帧上点的逆深度，相机内参

对相机位姿求导：对应代码中：Vec6f Jpdxi[2]

$\frac{\partial r_{k}}{\partial \delta \xi_{21}}=\frac{\partial r_k}{\partial x_2}*\frac{\partial x_{2}}{\partial \xi_{21}}=[d_x,d_y]*\frac{\partial x_{2}}{\partial \xi_{21}}$

$\frac{\partial x_{2}}{\partial \xi_{21}}=\left[\begin{array}{ccccc}f_{x} \rho_{2} & 0 & -f_{x} \rho_{2} u_{2}^{\prime} & -f_{x} u_{2}^{\prime} v_{2}^{\prime} & f_{x}\left(1+u_{2}^{\prime 2}\right) & -f_{x} v_{2}^{\prime} \\ 0 & f_{y} \rho_{2} & -f_{y} \rho_{2} v_{2}^{\prime} & -f_{y}\left(1+v_{2}^{\prime 2}\right) & f_{y} u_{2}^{\prime} v_{2}^{\prime} & f_{y} u_{2}^{\prime} \\ 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$

对逆深度求导：对应代码中：Vec2f Jpdd;

$\left[\begin{array}{l}f_{x} \rho_{1}^{-1} \rho_{2}\left(t_{21}^{x}-u_{2}^{\prime} t_{21}^{z}\right) \\ f_{y} \rho_{1}^{-1} \rho_{2}\left(t_{21}^{y}-v_{2}^{\prime} t_{21}^{z}\right)\end{array}\right]$

对相机内参求导：VecCf Jpdc[2];

$\begin{array}{c}\frac{\partial x_{2}}{\partial C}=\left[\begin{array}{cccc}\frac{\partial u_{2}}{\partial f_{x}} & \frac{\partial u_{2}}{\partial f_{y}} & \frac{\partial u_{2}}{\partial c_{x}} & \frac{\partial u_{2}}{\partial c_{y}} \\ \frac{\partial v_{2}}{\partial f_{x}} & \frac{\partial v_{2}}{\partial f_{y}} & \frac{\partial v_{2}}{\partial c_{x}} & \frac{\partial v_{2}}{\partial c_{y}}\end{array}\right] \\ \end{array}$

$\begin{aligned} \frac{\partial u_{2}}{\partial f_{x}} &=u_{2}^{\prime}+f_{x} \frac{\partial u_{2}^{\prime}}{\partial f_{x}} =u_{2}^{\prime}+\rho_{2} \rho_{1}^{-1}\left(r_{31} u_{2}^{\prime}-r_{11}\right) f_{x}^{-1}\left(u_{1}-c_{x}\right) \\ \frac{\partial u_{2}}{\partial f_{y}} &=f_{x} \frac{\partial u_{2}^{\prime}}{\partial f_{y}} =f_{x} f_{y}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{32} u_{2}^{\prime}-r_{12}\right) f_{y}^{-1}\left(v_{1}-c_{y}\right) \\ \frac{\partial u_{2}}{\partial c_{x}} &=f_{x} \frac{\partial u_{2}^{\prime}}{\partial c_{x}}+1 =\rho_{2} \rho_{1}^{-1}\left(r_{31} u_{2}^{\prime}-r_{11}\right)+1 \\ \frac{\partial u_{2}}{\partial c_{y}} &=f_{x} \frac{\partial u_{2}^{\prime}}{\partial c_{y}} =f_{x} f_{y}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{32} u_{2}^{\prime}-r_{12}\right) \\ \frac{\partial v_{2}}{\partial f_{x}} &=f_{y} \frac{\partial v_{2}^{\prime}}{\partial f_{x}} =f_{y} f_{x}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{31} v_{2}^{\prime}-r_{21}\right) f_{x}^{-1}\left(u_{1}-c_{x}\right) \\ \frac{\partial v_{2}}{\partial f_{y}} &=v_{2}^{\prime}+f_{y} \frac{\partial v_{2}^{\prime}}{\partial f_{y}} =v_{2}^{\prime}+\rho_{2} \rho_{1}^{-1}\left(r_{32} v_{2}^{\prime}-r_{22}\right) f_{y}^{-1}\left(v_{1}-c_{y}\right) \\ \frac{\partial v_{2}}{\partial c_{x}} &=f_{y} \frac{\partial v_{2}^{\prime}}{\partial c_{x}} =f_{y} f_{x}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{31} v_{2}^{\prime}-r_{21}\right) \\ \frac{\partial v_{2}}{\partial c_{y}} &=f_{y} \frac{\partial v_{2}^{\prime}}{\partial c_{y}}+1 =\rho_{2} \rho_{1}^{-1}\left(r_{32} v_{2}^{\prime}-r_{22}\right)+1 \end{aligned}$

对光度参数求导：对应代码VecNRf JabF[2];

$\begin{aligned} \delta_{\text {photo }} &=\left[-e^{a_{j i}},-b_{j i}\right] \\ \mathrm{J}_{\text {photo }} &=\frac{\partial r_{k}}{\partial \delta_{\text {photo }}}=\left[\frac{\partial r_{k}}{\partial-e^{a_{j i}}} \frac{\partial r_{k}}{\partial-b_{j i}}\right] \\ &=\left[I_{i}\left[\mathbf{p}_{i}\right]-b_{i} \quad 1\right] \end{aligned}$

3.优化过程中零空间的处理

对于纯视觉SLAM，其零空间有7维：3 rotation+3 translation+1 scale；对于VIO系统而言，充分的加速度计激励可以为系统提供尺度信息，加速度计同时可以提供重力方向，使得pitch和roll可观，因此其不客观的维度为4维：3 translation+1 yaw。

对于位置的不可观可以这样理解，当地图整体移动，整个SLAM的优化问题是不变的。所以SLAM中的零空间其实是整个优化问题的零空间，而不是说是优化中某个节点的零空间。就是说整个优化问题存在不可观的维度，这个不客观的维度会通过优化问题进而影响到某个节点的优化，导致那个节点出现问题，常见的比如说纯视觉SLAM在转弯的时候，表现在地图中可能是地图的尺度突然缩小，相机移动量也对应缩小，同时不会对能量函数造成影响

在数学上： 对于正规方程： $\mathbf{H} \Delta x=\mathbf{b}$ (1)

其中hession矩阵的零空间满足：$\mathbf{H} \Delta x_{n s}=0$ (2)

结合两个式子，一定有：$\mathbf{H}\left(\Delta x+\Delta x_{n s}\right)=\mathbf{b}$

因此在零空间上的漂移不会违反系统的约束,但是会对结果产生影响

初始化一般进行归一化，相当于是人为设了一个尺度，为什么还会有尺度问题呢？这是因为DSO采用的滑动窗口法进行优化，当前面的关键帧离开滑动窗口后，最多只