CL关于ORB_SLAM的那些事（LocalMapping）

LocalMapping部分的学习总结

刚刚看完关于Sim3方面的代码，那就趁热打铁继续阅读关于局部地图部分的代码吧，开始吧！

.h部分的阅读

头文件中包含的头文件有：
KeyFrame.h
Map.h
LoopClosing.h
Tracking.h
KeyFrameDatabase.h
mutex 线程锁的头文件

头文件中维护的变量都是protected的，有：
是否是单目的标志位
线程锁重启
线程锁完成
Map类型的指针
闭环检测类型的指针
跟踪类型的指针
存放关键帧指针的链表
关键帧指针（当前帧）
存放MapPoint指针的链表
线程锁（新的关键帧）
放弃进行BA的标志位
停止的标志位
停止要求的标志位
不停止的标志位
停止的线程锁
是否接收关键帧的标志位
接收的线程锁

头文件中维护的函数有：
基本的构造函数
设置闭环的函数
设置跟踪的函数
主要的函数：运行的函数
插入关键帧的函数
要求停止的函数
要求重启的函数
停止的函数
释放的函数
是否被停止的函数
是否要求停止的函数
接收关键帧的函数
设置接收关键帧的函数
设置不停止的函数
打断BA的函数
要求完成的函数
关键帧进入队列的函数
#######分割线以下的是protected的函数#########
校核新的关键帧
处理新的关键帧
创建新的MapPoint
剔除MapPoint的函数
邻域搜索的函数
剔除关键帧的函数
计算两帧关键帧之间的基础矩阵
斜对称矩阵的函数
如果要求就重启的函数
设置完成的函数

function by function

来看.cpp文件中包含的头文件（除自身的头文件）：

1. 闭环检测的头文件
2. ORB匹配的头文件
3. 优化的头文件
4. 线程锁的头文件

LocalMapping()
构造函数
函数的参数列表：地图类型的指针和常float的传感器类型
用列表初始化了单目的类型（传入的参数），重启的要求为false，完成的要求为false，完成的标志位为true，地图的指针赋值为传入的地图的指针，放弃BA的标志位为false，停止的标志位为false，要求停止的标志位为false，不停止的标志位为false，接收关键帧的标志位为true。

SetLoopCloser()
设置闭环检测函数
函数的参数列表：闭环检测类型的指针
将传入的参数赋值给维护变量中闭环检测类型的指针

SetTracker()
设置跟踪函数
函数的参数列表：跟踪类型的指针
将传入的参数赋值给维护变量中跟踪类型的指针

run()
主体运行函数
完成的标志位置false

• while死循环
  • 调用SetAcceptKeyFrames()函数，传入false
  • 调用CheckNewKeyFrames()函数
    • 如果是新的关键帧
      • 调用ProcessNewKeyFrame()函数进行词袋向量方面的转换，并将其插入地图内
      • 调用MapPointCulling()，校核新的MapPoint
      • 调用CreateNewMapPoints()函数，进行三角化，获取新的MapPoint
      • 调用CheckNewKeyFrames()函数
        • 如果返回的结果为false，调用SearchInNeighbors()函数，进行邻域关键帧内的搜索，找到更多的匹配
      • 将放弃BA的标志位置false
      • 调用CheckNewKeyFrames()函数和stopRequested()函数
        • 如果这两个函数都是false（没有新的关键帧且没要求停止），在地图中的关键帧大于2帧时，调用优化中的局部BA函数。
        • 调用KeyFrameCulling()函数，检查冗余的局部关键帧。
    • 如果不是新的关键帧，但是Stop()函数是true
      • while()调用isStopped() 函数和CheckFinish()函数
        • 如果要求被停止且检测没有完成，就一直休眠
      • 如果CheckFinish()函数为true，检测到完成，直接break。
  • 调用ResetIfRequested()函数，是否需要重启
  • 调用SetAcceptKeyFrames()函数，传入true
  • 调用CheckFinish()函数
    • 如果函数的结果为true，直接break。
  • 睡眠中
• 调用SetFinish()函数，设置完成。

InsertKeyFrame()
插入新的关键帧
函数参数列表：关键帧类型的指针
先将新关键帧上锁
将函数传入的关键帧存入新关键帧的容器内
将放弃BA的标志位置true

