ORB-SLAM2笔记

ORB-SLAM

https://www.bilibili.com/video/BV1DP41157dB

基于词袋的图像检索

TF_IDF：词频-逆文档频率
令 文档(图像)i 及 单词(特征)j，
TF = j在i中出现的总数 / i的单词总数 ： 对于特定i统计；
IDF = log(文档总数 / 包含j的文档数量)：对应特定j统计；
则有TF_IDF = TF * IDF：i中出现频率越高的词权重越高，包含j的文档越多权重越低。
后续查询帧q与图像di计算余弦相似度：

倒排索引：
暴力方法为：遍历所有图像逐个计算相似度，则图像越多耗时越长；
改进方法：维护一个查找表：key为特征编号j，
value为列表，每个元素为【图像编号i, TF值, j位于图中的坐标】
因此遍历所有特征，去计算上式sim，以降低耗时，因为特征数量是固定的。

（空间验证：仅基于词袋相似性还不够，还需考虑相对空间关系）

优化

核心数据库

视觉词典；
关键帧词袋数据库；
路标点：坐标；描述子；观测方向（所有观测到该点的视图方向均值）；
观测距离（所有观测到该点的视图的最大/小距离）
关键帧：摄像机位姿；内参；特征描述子及是否有地图点对应
共视图：无向有权图，节点为关键帧，边表示共视路标点数量是否大于阈值
本质图：共视图子图，节点为关键帧，边更少，用于加速会还修正
边 = 生成树（类似骨架） + 共视边权重超过100的边 + 回环边
生成树：任意两顶点仅一条通路：边的数量=顶点数 - 1

跟踪线程

地图初始化：特征匹配；计算F与H，选择合理的矩阵；三角化重构路标；全局BA；
初始位姿估计：基于前一帧运动模型推测当前帧位姿，并将路标投影至当前帧；
找到足够多对应点则PNP更新相机位姿；
否则对应点不足（相机运动可能突变），则使用全局重定位（relocalization）：
基于词袋找到相似关键帧（可能多个），对于每个关键帧：
与当前帧特征点匹配；建立2D-3D对应关系PNP估计位姿；
反复迭代：利用估计位姿找到更多对应点，RNASAC EPnP优化相机位姿
位姿优化：找到更多的2D-3D对应关系优化当前位姿
寻找候选局部地图点：共视图中与当前帧相连的关键帧集合K1
以及与K1相连的关键帧集合K2；得到K1中的最佳共视帧K_ref

得到K1K2中共视的全部路标点，并进行一系列筛选；
基于筛选后的路标对当前帧位姿进行优化
关键帧选取：只有关键帧中的点才能三角化
选取条件：距离上次重定位或关键帧超过20帧；建图线程空闲；
当前帧关键点数量充足；与K_ref（数据库中）共视点相似比小于90%

建图线程

关键帧插入：更新共视图/生成树/词袋数据库
地图点删除：不删除条件：实际被观测的帧的比例大于理论值的20% 且 创建后的三个关键帧均能观测到
新地图点生成：对当前帧Ki没有被匹配的特征点进行如下处理：
选择共视程度最高且极线宽度大于阈值的关键帧，与Ki中的特征进行三角化并进行一系列校验；
三角化后的地图点投影到其他视图中建立2D-3D关系
局部地图优化：参与局部BA的节点：
当前关键帧K1，共视关键帧K2，及他们共视的P1P2；
以及能看到P1P2的K3但其位姿固定：

局部关键帧剔除：机器人不运动时控制关键帧数量：当前帧>90%地图点能被>3个关键帧观测则被剔除；

回环修正线程

候选帧检测
原理：若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,
且这些闭环匹配关键帧组间是连续的,则认为实现闭环
备选回环帧筛选：计算与当前关键帧Ki的BoW相似性大于阈值且不直接相邻的关键帧；
备选回环帧分组：每个关键帧都搜索Top10相邻共视关键帧
进一步筛选：(见orb-slam2)

计算Sim3变换
当前帧与候选回环关键帧可能存在尺度漂移问题：

