【svopro】追踪梳理

1. SVO2系列之深度滤波DepthFilter
2. svo_note
3. SVO（SVO: fast semi-direct monocular visual odometry）
4. SVO 半直接视觉里程计
5. 【DepthFilter】深度滤波器
6. 【svopro】代码梳理

1.0 processFrame主流程

UpdateResult FrameHandlerStereo::processFrame() 

1.0 Tracking部分 前端主要流程

直接法具体过程如下：
  step1. 准备工作。假设相邻帧之间的位姿$T_{k,k-1}$已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。
  step2. 重投影。知道$I_{k-1}$中的某个特征在图像平面的位置(u,v)，以及它的深度d，能够将该特征投影到三维空间$p_{k-1}$，该三维空间的坐标系是定义在$I_{k-1}$摄像机坐标系的。所以，我们要将它投影到当前帧$I_k$中，需要位姿转换$T_{k,k-1}$，得到该点在当前帧坐标系中的三维坐标$p_k$。最后通过摄像机内参数，投影到$I_k$的图像平面$(u\prime ,v\prime )$，完成重投影。
  step3. 迭代优化更新位姿 。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿$T_{k,k-1}$。
  将上述过程公式化如下：通过不断优化位姿$T_{k,k-1}$最小化残差损失函数。其优化函数为:
$$T_{k-1}^k=argmin{\iint }_{\Re }\rho [\delta I(T,u)]du$$
其中:

$\rho [\ast ]=0.5\Vert \ast {\Vert }^2$，可见整个过程是一个最小二乘法的问题；

$\delta I(T,u)=I_k(\pi (T\ast {\pi }^{-1}(u,du)))-Ik-1(u)$，这个残差对比的是图像位置上的像素值的灰度，其中u ∈ ℜ ,ℜ表示即可以在k-1帧图像上看到，又可以通过投影在k帧上看到
对于比对灰度值的算法来说，一般都会用一个patch size中的全部像素的灰度进行对比，因此下面的公式中都不仅仅使用一点像素，而是使用多点像素进行求解的，但是在这个过程中，作者为了提高计算的速度，并没有进行patch的投影，算是以量取胜吧

NOTE:公式中 ${\pi }^{-1}(u,du)$ 为根据图像位置和深度逆投影到三维空间，第二步 $T\ast {\pi }^{-1}(u,du)$ 将三维坐标点旋转平移到当前帧坐标系下，第三步 $\pi (T\ast {\pi }^{-1}(u,du))$ 再将三维坐标点投影回当前帧图像坐标。当然在优化过程中，残差的计算方式不止这一种形式：有前向(forwards)，逆向(inverse)之分，并且还有叠加式(additive)和构造式(compositional)之分。这方面可以读读光流法方面的论文，Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。选择的方式不同，在迭代优化过程中计算雅克比矩阵的时候就有差别，一般为了减小计算量，都采用的是inverse compositional algorithm。

优化目标函数：

把上述的notation带入到优化函数中就可以得到
$$T_{k-1}^k=argmin\sum _{i\in \Re }1/2\Vert \delta I(T_{k-1}^k,u_i){\Vert }^2$$
其中雅克比矩阵为图像残差对李代数的求导，可以通过链式求导得到：
$$J=\frac{\partial \delta I(\xi ,u_i)}{\partial \xi }=\frac{\partial I_{k-1}(a)}{\partial a}{|}_{a=u_i}.\frac{\partial \pi (b)}{\partial b}{|}_{b=p_i}.\frac{\partial T(\xi )}{\partial \xi }{|}_{\xi =0}.p_i=\frac{\partial I}{\partial u}.\frac{\partial u}{\partial b}.\frac{\partial b}{\partial \xi }$$
对于上一帧的每一个特征点，都进行这样的计算， 在自己本来的层数上， 取那个特征点左上角的 4x4 图块。如果特征点映射回原来的层数时，坐标不是整数，就进行插值，其实，本来提取特征点的时候，在这一层特征点坐标就应该是整数。把图块往这一帧的图像上的对应的层数投影，然后计算雅克比和残差。 计算残差时， 因为投影的位置并不刚好是整数的像素，所以会在投影点附近插值，获取与投影图块对应的图块。最后，得到一个巨大的雅克比矩阵，以及残差矩阵。但是为了节省存储空间，提前就转换成了 H 矩阵 $ H =J *J ^ T$ 。

上面的非线性最小化二乘问题，可以用高斯牛顿迭代法求解，位姿的迭代增量ξ(李代数)可以通过下述方程计算：
$$\begin{gathered} J^{T}J\xi =-J^T\delta I(0) \\ H\xi =-J^T\delta I(0) \\ \xi =-H^{-1}{J}^T\delta I(0) \end{gathered}$$
然后，得到 $T(\xi )$ ，逆矩阵得到 $T(\psi )$ ，再更新出 $T_{k,k-1}$ 。然后，在新的位置上，再从像素点坐标，投影出新的点 $p_{k-1}$。每一层迭代 30 次。 因为这种 inverse-compositional 方法，用这种近似的思想，雅克比就可以不用再重新计算了。（因为重新投影出新的 p 点的位置， 这个过程没有在残差公式里面表现出来。） 这样子逐层下去，重复之前的步骤。