CheckNewKeyFrames()
校核新的关键帧
将新的关键帧上锁
返回存放新的关键帧容器内是否为空，空的话返回fasle，非空就返回true

ProcessNewKeyFrame()
处理新的关键帧
将新的关键帧上锁
取出front的关键帧，将取出的这帧pop掉
对当前关键帧计算词袋信息
获取当前关键帧中匹配的MapPoint

• 循环遍历这些MapPoint
  • 在是MapPoint且是good的基础上
    • 这个MapPoint在当前关键帧中没有观测信息
      • 就添加当前关键帧对这个MapPoint的观测
      • 更新法向量和深度
      • 计算该点独一无二的描述子
    • 如果当前关键帧观测过这个MapPoint（只有双目会发生这样的情况）
      • 将该点存入最近添加的MapPoint

调用当前帧中更新连接函数，更新新的关键帧和MapPoint之间的连接关系
将当前关键帧插入地图内

MapPointCulling()
剔除MapPoint
定义存放MapPoint类型指针的链表迭代器，指向链表的第一个元素
取出当前关键帧的帧号赋值给常量当前关键帧的ID
定义观测次数的阈值
如果是单目的话，观测的阈值就是2
如果不是，观测的阈值就是3
将此时计算的观测阈值赋值给新的观测阈值，当前观测阈值(cnThObs)

• 遍历最近添加的MapPoint
  • 取出其中的MapPoint
  • 如果该MapPoint是bad，直接从链表里删除
  • 如果该MapPoint被发现的频率少于0.25，将该点设置成bad，直接从链表里删除
  • 如果该点第一次被观测的关键帧的ID与当前关键帧的ID之差不小于2且观测的次数不大于阈值，将该点设置成bad，直接从链表里删除
  • 如果该点第一次被观测的关键帧的ID与当前关键帧的ID之差不小于3，直接从链表里删除
  • 其他情况，迭代器++

CreateNewMapPoints()
创建新的MapPoint
定义int类型的nn为10
如果是单目的话，nn为20
定义只读属性的存放关键帧指针的容器，存放当前关键帧连接的最好的Covisibility关键帧，单目取前20帧，其他的话取前10帧。
定义ORB的匹配对象
获取当前关键帧的旋转和平移矩阵，将其转换成变换矩阵
取出当前关键帧的相机中心坐标
取出当前关键帧的焦距、光心坐标和焦距的逆
取出当前关键帧的尺度因子乘上1.5得到频率因子
定义int类型的nnew，初始化为0