两个帧做特征匹配；利用RANSAC估计sim(3)，7个自由度；据此搜索更多对应点；
进一步优化sim(3)直到有足够多内点，则接受当前候选回环帧
回环融合
基于sim(3)修正当前帧及共视帧位姿，进行闭环；
回环帧及其共视帧的地图点投影至当前帧，进行特征匹配；
匹配成功的地图点进行融合；
更新共视图，增加回环边
位姿优化
基于本质图优化所有关键帧位姿，不优化地图点：
，w表示世界坐标系

ORB-SLAM2

https://www.bilibili.com/video/BV1bK4y197kB
https://www.yuque.com/chenhai-7zi1m/se4n14/udvggt
https://blog.csdn.net/ncepu_chen/category_9874746.html?spm=1001.2014.3001.5482

地图点MapPoint

cv::Mat mWorldPos 地图点的世界坐标
std::map<KeyFrame*, size_t> mObservations 当前地图点在某KeyFrame中的索引
当前地图点对某KeyFrame的增删改查
int nObs 记录当前地图点被多少相机观测到

观测尺度
float mfMinDistance 地图点匹配在ORB金字塔第7层上的某特征点,观测距离值
float mfMaxDistance 地图点匹配在ORB金字塔第0层上的某特征点,观测距离值
UpdateNormalAndDepth() 更新平均观测方向和距离：
- 方向：mObservations中所有观测到本地图点的关键帧取平均得到；
- 距离：根据参考关键帧
- 调用时机：创建地图点初始化观测信息；mObservations更新；
- BA优化后mWorldPos变化

特征描述子
cv::Mat mDescriptor 当前地图点的特征描述子(所有观测关键帧描述子的中位数)
- 更新时机：地图点对关键帧的观测mObservations发生改变
- 用途：ORBmatcher::SearchByProjection()比较3D地图点-2D图像点描述子实现匹配

地图点删除
mbBad：表征当前地图点是否被删除
SetBadFlag()删除关键点：先mbBad置为true，再清空当前地图点各成员变量

帧Frame

相机参数信息：内参
特征点提取:ORBextractor* mpORBextractorLeft/Right
特征点分配:AssignFeaturesToGrid()
- 将特征点分配到48行64列的网格中以加速匹配
- std::vectorstd::size_t mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]:
- - 每个网格内特征点编号列表
- vector<size_t> GetFeaturesInArea(float &x, float &y, float &r):
- - 获取点(y,x)周围半径为r的圆域内所有特征点编号.
构造函数: Frame()步骤：
- 帧的ID自增、计算图像金字塔的参数、单目图像提取特征点、
- 畸变矫正、将特征点分配到网格中

关键帧KeyFrame

std::map<KeyFrame*, int> mConnectedKeyFrameWeights 无序保存共视关键帧及权重
std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames权重降序保存共视关键帧列表
std::vector<int> mvOrderedWeights权重降序保存共视权重列表

void UpdateConnections() 更新关键帧之间的连接图
- 调用时机：创建关键帧；局部地图构造；闭环矫正
- 获得该关键帧的所有MapPoint点
- 1. 首先获得该关键帧的所有MapPoint点，统计每一个地图点都有多少关键帧与当前关键帧存在共视关系KFcounter
- 对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
- 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边
- 3. 对这些连接按照权重从大到小进行排序并保存，以及更新生成树的连接

生成树
- bool mbFirstConnection 当前关键帧是否还未加入到生成树
- KeyFrame* mpParent 当前关键帧在生成树中的父节点
- std::set<KeyFrame*> mspChildrens 当前关键帧在生成树中的子节点列表
- 关键帧增加到生成树时机：关键帧创建后调用上述UpdateConnections：父关键帧为最大共视帧

关键帧删除
- bool mbBad 标记是坏帧
- bool mbNotErase 具有不被删除的特权(回环帧)
- SetBadFlag()对KeyFrame删除
- - 也是采取先标记再清除的方式，同MapPoint
- - 同时维护共视图和生成树，为其所有子关键帧寻找新的父关键帧