1.1 初始化追踪

FeatureTracker是初始化阶段的光流追踪实现，类中对象包含和相机个数对应的AbstractDetector和FeatureTracks，其中FeatureTracks是FeatureTrack类型的vector。

size_t FeatureTracker::trackFrameBundle(const FrameBundlePtr& nframe_kp1) {...}

功能描述：
该段代码实现了对帧捆绑进行特征跟踪的功能。

输入：

1. nframe_kp1: 帧束智能指针，指向要跟踪的一组连续帧。

输出：
返回值：返回当前所有跟踪到的特征点数量总和。

实现功能：
针对输入的帧束，对每个frame_index对应的帧进行特征跟踪，将每个已有轨迹上的特征点在下一帧视野内跟踪。已终止轨迹的特征点不需要跟踪，并且不会存储在轨迹中。特征点的轨迹用 “FeatureTracks” 数据类型存储，轨迹矢量列为 “active_tracks_”，已终止轨迹以及其尾缀存储在 “terminated_tracks_” 中。跟踪结果（即新提取的关键点）存储在输出帧 “nframe_kp1” 的 “px_vec_” 、“score_vec_” 和 “track_id_vec_” 中。

算法步骤：

1. 将已终止的轨迹进行重置。
2. 遍历帧束中的每一帧：
   • 获取帧索引 frame_index 对应的特征矢量列 tracks 和帧智能指针 cur_frame。
   • 创建空向量 remove_indices，表示需要删除的关键点索引。
   • 创建新的像素坐标、得分和轨迹ID矢量列（类型为Keypoints, Scores, 以及 TrackIds），长度为当前矢量列 tracks 的长度。
   • 遍历每一个 track_index 相应的跟踪信息：
     • 获取该轨迹的最后一个或

1.1 帧间追踪sparseImageAlignment 解决如何计算帧与帧之间位姿变换的问题

该函数的作用是使用稀疏光流法优化当前帧相对于前一帧之间的相机位姿。稀疏光流求解器通过在前一帧中选取一些特征点，并在新帧（当前帧）中寻找相应的特征点，从而做到优化相机位姿的效果。该过程的具体步骤如下：

1. 重置和设置稀疏光流优化器，包括权重、最大要跟踪的特征数量等参数。
2. 如果拥有运动先验，则将其应用于图像对齐（优化相机位姿）的过程中。该步骤可以通过添加加权先验项，将imu传感器的运动信息融合到图像对齐的过程中，从而增强优化估计的鲁棒性。
3. 通过运行稀疏光流求解器，找到尽可能多的特征点，以优化相机位姿，并返回拥有匹配的特征点数。
4. 返回匹配到特征点的数量。

1.1.1 核心实现：

size_t SparseImgAlign::run(const FrameBundle::Ptr &ref_frames, 
                           const FrameBundle::Ptr &cur_frames) 

该函数的作用是运行稀疏光流法进行图像对齐，即通过优化相机的相对运动来最大化场景中的特征点在两帧图像中的匹配程度。该过程的具体步骤如下：

1. 检查并保证输入帧的有效性，获取其中的跟踪特征，并在必要时对其进行子采样。
2. 设置必要的成员变量，例如当前帧、参考帧、参考帧相对于世界坐标系的位姿。
3. 准备缓存以存储需要用到的中间变量，包括特征在两帧图像中的坐标（像素坐标）和空间中的位置（3D坐标），两帧图像之间的采样点之间的雅可比矩阵等。其中有一些变量可以在多个金字塔层次上共享使用，以加速算法的运行速度。
4. 在金字塔的每一层上，先计算一些预处理的通用变量。然后使用LM算法对特征点进行优化，最终得到两帧之间最佳的像素级对齐结果。
5. 将优化后的结果保存，并返回被跟踪的特征点数。

1.1.2 输入输出与更新全局变量：

输入：- ref_frames：指向参考帧的指针，存储了参考帧中的特征点和图像信息。
- cur_frames：指向当前帧的指针，存储了当前帧中的特征点和图像信息。
输出：- n_fts_to_track：表示成功跟踪的特征点数。
- 
在该函数的主体结构中，更新了以下几个全局变量：

- ref_frames_：参考帧。
- cur_frames_：当前帧。
- T_iref_world_：参考帧的imu坐标系到世界坐标系之间的变换。
- uv_cache_：特征点在两帧图像中的像素坐标，作为缓存提高运行效率。
- xyz_ref_cache_：特征点在参考帧中的3D空间坐标，作为缓存提高运行效率。
- jacobian_proj_cache_：两帧图像之间的采样点之间的雅可比矩阵，作为缓存提高运行效率。
- alpha_init_：初始化的亮度增益。
- beta_init_：初始化的亮度偏移。
- level_：当前运行所在的金字塔等级。
- mu_：LM算法中的缩放因子。
- have_cache_：一个布尔变量，用于表示是否已经准备好计算雅可比矩阵的缓存。