• 循环遍历邻域的关键帧
  • 如果循环索引大于0且CheckNewKeyFrames()函数返回true，直接return
  • 取出邻域内的关键帧
  • 获取该关键帧的相机中心坐标
  • 计算基线向量
  • 计算得到基线的长度
  • 如果不是单目
    • 如果基线的长度小于关键帧本身基线长度，直接continue
  • 如果是单目
    • 计算场景中的深度中间值
    • 将基线长度除上深度中值得到基线深度率
    • 如果基线深度率小于0.01，直接continue
  • 调用计算基础矩阵的函数ComputeF12()，传入两帧关键帧，最后得到两帧之间的基础矩阵
  • 定义存放索引和索引的pair容器
  • 调用匹配中的SearchForTriangulation()函数进行三角化
  • 获取帧2的旋转矩阵，计算得到旋转的逆矩阵
  • 获取帧2的平移矩阵，计算得到平移的逆矩阵
  • 获取帧2的逆焦距
  • 获取匹配上的索引值
  • 循环遍历匹配索引信息
    • 取出对应的索引值
    • 按着索引值找出对应的关键点坐标、在右图上值
    • 根据右图上的值判断是否是双目
    • 计算得到x和y坐标，存放在xn变量内
    • 乘上旋转矩阵，得到世界坐标系下的x和y
    • 计算这两条射线的夹角的余弦值
    • 双目的Parallax夹角余弦值为上面的余弦值+1
    • 双目1的Parallax夹角余弦值等于上面的
    • 双目2同上
    • 如果是双目的情况，如下图所示
      
    • 取较小的cos值作为双目的cos值
    • 定义一个Mat的3D坐标
    • 如果射线的夹角cos小于双目的cos且射线的夹角cos值大于0且（是双目或者射线的夹角小于1）
      • 利用线性三角化的方法，构建A矩阵，4行4列
        
      • 利用SVD分解，计算得到3D点的坐标
      • 进行归一化处理（内心一开始的想法是为什么这边Z可以取1呢，后来才想到，这边取其他值应该也是可以，只不过将Z选到归一化平面上感觉会好点，如图可能解释得好点
        
    • 如果帧1是双目且双目1的夹角cos小于双目2的夹角cos
      • 那个3D点就是当前帧中立体索引点的坐标
    • 如果帧2是双目且双目2的夹角cos小于双目1的夹角cos
      • 那个3D点就是帧2中立体索引点的坐标
    • 其他情况就直接continue
    • 取出计算得到的3D坐标
    • 将其转换到相机坐标系内，检查Z的值
    • 如果不大于0，就直接continue（两个视角内都要检查）
    • 取出当前帧该点位置的关键点的sigma的平方值
    • 计算得到相机坐标系下的坐标（x和y）
    • 如果帧1不是双目
      • 计算投影的像素坐标
      • 计算像素误差
      • 如果像素误差的平方和大于5.991*sigma的平方，直接continue
    • 如果帧1是双目
      • 计算的误差项还要加右图的值，阈值也会大一点，其他差不多
    • 在帧2中进行同样的操作
    • 计算该3D点到光心1和光心2的距离
    • 如果其中有个距离等于0，直接continue
    • 计算两个距离之比
    • 计算对应的关键帧的金字塔层数之比
    • 如果距离之比×因子比例小于尺度之比或者距离之比大于尺度之比×因子比例，直接continue
    • 三角化成功，创建新的MapPoint
    • 添加该点在这两关键帧内的观测
    • 添加这两关键帧对该点的观测
    • 对该点进行唯一描述子的计算
    • 更新该MapPoint的法向量和深度值
    • 在地图中添加这个MapPoint
    • 将这个MapPoint存入最近添加的MapPoint的容器内
    • 新的MapPoint计数变量++

SearchInNeighbors()
在邻域关键帧内进行搜索
定义int类型的数量，初始化为10
如果是单目的话，该数量值初始化为20
获取当前关键帧连接的前nn帧关键帧
定义存放关键帧指针的容器（目标关键帧）

• 循环遍历邻域内的关键帧
  • 取出一帧关键帧
  • 如果这帧关键帧是bad或者已经被当前帧使用过，直接continue
  • 满足要求的关键帧存入目标关键帧容器内
  • 将这帧关键帧内的使用帧信息赋值为当前帧的帧号
  • 再取这帧关键帧的前5关键帧
  • 循环遍历这前5的关键帧
    • 取出一帧关键帧
    • 如果这帧关键帧是bad或者已经被当前帧使用过或者被第二连接的帧使用过，直接continue
    • 满足要求的存入目标关键帧容器内
      以上获取了所有目标关键帧
      定义ORB的匹配对象
      获取当前帧中所有有匹配关系的MapPoint
• 循环遍历目标关键帧
  • 取出每一帧关键帧
  • 调用matcher中的Fuse()函数，传入这帧关键帧和匹配MapPoint信息，寻找匹配关系
    定义存放MapPoint指针的容器
    容器的大小设置成目标关键帧的数量*匹配上的MapPoint的数量
• 循环遍历目标关键帧
  • 取一帧关键帧
  • 获取这帧关键帧内MapPoint的匹配关系
  • 循环遍历这些匹配的MapPoint
    • 取一个MapPoint
    • 如果不是，直接continue
    • 如果是bad或者这个点被当前帧使用过，直接continue
    • 满足条件，就把这个MapPoint的使用信息赋值为当前帧的帧号
    • 将这个MapPoint存入候选MapPoint的容器内
      调用matcher中的Fuse()函数，传入当前关键帧和候选MapPoint，寻找匹配关系
      获取当前关键帧内匹配上的MapPoint
• 循环遍历匹配上的MapPoint
  • 取一个MapPoint
  • 在是MapPoint且好的基础上，计算该点唯一的描述子
  • 更新这个MapPoint的法向量和深度值
    最后调用当前关键帧的UpdateConnections()函数，更新covisibility graph内的连接关系

