Staskaer

Hi, this is Staskaer

0%

ORBSLAM2源码小记(5)--跟踪器Tracking

Posted on In Valine:

ORBSLAM2的跟踪器Tracking

主要是记录下相对容易忘的内容，不会写的很细

跟踪器Tracking

Tracking的输入是普通帧，负责跟踪每一帧的位姿，并在恰当的时候插入关键帧。在SLAM系统的初始阶段，如果是单目相机会首先进行系统初始化，这时会确定单目SLAM的尺度；初始化完成后，会使用恒速模型与关键帧模型两种方法来初始化当前帧的位姿，并通过BA的方式解PnP来求解出当前帧的位姿；当然如果上述两种方法都跟踪失败了，系统会进行重定位，这时会通过Bow来选出重定位候选帧，使用EPnP来求解出当前帧的还算准确的位姿，并通过BA进一步解PnP优化出当前帧的位姿。值得一提的是，用于优化PnP的BA仅仅只会通过重投影误差优化当前帧的位姿，而不会优化地图点。

在求解出当前帧的位姿后，会使用帧的共视关系来创建局部地图，主要包括：

  • 当前帧的一级共视关键帧
  • 当前帧的一级共视关键帧的父子关键帧
  • 当前帧的二级共视关键帧二级共视选共视关系最好的前10个）

这些帧的地图点就构成了局部地图点，局部地图点主要有两个作用：

  • 更新地图点的评价指标，通过isInFrustum函数判断地图点是否在当前帧的视野内来增加对应地图点应该被观测到的次数，从而用于后续地图点的剔除
  • 对当前帧扩大匹配，从而得到更多的匹配点用于后续的优化

扩大匹配完成后，就会仅优化当前帧的位姿，来更精确的完成求解。（这里提到的所有所有优化都是通过重投影误差来优化这一帧的位姿，参与优化的其他参数为地图点的3D坐标）

最后，还会判断判断是否需要插入关键帧。

初始化

这里的初始化仅仅是对Initializer类的小封装，当系统经过两帧后，会首先检测这两帧的特征点是否超过100个，如果超过就会尝试匹配，并通过Initializer类恢复位姿并设定初始尺度。三角化成功后就会将这两帧设置为关键帧同时更新地图，并进行全局BA，这会同时优化地图点和这两个关键帧的位姿以确保求解精准。

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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
/*
 * @brief 单目的地图初始化
 *
 * 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
 * 得到初始两帧的匹配、相对运动、初始MapPoints
 *
 * Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
 * Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
 * Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
 * Step 4：如果初始化的两帧之间的匹配点太少，重新初始化
 * Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
 * Step 6：删除那些无法进行三角化的匹配点
 * Step 7：将三角化得到的3D点包装成MapPoints
 */
void Tracking::MonocularInitialization()
{
    // 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
    if (!mpInitializer)
    {
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if (mCurrentFrame.mvKeys.size() > 100)
        {
            // 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
            mInitialFrame = Frame(mCurrentFrame);
            // 用当前帧更新上一帧
            mLastFrame = Frame(mCurrentFrame);
            ...
            // 由当前帧构造初始器 sigma:1.0 iterations:200
            mpInitializer = new Initializer(mCurrentFrame, 1.0, 200);
            ...
            return;
        }
    }
    else // 如果单目初始化器已经被创建
    {
        // 如果当前帧特征点数太少（不超过100），则重新构造初始器
        // NOTICE 只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
        if ((int)mCurrentFrame.mvKeys.size() <= 100)
        {
            ...
            return;
        }

        // 在mInitialFrame与mCurrentFrame中找匹配的特征点对
        ORBmatcher matcher(
            0.9,   // 最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
            true); // 检查特征点的方向
        int nmatches = matcher.SearchForInitialization(
            mInitialFrame, mCurrentFrame, // 初始化时的参考帧和当前帧
            mvbPrevMatched,               // 在初始化参考帧中提取得到的特征点
            mvIniMatches,                 // 保存匹配关系
            100);                         // 搜索窗口大小

        // 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
        if (nmatches < 100)
        {
            ...
            return;
        }
        ...

        // 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
        if (mpInitializer->Initialize(
                mCurrentFrame,   // 当前帧
                mvIniMatches,    // 当前帧和参考帧的特征点的匹配关系
                Rcw, tcw,        // 初始化得到的相机的位姿
                mvIniP3D,        // 进行三角化得到的空间点集合
                vbTriangulated)) // 以及对应于mvIniMatches来讲,其中哪些点被三角化了
        {
            // 初始化成功后，删除那些无法进行三角化的匹配点
            for (size_t i = 0, iend = mvIniMatches.size(); i < iend; i++)
            {
                if (mvIniMatches[i] >= 0 && !vbTriangulated[i])
                {
                    mvIniMatches[i] = -1;
                    nmatches--;
                }
            }

            // 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
            mInitialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
            ...

            // 创建初始化地图点MapPoints
            // Initialize函数会得到mvIniP3D，
            // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
            // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
            CreateInitialMapMonocular();
        } // 当初始化成功的时候进行
    } // 如果单目初始化器已经被创建
}