1.2 projectMapInFrame 解决如何计算帧与地图之间的位姿变换问题

函数的作用是在帧中投影地图（包含3D点和特征），并在图像中搜索匹配的特征。在1.1求解之后，我们得到了当前帧位姿的粗略估计。因为它是基于上一帧的结果来计算的，所以如果把它当作真实位姿估计的话，将有较大的累积误差。因此，需要进一步和地图之间进行特征点比对，来对当前帧位姿进一步优化。
该过程的具体步骤如下：

1. 遍历地图中的所有点，计算在当前帧的投影位置。由于当前帧有粗略的位姿估计，这个投影位置应该与真实位置有少量误差（2~3个像素）。
2. 对每个成功投影的地图点，比较这些点的初始观测图像与当前帧的图像。通过计算光度的误差，求取更精准的投影位置。这步类似于光流，在SVO中称为Refinement。
3. 根据更精确的投影位置，进行位姿与地图点的优化。这一步类似于Bundle Adjustment，但SVO实现中，是把Pose和Point两个问题拆开优化的，速度更快。判断是否生成关键帧，处理关键帧的生成。
4. 计算有重叠的关键帧，以便后续在这些帧中搜索匹配特征。
5. 如果启用了异步重投影，则为每个相机启动一个异步重投影任务，实现并行处理。
   否则，为每个相机执行投影任务

reprojectors_.at(camera_idx)->reprojectFrames(frame, overlap_kfs_.at(camera_idx), trash_points.at(camera_idx));

删除对于某一相机而言不再视野范围内的3D点，释放内存。
统计所有重投影器的匹配特征总数，并根据特定约束来判断是否匹配特征数量是否满足要求。
返回匹配特征的总数。

这里理解的难点是，地图点初次被观测到的图像与当前帧的图像进行比对时，不能直接对两个图像块求差，而需要计算一个仿射变换（Affine Warp）。这是因为初次观测和当前帧的位移较远，并且可能存在旋转，所以不能单纯地假设图像块不变。仿射变换的计算方式在PTAM论文的5.3节有介绍，似乎是一种比较标准的处理方式。（其实SVO的追踪部分和PTAM整个儿都挺像。）实现当中可能还需要注意一些细节。例如有些地方使用了网格，以保证观测点的均匀分布。还有Affine Warp当中需要注意特征点所在的金字塔层数，等等，参考高博。

1.2.1 实现步骤：

该函数的作用是向当前帧中重新投影3D地图点，从而找到匹配的二维特征点，以便在后续的位姿估计和优化中使用。实现中，函数首先将输入的相邻关键帧中的所有3D地图点与当前帧进行投影匹配，并将候选匹配点的相应信息存储在一个候选列表candidates_中。然后，该函数通过一系列的筛选条件对候选匹配点进行过滤和排序，并找到其中最佳的匹配点，并计算这些点的重投影误差（Reprojection Error），并将其存储在栅格地图（Occupandy Grid）中以进行后续的处理。

具体实现步骤如下：

1. 组合所有相邻关键帧中的有效3D地图点，并检查其有效性（是否超过最大重投影次数、是否足够成熟、是否已被标记为Outlier等）。
2. 将有效的3D地图点投影到2D图像平面上，并得到对应的2D特征点，将其保存到一个候选列表candidates_中。
3. 根据候选列表中的2D特征点，检查它们是否与当前帧中的2D特征点重叠，若重叠，则进行匹配。
4. 对匹配结果进行排序，并筛选最优的匹配结果。同时更新栅格地图中的区域占用情况，以避免在后续的迭代中提取过多的新特征点。如果特征点数已达到阈值，则结束投影过程。

1.2.2 输入输出与更新全局变量：

输入：

• cur_frame: 当前帧，带有以下信息：
  • 相机内参矩阵K和畸变矩阵distortion
  • 当前帧的id、世界坐标系下的位姿、2D特征点和对应的描述子等信息。
• visible_kfs: 相邻关键帧的列表。
• trash_points: 需要删除的3D地图点的列表，超出最大重投影次数或观测次数太少的3D地图点属于垃圾。
• max_n_fixed_lm: 从全局地图中选择的最大不可变地标数目。
• max_total_n_features: 当前帧可以使用的最大特征点个数，其中包括当前帧计算出来的点，以及从相邻关键帧中反投影得到的点的个数之和。

输出：

• 无。

更新的全局变量：

• candidates_: 存储在栅格化坐标系中的候选匹配列表（Candidate），其中包括从相邻关键帧中投影得到的匹配点和已经估计出的3D地图点。
• 栅格地图（Occupandy Grid）: 记录每个栅格中是否有特征点、是否被访问过等信息，用于后续的特征点更新和位姿估计等操作。
• trash_points: 需要删除的3D地图点的列表。

1.2.3 算法步骤1.1 ~ 1.2特点小结：