ComputeF12()
计算基础矩阵
分别获取两帧原来的旋转和平移矩阵
根据矩阵乘法，计算得到两帧之间的旋转和平移矩阵
调用SkewSymmetricMatrix()函数，传入平移向量，最后输出反对称矩阵
获取两帧的内参矩阵
利用现有的矩阵包装成基础矩阵（如果怎么包装不会的，可以查看单目初始化那一块，对本质矩阵、单应矩阵和基础矩阵进行了详细的阐述）

RequestStop()
要求停止局部地图
上锁停止
将要求停止的标志位置true
上锁新的关键帧
放弃BA的标记位置true

Stop()
停止局部地图
上锁停止

• 如果要求停止且还没停止，将停止的标志位置true，屏幕上输出“局部地图停止”，返回true
• 如果不满足条件，直接返回false

isStopped()
判断是否被停止
上锁停止
返回停止的标记位

stopRequested()
是否有停止要求
上锁停止
返回要求停止的标志位

Release()
释放局部地图
上锁停止
上锁完成

• 如果是完成，直接return
  将停止的标记位置false
  将要求停止的标记位置false
• 循环遍历新的关键帧
  • 释放掉所有的指针
    将容器清空
    屏幕上输出“局部地图释放”

AcceptKeyFrames()
接收新的关键帧
上锁接收
返回接收关键帧的状态

SetAcceptKeyFrames()
设置是否接收关键帧
函数的参数列表：bool的标志位
将传入的标志位赋值给接收关键帧的标志位

SetNotStop()
设置不停止
函数的参数列表：bool类型的标志位
上锁停止

• 如果传入的标志位为true且停止的标志位为true，直接返回false
  将传入的标志位赋值给不停止的标志位
  返回true

InterruptBA()
中断BA
将放弃BA的标志位置true

KeyFrameCulling()
剔除不好的关键帧
获取当前帧的向量Covisible关键帧，存入局部关键帧的容器内

• 循环遍历局部关键帧
  • 取一帧关键帧
  • 如果是第一帧，直接continue
  • 获取这一帧匹配的MapPoint
  • 设置观测的阈值为3词
  • 循环遍历匹配上的MapPoint
    • 取一个MapPoint
    • 如果这个点是MapPoint且这个点是好点
      • 如果不是单目的模式
        • 判断这个点在这个关键帧内的深度，如果大于深度阈值或者小于0，直接continue
      • MapPoint的数量++
      • 如果是单目模式
        • 如果取出的MapPoint被观测的次数大于阈值
          • 取出对应关键点的金字塔层数
          • 获取观测这个MapPoint的关键帧和观测索引的map信息
          • 定义int类型的观测次数，初始化为0
          • 循环遍历这个MapPoint的观测信息
            • 先取出关键帧
            • 如果这帧关键帧就是上面的那一帧，直接continue
            • 取出索引对应的金字塔层数
            • 如果取出的金字塔层数不大于上面关键帧内该点对应的金字塔层数+1
              • 观测次数++
              • 如果观测的次数不小于阈值，直接break
          • 如果观测的次数不小于阈值，最近观测点数++
  • 如果新增的观测点数大于0.9倍的进来的所有的好的MapPoint，调用SetBadFlag()函数，将这个关键帧设置成bad

SkewSymmetricMatrix()
hat运算符，将一个向量转换成一个反对称矩阵

RequestReset()
发出要求重启
上锁重启
将要求重启的标志位置true

• while()死循环
  • 上锁2重启
  • 如果重启要求的标志位为false，才break
  • 睡眠

ResetIfRequested()
是否要求重启
上锁重启

• 如果重启要求的标志位为true
  • 将存放新的关键帧的容器清空
  • 将最近添加的MapPoint的容器清空
  • 将重启要求的标志位置false

RequestFinish()
重启完成
上锁完成
将完成要求的标志位置true

CheckFinish()
校核是否完成重启
上锁完成
返回完成要求的标志位

SetFinish()
设置完成的标志位
上锁完成
将完成的标志位置true
上锁停止
将停止的标志位置true

isFinish()
判断是否完成
上锁完成
返回是否完成的标志位

时间：2019年09月16日
作者：hhuchen
机构：河海大学机电工程学院