Staskaer

Hi, this is Staskaer

0%

ORBSLAM2源码小记(4)--匹配器ORBmatcher

Posted on In Valine:

ORBSLAM2的匹配器ORBmatcher

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

匹配器ORBmatcher

匹配器的目的是将A帧中的特征点与B帧中的特征点进行匹配，这样才能确定位姿与三角化地图点。除此之外，匹配器还负责融合地图点，这主要是在LocalMapping过程中融合重复地图点以及闭环后融合重复地图点。

最理想的匹配方式是暴力匹配，但是这过于耗时。为了降低开销，ORBSLAM2使用了多种方法来降低搜索范围，比如通过词袋、投影等方法来筛选区域。

另外，匹配基于目的还可以分为两类：

  • 搜索匹配：两帧之间没有任何匹配点，比如初始化时以及当一个新的普通帧到达时
  • 扩大匹配：两帧之间已经有了一些匹配点，但是为了后续优化准确性，需要把共视帧的地图点通过投影(sim3或pose)到当前帧，从而进一步得到更多的匹配点

搜索匹配

  • SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame ...)：一个普通帧到达，且恒速模型可用，用恒速模型预测当前普通帧的位姿，并基于此预测的位姿把前一阵匹配点对应的地图点投影到当前帧并在一定范围内搜索
  • SearchByBoW(KeyFrame *pKF, Frame &F ...)：一个普通帧到达，但恒速模型不可用，转而使用关键帧跟踪或重定位，使用词袋匹配当前普通帧与关键帧的特征点
  • SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2 ...)：闭环矫正时，通过词袋搜索两个关键帧之间的匹配点，以用于初始计算两帧之间的Sim3
  • SearchForInitialization(Frame &F1, Frame &F2 ...：单目初始化，与SearchByProjection类似，但是由于不知道初始两帧的位姿，缺少了投影的步骤，转而在搜索时使用了一个较大区域来尽可能多的匹配特征点

扩大匹配

  • SearchByProjection(Frame &F, const std::vector<MapPoint *> &vpMapPoints ...)：Tracking中用局部关键帧中的局部地图点投影到当前普通帧来扩大匹配
  • SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF ...)：重定位时，将重定位候选关键帧的地图点投影到当前普通帧以扩大匹配
  • SearchByProjection(KeyFrame *pKF, cv::Mat Scw, const std::vector<MapPoint *> &vpPoints ...)：回环计算Sim3时，用已经粗略计算出来的Sim3把共视帧的地图点投影到当前帧，以扩大匹配来进行优化
  • SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2 ...)：LocalMapping中为了三角化更多地图点，使用基础矩阵的极线约束，来进一步扩大匹配
  • SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2 ...)：回环计算Sim3时，用计算好的Sim3把闭环候选关键帧的地图点投影到当前关键帧以扩大匹配

匹配步骤

