(01)ORB-SLAM2源码无死角解析-(45) 跟踪线程→局部地图跟踪TrackLocalMap():主体流程

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

有兴趣的朋友可以加微信 17575010159 相互讨论技术 - 文末公众号也可关注

一、前言

通过前面的博客，已经对 参考关键帧追踪TrackReferenceKeyFrame()，恒速模型跟踪当前普通帧TrackWithMotionModel() 以及 重定位跟踪 Relocalization()。他们分别适用于不同的情况，具体就不再这里提及了，因为在前面都进行了细致的分析，其中 恒速模型跟踪当前普通帧TrackWithMotionModel() 被使用得最多。

也就是说，对当前帧进行追踪，是从上诉的三种选择一种进行追踪，或者说位姿估算。这里可以很明显的看到，TrackReferenceKeyFrame() 与 TrackWithMotionModel() 追踪利用的都是一帧图像(最近关键帧或者上一帧)，可想而知，其忽略了很多有价值的信息。虽然 Relocalization() 追踪使用了很多关键帧，利用的信息还是比较充足的，但是只有在追踪丢失的情况下才使用重定位追踪。

首先我们在src/Tracking.cc文件中，可以看到如下代码：

        // If we have an initial estimation of the camera pose and matching. Track the local map.
        // Step 3：在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
        // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
        if(!mbOnlyTracking)
        {
            if(bOK)
                bOK = TrackLocalMap();
        }
        else
        {
            // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
            // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
            // the camera we will use the local map again.

            // 重定位成功
            if(bOK && !mbVO)
                bOK = TrackLocalMap();
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

可以看到，使用上诉三种跟踪方式中任意一种，只要跟踪成功，紧接着就会进行 局部地图追踪 TrackLocalMap()。局部地图追踪，可以理解为对上诉上种追踪获得位姿的优化。因为 局部地图跟踪 会采用更多的地图点进行追踪。

$核心思想:$ 在追踪的过程中，随着当前帧在一帧一帧的变化，可想而知，当前帧可以观测到的地图点也在变化，利用已经存在的地图点可以计算出相机位姿，同时当前帧会剩下一些未匹配上的关键点，那么这些关键点结合位姿则可以产生新的地图点，这些新的地图点可以提供给下一帧用于位姿估算。

大家这里注意一个点，当前帧生成新的地图点(核心代码为Tracking::UpdateLastFrame())，一般只会只会用于更新局部地图，只有当前帧为关键帧时，进行关键帧创建才会生成新的全局地图点。下面我们先来了解局部关键帧以及局部地图点的概念。

 

二、局部关键帧

从命名可以看到出来，所谓局部地图跟踪那么肯定是根据 局部地图点 进行跟踪，在了解 局部地图点 之前，先来看看局部关键帧。首先来查看下图:

$一级共视关键帧:$ 假若当前帧为F，其能观测到地图中N个地图点，那么能够观测到这些地图点(至少一个)的关键帧，称为一级共视关键帧。如上图，当前帧F能观测到红色的三个地图点，同时这三个地图点又被 KF1与 KF2 观测到，那么那么称 KF1与 KF2 为 前帧F 的一级共视关键帧。

$二级共视关键帧:$ F的一级共视关键帧 KF1与 KF2 分别能够观测到上图的 KF1地图点(绿色)，KF2地图点(绿色)。那么称能够观测到 KF1地图点 的关键帧为 KF1的共视关键帧，也叫 KF1的一级共视关键帧。此时 KF1的共视关键帧 对于 F来说，就是 二级共视关键帧。同理，KF2 对于 F 来说，也就是 二级共视关键帧。

$当前帧:$ mCurrentFrame(当前帧是普通帧)
$参考关键帧:$ 与当前帧共视程度最高的关键帧作为参考关键帧，mCurrentFrame.mpReferenceKF，在KeyFrame::UpdateConnections() 里确定关键帧的父子关系（当前帧必须是关键帧）
$父关键帧:$ 和当前关键帧共视程度最高的关键帧
$子关键帧:$ 是上述父关键帧的子关键帧
$mvpLocalMapPoints:$ 示意图

$局部关键帧:$
        ①当前帧的一级共视关键帧(包含所有)。
        ②当前帧的二级共视关键帧(只取了共视程度最高的一帧)。
        ③前面两种共视帧的父关键帧与子关键帧

局部关键帧主要包含了上述三种关键帧，可想而只，随着当前帧在变化，那么当前帧观测到的地图点也随着变化，从而局部关键帧也在变化。更新局部关键帧的代码位于 sec/Tracking.cc 文件中，注释如下：

/**
 * @brief 跟踪局部地图函数里，更新局部关键帧
 * 方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames
 * Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧 
 * Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧包括以下3种类型
 *      类型1：能观测到当前帧地图点的关键帧，也称一级共视关键帧
 *      类型2：一级共视关键帧的共视关键帧，称为二级共视关键帧
 *      类型3：一级共视关键帧的子关键帧、父关键帧
 * Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
 */
void Tracking::UpdateLocalKeyFrames()
{
    // Each map point vote for the keyframes in which it has been observed
    // Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
    map<KeyFrame*,int> keyframeCounter;
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
            if(!pMP->isBad())
            {
                // 得到观测到该地图点的关键帧和该地图点在关键帧中的索引
                const map<KeyFrame*,size_t> observations = pMP->GetObservations();
                // 由于一个地图点可以被多个关键帧观测到,因此对于每一次观测,都对观测到这个地图点的关键帧进行累计投票
                for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
                    // 这里的操作非常精彩！
                    // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
                    // it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
                    // 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，也就是共视程度
                    keyframeCounter[it->first]++;      
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }

    // 没有当前帧没有共视关键帧，返回
    if(keyframeCounter.empty())
        return;

    // 存储具有最多观测次数（max）的关键帧
    int max=0;
    KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);

    // Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型
    // 先清空局部关键帧
    mvpLocalKeyFrames.clear();
    // 先申请3倍内存，不够后面再加
    mvpLocalKeyFrames.reserve(3*keyframeCounter.size());

    // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
    // Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧） 
    for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
    {
        KeyFrame* pKF = it->first;

        // 如果设定为要删除的，跳过
        if(pKF->isBad())
            continue;
        
        // 寻找具有最大观测数目的关键帧
        if(it->second>max)
        {
            max=it->second;
            pKFmax=pKF;
        }

        // 添加到局部关键帧的列表里
        mvpLocalKeyFrames.push_back(it->first);
        
        // 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id
        // 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧
        pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
    }


    // Include also some not-already-included keyframes that are neighbors to already-included keyframes
    // Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧 
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        // Limit the number of keyframes
        // 处理的局部关键帧不超过80帧
        if(mvpLocalKeyFrames.size()>80)
            break;

        KeyFrame* pKF = *itKF;

        // 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙）
        // 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧
        const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
        // vNeighs 是按照共视程度从大到小排列
        for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
        {
            KeyFrame* pNeighKF = *itNeighKF;
            if(!pNeighKF->isBad())
            {
                // mnTrackReferenceForFrame防止重复添加局部关键帧
                if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pNeighKF);
                    pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //? 找到一个就直接跳出for循环？
                    break;
                }
            }
        }

        // 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙）
        const set<KeyFrame*> spChilds = pKF->GetChilds();
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
        {
            KeyFrame* pChildKF = *sit;
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //? 找到一个就直接跳出for循环？
                    break;
                }
            }
        }

        // 类型3:将一级共视关键帧的父关键帧（将邻居的父母们拉拢入伙）
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
            // mnTrackReferenceForFrame防止重复添加局部关键帧
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                //! 感觉是个bug！如果找到父关键帧会直接跳出整个循环
                break;
            }
        }

    }

    // Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
    if(pKFmax)
    {
        mpReferenceKF = pKFmax;
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
    }
}
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
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

 