1. 金字塔的处理。这一步估计是从金字塔的顶层开始，把上一层的结果作为下一层估计的初始值，最后迭代到底层的。顶层的分辨率最小，所以这是一个由粗到精的过程（Coarse-to-Fine），使得在运动较大时也能有较好的结果。
2. SVO自己实现了高斯——牛顿法的迭代下降，并且比较取巧地使用了一种反向的求导方式：即雅可比在k-1帧图像上进行估计，而不在k帧上估计。这样做法的好处是在迭代过程中只需计算一次雅可比，其余部分只需更新残差即可（即G-N等式右侧的）。这能够节省一定程度的计算量。另一个好处是，我们能够保证k-1帧的像素具有梯度，但没法保证在k帧上的投影也具有梯度，所以这样做至少能保证像素点的梯度是明显的。
3. 根据更精确的投影位置，进行位姿与地图点的优化。这一步类似于Bundle Adjustment，但SVO实现中，是把Pose和Point两个问题拆开优化的，速度更快。
4. 地图点初次被观测到的图像与当前帧的图像进行比对时，不能直接对两个图像块求差，而需要计算一个仿射变换（Affine Warp）。这是因为初次观测和当前帧的位移较远，并且可能存在旋转，所以不能单纯地假设图像块不变。

1.3 optimizeStructure （类似Bundle Adjustment，把Pose和Point拆开优化的，速度更快）

该函数的作用是通过最小化各个特征点在不同关键帧之间的投影误差，以优化场景中所有的3D点的空间坐标。该过程的具体步骤如下：

1. 获取所有关键帧（frames）中的所有特征点，筛选出需要优化的特征点。特定的筛选条件为：该特征点必须有对应的3D点，并且不能是边缘点。
2. 根据空间坐标，对于所有待优化的3D点，运用LM算法进行优化，获取其在空间中的最优坐标。
3. 更新每个3D点上一次被优化的时间。
4. 返回优化完成时，被优化的3D点数量。

1.3.1 主接口输入输出与更新全局变量：

输入： frames: 存储有所有关键帧的指针。
输出：该函数没有明确的输出，但是在函数内部有一些全局变量被更新，包括：

• 所有3D点的空间坐标将被优化，并更新其优化的时间戳（last_structure_optim_）。

1.3.2 核心接口实现：

size_t FrameHandlerBase::optimizePose() 

该函数的作用是**对于所有帧（当前时刻对应的frame_bundle对应的观测），在全局范围内优化它们的相对位姿，即在不同图像帧之间找到特征点的对应关系以计算相机位置姿态**。该过程的具体步骤如下：

1. 重置位姿优化器（pose_optimizer_），以便于从一个干净的状态开始对所有帧进行位姿优化。
2. 如果有运动先验，则应用它们来约束该过程中优化的相机位姿，即通过在LM优化算法中添加先验权重的方式来衡量优化解决方案的可靠性。自我运动模型可以通过另一个数据流获得，也可以在相机的运动中进行模拟。
3. 运行位姿优化器，以估计所有帧之间的相对位姿，并在过程中尽量减少特征点匹配之间的重投影误差。
4. 在最后，返回优化后 SFBA 图中所有边的数量，表示优化解决方案中的约束数，用于求解相机位姿。

1.3.3 FrameHandlerBase::optimizePose接口输入输出与更新全局变量：

输入：该函数无任何输入。
输出：- sfba_n_edges_final：表示在位姿优化过程中使用的边的数量，用于表明位姿优化解决方案的约束数量。

更新了的全局变量：

该函数主要使用了位姿优化器（pose_optimizer_），对所有的帧进行优化，以更新帧之间的相对位姿。位姿优化过程可能会产生一个新的SFBA图，但这个图是保存在优化器内部的，而不是在函数之外的类中。因此，该函数没有明确更新全局变量的行为。

1.3.4 PoseOptimizer::run算法步骤：

size_t PoseOptimizer::run(const FrameBundle::Ptr &frame_bundle, 
						  double reproj_thresh_px)

该函数的作用是运行Gauss-Newton优化算法，**对输入的关键帧bundle中的所有帧进行位姿优化，以计算每个帧之间的相对位姿，从而使得所有3D点的投影误差最小化。**该过程的具体步骤如下：

1. 检查输入的帧是否满足要求，如果无法进行计算，则直接返回。
2. 获取第一个帧的IMU到世界坐标系中的变换（T_imu_world），用于初始求解。
3. 根据初始值计算初始状态下的误差，得到初始的测量方差（measurement_sigma_）。
4. 使用Gauss-Newton算法对所有帧的相对位姿进行优化，使得重投影误差最小化。
5. 将每个帧在世界坐标系下的位姿设置为相机到世界坐标系的转换矩阵（frame->T_f_w_ = frame->T_cam_imu() * T_imu_world）。
6. 移除重投影误差较大的噪点，并计算剩余边的数量。

1.3.5 位姿优化（重投影误差）PoseOptimizer::run接口输入输出与更新全局变量：

