(01)ORB-SLAM2源码无死角解析-(50) 局部建图线程→SearchInNeighbors():融合重复地图点

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

上一篇博对 CreateNewMapPoints() 函数进行了讲解，其主要功能为: 对当前关键帧与共视关键帧匹配的特征点，进行三角话，生成新的地图点。但是这样有一个后遗症，那就是会有很多冗余的地图点。比如共视关键帧 pKFi2 中的一个关键点 kp2 原本就存在对应的地图点 pMP，kp2 与当前关键帧中的关键点 kp1匹配，匹配关键点对 kp2 与 kp1 经过三角话之后，会形成一个新的地图点 new_pMP。那么此时就存在 pMP 与 new_pMP 两个地图点，无可厚非针对于当前帧只能选择使用 new_pMP，但是共视关键帧 pKFi2，我们应该选择那个地图点呢？或者说是否有必要对这两个地图点做一个融合呢？那么下面就来看看源码是如何操作的吧
 

二、多级共视关键帧

在前面的博客中，已经讲解过共视图的概念，这里就再做深入的讲解，简单的说就是与能看到当前关键帧地图点的其他关键帧都称为一级共视关键帧，能够看到一级共视关键帧地图点的关键帧，称为二级共视关键帧，如下图所示：

 

三、SearchInNeighbors() 总体流程

在深入了解源码之前，下来看看 SearchInNeighbors() 函数的总体流程：
$(1):$ 获得所有一级相邻关键帧以及二级相邻关键帧，存放于 vpTargetKFs 变量之中。然后进行循环遍历。

$(2):$ 循环取出 vpTargetKFs 的每一共视关键帧，将 $当前帧地图点投影到共视关键帧$ 中进行匹配和融合，与核型函数为 matcher.Fuse(pKFi,vpMapPointMatches); 第一个参数为共视关键帧，第二个参数为当前关键帧的地图点。
①如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
②如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点
具体融合过程在后面分析。

$(3):$ 获得共视关键帧的所有地图点，存储于vpFuseCandidates中， 然后进行反向融合，这里的操作是把 $共视关键帧地图点$$投影到当前关键帧$，相当于步骤(2)的反向融合。
①如果共视地图关键帧的地图点能匹当前关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
②如果共视地图关键帧的地图点能匹当前关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

$(4):$ 因为单前关键帧的地图点可能变了，所以需要更新当前帧地图点的描述子、深度、平均观测方向等属性，最后再更新当前关键帧与其它关键帧的共视连接关系
 

四、SearchInNeighbors() 代码注释

该函数实现于 src/Tracking.cc 文件中，在 LocalMapping::Run() 函数中被调用，这里需要注意一个点，当没有新的关键帧，也就是已经处理完队列中的最后的一个关键帧，即满足 !CheckNewKeyFrames() 时，才会调用该函数。LocalMapping::SearchInNeighbors() 注释如下：

/**
 * @brief 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点
 * 
 */
void LocalMapping::SearchInNeighbors()
{
    // Retrieve neighbor keyframes
    // Step 1：获得当前关键帧在共视图中权重排名前nn的邻接关键帧
    // 开始之前先定义几个概念
    // 当前关键帧的邻接关键帧，称为一级相邻关键帧，也就是邻居
    // 与一级相邻关键帧相邻的关键帧，称为二级相邻关键帧，也就是邻居的邻居

    // 单目情况要20个邻接关键帧，双目或者RGBD则要10个
    int nn = 10;
    if(mbMonocular)
        nn=20;

    // 和当前关键帧相邻的关键帧，也就是一级相邻关键帧
    const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
    
    // Step 2：存储一级相邻关键帧及其二级相邻关键帧
    vector<KeyFrame*> vpTargetKFs;
    // 开始对所有候选的一级关键帧展开遍历：
    for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        // 没有和当前帧进行过融合的操作
        if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
            continue;
        // 加入一级相邻关键帧    
        vpTargetKFs.push_back(pKFi);
        // 标记已经加入
        pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;

        // Extend to some second neighbors
        // 以一级相邻关键帧的共视关系最好的5个相邻关键帧 作为二级相邻关键帧
        const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
        // 遍历得到的二级相邻关键帧
        for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
        {
            KeyFrame* pKFi2 = *vit2;
            // 当然这个二级相邻关键帧要求没有和当前关键帧发生融合,并且这个二级相邻关键帧也不是当前关键帧
            if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
                continue;
            // 存入二级相邻关键帧    
            vpTargetKFs.push_back(pKFi2);
        }
    }

    // Search matches by projection from current KF in target KFs
    // 使用默认参数, 最优和次优比例0.6,匹配时检查特征点的旋转
    ORBmatcher matcher;

    // Step 3：将当前帧的地图点分别投影到两级相邻关键帧，寻找匹配点对应的地图点进行融合，称为正向投影融合
    vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;

        // 将地图点投影到关键帧中进行匹配和融合；融合策略如下
        // 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
        // 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点
        // 注意这个时候对地图点融合的操作是立即生效的
        matcher.Fuse(pKFi,vpMapPointMatches);
    }

    // Search matches by projection from target KFs in current KF
    // Step 4：将两级相邻关键帧地图点分别投影到当前关键帧，寻找匹配点对应的地图点进行融合，称为反向投影融合
    // 用于进行存储要融合的一级邻接和二级邻接关键帧所有MapPoints的集合
    vector<MapPoint*> vpFuseCandidates;
    vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());
    
    //  Step 4.1：遍历每一个一级邻接和二级邻接关键帧，收集他们的地图点存储到 vpFuseCandidates
    for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
    {
        KeyFrame* pKFi = *vitKF;
        vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();

        // 遍历当前一级邻接和二级邻接关键帧中所有的MapPoints,找出需要进行融合的并且加入到集合中
        for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
        {
            MapPoint* pMP = *vitMP;
            if(!pMP)
                continue;
            
            // 如果地图点是坏点，或者已经加进集合vpFuseCandidates，跳过
            if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
                continue;

            // 加入集合，并标记已经加入
            pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
            vpFuseCandidates.push_back(pMP);
        }
    }
    // Step 4.2：进行地图点投影融合,和正向融合操作是完全相同的
    // 不同的是正向操作是"每个关键帧和当前关键帧的地图点进行融合",而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合"
    matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);

    // Update points
    // Step 5：更新当前帧地图点的描述子、深度、平均观测方向等属性
    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
        MapPoint* pMP=vpMapPointMatches[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                // 在所有找到pMP的关键帧中，获得最佳的描述子
                pMP->ComputeDistinctiveDescriptors();

                // 更新平均观测方向和观测距离
                pMP->UpdateNormalAndDepth();
            }
        }
    }

    // Update connections in covisibility graph
    // Step 6：更新当前帧与其它帧的共视连接关系
    mpCurrentKeyFrame->UpdateConnections();
}
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

 