/**
 * @brief 单目相机成功初始化后用三角化得到的点生成MapPoints
 *
 */
void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧
    KeyFrame *pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB); // 第一帧
    KeyFrame *pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB); // 第二帧

    // 将初始关键帧,当前关键帧的描述子转为BoW
    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

    // 将关键帧插入到地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

    // 用初始化得到的3D点来生成地图点MapPoints
    //  mvIniMatches[i] 表示初始化两帧特征点匹配关系。
    //  具体解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值,没有匹配关系的话，vMatches12[i]值为 -1
    for (size_t i = 0; i < mvIniMatches.size(); i++)
    {
        // 没有匹配，跳过
        if (mvIniMatches[i] < 0)
            continue;

        //  用三角化点初始化为空间点的世界坐标
        cv::Mat worldPos(mvIniP3D[i]);

        // 用3D点构造MapPoint
        MapPoint *pMP = new MapPoint(
            worldPos,
            pKFcur,
            mpMap);

        // 为该MapPoint添加属性：
        // a.观测到该MapPoint的关键帧
        // b.该MapPoint的描述子
        // c.该MapPoint的平均观测方向和深度范围

        // 表示该KeyFrame的2D特征点和对应的3D地图点
        pKFini->AddMapPoint(pMP, i);
        pKFcur->AddMapPoint(pMP, mvIniMatches[i]);

        // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
        pMP->AddObservation(pKFini, i);
        pMP->AddObservation(pKFcur, mvIniMatches[i]);

        // b.从众多观测到该MapPoint的特征点中挑选最有代表性的描述子
        pMP->ComputeDistinctiveDescriptors();
        // c.更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();

        // mvIniMatches下标i表示在初始化参考帧中的特征点的序号
        // mvIniMatches[i]是初始化当前帧中的特征点的序号
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        // Add to Map
        mpMap->AddMapPoint(pMP);
    }

    // 更新关键帧间的连接关系
    // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

    // 全局BA优化，同时优化所有位姿和三维点
    Optimizer::GlobalBundleAdjustemnt(mpMap, 20);

    // S取场景的中值深度，用于尺度归一化
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f / medianDepth;

    // 两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100
    if (medianDepth < 0 || pKFcur->TrackedMapPoints(1) < 100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // 将两帧之间的变换归一化到平均深度1的尺度下
    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1
    Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // 把3D点的尺度也归一化到1
    vector<MapPoint *> vpAllMapPoints = pKFini->GetMapPointMatches();
    for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++)
    {
        if (vpAllMapPoints[iMP])
        {
            MapPoint *pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
        }
    }

    // 将关键帧插入局部地图，更新归一化后的位姿、局部地图点
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);

    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;

    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    // 单目初始化之后，得到的初始地图中的所有点都是局部地图点
    mvpLocalMapPoints = mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;

    mCurrentFrame.mpReferenceKF = pKFcur;
    mLastFrame = Frame(mCurrentFrame);
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
    mpMap->mvpKeyFrameOrigins.push_back(pKFini);
    mState = OK; 
}

普通帧跟踪

