0. 局部建图线程介绍

   在 Tracking 线程中每次跟踪成功之后会判断是否将当前帧作为关键帧并送入到局部建图线程，关键帧的判断在 Tracking 线程中进行，但关键帧、地图点插入到地图在局部建图线程进行。
局部建图的主要任务包括：

  • 1. 插入关键帧；
  • 2. 当前帧地图点剔除；
  • 3. 创建新的地图点；
  • 4. 局部 BA；
  • 5. 局部关键帧剔除。

0.1 局部建图线程创建与运行

  • 局部建图线程的创建与运行在初始化 SLAM 系统时，线程并行运行在 ==LocalMapping::Run()== 函数中。 
    1
2
3
4
5

    mpMap = new Map();
mpLocalMapper = new LocalMapping(mpMap, 
                                 mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,	//这个线程会调用的函数
                 mpLocalMapper);				//这个调用函数的参数
    • 步骤一： 告诉跟踪线程 LocalMapping 线程正在处理关键帧，Tracking 线程最好不要发送太快，将 mbAcceptKeyFrames 设置为 false； 
      1

      SetAcceptKeyFrames(false);
    • 步骤二： 检查待处理的关键帧队列列表 mlNewKeyFrames 不为空，==LocalMapping::CheckNewKeyFrames()==；
    • 步骤三： 计算关键帧特征点的 BoW 向量，并将关键帧插入地图， ==LocalMapping::ProcessNewKeyFrame()==；
    • 步骤四： 剔除上一步引入的不合格的地图点，==LocalMapping::MapPointCulling()==；
    • 步骤五： 与相邻关键帧三角化创新新的地图点，==LocalMapping::CreateNewMapPoints()==；
    • 步骤六： 相邻关键帧融合优化， ==LocalMapping::SearchInNeighbors()==；
    • 步骤七： 局部地图 BA 优化，最小化重投影误差，==Optimizer::LocalBundleAdjustment()==；
    • 步骤八： 剔除冗余关键帧，==LocalMapping::KeyFrameCulling()==；
    • 步骤九： 将当前关键帧插入到闭环检测队列中； 
      1

      mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
    • 步骤十： 告诉 Tracking 线程，Local Mapping 线程现在空闲，可一处理接收下一个关键帧。 
      1

      SetAcceptKeyFrames(true);

1. 插入关键帧

  ==LocalMapping::ProcessNewKeyFrame()== 函数的功能是计算关键帧的 BoW 向量，加速三角化新的 MapPoints，关联当前关键帧到 MapPoints，并更新 MapPoints 的平均观测方向和观测距离范围，插入关键帧，更新 Covisibility 图和 Essential 图。

  • 步骤一： 从 Tracking 线程获取的 mlNewKeyFrames 缓冲队列中取出一帧关键帧 
    1
2
3
4
5
6
7

    // STEP 步骤1：从缓冲队列中取出一帧关键帧
{
    unique_lock<mutex> lock(mMutexNewKFs);
    // 从列表中获得一个等待被插入的关键帧
    mpCurrentKeyFrame = mlNewKeyFrames.front();
    mlNewKeyFrames.pop_front();     // 删除队列中第一个，也即刚取出的.
}
  • 步骤二： 计算当前关键帧的 BoW 向量，在函数 ==KeyFrame::ComputeBoW()== 中实现，与跟踪线程中计算普通帧的 BoW ==Frame::ComputeBoW()== 类似；
  • 步骤三： 跟踪局部地图过程中新匹配上的 MapPoints 和当前关键帧关联，更新当前关键帧所看到的地图点的属性（包括平均观测方向，最优描述子）；
  • 步骤四： 更新关键帧间的连接关系，Covisibility 图和 Essential 图，在函数 ==KeyFrame::UpdateConnections()== 中实现
    • 步骤 1： 首先获得该关键帧的所有 MapPoint 点，统计观测到这些 3d 点的每个关键与其它所有关键帧之间的共视程度，存储在 KFcounter 中，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共 3d 点的个数； 
      1

      map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数
    • 步骤 2： 要求该权重必须大于一个阈值（设置为 15），如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高），并对这些连接按照权重从大到小进行排序 
       1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17

      if(mit->second>=th)
{
    // 对应权重需要大于阈值，对这些关键帧建立连接
    vPairs.push_back(make_pair(mit->second,mit->first));
    // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
    // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
    (mit->first)->AddConnection(this,mit->second);
}
// vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小
sort(vPairs.begin(),vPairs.end());
list<KeyFrame*> lKFs;
list<int> lWs;
for(size_t i=0; i<vPairs.size();i++)
{
    lKFs.push_front(vPairs[i].second);
    lWs.push_front(vPairs[i].first);
}
    • 步骤 3： 更新 covisibility 图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树 
       1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19

      {
    unique_lock<mutex> lockCon(mMutexConnections);

    // mspConnectedKeyFrames = spConnectedKeyFrames;
    // 更新图的连接(权重)
    mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
    mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

    // 更新生成树的连接
    if(mbFirstConnection && mnId!=0)
    {
        // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
        mpParent = mvpOrderedConnectedKeyFrames.front();
        // 建立双向连接关系
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }
}
  • 步骤五： 将该关键帧插入到地图中 
     1
 2
 3
 4
 5
 6
 7
 8
 9