五、ORBmatcher::Fuse()代码流程

从上面的讲解，可以很明显的 SearchInNeighbors() 中最核型的代码就是：

	matcher.Fuse(pKFi,vpMapPointMatches);
	matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
1
2

那么现在我们就来看看其具体实现过程：

$(01):$ 获取传入关键帧 pKF 的位姿、内参、光心在世界坐标系下坐标，然后对待投影地图点进行遍历。

$(02):$ 如果该地图点是无效的，或者已经是该关键帧 pKF 的地图点。则直接跳过。

$(03):$ 把地图点投影到到关键帧的图像坐标:
①投影点需要在图像有效范围
②地图点到关键帧相机光心距离需满足在有效范围内。
③地图点到光心的连线与该地图点的平均观测向量之间夹角要小于60°

$(04):$ 根据预测匹配点所在的金字塔尺度，在投影点附近搜索窗口内找到候选匹配点的索引(可能多个)。

$(05):$ 根据索引对找到的候选匹配点进行遍历(遍历寻找最佳匹配点):
①匹配关键点的金字塔层级要接近（同一层或小一层），否则跳过
②计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过
③和投影点的描述子距离最小

$(06):$ 找到投影点对应的最佳匹配特征点，根据是否存在地图点来融合或新增.
①如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换
②如果最佳匹配点没有对应地图点，添加观测信息(把地图点添加到关键帧中)

 

六、ORBmatcher::Fuse()代码注释

该代码位于 src/ORBmatcher.cc 文件中

/**
 * @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)
{
    // 取出当前帧位姿、内参、光心在世界坐标系下坐标
    cv::Mat Rcw = pKF->GetRotation();
    cv::Mat tcw = pKF->GetTranslation();

    const float &fx = pKF->fx;
    const float &fy = pKF->fy;
    const float &cx = pKF->cx;
    const float &cy = pKF->cy;
    const float &bf = pKF->mbf;

    cv::Mat Ow = pKF->GetCameraCenter();

    int nFused=0;

    const int nMPs = vpMapPoints.size();

    // 遍历所有的待投影地图点
    for(int i=0; i<nMPs; i++)
    {
        
        MapPoint* pMP = vpMapPoints[i];
        // Step 1 判断地图点的有效性 
        if(!pMP)
            continue;
        // 地图点无效 或 已经是该帧的地图点（无需融合），跳过
        if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
            continue;

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

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

        // Step 2 得到地图点投影到关键帧的图像坐标
        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;

        // Point must be inside the image
        // 投影点需要在有效范围内
        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);

        // Depth must be inside the scale pyramid of the image
        // Step 3 地图点到关键帧相机光心距离需满足在有效范围内
        if(dist3D<minDistance || dist3D>maxDistance )
            continue;

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

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

        if(vIndices.empty())
            continue;

        // Match to the most similar keypoint in the radius
         // Step 6 遍历寻找最佳匹配点
        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++)// 步骤3：遍历搜索范围内的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)
            {
                // Check reprojection error in stereo
                // 双目情况
                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 there is already a MapPoint replace otherwise add new measurement
        // Step 7 找到投影点对应的最佳匹配特征点，根据是否存在地图点来融合或新增
        // 最佳匹配距离要小于阈值
        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;
}
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

 

七、结语

总体来说，该篇博客的思想还是很好理解的，首先就是找到一级和二级共视帧关键帧，然后把当前帧地图点投影到共视帧关键帧，在投影点一定范围内进行搜索匹配，找到描述子距离最小的特征点，进一步判断这个特征点是否已经存在地图点，如果已经存在了，则把对两个地图都替换成观测到次数多的地图点。然后还做了一个逆的操作，就是把所有共视帧关键帧的所有地图点投影到当前帧，然后做同样的操作。