每当一个普通帧到达时，会有限选择恒速模型进行跟踪；如果速度为空或刚刚完成重定位或恒速模型跟踪失败，那么才会使用关键帧模型进行跟踪；如果都跟踪失败，就只能进行重定位了。

  • 恒速模型指的是：假设已知了第i-1帧和第i帧的位姿分别为和，那么取速度为两者的相对变化，当第i+1帧到达时，粗略估计第i+1帧的位姿为（即认为第i帧和第i+1帧的相对位姿与第i-1帧和第i帧的相对位姿是一样的）

  • 关键帧模型指的是：由于每个普通帧的参考关键帧都是最新的关键帧，因此可以用Bow匹配当前帧和参考关键帧的特征点，并通过匹配点数目，来判断当前帧是否真的能和参考关键帧对应的上，如果匹配点数目比较多，就认为当前普通帧与上一个普通帧应该大差不差，于是直接拿上一个普通帧的位姿来粗略估计当前帧的位姿

  • 重定位指的是：由于恒速模型和关键帧模型都失败了，说明当前普通帧与最近一段的普通帧离的比较远了，只能尝试看看当前普通帧是否还在已经获取到的地图里，如果在的话还可以勉强拉回来（不在当前地图的话就没办法了）。这会使用Bow来匹配当前普通帧与所有关键帧，并选出重定位候选关键帧，同时使用EPnP算法来当前帧估计一个还算可靠的位姿。

值得一提的是，上面三种模型得到的位姿都只是"粗略估计"，只是为接下来的优化提供一个可靠的初始值。在完成上述三种方法的"粗定位"后，会利用这个"粗定位"来进行扩大匹配，并使用扩大匹配后的匹配点构建一个PnP的优化，通过仅优化当前普通帧位姿的方式来得到一个"细定位"的结果。

在得到"细定位"的结果后会立马更新局部地图，并进行局部地图优化，关于局部地图前文已经提到一点了，后文会更加详细阐述。

恒速模型

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
/**
 * @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
 * Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
 * Step 2：根据上一帧特征点对应地图点进行投影匹配
 * Step 3：优化当前帧位姿
 * Step 4：剔除地图点中外点
 * @return 如果匹配数大于10，认为跟踪成功，返回true
 */
bool Tracking::TrackWithMotionModel()
{
    ...

    // 根据之前估计的速度，用恒速模型得到当前帧的初始位姿。
    mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);

    ...

    // 设置特征匹配过程中的搜索半径
    int th;
    if (mSensor != System::STEREO)
        th = 15; // 单目
    else
        th = 7; // 双目

    // 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
    int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR);
    if (nmatches < 20)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th, mSensor == System::MONOCULAR); // 2*th
    }

    // 如果还是不能够获得足够的匹配点,那么就认为跟踪失败
    if (nmatches < 20)
        return false;

    // 利用3D-2D投影关系，优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);

    // 剔除地图点中外点，外点是在优化时通过卡方检验判断的
    int nmatchesMap = 0;
    for (int i = 0; i < mCurrentFrame.N; i++)
    {
        if (mCurrentFrame.mvpMapPoints[i])
        {
            if (mCurrentFrame.mvbOutlier[i])
            {
                // 如果优化后判断某个地图点是外点，清除它的所有关系
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];

                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                mCurrentFrame.mvbOutlier[i] = false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                // 累加成功匹配到的地图点数目
                nmatchesMap++;
        }
    }

    // 匹配超过10个点就认为跟踪成功
    return nmatchesMap >= 10;
}

关键帧模型

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
/*
 * @brief 用参考关键帧的地图点来对当前普通帧进行跟踪
 *
 * Step 1：将当前普通帧的描述子转化为BoW向量
 * Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配
 * Step 3: 将上一帧的位姿态作为当前帧位姿的初始值
 * Step 4: 通过优化3D-2D的重投影误差来获得位姿
 * Step 5：剔除优化后的匹配点中的外点
 * @return 如果匹配数超10，返回true
 *
 */
bool Tracking::TrackReferenceKeyFrame()
{
    // 将当前帧的描述子转化为BoW向量
    mCurrentFrame.ComputeBoW();

    ORBmatcher matcher(0.7, true);
    vector<MapPoint *> vpMapPointMatches;

    // 通过词袋BoW加速当前帧与参考帧之间的特征点匹配
    int nmatches = matcher.SearchByBoW(
        mpReferenceKF,      // 参考关键帧
        mCurrentFrame,      // 当前帧
        vpMapPointMatches); // 存储匹配关系

    // 匹配数目小于15，认为跟踪失败
    if (nmatches < 15)
        return false;

    // 将上一帧的位姿态作为当前帧位姿的初始值
    mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    mCurrentFrame.SetPose(mLastFrame.mTcw); 

    // 通过优化3D-2D的重投影误差来获得位姿
    Optimizer::PoseOptimization(&mCurrentFrame);

    // 剔除优化后的匹配点中的外点，同样是在优化过程中通过卡方检验来判断的
    int nmatchesMap = 0;
    for (int i = 0; i < mCurrentFrame.N; i++)
    {
        if (mCurrentFrame.mvpMapPoints[i])
        {
            // 如果对应到的某个特征点是外点
            if (mCurrentFrame.mvbOutlier[i])
            {
                // 清除它在当前帧中存在过的痕迹
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];

                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                mCurrentFrame.mvbOutlier[i] = false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                // 匹配的内点计数++
                nmatchesMap++;
        }
    }
    // 跟踪成功的数目超过10才认为跟踪成功，否则跟踪失败
    return nmatchesMap >= 10;
}