输入：

• frame_bundle：存储有所有关键帧的数据集指针。
• reproj_thresh_px：重投影误差阈值，用于剔除噪点。

输出：

• 优化后剩余边的数量，返回值减去被移除的边的数量。

更新了的全局变量：

• focal_length_：库伯恩-图克法测量的初始值标度长度.
• frame_bundle_：输入的关键帧束指针。
• T_imu_world: 表示第一个帧IMU到世界坐标系的变换。
• measurement_sigma_：估计的初始测量方差。
• n_meas_：测量（边）的总数。

1.3.6 三维点优化 FrameHandlerBase::optimizeStructure接口算法步骤

void FrameHandlerBase::optimizeStructure(
    const FrameBundle::Ptr &frames, int max_n_pts, int max_iter)

该函数的**作用是对3D点进行优化**。在优化过程中，使用多种观测作为约束条件来估计点的位置，从而提高点的位置精度。同时，通过最小化重投影误差，使点能更好地适应相机移动和姿态变化。具体来说，该函数采用LM算法或者高斯-牛顿法来迭代计算点的位置xyz坐标。每迭代一次，都计算一组新的重投影误差，更新雅克比矩阵、梯度向量，直到误差足够小或者达到迭代上限时停止。

此函数中使用的优化算法可以采用高斯-牛顿法或Levenberg-Marquardt算法等。其中包括两个重要步骤：

1.计算重投影误差: 遍历之前所有观测到该特征点的帧，计算投影点和特征点之间的误差（或描述为残差，如unit plane residual）。如果采用透视相机模型，则使用透视变换将特征点从相机坐标系转换到世界坐标系中，并在每个观测点处计算预测值，并以此计算重投影误差。

2.更新雅克比矩阵: 为了计算优化方程，需要计算雅克比矩阵（或描述为导数矩阵）。雅可比矩阵的元素由重投影误差对点位置的偏导数计算而来。

在计算过程中，需要对新误差进行比较并更新点的位置。如果重投影误差有所增加或更新后的点位置不满足优化要求，则需要撤销更新前的点位置。

1.4 makeKeyframe关键帧的生成

UpdateResult FrameHandlerStereo::makeKeyframe() 

1.4.1 关键帧的生成步骤

该函数用于决定当前帧是否需要成为新的关键帧，并且如果需要成为关键帧，则执行以下操作：

1. 如果当前帧的特征点数低于下限阈值（kfselect_numkfs_lower_thresh），则增加特征点数量，将相邻关键帧的一个帧作为新的关键帧，将所有新特征点加入到地图中，并进行三角化。
2. 选择当前帧作为新的关键帧，将其加入到地图中，将当前帧的所有新特征点加入到地图中。
3. 初始化新的深度滤波器、并将其与相应的特征检测器绑定，将当前帧中已经提取出的特征点对应的栅格置为已占用状态，将当前帧加入到深度滤波器中。
4. 根据相邻关键帧协助进行新特征点的深度滤波（Depth Filter）更新。
5. 如果当前地图中关键帧数量已经达到极限，则删除距离当前帧最远的关键帧（使用Ceres优化时，区别对待地图的删除操作）。

该函数的输出是一个枚举类型UpdateResult，代表着新一个新的关键帧已经被选定。

1.4.2 生成关键帧的主要条件如下：

1. 当前帧距离上一个关键帧的时间超过设定的阈值，即时间间隔超过options_.keyframe_min_rot、options_.keyframe_min_trans、options_.keyframe_min_inliers 中的任意一个。
2. 当前帧到与上一个关键帧的重叠帧（Overlapping frames）之一的运动量大于设定的阈值 options_.keyframe_max_overlap_R、options_.keyframe_max_overlap_t。
3. 当前有足够的2D特征点符合基础矩阵假设，即当前帧中符合基础矩阵投影约束的2D特征点数量需要达到 options_.keyframe_min_features个。

其中，条件（1）和（2）是为了控制关键帧之间的密度，防止在时间或空间上有大量的冗余关键帧。同时，条件（2）也是为了保证运动的光滑性，避免给VO模型带来过多的噪声。条件（3）则是为了保证特征点的分布均匀和数量合理。需要注意的是，除了这些条件之外，还可能会有其他条件：例如，当**当前帧与所有已有地图点的距离太远时，也可以将其作为关键帧以增加地图的范围。

1.4.3 关键帧的生成接口输入输出与更新全局变量：

输入：

• new_frames_: 存储最新帧信息的指针，包括当前帧和相邻关键帧。
• overlap_kfs_: 存储当前帧与最新相邻关键帧之间的重叠帧（Overlapping frames）列表。
• depth_filter_: 深度滤波器，用于更新3D点的深度估计。
• map_: 地图，存储关键帧和3D点等信息。
• bundle_adjustment_type_: 前端图优化的类型，目前主要支持两种优化算法：Ceres和g2o。
• options_: 系统参数类，包括多种系统参数，如特征点数量下限、生成关键帧条件等。

输出：