用于匹配的函数虽然有很多，但是基本上大同小异，主要就是一些约束条件不一样，这里就以SearchByProjection为例来简述匹配流程

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
/**
 * @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧
 * 步骤
 * Step 1 建立旋转直方图，用于检测旋转一致性
 * Step 2 计算当前帧和前一帧的平移向量
 * Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
 * Step 4 根据相机的前后前进方向来判断搜索尺度范围
 * Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点
 * Step 6 计算匹配点旋转角度差所在的直方图
 * Step 7 进行旋转一致检测，剔除不一致的匹配
 * @param[in] CurrentFrame          当前帧
 * @param[in] LastFrame             上一帧
 * @param[in] th                    搜索范围阈值，默认单目为7，双目15
 * @param[in] bMono                 是否为单目
 * @return int                      成功匹配的数量
 */
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
    int nmatches = 0;

    // 建立旋转直方图，用于检测旋转一致性
    vector<int> rotHist[HISTO_LENGTH];
    for (int i = 0; i < HISTO_LENGTH; i++)
        rotHist[i].reserve(500);
    const float factor = HISTO_LENGTH / 360.0f;

    // 计算当前帧和前一帧的相对变换
    const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0, 3).colRange(0, 3);
    const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0, 3).col(3);
    const cv::Mat twc = -Rcw.t() * tcw;
    const cv::Mat Rlw = LastFrame.mTcw.rowRange(0, 3).colRange(0, 3);
    const cv::Mat tlw = LastFrame.mTcw.rowRange(0, 3).col(3); 
    const cv::Mat tlc = Rlw * twc + tlw;

    // 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
    for (int i = 0; i < LastFrame.N; i++)
    {
        MapPoint *pMP = LastFrame.mvpMapPoints[i];

        if (pMP)
        {
            if (!LastFrame.mvbOutlier[i])
            {
                // 对上一帧有效的MapPoints投影到当前帧坐标系
                cv::Mat x3Dw = pMP->GetWorldPos();
                cv::Mat x3Dc = Rcw * x3Dw + tcw;

                const float xc = x3Dc.at<float>(0);
                const float yc = x3Dc.at<float>(1);
                const float invzc = 1.0 / x3Dc.at<float>(2);

                if (invzc < 0)
                    continue;

                // 投影到像素坐标系
                float u = CurrentFrame.fx * xc * invzc + CurrentFrame.cx;
                float v = CurrentFrame.fy * yc * invzc + CurrentFrame.cy;

                if (u < CurrentFrame.mnMinX || u > CurrentFrame.mnMaxX)
                    continue;
                if (v < CurrentFrame.mnMinY || v > CurrentFrame.mnMaxY)
                    continue;

                // 上一帧中地图点对应二维特征点所在的金字塔层级
                int nLastOctave = LastFrame.mvKeys[i].octave;

                // 单目：th = 7，双目：th = 15
                float radius = th * CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大

                // 记录候选匹配点的id
                vector<size_t> vIndices2;

                // 根据相机的前后前进方向来判断搜索尺度范围。
                // 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
                // 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
                // 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来
                if (bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nLastOctave);
                else if (bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, 0, nLastOctave);
                else // 在[nLastOctave-1, nLastOctave+1]中搜索
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nLastOctave - 1, nLastOctave + 1);

                if (vIndices2.empty())
                    continue;

                const cv::Mat dMP = pMP->GetDescriptor();

                int bestDist = 256;
                int bestIdx2 = -1;

                // 遍历候选匹配点，寻找距离最小的最佳匹配点
                for (vector<size_t>::const_iterator vit = vIndices2.begin(), vend = vIndices2.end(); vit != vend; vit++)
                {
                    const size_t i2 = *vit;

                    // 如果该特征点已经有对应的MapPoint了,则退出该次循环
                    if (CurrentFrame.mvpMapPoints[i2])
                        if (CurrentFrame.mvpMapPoints[i2]->Observations() > 0)
                            continue;

                    if (CurrentFrame.mvuRight[i2] > 0)
                    {
                        // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内
                        const float ur = u - CurrentFrame.mbf * invzc;
                        const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
                        if (er > radius)
                            continue;
                    }

                    const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);

                    const int dist = DescriptorDistance(dMP, d);

                    if (dist < bestDist)
                    {
                        bestDist = dist;
                        bestIdx2 = i2;
                    }
                }

                // 最佳匹配距离要小于设定阈值
                if (bestDist <= TH_HIGH)
                {
                    CurrentFrame.mvpMapPoints[bestIdx2] = pMP;
                    nmatches++;

                    // Step 6 计算匹配点旋转角度差所在的直方图
                    if (mbCheckOrientation)
                    {
                        float rot = LastFrame.mvKeysUn[i].angle - CurrentFrame.mvKeysUn[bestIdx2].angle;
                        if (rot < 0.0)
                            rot += 360.0f;
                        int bin = round(rot * factor);
                        if (bin == HISTO_LENGTH)
                            bin = 0;
                        assert(bin >= 0 && bin < HISTO_LENGTH);
                        rotHist[bin].push_back(bestIdx2);
                    }
                }
            }
        }
    }

    // 进行旋转一致检测，剔除不一致的匹配
    if (mbCheckOrientation)
    {
        int ind1 = -1;
        int ind2 = -1;
        int ind3 = -1;

        ComputeThreeMaxima(rotHist, HISTO_LENGTH, ind1, ind2, ind3);

        for (int i = 0; i < HISTO_LENGTH; i++)
        {
            // 对于数量不是前3个的点对，剔除
            if (i != ind1 && i != ind2 && i != ind3)
            {
                for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++)
                {
                    CurrentFrame.mvpMapPoints[rotHist[i][j]] = static_cast<MapPoint *>(NULL);
                    nmatches--;
                }
            }
        }
    }

    return nmatches;
}

地图点融合

在LocalMapping会融合相邻帧中重复地图点，而在LoopClosing中，则需要融合闭环后潜在重复地图点。融合时需要比较两个地图点的相似性，所以这项任务也交给匹配器来完成。

融合思路其实比较简单：

  • 首先判断地图点投影后是否在当前帧视野内，（与isInFrustum函数类似）主要涉及相机坐标系深度为正、像素坐标系在校正后有效区域内、地图点与光心的距离在有效区域内以及地图点方向与光心夹角小于一定阈值
  • 然后判断当前地图点可能出现在金字塔的哪些层中，从而确定特征点的搜索范围
  • 遍历匹配该区域内的特征点，并使用描述子距离和卡方检验来排除潜在的误匹配
  • 通过上述步骤的特征点如果有地图点，则选一个较好的（被观测到次数更多的地图点）；否则则为该特征点添加地图点
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
/**
 * @brief 将地图点投影到关键帧中进行匹配和融合；融合策略如下
 * 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
 * 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

 * @param[in] pKF           关键帧
 * @param[in] vpMapPoints   待投影的地图点
 * @param[in] th            搜索窗口的阈值，默认为3
 * @return int              更新地图点的数量
 */