重定位

关于重定位候选帧是怎么获取的，前文已经阐述了，它会计算组得分不要忘了！

在重定位的过程中，会多次尝试扩大匹配，尽可能的挽救系统，如果实在匹配不上那也只能寄了。

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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
/**
 * @details 重定位过程
 * @return true
 * @return false
 *
 * Step 1：计算当前帧特征点的词袋向量
 * Step 2：找到与当前帧相似的候选关键帧
 * Step 3：通过BoW进行匹配
 * Step 4：通过EPnP算法估计姿态
 * Step 5：通过PoseOptimization对姿态进行优化求解
 * Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
 */
bool Tracking::Relocalization()
{
    // 计算当前帧特征点的词袋向量
    mCurrentFrame.ComputeBoW();

    // 用词袋找到与当前帧相似的候选关键帧
    vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

    ...

    // 有效的候选关键帧数目
    int nCandidates = 0;

    // Step 3：遍历所有的候选关键帧，通过词袋进行快速匹配，用匹配结果初始化PnP Solver
    for (int i = 0; i < nKFs; i++)
    {
        KeyFrame *pKF = vpCandidateKFs[i];
        if (pKF->isBad())
            vbDiscarded[i] = true;
        else
        {
            // 当前帧和候选关键帧用BoW进行快速匹配，匹配结果记录在vvpMapPointMatches，nmatches表示匹配的数目
            int nmatches = matcher.SearchByBoW(pKF, mCurrentFrame, vvpMapPointMatches[i]);
            // 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧
            if (nmatches < 15)
            {
                vbDiscarded[i] = true;
                continue;
            }
            else
            {
                // 如果匹配数目够用，用匹配结果初始化EPnPsolver
                PnPsolver *pSolver = new PnPsolver(mCurrentFrame, vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(
                    0.99,   // 用于计算RANSAC迭代次数理论值的概率
                    10,     // 最小内点数, 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个
                    300,    // 最大迭代次数
                    4,      // 最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中
                    0.5,    // 这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的
                    5.991); // 自由度为2的卡方检验的阈值,程序中还会根据特征点所在的图层对这个阈值进行缩放
                vpPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }

    ...

    // Step 4: 通过一系列操作,直到找到能够匹配上的关键帧
    // 为什么搞这么复杂？答：是担心误闭环
    while (nCandidates > 0 && !bMatch)
    {
        // 遍历当前所有的候选关键帧
        for (int i = 0; i < nKFs; i++)
        {
            ...

            // 通过EPnP算法估计姿态，迭代5次
            PnPsolver *pSolver = vpPnPsolvers[i];
            cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers);

            ...

            if (!Tcw.empty())
            {
                // 如果EPnP 计算出了位姿，对内点进行BA优化
                Tcw.copyTo(mCurrentFrame.mTcw);

                ...

                // 遍历所有内点
                for (int j = 0; j < np; j++)
                {
                    if (vbInliers[j])
                    {
                        mCurrentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];
                        sFound.insert(vvpMapPointMatches[i][j]);
                    }
                    else
                        mCurrentFrame.mvpMapPoints[j] = NULL;
                }

                // 只优化位姿，不优化地图点的坐标，返回的是内点的数量
                int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

                // 如果优化之后的内点数目不多，跳过了当前候选关键帧,但是却没有放弃当前帧的重定位
                if (nGood < 10)
                    continue;

                // 删除外点对应的地图点
                for (int io = 0; io < mCurrentFrame.N; io++)
                    if (mCurrentFrame.mvbOutlier[io])
                        mCurrentFrame.mvpMapPoints[io] = static_cast<MapPoint *>(NULL);

                // 如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
                // 前面的匹配关系是用词袋匹配过程得到的
                if (nGood < 50)
                {
                    // 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配
                    int nadditional = matcher2.SearchByProjection(
                        mCurrentFrame,     // 当前帧
                        vpCandidateKFs[i], // 关键帧
                        sFound,            // 已经找到的地图点集合，不会用于PNP
                        10,                // 窗口阈值，会乘以金字塔尺度
                        100);              // 匹配的ORB描述子距离应该小于这个阈值

                    // 如果通过投影过程新增了比较多的匹配特征点对
                    if (nadditional + nGood >= 50)
                    {
                        // 根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿
                        nGood = Optimizer::PoseOptimization(&mCurrentFrame);

                        // 如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 最后垂死挣扎
                        // 重新执行上一步的过程，只不过使用更小的搜索窗口
                        // 这里的位姿已经使用了更多的点进行了优化,应该更准，所以使用更小的窗口搜索
                        if (nGood > 30 && nGood < 50)
                        {
                            // 用更小窗口、更严格的描述子阈值，重新进行投影搜索匹配
                            sFound.clear();
                            for (int ip = 0; ip < mCurrentFrame.N; ip++)
                                if (mCurrentFrame.mvpMapPoints[ip])
                                    sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
                            nadditional = matcher2.SearchByProjection(
                                mCurrentFrame,     // 当前帧
                                vpCandidateKFs[i], // 候选的关键帧
                                sFound,            // 已经找到的地图点，不会用于PNP
                                3,                 // 新的窗口阈值，会乘以金字塔尺度
                                64);               // 匹配的ORB描述子距离应该小于这个阈值

                            // 如果成功挽救回来，匹配数目达到要求，最后BA优化一下
                            if (nGood + nadditional >= 50)
                            {
                                nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                                // 更新地图点
                                for (int io = 0; io < mCurrentFrame.N; io++)
                                    if (mCurrentFrame.mvbOutlier[io])
                                        mCurrentFrame.mvpMapPoints[io] = NULL;
                            }
                            // 如果还是不能够满足就放弃了
                        }
                    }
                }

                // 如果对于当前的候选关键帧已经有足够的内点(50个)了,那么就认为重定位成功
                if (nGood >= 50)
                {
                    bMatch = true;
                    // 只要有一个候选关键帧重定位成功，就退出循环，不考虑其他候选关键帧了
                    break;
                }
            }
        } // 一直运行,直到已经没有足够的关键帧,或者是已经有成功匹配上的关键帧
    }

    // 折腾了这么久还是没有匹配上，重定位失败
    if (!bMatch)
    {
        return false;
    }
    else
    {
        // 如果匹配上了,说明当前帧重定位成功了(当前帧已经有了自己的位姿)
        // 记录成功重定位帧的id，防止短时间多次重定位
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    }
}

局部地图优化

前文提到，在求解出当前帧的"细定位"位姿后，会使用帧的共视关系来创建局部地图，并通过局部地图优化来求解"精确定位"，其中局部地图主要包括：

  • 当前帧的一级共视关键帧
  • 当前帧的一级共视关键帧的父子关键帧
  • 当前帧的二级共视关键帧二级共视选共视关系最好的前10个）

注意Tracking中的局部地图优化不要和LocalMapping中的局部BA搞混了：

  • Tracking中的局部地图优化过程参与优化的仅有当前普通帧和一些局部地图点，目的是通过扩大匹配的方式来进一步求解当前普通帧的"精确定位"，仅仅只会优化当前普通帧的位姿，不会优化地图点的位姿
  • LocalMapping中的局部BA过程参与优化的是多个关键帧（一二级共视）和这些关键帧的地图点，目的是同时调整一大堆关键帧和地图点的位姿，会优化关键帧的位姿（不过二级共视关键帧不会优化位姿）和地图点的位姿

当获取到局部地图点后，会使用这些局部地图点来扩大匹配，同时调整每个地图点的指标（前文提到的通过isInFrustum函数判断地图点是否在当前帧的视野内来增加对应地图点应该被观测到的次数，从而用于后续地图点的剔除）。最后执行和恒速|关键帧|重定位的同款优化来解PnP求出当前普通帧的"精确定位"

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
/**
 * @brief 用局部地图进行跟踪，进一步优化位姿
 *
 * 1. 更新局部地图，包括局部关键帧和关键点
 * 2. 对局部MapPoints进行投影匹配
 * 3. 根据匹配对估计当前帧的姿态
 * 4. 根据姿态剔除误匹配
 * @return true if success
 *
 * Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
 * Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪
 * Step 3：更新局部所有MapPoints后对位姿再次优化
 * Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
 * Step 5：决定是否跟踪成功
 */
bool Tracking::TrackLocalMap()
{
    // 更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
    UpdateLocalMap();

    // 筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系
    SearchLocalPoints();

    // Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 的同款优化
    // 前面新增了更多的匹配关系，BA优化得到更准确的位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
    mnMatchesInliers = 0;

    // 更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目
    for (int i = 0; i < mCurrentFrame.N; i++)
    {
        if (mCurrentFrame.mvpMapPoints[i])
        {
            // 由于当前帧的地图点可以被当前帧观测到，其被观测统计量加1
            if (!mCurrentFrame.mvbOutlier[i])
            {
                // 找到该点的帧数mnFound 加 1
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                // 查看当前是否是在纯定位过程
                if (!mbOnlyTracking)
                {
                    // 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1
                    // nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2
                    if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                        mnMatchesInliers++;
                }
                else
                    // 记录当前帧跟踪到的地图点数目，用于统计跟踪效果
                    mnMatchesInliers++;
            }
            ...
        }
    }

    // 根据跟踪匹配数目及重定位情况决定是否跟踪成功
    // 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪
    if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 50)
        return false;

    // 如果是正常的状态话只要跟踪的地图点大于30个就认为成功了
    if (mnMatchesInliers < 30)
        return false;
    else
        return true;
}

