ORB-SLAM学习（三）：地图点、关键帧和图结构
#SLAM 2022-07-02

地图点

地图点就是真实世界中的点，来自真实世界的三维物体，有唯一的ID，不同图像帧里面的特征点可能对应三维空间中的同一个地图点。

地图点生成

地图点生成在ORB-SLAM2中主要存在于下面几个地方：

  • 单目初始化时前两帧会生成一部分地图点，双目初始化通过左右目匹配第一帧就可以生成地图点，RGB-D相机通过测量深度也可以在第一帧生成部分地图点；
  • 局部建图线程里面，共视关键帧之间通过LocalMapping::CreateNewMapPoints()函数生成地图点；
  • 跟踪线程里面，Tracking::UpdateLastFrame()和Tracking::CreateNewKeyFrame()函数中为双目和RGB-D相机生成新的临时地图点，单目相机不生成地图点。

计算地图点最具代表性的描述子

由于一个地图点会被多个相机图像观测到，因此在插入关键帧后，需要判断是否更新当前点的描述子。

找出合适的描述子描述该地图点

那么代表性描述子如何找出来呢？具体的操作为首先获得当前点的所有描述子，然后计算描述子之间的两两距离。在ORB-SLAM2中，最好的描述子与其他的描述子之间应该具有最小的距离中值。

距离中值

计算描述子距离

在函数ORBmatcher::DescriptorDistance()中，通过汉明距离进行计算

汉明距离：汉明距离是一个概念，它表示两个（相同长度）字符串对应位置的不同字符的数量，我们以d（x,y）表示两个字x,y之间的汉明距离。对两个字符串进行异或运算，并统计结果为1的个数，那么这个数就是汉明距离。

举个例子，1011101和1001001之间的汉明距离是2，因为第三位和第六位不同。

由于BRIEF描述子是一堆二进制串，所以通过汉明距离计算非常方便

ORB-SLAM2中计算BRIEF描述子汉明距离的代码如下：

1
2
3
4
5
6
7
8
9
//pa,pb是两BRIEF描述子，每个描述子8*32=256位

for(int i=0; i<8; i++, pa++, pb++)
    {
        unsigned  int v = *pa ^ *pb;    //异或操作，相等为0，不等为1     
        v = v - ((v >> 1) & 0x55555555);
        v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
        dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
    }

对这段代码进行解读，这段代码实际上使用了一个泛用的算法，叫SWAR算法，用于计算二进制数中1的个数。整体思路就是先将相邻2位的1的数量计算出来，结果存放在这2位。然后将相邻4位的结果相加，结果存放在这4位，将相邻8位的结果相加，结果存放在这8位。最后计算整体1的数量，记录在高8位，然后通过右移运算，将结果放到低8位，得到最终结果。

SWAR算法通过先将数字进行最细力粒度的拆分，然后每一步都对上一次的计算结果进行整合，最终得到整体结果。由于这里处理的是32位数，所以这个算法比遍历算法快32倍，也不需要消耗额外内存空间。

地图点平均观测方向、观测距离范围

地图点法线朝向的计算

对于观测到某一地图点的全部关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向。

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
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
void MapPoint::UpdateNormalAndDepth()
{
    // Step 1 获得观测到该地图点的所有关键帧、坐标等信息
    map<KeyFrame*,size_t> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;

        observations=mObservations; // 获得观测到该地图点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
        Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    // Step 2 计算该地图点的平均观测方向
    // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
    // 初始值为0向量，累加为归一化向量，最后除以总数n
    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        // 获得地图点和观测到它关键帧的向量并归一化
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);                       
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 观测到该地图点的当前帧的特征点在金字塔的第几层
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数
    const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔总层数，默认为8

    {
        unique_lock<mutex> lock3(mMutexPos);
        // 使用方法见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
        mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
    }
}

关键帧

关键帧就是几帧图像里面比较有代表性的那一帧，可以大概的把它的重要程度比作一幅图像中的特征点。

ORB-SLAM中的关键帧

关键帧的好处

  • 相邻帧之间的信息冗余度很高，通过关键帧可以降低信息冗余度。比如相机放在原地不动，普通帧一直在累加，但是关键帧始终不变；
  • 关键帧是普通帧滤波和优化的结果，可以增加定位的准确性；
  • 关键帧的主要作用是面向后端的算力和精度的折中，使得有限的资源能够用在刀刃上。就好像现在还处于社会主义初级阶段，不能够使所有人都富起来，只好让一部分人先富起来，先富带动后富，最后等生产力极大发展（算力爆炸）后，才能实现所有人一视同仁。

如何选择关键帧

目前来说，主要的指标主要有下面这些：

  • 时间尺度，距离上一关键帧之间的帧数不能太少，但是搁多少帧选择关键帧是个问题；
  • 运动尺度，计算运动累积的多少，每移动一定的距离就选一个关键帧，但是这样的话如果对着一个物体来回扫就会出现大量相同关键帧；
  • 根据共视特征点的数量，记录下当前视角下的特征点的数量或者比例，当相机离开场景时才新建关键帧，缺点是数据结构和逻辑比较复杂。

在ORB-SLAM2中，后期的局部建图和全局BA都是只用关键帧来操作了，在跟踪线程中选择关键帧，这时候的选择标准还比较宽松，在局部建图线程，会根据共视冗余度对关键帧进行剔除，剩下这些才是真正用到的关键帧。

共视图

共视图是无向加权图，每个节点都是关键帧，如果关键帧之间满足一定的共视关系（在ORB-SLAM2中的标准是至少有15个共视地图点），就连成一条边，这条边的权重就是共视地图点的数目。

共视图

共视图的作用

  • 跟踪局部地图，扩大搜索范围；
  • 关键帧之间新建地图点；
  • 闭环检测以及重定位检测；
  • 优化。

本质图

共视图比较稠密，本质图比共视图稀疏很多，关键帧作为节点，但是连接边更少，只保留了联系紧密的边。在本质图中，主要包含了

  • 生成树之间的连接关系；
  • 形成闭环的连接关系；
  • 共视关系非常好的连接关系。
本质图

生成树

生成树由子关键帧和父关键帧构成，也就是共视关系最高的帧，只要子关键帧存在父关键帧就生成连接关系，不然就被孤立，如下图中黄色的线。

生成树