地图点的观测
- std::vector<MapPoint*> mvpMapPoints 观测到的地图点列表
- 对应地图点的增删改查函数
- 增加地图点时机：
- - Tracking和LocalMapping创建新地图点；LocalMapping融合共视帧间地图点；
- - LoopClosing融合闭环帧与匹配帧地图点
- 删除替换地图点时机：
- - LocalMapping局部BA删除大误差地图点
- - MapPoint类中会删改所对应的关键帧观测信息

回环检测与本质图
- std::set<KeyFrame*> mspLoopEdge 和当前帧形成回环的关键帧集合

单目初始化器Initializer

vectorcv::KeyPoint mvKeys1/2 参考帧/当前帧特征点
vector<pair<int,int>> mvMatches12 参考帧到当前帧的匹配特征点对
vector<vector<size_t>> mvSets 每一层保存RANSAC计算H和F矩阵所需的八对点
vector mvbMatched1 参考帧特征点是否在当前帧存在匹配特征点

计算变换矩阵步骤：FindHomography() FindFundamental()
特征点坐标归一化Normalize()
RANSAC循环：选8点计算F/H ComputeF21() ComputeH21()
卡方检验：CheckFundamental() CheckHmography()

恢复运动：
ReconstructF()：

ReconstructH()：

CheckRT()：
https://blog.csdn.net/qq_41694024/article/details/127945657
输入：
- 两帧R/t，两帧特征点及匹配关系，是否为内点
输出：
- 三角化的空间坐标，比较小的视差角(视差角过小则不靠谱)
过程：
- 基于R/t求投影矩阵，并遍历所有匹配对，做三角化
- 校验三角化结果（坐标非无穷，景深正负，视差角，重投影）
- 合格三维点视差做排序，找到较小视差角

其中Triangulate() ：
- 基于一对匹配2D点以及对应的2个投影矩阵（空间点=>图像点）恢复空间点坐标。原理：

跟踪线程Tracking

成员变量：
- Frame mCurrentFrame - 当前帧
- Frame mLastFrame 上一帧
- Frame mInitialFrame 单目初始化参考帧(实际上就是前一帧)
- Initializer* mpInitializer 单目初始化器
- KeyFrame* mpReferenceKF - 参考关键帧 初始化成功的帧会被设为参考关键帧
- std::vector<KeyFrame*> mvpLocalKeyFrames - 局部关键帧列表,初始化成功后向其中添加局部关键帧
- std::vector<MapPoint*> mvpLocalMapPoints - 局部地图点列表,初始化成功后向其中添加局部地图点

跟踪状态(紫框)转移图：GrabImageMonocular

持续调用GrabImageMonocular
- - 更新mCurrentFrame
- - 调用Track()：
- - - - 跟踪状态为NOT_INITIALIZED：
- - - - - - 单目初始化MonocularInitialization()：相邻两帧三角化超过100个点则成功
- - - - - - - - 初始化器mpInitializer未创建：
- - - - - - - - - - mCurrentFrame特征点数>100则用其初始化上一帧，并创建初始化器
- - - - - - - - 初始化器已创建：
- - - - - - - - - - mCurrentFrame特征点数<100则删除初始化器并退出
- - - - - - - - - - mInitialFrame与mCurrentFrame特征匹配
- - - - - - - - - - - - 内点太少则删除初始化器并退出
- - - - - - - - - - mpInitializer进行单目初始化，若成功：
- - - - - - - - - - - - 删除外点，设置mInitialFrame/mCurrentFrame位姿
- - - - - - - - - - - - CreateInitialMapMonocular() 地图初始化(见下)
- - - - 跟踪状态为OK或LOST，进入初始位姿估计：
- - - -
- - - - - - 分为三种方法先后尝试估计：
- - - - - - 根据恒速运动模型估计位姿TrackWithMotionModel()
- - - - - - 根据参考帧估计位姿TrackReferenceKeyFrame()
- - - - - - 通过重定位估计位姿Relocalization()
- - - - 执行局部地图跟踪：流程如下
- - - -
- - - -
- - - - - - UpdateLocalMap() 更新局部关键帧和局部地图点
- - - - - - - - UpdateLocalKeyFrames() 更新局部关键帧
- - - - - - - - - - 统计各当前帧的地图点：有多少关键帧与当前关键帧存在共视关系
- - - - - - - - - - 更新局部关键帧：一级二级共视关键帧/一级共视关键帧的父子关键帧
- - - - - - - - - - 最高共视帧定义为参考关键帧
- - - - - - - - UpdateLocalPoints() mvpLocalMapPoints被赋为mvpLocalKeyFrames的所有地图点
- - - - - - SearchLocalPoints() 局部地图点投影至当前帧，isInFrustum进行筛选，做2D-3D匹配
- - - - - - 最后一步：需根据是否刚发生重定位来确定阈值，若是则更严格
- - - - NeedNewKeyFrame() 关键帧判断
- - - - - - 距上次插入间隔、最近是否重定位、与参考帧观测>=3次的点共视点比例<90%
- - - - CreateNewKeyFrame() 创建新关键帧