10

    mpMap->AddKeyFrame(mpCurrentKeyFrame);
  
// BRIEF 在地图中插入关键帧,同时更新关键帧的最大id
void Map::AddKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexMap);
    mspKeyFrames.insert(pKF);
    if(pKF->mnId>mnMaxKFid)
        mnMaxKFid=pKF->mnId;
}

2. 剔除当前帧地图点

  为了保存地图点，必须在创建该地图点的需要满足下面三个条件的约束，才能真正被保存，这样才能保证可跟踪且不容易在三角化时出现较大误差，在函数 LocalMapping::MapPointCulling() 中实现，

  • 条件一： 已经是坏点的 MapPoints 直接从检查链表中删除； 
    1
2
3
4

    if(pMP->isBad())
{
    lit = mlpRecentAddedMapPoints.erase(lit);
}
  • 条件二： 要求跟踪到该 MapPoint 的 Frame 数相比预计可观测到该 MapPoint 的 Frame 数的比例需大于 25% ，即 (mnFound)/mnVisible > 0.25 
    1
2
3
4
5
6
7

    else if(pMP->GetFoundRatio()<0.25f)
{
    // 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%
    // IncreaseFound / IncreaseVisible < 25%，注意不一定是关键帧。
    pMP->SetBadFlag();
    lit = mlpRecentAddedMapPoints.erase(lit);
}
  • 条件三： 如果一个地图点被构建，它必须被超过三个关键帧观察到（单目时为 2 个）
  • 步骤四： 从建立该点开始，已经过了 3 个关键帧而没有被剔除，则认为是质量高的点，因此没有 SetBadFlag()，仅从队列中删除，放弃继续对该 MapPoint 的检测；

3. 创建新的地图点

  相机运动过程中和共视程度比较高的关键帧，通过三角化恢复出一些新的地图点，不包括和当前关键帧匹配的局部地图点（已经在 ProcessNewKeyFrame() 中处理了），先处理新关键帧与局部地图点之间的关系，然后对局部地图点进行检查，最后再通过新关键帧恢复。

  • 步骤一： 在当前关键帧的共视关键帧中找到共视程度最高的 nn 帧相邻帧 vpNeighKFs，获取在函数 ==KeyFrame::GetBestCovisibilityKeyFrames== 中进行；
  • 步骤二： 遍历前面选出的共视程度高的相邻帧；
  • 步骤三： 判断相机运动的基线是不是足够长；
    • 如果是立体相机，关键帧间的运动太小时不能生成地图点；
    • 如果是单目相机，特别远（基线与景深比例特别小）则不生成地图点，场景深度中值由函数 ==KeyFrame::ComputeSceneMedianDepth()== 来计算；
     1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

    // 计算运动基线
// 得到当前关键帧在世界坐标系中的坐标
cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
// 邻接的关键帧在世界坐标系中的坐标
cv::Mat Ow2 = pKF2->GetCameraCenter();
// 基线向量，两个关键帧间的相机位移
cv::Mat vBaseline = Ow2-Ow1;
// 基线长度
const float baseline = cv::norm(vBaseline);
  