三、局部地图点

局部关键帧能够观测到的所有地图点，总和起来，称为局部地图点，如下图，其中红色的部分称为就是局部地图点：

其代码还是十分简单的，位于 sec/Tracking.cc 文件中，注释如下：

/*
 * @brief 更新局部关键点。先把局部地图清空，然后将局部关键帧的有效地图点添加到局部地图中
 */
void Tracking::UpdateLocalPoints()
{
    // Step 1：清空局部地图点
    mvpLocalMapPoints.clear();

    // Step 2：遍历局部关键帧 mvpLocalKeyFrames
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        KeyFrame* pKF = *itKF;
        const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();

        // step 2：将局部关键帧的地图点添加到mvpLocalMapPoints
        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
        {
            MapPoint* pMP = *itMP;
            if(!pMP)
                continue;
            // 用该地图点的成员变量mnTrackReferenceForFrame 记录当前帧的id
            // 表示它已经是当前帧的局部地图点了，可以防止重复添加局部地图点
            if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
                continue;
            if(!pMP->isBad())
            {
                mvpLocalMapPoints.push_back(pMP);
                pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
            }
        }
    }
}
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

 

四、结语

上述的的两个函数 UpdateLocalKeyFrames 与 UpdateLocalPoints 都是在 Tracking::UpdateLocalMap() 中被调用，如下所示：

/**
 * @brief 更新LocalMap
 *
 * 局部地图包括： 
 * 1、K1个关键帧、K2个临近关键帧和参考关键帧
 * 2、由这些关键帧观测到的MapPoints
 */
void Tracking::UpdateLocalMap()
{
    // This is for visualization
    // 设置参考地图点用于绘图显示局部地图点（红色）
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    // Update
    // 用共视图来更新局部关键帧和局部地图点
    UpdateLocalKeyFrames();
    UpdateLocalPoints();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

可以明显的知道，更新了局部关键帧与局部地图点之后，相当于对局部地图进行了更新，那么其更新的目的时什么呢？当然是为了 局部地图跟踪TrackLocalMap() 服务，在下一篇博客中进行详细的讲解。