ORB-SLAM 笔记
Feature choice
orb 描述子具备旋转不变性，而且匹配比较快。
Tracking 线程
constant velocity motion model 跟踪
如果上一帧相机跟踪成功了，则用 constant velocity motion model 预测当前帧相机的位姿，并且用上一帧图像的地图点在当前帧图像中做 guided search，如果没有足够的匹配点（motion model is clearly violated），再将搜索半径扩大，再重新寻找匹配点，然后用 motion-only BA 得到的匹配点计算位姿。
motion-only BA:
已知匹配的 3D 特征点坐标，和 2D 特征图像坐标，计算当前帧 pose，3D-2D 投影误差构成优化的目标函数，优化的变量只有当前帧 pose。
如果速度为 0，或者刚做了重定位，这时和当前关键帧进行匹配，匹配用 vocabulary tree 的方式，而不是 guided search 的方式，匹配完后也用 motion-only BA 优化。
如果跟踪跟丢了，将当前帧转换成 bag of words，并且在 recognition database 中找 candidates，然后和 candidate 帧特征匹配，用 RANSAC 和 PnP 算法计算位姿，如果能得到足够的匹配点用 motion-only BA 优化位姿，并且根据优化的再用 guided match 寻找更多的匹配点，然后再用 motion-only BA 优化相机的位姿。如果能得到足够的匹配点，跟踪过程继续。
Track local map
如果通过上步可以得到初始的位姿估计，再将地图点投影到当前帧中获取更多的匹配点。投影到当前帧的地图点包括，和当前帧有共视 地图点 的关键帧 K1 ，和在 convisibility graph 中和 K1 相连的关键帧 K2 的地图点。
筛选 K1 和 K2 中的地图点，在当前帧中找匹配点。
相机的位姿最终通过 motion-only BA 优化在上步得到的匹配点得到。
New Keyframe Decision
关键帧插入的越快越好：As there is a mechanism in the local mapping to cull redundant keyframes, we will try to insert keyframes as fast as possible, because that makes the tracking more robust to challenging camera movements, typically rotations.
关键帧插入时必须满足的条件:
1，上次重定位后已经过去 20 帧图像了（ensures a good relocalization and condition）。
2，Local mapping 线程是空闲的，或者从上次关键帧插入开始已经过去 20 帧了。
3，当前帧跟踪至少 50 个特征点（ensure a good tracking）。
4，Current frame tracks less than 90% points than Kref (minimum visual change).
如果关键帧插入的时候 local mapping busy，这时已经过去 20 帧图像了，这时会发出信号停止掉 local bundle adjustment，以便可以尽快处理的新的关键帧。
local mapping 线程
local mapping 线程处理的是关键帧，关键帧是 local tracking 线程给的。
做 local BA 优化。
对于新的关键帧中未匹配的特征点，和根据 convisibility graph 找到的和新关键帧相连接的关键帧，找新的匹配点。
执行点的剔除保证只保留高质量点，执行关键帧的剔除去掉冗余的特征点。
Keyframe Insertion
更新 convisibility graph（插入新的节点，更新插入节点边的连接），
更新 spanning tree，
计算新关键帧 bags of words。
Recent map points culling
判断新创建的地图点有效的条件：
1) The tracking must find the point in more than the 25% of the frames in which it is predicted to be visible.
2) If more than one keyframe has passed from map point creation, it must be observed from at least three keyframes.
New Map Point Creation
新地图点通过当前处理的关键帧和当前处理的关键帧相连接的关键帧 Kc 搜索匹配点，然后三角化初始化。对于当前处理的关键帧中没有匹配的特征点，在 Kc 中，搜索匹配点用 vocabulary tree，对于三角化的新特征点，还要检测是否满足一系列条件。对于新初始化的特征点，可能在 Kc 中的其它帧也有观测，还需要再其它帧中也搜索，并且做处理。
local Bundle Ajustment
local BA：优化当前关键帧 Ki ，和当前关键帧在 convisibility graph 有连接的关键帧 Kc 的 pose， Ki 和 Kc 能看到的地图点 map 也放在一起优化。能看到地图点 map 但是和当前关键帧在 convisibility graph 没有连接的关键帧也会包含在优化中，但是在优化的时候会设置为固定。
优化的变量：当前帧的 pose，和当前帧相连的关键帧的 pose，当前帧，和当前帧相连的关键帧的地图点 map ，能看到地图点 map 但是和当前帧不想连的关键帧在优化的是时候设置为 fixed。
优化的目标函数是 map 点所有观测的投影误差。
loop closing 线程
loop candidates detection
首先计算 Ki 和 convisibility graph 相连的帧最小相似度 smin ，回环检测时在 recognition database 中去掉相似度小于 smin 的帧，并且去掉 convisibility graph 中和当前帧相连的帧。在判断某一帧为回环时，还需要判断连续的 3 帧都为回环（To accept a loop candidate we must detect consecutively three loop candidates that are consistent (keyframes connected in the covisibility graph).）
单目的 SLAM 中，重建的地图在旋转，平移和尺度上都可以有漂移，所以在 close a loop 时，需要计算当前帧和回环帧之间的相似变换。首先计算当前帧和回环帧之间的匹配点，匹配点用 vocabulary tree 搜索，找到的匹配点是 3D-3D 的点，然后用 RANSAC 算法对这这些点计算一个相似变换，对于得到的相似变换如果能有足够的 inliers，则接受回环。
Loop Fusion
回环的地方，地图会被重建多次，所以在 loop fusion 时，需要把回环两端的地图融合在一起，并且回环两端的 convisibility graph 也需要添加新的边。
首先将计算的相似变换作用到当前帧，并且传播到和当前帧连接的关键帧，通过这样做回环两端的地图可以对齐。
然后回环帧，和回环帧相连的关键帧的所有地图点，向当前帧，和当前帧相连接的关键帧投影寻找匹配点，所有的这些匹配点和计算相似变换时的匹配点进行融合，融合会使得回环两端的关键帧建立新的共视点，从而需要 convisibility graph 中更新关键帧之间的边连接。
Essential Graph Optimization
Essential Graph /pose graph Optimization：To effectively close the loop, we perform a pose graph optimization over the Essential Graph，对于单目 SLAM 优化是针对相似变换做的，优化后地图点根据能观测到它的一帧关键帧进行位置的更新。
优化的参数：每个关键帧的 pose，优化参数 pose 是个相似变换，优化完后再作用到关键帧位姿，作用后，关键帧位姿仍然是个欧式变换，优化后也作用到地图点。
优化约束
这里的约束都是位姿约束，位姿约束指帧和帧之间的位姿变换。
约束有：
当前回环两端新建立的边（回环两端地图点融合产生连接回环两端新边）形成的位姿约束。
spanning tree 位姿约束。
每个关键帧的回环约束。
convisibility graph 中权值大于一定阈值的边约束。
Global BA
Global BA：优化地图中所有关键帧的 pose 和所有地图点的坐标。优化的变量包括所有关键帧 pose，所有特征点坐标，优化的约束是所有特征点所有观测的投影误差。
Map points, Keyframes and their Selection
每个 map point 存储的信息
在世界系下特征点的位置。
观测方向（特征点被观测的平均方向，方向指光心到特征点光线的方向）
特征描述子，特征可能被好多帧观测到，which is the associated ORB descriptor whose hamming distance is minimum with respect to all other associated descriptors in the keyframes in which the point is observed.
该特征可以被观测到的最小和最大距离，最小和最大距离根据 orb 特征尺度不变性的上限值计算得到，原文如下：The maximum d max and minimum d min distances at which the point can be observed, according to the scale invariance limits of the ORB features.
每个关键帧存储的信息
世界坐标系到该关键帧相机坐标系的转换矩阵。
相机内参。
该关键帧提取到的所有 ORB 特征，并且这些特征是否和 map point 关联起来的信息。
地图点和关键帧创建时 generous policy
地图点和关键帧在创建的时候要求不严格（generous policy），但是后续会有一个剔除操作，剔除操作负责检测冗余的关键帧，和错误匹配或者不能跟踪的地图点（wrongly matched or not trackable map points）。这样做好处：This permits a flexible map expansion during exploration, which boost tracking robustness under hard conditions (e.g. rotations, fast movements), while its size is bounded in continual revisits to the same environment, i.e. lifelong operation. Additionally our maps contain very few outliers compared with PTAM, at the expense of containing less points.
Convisibility graph and Essential graph
consistency graph 中节点由关键帧组成，如果两个 keyframe 至少有 15 个共同的观测特征点，则节点之间用无向边连接，边的权值设为两个关键帧共同观测的特征点的数目。
当检测到回环时，执行 pose graph optimization that distributes the loop closing error along the graph. 。pose graph 优化的时候为了不包含 convisibility graph 中所有的边（very dense），算法先构造一个 Essential graph，Essential graph 包含 convisibility graph 中所有的节点（所有的关键帧），但是包含的边要比 convisibility grap 少（still preservation a network that yields accurate results）。
pose graph 优化的 Essential graph 包含的边有：
1，spanning tree：incrementally build。每个关键帧在插入的时候，关键帧和共视点最多的关键帧相连接，关键帧在删除的时候，系统也会更新和删除关键帧相连的关键帧的连接。
2，权值大于 100 边
3，回环边
对于 pose graph 优化作者评价：
when performing the pose graph optimization, the solution is so accurate that an additional full bundle adjustment optimization barely improves the solution.
Bags of Words Place Recognition
DBoW2 系统创建了一个 invert index，对于 vocabulary 中的每个 visual word（不仅仅是最后一层的 word），保存了看以观测到这个 word 的关键帧，这样在回环检测寻找相似的关键帧时，可以提高效率。
在存在回环的地方，关键帧会有 overlap，也就说不会仅有一帧得分比较高，DBoW2 会把时间因素考虑进去，把时间上相近的关键帧的得分加在一起。但是这样做不能解决不同时间扫描同一区域问题，系统还会把 convisibility graph 相连的回环帧 group 到一起。
用 bag of word 在特征匹配的时候也有好处，当匹配两个集合内的 ORB 特征点时，可以不用 burte fouce 搜索，而仅仅搜索在 vocabulary tree 中属于相同节点的特征（这里系统选择第二层节点）。系统在寻找新的匹配点三角化新的特征点时，和在闭环检测和重定位时，使用这策略。
系统还用 orientation consistency test 再精炼匹配。
参考文献：
Mur-Artal R, Montiel J M M, Tardós J D. Orb-slam: a versatile and accurate monocular slam system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.