• 返回一个枚举类型UpdateResult，代表着新一个新的关键帧已经被选定。

更新的全局变量：

• map_: 将新的关键帧加入到地图中，并更新与相邻关键帧的关系。
• depth_filter_: 将当前帧和相邻关键帧添加到深度滤波器中，并更新3D点的深度估计。
• candidates_: 存储在栅格化坐标系中的候选匹配列表（Candidate），其中包括从相邻关键帧中投影得到的匹配点和已经估计出的3D地图点。
• 栅格地图（Occupandy Grid）: 记录每个栅格中是否有特征点、是否被访问过等信息，用于后续的特征点更新和位姿估计等操作。

1.5. problem

1.5.1 两个主要问题：

1. 关键帧选择：当前，只有当移动距离达到场景平均深度的15％时，才会选择关键帧。如果相机朝向前方，则场景深度非常大，并且不会选择新的关键帧。
2. 种子点更新：向前移动时，种子点的收敛速度不够快，因为视差非常小。问题在于，我们仅使用新种子点（seeds）创建之后的帧更新种子点。对于向前拍摄的相机，这是可以的，因为早期的帧尚未观察到该表面。但是，对于向前拍摄的相机，我们也应使用旧的关键帧来更新种子点。

1.5.2 关于种子点更新

将N个最近或重叠关键帧添加到新帧的深度滤波器中的困难是什么？

在DepthFilter中一次性添加多个关键帧（如新帧及其最近的N个关键帧）需要解决诸多问题：

1. 关键帧的数量：将N个最接近的关键帧添加进去需要考虑深度滤波器队列（queue）中能存放的关键帧的数量限制。队列中存放的关键帧数量多了，可能会增加计算负担，而数量太少则会减慢3D点深度收敛的速度。
2. 关键帧之间的距离：例如，有一组新的关键帧，其中有些关键帧越近，则其视角下的图像内容差别较小，它们之间的深度读数可以相互帮助更快地收敛（seed 合并）。
3. 帧之间的时间和大小：如果在关键帧之间差异很大，例如，相机姿态变化剧烈或场景遮挡很大，那么不同帧之间可能会产生很大的不一致性，导致更高的滤波误差。因此需要选择时间尺度类似、帧大小相似的关键帧来更新种子点。
4. 代码复杂性：同时处理多个帧的数据往往需要更多的代码来管理程序复杂度，并且需要处理许多输入和输出数据的交互，而这些数据涉及多方面的运算，会增加代码难度和执行时间。

在代码中，首先使用setCoreKfs()函数设定了最近的3个重叠关键帧，把它们存储在core_kfs_ 中。然后把这个集合作为参数传给DepthFilter的updateSeeds()函数用于对新帧种子点的更新。优先级方面，可以按照一定的规则确定要使用哪些最接近的关键帧。但需要注意的是，在添加新帧之前，必须确保队列中已经存储足够的关键帧来使得新帧具有足够多的观测信息以进行深度更新。

https://github.com/uzh-rpg/rpg_svo/blob/master/svo/src/frame_handler_mono.cpp#L191

1.5.3 关键帧插入条件判断

bool FrameHandlerBase::needNewKf(const Transformation &)

这个函数的作用是判断是否需要新的关键帧。该函数有一个输入参数Transformation&是当前帧与最近关键帧的位姿变换。在函数中，通过多种条件判断是否需要新的关键帧，这些条件可以在系统参数类（options_）中设置。如：

• 当前帧距离上一关键帧的时间大于设定的上限时间时，需要新的关键帧；
• 当前已匹配的特征点数量不足时，需要新的关键帧；
• 当前帧与最近关键帧的视差大于设定的最小视差时，需要新的关键帧；
• 当前帧与最近关键帧的角度和距离大于设定的阈值时，需要新的关键帧。

这个函数的输出结果是一个bool型的值，表示是否需要一个新的关键帧。如果需要，该函数返回值为true，否则为false。该函数是SLAM算法中一个非常重要的环节，控制关键帧的选取，保证系统的稳定性和速度。

1.6 frame中信息计算与更新 stereo_triangulation_->compute（FrameHandlerStereo::makeKeyframe() 中）

  // add extra features when num tracked is critically low!
  if (new_frames_->numLandmarks() < options_.kfselect_numkfs_lower_thresh) {
    setDetectorOccupiedCells(0, stereo_triangulation_->feature_detector_);
    new_frames_->at(other_id)->setKeyframe();
    map_->addKeyframe(new_frames_->at(other_id),
        bundle_adjustment_type_ == vi_estimator::BundleAdjustmentType::kCeres);
    upgradeSeedsToFeatures(new_frames_->at(other_id));
    stereo_triangulation_->compute(new_frames_->at(0), new_frames_->at(1));
  }

这个函数实现了SVO（Semi-Direct Visual Odometry）算法中的双目三角化过程。它将前一帧和当前帧之间的图像特征点恢复为三维空间中的点，并添加这些新点到前一帧的特征点集合中作为新的地标点。