int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)
{
    ...

    // 遍历所有的待投影地图点
    for (int i = 0; i < nMPs; i++)
    {

        MapPoint *pMP = vpMapPoints[i];
        if (!pMP)
            continue;
        // 地图点无效 或 已经是该帧的地图点（无需融合），跳过
        if (pMP->isBad() || pMP->IsInKeyFrame(pKF))
            continue;

        // 将地图点变换到关键帧的相机坐标系下
        cv::Mat p3Dw = pMP->GetWorldPos();
        cv::Mat p3Dc = Rcw * p3Dw + tcw;

        // 深度值为负，跳过
        if (p3Dc.at<float>(2) < 0.0f)
            continue;

        // 得到地图点投影到关键帧的图像坐标
        const float invz = 1 / p3Dc.at<float>(2);
        const float x = p3Dc.at<float>(0) * invz;
        const float y = p3Dc.at<float>(1) * invz;

        const float u = fx * x + cx;
        const float v = fy * y + cy;

        // 投影点需要在有效范围内
        if (!pKF->IsInImage(u, v))
            continue;

        const float ur = u - bf * invz;
        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        cv::Mat PO = p3Dw - Ow;
        const float dist3D = cv::norm(PO);

        // 地图点到关键帧相机光心距离需满足在有效范围内
        if (dist3D < minDistance || dist3D > maxDistance)
            continue;

        // 地图点到光心的连线与该地图点的平均观测向量之间夹角要小于60°
        cv::Mat Pn = pMP->GetNormal();
        if (PO.dot(Pn) < 0.5 * dist3D)
            continue;
        // 根据地图点到相机光心距离预测匹配点所在的金字塔尺度
        int nPredictedLevel = pMP->PredictScale(dist3D, pKF);

        // 确定搜索范围
        const float radius = th * pKF->mvScaleFactors[nPredictedLevel];
        // 在投影点附近搜索窗口内找到候选匹配点的索引
        const vector<size_t> vIndices = pKF->GetFeaturesInArea(u, v, radius);

        if (vIndices.empty())
            continue;

        // 遍历寻找最佳匹配点
        const cv::Mat dMP = pMP->GetDescriptor();

        int bestDist = 256;
        int bestIdx = -1;
        for (vector<size_t>::const_iterator vit = vIndices.begin(), vend = vIndices.end(); vit != vend; vit++) // 遍历搜索范围内的features
        {
            const size_t idx = *vit;

            const cv::KeyPoint &kp = pKF->mvKeysUn[idx];

            const int &kpLevel = kp.octave;
            // 金字塔层级要接近（同一层或小一层），否则跳过
            if (kpLevel < nPredictedLevel - 1 || kpLevel > nPredictedLevel)
                continue;

            // 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过
            if (pKF->mvuRight[idx] >= 0)
            {
                // 双目情况
                const float &kpx = kp.pt.x;
                const float &kpy = kp.pt.y;
                const float &kpr = pKF->mvuRight[idx];
                const float ex = u - kpx;
                const float ey = v - kpy;
                // 右目数据的偏差也要考虑进去
                const float er = ur - kpr;
                const float e2 = ex * ex + ey * ey + er * er;

                // 自由度为3, 误差小于1个像素,这种事情95%发生的概率对应卡方检验阈值为7.82
                if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 7.8)
                    continue;
            }
            else
            {
                // 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过
                // 单目情况
                const float &kpx = kp.pt.x;
                const float &kpy = kp.pt.y;
                const float ex = u - kpx;
                const float ey = v - kpy;
                const float e2 = ex * ex + ey * ey;

                // 自由度为2的，卡方检验阈值5.99（假设测量有一个像素的偏差）
                if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 5.99)
                    continue;
            }

            const cv::Mat &dKF = pKF->mDescriptors.row(idx);

            const int dist = DescriptorDistance(dMP, dKF);
            // 和投影点的描述子距离最小
            if (dist < bestDist)
            {
                bestDist = dist;
                bestIdx = idx;
            }
        }

        // 找到投影点对应的最佳匹配特征点，根据是否存在地图点来融合或新增
        // 最佳匹配距离要小于阈值
        if (bestDist <= TH_LOW)
        {
            MapPoint *pMPinKF = pKF->GetMapPoint(bestIdx);
            if (pMPinKF)
            {
                // 如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换
                if (!pMPinKF->isBad())
                {
                    if (pMPinKF->Observations() > pMP->Observations())
                        pMP->Replace(pMPinKF);
                    else
                        pMPinKF->Replace(pMP);
                }
            }
            else
            {
                // 如果最佳匹配点没有对应地图点，添加观测信息
                pMP->AddObservation(pKF, bestIdx);
                pKF->AddMapPoint(pMP, bestIdx);
            }
            nFused++;
        }
    }

    return nFused;
}