关键帧插入条件

当跟踪完成后，会判断是否需要插入关键帧，满足以下条件就会插入：

  • 当前帧的id与上次插入关键帧的id差距较大（很长时间没有插入关键帧）
  • 满足插入关键帧的最小间隔并且localMapper处于空闲状态
  • 和参考帧相比当前跟踪到的点太少；同时跟踪到的内点还不能太少
  • 满足上述条件且LocalMapping不是很忙（因为插入是push到一个列表中，然后LocalMapping逐个pop并处理）
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
/**
 * @brief 判断当前帧是否需要插入关键帧
 *
 * Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧
 * Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧
 * Step 3：得到参考关键帧跟踪到的地图点数量
 * Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
 * Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量
 * Step 6：决策是否需要插入关键帧
 * @return true         需要
 * @return false        不需要
 */
bool Tracking::NeedNewKeyFrame()
{
    // 纯VO模式下不插入关键帧
    if (mbOnlyTracking)
        return false;

    // 如果局部地图线程被闭环检测使用，则不插入关键帧
    if (mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;

    ...

    // 如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧
    if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs > mMaxFrames)
        return false;

    ...

    // 查询局部地图线程是否繁忙，当前能否接受新的关键帧
    bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

    ...

    // 设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧
    float thRefRatio = 0.75f;

    // 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低
    if (nKFs < 2)
        thRefRatio = 0.4f;

    // 单目情况下插入关键帧的频率很高
    if (mSensor == System::MONOCULAR)
        thRefRatio = 0.9f;

    // 很长时间没有插入关键帧，可以插入
    const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames;

    // 满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
    const bool c1b = (mCurrentFrame.mnId >= mnLastKeyFrameId + mMinFrames && bLocalMappingIdle);

    // 在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose
    const bool c1c = mSensor != System::MONOCULAR &&           // 只考虑在双目，RGB-D的情况
                     (mnMatchesInliers < nRefMatches * 0.25 || // 当前帧和地图点匹配的数目非常少
                      bNeedToInsertClose);                     // 需要插入

    // 和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少
    const bool c2 = ((mnMatchesInliers < nRefMatches * thRefRatio || bNeedToInsertClose) && mnMatchesInliers > 15);

    if ((c1a || c1b || c1c) && c2)
    {
        // local mapping空闲时可以直接插入，不空闲的时候要根据情况插入
        if (bLocalMappingIdle)
        {
            // 可以插入关键帧
            return true;
        }
        else
        {
            mpLocalMapper->InterruptBA();
            if (mSensor != System::MONOCULAR)
            {
                // 队列里不能阻塞太多关键帧
                // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
                // 然后localmapper再逐个pop出来插入到mspKeyFrames
                if (mpLocalMapper->KeyframesInQueue() < 3)
                    // 队列中的关键帧数目不是很多,可以插入
                    return true;
                else
                    // 队列中缓冲的关键帧数目太多,暂时不能插入
                    return false;
            }
            else
                return false;
        }
    }
    else
        // 不满足上面的条件,自然不能插入关键帧
        return false;
}