1.6.1 核心接口算法实现

具体实现过程如下：

1. 首先，检查前一帧已有地标点的数量，如果已有的地标点数量足够，就不做处理，直接返回。

2. 使用特征检测器（feature detector）检测当前帧中的新特征点，然后计算这些特征点的深度和空间坐标。

3. 将这些特征点转换成3D点，并将新的地标点添加到前一帧的特征点集合中。如果新的地标点被匹配到，还将这些点添加到当前帧的特征点集合中，确定它们的观测关系，最后返回新的地标点列表。

4. 当需要计算新的地标点时，重新检测图像中的特征点，直到前一帧中有足够的地标点。

该函数的输入是两个帧的指针 FramePtr frame0 和 FramePtr frame1 ，表示前一帧和当前帧。输出则是计算得到的新的地标点，它们在前一帧图像的特征点集合中添加。该函数使用了一些常见的计算机视觉算法如特征检测和匹配、双目几何等。

1.6.2 主接口输入输出与更新全局变量

该函数的输入是两个帧的指针 FramePtr frame0 和 FramePtr frame1 ，表示前一帧和当前帧。这两个输入变量都被视为输入，因为函数在函数体内部并不修改这些帧的指针，而只是使用它们来计算新的地标点。因此，此函数没有修改输入参数。而此函数的输出为新计算得到的地标点，它们被添加到第一帧图像的特征点集中，并在第二帧图像中生成新的特征点。同时，该函数还会更新两个帧的一些成员变量，如特征点总数、特征点坐标、特征点焦点、特征点分数、特征点层级、特征类型等。

此外，该函数还会将新的特征点添加到SLAM系统的地图中，作为新的地标点，从而更新全局地图。因此，该函数会修改全局地图，但这并不是直接在此函数中实现的，而是通过调用其他函数来实现的，如在addObservation函数中添加观测信息以及在Map类中添加新的地标点。

1.7 stereo_triangulation_->compute中Matcher

 Matcher::MatchResult res = matcher.findEpipolarMatchDirect(*frame0, *frame1, T_f1f0, ref_ftr,
        options_.mean_depth_inv, options_.min_depth_inv, options_.max_depth_inv, depth);

1.7.1 核心接口算法实现

该函数实现了双目三角化的核心算法，用于在当前帧中寻找与参考帧中的一个特征点相关的地标点以及计算出其深度值。该函数的主要步骤如下：

1. 根据当前相机的位姿和参考帧中的特征点，计算在当前帧中与参考帧中的特征点的视线相交的线段的起始点和终止点，即计算出双目基线（Baseline）的起点 A 和终点 B。

2. 根据双目基线、参考帧中的特征点、当前相机的位姿等参数，计算出当前相机中对应深度值的取值范围（d_min_inv与d_max_inv）。

3. 根据参考帧中的特征点和当前帧中进行搜索的像素级别和方向，对当前帧中可能匹配的像素点进行预先筛选，去除掉一些不合适的匹配点（例如较大的视差角度或视差过近或过远的像素点）。

4. 根据像素点的预先筛选结果，对参考帧中的特征点进行向当前帧中的搜索。首先，根据参考帧中的特征点，计算出另外一条视线上的点 C，并在 C 与 A、B的连线方向上进行搜索，找到一个最优的匹配点，以计算出其深度值厚，并返回匹配结果。

5. 如果找到的匹配结果不理想，则进一步采用亚像素级别的精细匹配方法进行匹配，得到更精确的匹配结果和深度值。

6. 最后，返回匹配结果，指示是否成功找到匹配点，并将计算得到的深度值返回给调用函数。

1.7.2 feature_alignment 与 feature_alignment_utils中align1D 与 align2D

2. Mapping部分 depth_filter_->updateSeeds线程

2.1 updateSeeds

它的工作逻辑大概是这样的：如果进来一个关键帧，就提取关键帧上的新特征点，作为种子点放进一个种子队列中。如果进来一个普通帧，就用普通帧的信息，更新所有种子点的概率分布。如果某个种子点的深度分布已经收敛，就把它放到地图中，供追踪线程使用。当然实现当中还有一些细节，比如删掉时间久远的种子点、删掉很少被看到的种子点等等。
要理解Depth Filter，请搞清楚这两件事：
基于高斯——均匀的滤波器，在理论上的推导是怎么样的？
【DepthFilter】深度滤波器
Depth Filter又是如何利用普通帧的信息去更新种子点的？
SVO2系列之深度滤波DepthFilter

2.1.1 updateSeeds算法步骤

该函数的作用是更新当前帧和一组参考帧之间的深度估计，以剔除那些估计结果不好的点，并为后续的地图点生成提供更好的初始观测。函数中采用了多线程的方式，并将每个深度估计任务转换为独立的 Job，并异步处理。

该函数的主要步骤如下：

1. 遍历一组参考帧，对其中的每一个种子特征点进行检查，判断特征点的类型是否为种子点。