CreateInitialMapMonocular()地图初始化：
- - mCurrentFrame/mInitialFrame均设置为关键帧pKFini/pKFcur，插入全局地图mpMap
- - 单目初始化得到的3D点生成地图点建立与两帧的双向连接，也插入mpMap(仅两帧)
- - 调用UpdateConnections()更新连接，mpMap做全局BA
- - 建立局部地图mpLocalMapper

恒速运动模型估计位姿: TrackWithMotionModel()
- - 假定帧间速度恒定，用最近普通帧跟踪当前帧
- - cv::Mat mVelocity 前一帧运动速度
- - 更新上一帧位姿?，基于已估计的mVelocity计算当前帧位姿
- - 用上一帧地图点进行投影匹配，最小化重投影优化当前帧位姿
- - 剔除外点，剩余匹配点>10则成功

根据参考帧估计位姿: TrackReferenceKeyFrame()
- - 参考关键帧mpReferenceKF来源：
- - - - CreateNewKeyFrame()，设当前帧为自身的参考关键帧；
- - - - TrackLocalMap() => UpdateLocalMap()设置与当前帧共视点最多的局部关键帧
- - 当前帧-参考帧特征匹配，当前帧位姿以上一帧为初值进行优化
- - 剔除外点后，跟踪>10对点则成功

重定位估计位姿：Relocalization()
- - 基于词袋找到相似关键帧集合，对于每个关键帧：
- - - - 与当前帧特征点匹配；建立2D-3D对应关系PNP估计位姿；
- - - - 反复迭代：利用估计位姿找到更多对应点，RNASAC EPnP优化相机位姿

局部建图线程 LocalMapping

Run() 内部While(1)循环：
- ProcessNewKeyFrame() 关键帧插入地图
- MapPointCulling()
- - 该地图点被观测的实际帧数mnFound/理论帧数mnVisible<0.25
- - - mnFound ：Tracking::TrackLocalMap()对观测到的地图点调用
- - - mnVisible：局部地图跟踪SearchLocalPoints()中判断该点是否在视锥范围内
- - 创建的3帧内观测数目少于2

CreateNewMapPoints()
- 与共视程度最高的前20个共视关键帧两两进行特征匹配，并三角化生成地图点.

SearchInNeighbors()
- 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点
- 1、找到一级相邻关键帧及其二级相邻关键帧，存至vpTargetKFs
- 2、遍历vpTargetKFs做地图点投影融合：
- - 正向(当前帧地图点投影至vpTargetKFs)+反向(vpTargetKFs地图点投影至当前帧)
- - 其中调用matcher.Fuse：
- - - 在反投影附近搜索窗口内找到最佳匹配点
- - - 存在地图点则直接添加观测，不存在则将两个地图点合并到其中观测较多的那个
- 3、更新地图点描述子、深度、平均观测方向、共视关系

KeyFrameCulling()
- 剔除冗余关键帧：
- 遍历所有共视关键帧的所有地图点：
- - 至少被3个关键帧观测到，就记录为冗余点nRedundantObservations++
- 90%以上的有效地图点被判断为冗余，则为冗余关键帧，则删除