// STEP 步骤3：判断相机运动的基线是不是足够长
if(!mbMonocular)
{
    // 如果是立体相机，关键帧间距太小时不生成3D点
    if(baseline<pKF2->mb)
        continue;
}
else
{
    // 邻接关键帧的场景深度中值
    const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
    // baseline与景深的比例
    const float ratioBaselineDepth = baseline/medianDepthKF2;
    // 如果特别远(比例特别小)，那么不考虑当前邻接的关键帧，不生成3D点
    if(ratioBaselineDepth<0.01)
        continue;
}
  • 步骤四： 根据两个关键帧的位姿计算他们之间的基础矩阵 F = inv(K1 转置) * t12 叉乘 R12 * inv(K2)，计算在函数 ==LocalMapping::ComputeF12()== 中进行；
  • 步骤五： 通过帧间词典向量加速匹配，极线约束限制匹配时的搜索范围，进行特征点匹配，实现在 ==ORBmatcher::SearchForTriangulation()== 函数中；
  • 步骤六： 对每对匹配点 2d-2d 通过三角化生成 3D 点,和 Triangulate 函数差不多
    • 步骤 1： 取出上一步极限匹配的特征点对；
    • 步骤 2： 利用匹配点反投影得到视差角，用来决定使用三角化恢复(视差角较大) 还是直接 2d 点反投影(视差角较小) ；
    • 步骤 3： 对于双目情况，利用双目基线、深度得到视差角；
    • 步骤 4： 视差角较大时 使用三角化恢复 3D 点 (两个点，四个方程，奇异值分解求解)
    • 对于双目视差角较小时, 2d 点利用深度值反投影成三维点，单目的话直接跳过；
    • 步骤 5： 检测生成的 3D 点是否在相机前方(Z>0) ；
    • 步骤 6： 计算 3D 点在当前关键帧下的重投影误差，误差较大的跳过；
    • 步骤 7： 计算 3D 点在邻接关键帧下的重投影误差，误差较大的跳过；
    • 步骤 8： 检查尺度连续性；
    • 步骤 9： 三角化生成 3D 点成功，构造成地图点 MapPoint；
    • 步骤 10： 为该 MapPoint 添加属性；
    • 步骤 11： 新产生的点放入待检测队列 mlpRecentAddedMapPoints，交给 MapPointCulling() 检查生成的点是否合适。

4. 相邻关键帧融合优化

  在前面已经创建了关键帧和更新地图点之后考虑对相邻帧之间重复的地图点进行融合，分别对当前帧和当前帧连接的一二级关键帧进行更新。

  • 步骤一： 获得当前关键帧在 covisibility 图中权重排名前 nn 的邻接关键帧，找到当前帧一级相邻与二级相邻关键帧；
  • 步骤二： 将当前帧的 MapPoints 分别与一级二级相邻帧的 MapPoints 进行融合，融合操作在函数 ==ORBmatcher::Fuse()== 中进行；
  • 步骤三： 同样将一级二级相邻帧的MapPoints分别与当前帧的 MapPoints 进行融合；
  • 步骤四： 更新当前帧 MapPoints 的描述子，深度，观测主方向等属性；
  • 步骤五： 更新当前帧的 MapPoints 后更新与其它帧的连接关系，在函数 ==KeyFrame::UpdateConnections()== 中实现。

5. 局部 BA 优化

  在已经处理完队列中的最后一个关键帧之后，并且闭环检测线程没有请求停止局部建图线程，则开始对当前帧进行局部 BA 优化，在 ==Optimizer::LocalBundleAdjustment== 函数中进行。

6. 冗余关键帧剔除

  在 Covisibility 图中的关键帧，其 90% 以上的 MapPoints 能被其他至少 3 个关键帧观测到，则认为该关键帧为冗余关键帧，在函数 ==LocalMapping::KeyFrameCulling()== 中实现。

  • 步骤一： 根据 Covisibility Graph 提取当前帧的共视关键帧；
  • 步骤二： 遍历每一个共视帧并提取其地图点；
  • 步骤三： 遍历该局部关键帧的 MapPoints，判断是否 90% 以上的 MapPoints 能被其它至少 3 个关键帧观测到，如果是则将其设置为空余关键帧 
    1
2

    if(nRedundantObservations>0.9*nMPs)
    pKF->SetBadFlag();

【R】参考资料

【Q】问题

  • ORBmatcher::SearchForTriangulation() 函数判断两帧之间点是否满足极限约束，没有详细展开；
  • ORBmatcher::Fuse() 相邻关键帧之间的特征点融合没有展开，后续补上；

2019.05.30
wuyanminmax@gmail.com