2. 对于每一个符合条件的种子点，调用 depth_filter_utils::updateSeed() 函数进行深度估计更新，以确定其深度、状态和观察范围等信息，并返回一个成功标志，记录成功更新的种子点数。

3. 如果当前线程非空（即为主线程），则直接处理所有更新深度估计的任务；如果当前线程为空（即为子线程），则将需要更新的任务转换为独立的 Job，并加入任务队列中。

4. 在队列中添加完所有任务后，调用 jobs_condvar_.notify_all() 函数唤醒所有等待该条件变量的子线程，开始处理任务队列中的所有 Job，并进行深度估计更新、状态变更等操作，同时记录并返回成功更新深度估计的种子点数量。

5. 子线程处理任务的过程中，每处理完一个 Job，就会检测是否需要发送异步消息或修改共享状态等操作，并等待新的 Job 的到达等待队列。如果任务队列为空，则等待条件变量被唤醒。

6. 所有的任务处理完成后，返回成功更新深度估计的种子点数量。

2.1.2 updateSeeds输入输出与更新全局变量

该函数接受以下参数输入：

1. ref_frames_with_seeds：一个 FramePtr 类型的 vector，包含了需要更新的特征点所在的参考帧。

2. cur_frame：一个 FramePtr 类型的指针，指向需要更新深度估计的当前帧。

3. options_：一个 Options 类型的结构体，其中包含了深度滤波器的相关参数。

4. matcher_：一个 Matcher 类型的指针，用于进行匹配操作。

该函数没有任何输出，但是通过对 depth_filter_ 全局变量进行修改来更新地图点和相机的状态。

其中，depth_filter_ 全局变量是一个 DepthFilter 类型的对象，用于管理地图点和相机状态的更新。通过调用 depth_filter_ 对象的成员函数来对该变量进行更新。

2.2 SVO2系列之深度滤波DepthFilter

2.3 【DepthFilter】深度滤波器

3. 小结

那么，这样一套系统实际工作起来效果如何呢？相比于其他几个开源方案有何优劣呢？首先要澄清一点的是：开源版本的SVO，是一个比较挫的版本。相比于LSD或ORB，我还很少看到有人能一次性把SVO跑通的。但是从论文上看，开源版本并不能代表SVO的真实水平。所以应该是心机弗斯特开源了一个只有部分代码的，不怎么好用的版本，仅供学习研究使用。相比之下，DSO，LSD，ORB至少能够在自己数据集上顺利运行，而ORB、LSD还能在大部分自定义的相机上运行，这点开源版本的SVO是做不到的。那么，抛开开源实现，从理论和框架上来说，SVO有何优劣呢？

优点：
着实非常快，不愧为稀疏直接法；
关键点分布比较均匀；

缺点：
追踪部分：SVO首先将当前帧与上一个追踪的帧比较，以求得粗略的位姿估计。这里存在一个问题：这必须要求上一个帧是足够准确的！那么问题就来了：怎么知道上一个帧是准的呢？开源SVO里甚少考虑出错的情况。

如果上一个帧由于遮挡、模糊等原因丢失，那么当前帧也就会得到一个错误的结果，导致和地图比对不上。
既然是直接法，SVO就有直接法的所有缺点
怕模糊（需要全局曝光相机）
怕大运动（图像非凸性）
怕光照变化（灰度不变假设）

---

地图部分 ：主要是计算特征点的深度。
Depth Filter收敛较慢，结果比较严重地依赖于准确的位姿估计。统计收敛的种子点的比例，会发现并不高，很多计算浪费在不收敛的点上。

单目VO中，刚刚从图像中提取的热乎的关键点是没有深度的，需要等相机位移之后再通过三角化，再估计这些点的深度。这些尚未具备有效深度信息的点，不妨称之为种子点（或候选点）。然而，三角化的成功与否（以及精度），取决于相机之间的平移量和视线的夹角，所以我们通常要维护种子点的深度分布，而不是单纯的一个深度值。

实际可以操作的只有高斯分布一种——高斯只要在计算机里存均值和协方差即可。在逆深度[8]流行起来之后，用逆深度的高斯分布成了SLAM中的标配。然而在特征点法中我们也能够通过描述来判断outlier，所以并不具有明显优势。SVO却使用了一种高斯——均匀混合分布的逆深度（由四个参数描述），推导并实现了它的更新方式，称为Depth Filter。

SVO使用一种称为均匀混合分布的逆深度模型，用四个参数描述。相比于纯高斯的逆深度，SVO的Depth Filter主要特点能够通过Beta分布中的两个参数a和b来判断一个种子点是否为outlier。算法逻辑如下：
当进来一个关键帧时，提取该关键帧上的新特征点，并将其作为种子点放入种子队列中。
当进来一个普通帧时，利用普通帧的信息更新所有种子点的概率分布。
如果某个种子点的深度分布已经收敛，那么将其放入地图中，供追踪线程使用。
总之，SVO的Depth Filter采用了一种均匀混合分布的逆深度模型，并通过Beta分布的参数来判断outlier，从而对逆深度进行更新和筛选。