(01)ORB-SLAM2源码无死角解析-(49) 局部建图线程→MapPointCulling,CreateNewMapPoints

江南才尽，年少无知！

已于 2022-09-21 09:04:14 修改

阅读量850

点赞数

分类专栏： # (01)ORB-SLAM2源码无死角解析-免费文章标签：计算机视觉智能驾驶机器人 SLAM 增强现实

于 2022-08-11 17:37:28 首次发布

本文链接：https://blog.csdn.net/weixin_43013761/article/details/126285793

版权

(01)ORB-SLAM2源码无死角解析-免费专栏收录该内容

50 篇文章 103 订阅

订阅专栏

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

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过上一篇博客，对局部建图线程 LocalMapping::Run() 做了整体的介绍，然后分析了 ProcessNewKeyFrame() 函数，该函数主要功能为计算关键帧的BoW、更新观测、描述子、共视图，插入到地图等。下面主要是对 LocalMapping::Run() 中调用的 MapPointCulling() 函数进行讲解，其位于 src/LocalMapping.cc 文件中。在讲解之前我们回顾一下上一篇博客讲解的 ProcessNewKeyFrame() 函数，可以看到如下代码：

	// 如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息
	// 这些地图点可能来自双目或RGBD跟踪过程创建新关键帧生成的地图点，或者是CreateNewMapPoints 中通过三角化产生
	// 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验
	mlpRecentAddedMapPoints.push_back(pMP);

这里存在一个变量 mlpRecentAddedMapPoints，对接下来的讲解有一个引导作用。其中的元素来自于双目或RGBD跟踪过程创建新关键帧生成的地图点，以及 CreateNewMapPoints 中通过三角化产生，下面就来分析 MapPointCulling 以及 CreateNewMapPoints 函数

二、MapPointCulling()

$\color{blue}{(1):}$ 获得需要检查的新增地图点，根据相机类型设置不同的观测阈值。然后循环检查每个地图点

$\color{blue}{(2):}$ 已经是坏点的地图点仅从队列中删除

$\color{blue}{(3):}$ 跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除。
①mnVisible: 在局部地图追踪函数TrackLocalMap()中调用了SearchLocalPoints()，其会判断该地图点是否在当前帧的视野范围内，如果在，说明其理应被当前帧(含普通帧)观测到，则会调用 IncreaseVisible() 对 mnVisible 执行+1操作。
②mnFound: 在局部地图追踪函数 TrackLocalMap() 中，通过 SearchLocalPoints() 函数获得了更多的匹配关系之后，当前帧的每个地图点都会调用 IncreaseFound() 函数，对 mnFound 执行 +1 操作。

简单的说，mnVisible表示该地图点理论被观测到的次数，mnFound为局部地图追踪过程中，该地图点被实际观测到的次数，如果 mnFound/mnVisible<0.25 则设置为坏点。从队列中删除

$\color{blue}{(4):}$ 从该点建立开始，到现在已经过了不小于2个关键帧但是观测到该点的相机数却不超过阈值cnThObs，设置为坏点，从地图中删除

$\color{blue}{(5):}$ 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点因此没有SetBadFlag()，仅从队列中删除。

大家注意，如果认为只坏点，仅仅设置为 SetBadFlag() 而已，是从 mlpRecentAddedMapPoints 移除，并不是直接从地图中删除这个坏点。代码位于 LocalMapping.cc 文件中，注释如下：

/**
 * @brief 检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点
 * mlpRecentAddedMapPoints：存储新增的地图点，这里是要删除其中不靠谱的
 */
void LocalMapping::MapPointCulling()
{
    // Check Recent Added MapPoints
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    // Step 1：根据相机类型设置不同的观测阈值
    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;
	
	// Step 2：遍历检查新添加的地图点
    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;
        if(pMP->isBad())
        {
            // Step 2.1：已经是坏点的地图点仅从队列中删除
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(pMP->GetFoundRatio()<0.25f)
        {
            // Step 2.2：跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除
            // (mnFound/mnVisible） < 25%
            // mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好
            // mnVisible：地图点应该被看到的次数
            // (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
            // Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧
            // 但是观测到该点的相机数却不超过阈值cnThObs，从地图中删除
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            // Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
            // 因此没有SetBadFlag()，仅从队列中删除
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

三、CreateNewMapPoints→流程

从上面的过程可以知道， MapPointCulling 函数是比较简单容易理解，现在来看看 CreateNewMapPoints() 函数，其位于 src/LocalMapping.cc 文件中。过程还是比较繁琐的，不过没关系，下面会进行十分详细的讲解。

$\color{red}{注意点1:}$ 如果是双目或者深度相机，在 Frame 初始化的时候，会计算关键点的的深度值，参考函数为 src/Frame.cc 文件中的 ComputeStereoFromRGBD() 与 ComputeStereoMatches()。后续在创建关键帧，也就是执行 Tracking::CreateNewKeyFrame() 函数的时候，若关键点存在深度值，则会创建地图点。详细细节可以参考博客:(01)ORB-SLAM2源码无死角解析-(25) 关键帧KeyFrame→如何创建、插入关键帧，这里只为有深度的关键点创建了关键点。

$\color{red}{注意点2:}$ Tracking::CreateNewKeyFrame() 函数创建的地图点，是根据深度值进行创建的，如果没有深度值则不会创建，比如单目相机。也就是说，其创建关键点生成的地图点，都是依赖相机的硬件属性(如双目或者RGBD)。但是CreateNewMapPoints() 函数创建地图点是利用多个关键帧关键点的匹配属性。所以其也会为单目相机创建关键点。

在讲解之前，请大家仔细阅读这篇博客: (01)ORB-SLAM2源码无死角解析-(32) ORB特征匹配→局部建图BoW加速匹配，三角化SearchForTriangulation

$\color{blue}{(1):}$ 首先设置搜索最佳共视关键帧的数目nn，然后在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧。获取当当前帧从世界坐标系到相机坐标系的变换矩阵，以及得到当前关键帧（左目）光心在世界坐标系中的坐标、内参。然后循环对所有相邻关键帧进行处理。

$\color{blue}{(2):}$ 获取一个相邻关键帧 pKF2，得到 pKF2 光心在世界坐标系中的坐标，获得基线向量(两个关键帧间的相机位移)，如果相机运动的基线太短，那么跳过当前邻接的关键帧，不生成3D点。如果是双目相机关键帧间距需要大于本身的基线，单目相机则需要关键帧间距大于场景深度中值（其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值作为当前场景的平均深度，q=2表示中值）。

$\color{blue}{(3):}$ 根据两个关键帧的位姿计算它们之间的基础矩阵，不是很明白基本矩阵的朋友可以参考这篇博客史上最简SLAM零基础解读(2) - 对极约束→Essential矩阵、Fundamental矩阵推导。然后利用调用 ORBmatcher::SearchForTriangulation 函数，源码中把公式 $v_1^T\mathbf F_{12}v_2=0$ 中的 $v_1^T\mathbf F_{12}$ 看作极线，也就是极线位于帧 pKF2 中。利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对。

$\color{blue}{(4):}$ 源码中的 cosParallaxStereo1表示当前关键帧视差角度的余弦值，cosParallaxStereo2 表示 pKF2 视差角度的余弦值。然后获得他们最小的一个赋值给 cosParallaxStereo:
①如果匹配点对夹角合理(双目还需大于双目本身观察三维点夹角)，则用三角法恢复3D点。具体过程可以参考博客(01)ORB-SLAM2源码无死角解析-(21) 特征点三角化、深度计算、三维点筛选
②如果是双目，但是匹配点对夹角小(两关键帧距离太近)，且该关键点具备深度信息，则直接用3D点反投影了，也就是UnprojectStereo()函数。

如果不是双目，且匹配关键点的视差很低，则放弃当前匹配点，生成3D点之后，再对其进行检测，判断3D点是否在相机前方,不在的话就放弃这个点。

$\color{blue}{(5):}$ 计算3D点在当前关键帧下以及相邻关键帧的重投影误差，如果为双目，其还会计算右目的重投影误差。另外，如果已知关键点在左目的坐标，求解其匹配点在右目的坐标，公式为 $d=\frac{f}{d}/Z$ ，具体细节可以参考博客(01)ORB-SLAM2源码无死角解析-(27) 双目Stereo相机立体匹配,SAD算法→深度求解

$\color{blue}{(6):}$ 判断3D点到当前关键帧与相邻关键帧的距离是否合理，如果距离太近，比如为0，则放弃这个点。

$\color{blue}{(7):}$ 通过层层筛选，最终成功创建一个3D地图点，为该地图点添加观测帧，很明显当前关键帧与相邻关键帧都能够观测到该关键点，同时要为前关键帧与相邻关键帧添加该地图点。宁外，还需要计算该地图点的描述子，更新平均观测方向。最后把地图添加进全局地图mpMap，同时添添加到 mlpRecentAddedMapPoints 之中，与上一篇博客联系到一起，这些MapPoints都会经过MapPointCulling函数的检验。

四、CreateNewMapPoints→代码注释

其位于 src/LocalMapping.cc 文件中

/**
 * @brief 用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
 * 
 */
void LocalMapping::CreateNewMapPoints()
{
    // Retrieve neighbor keyframes in covisibility graph
    // nn表示搜索最佳共视关键帧的数目
    // 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图
    int nn = 10;
    if(mbMonocular)
        nn=20;

    // Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
    const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

    // 特征点匹配配置 最佳距离 < 0.6*次佳距离，比较苛刻了。不检查旋转
    ORBmatcher matcher(0.6,false); 

    // 取出当前帧从世界坐标系到相机坐标系的变换矩阵
    cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
    cv::Mat Rwc1 = Rcw1.t();
    cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
    cv::Mat Tcw1(3,4,CV_32F);
    Rcw1.copyTo(Tcw1.colRange(0,3));
    tcw1.copyTo(Tcw1.col(3));

    // 得到当前关键帧（左目）光心在世界坐标系中的坐标、内参
    cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
    const float &fx1 = mpCurrentKeyFrame->fx;
    const float &fy1 = mpCurrentKeyFrame->fy;
    const float &cx1 = mpCurrentKeyFrame->cx;
    const float &cy1 = mpCurrentKeyFrame->cy;
    const float &invfx1 = mpCurrentKeyFrame->invfx;
    const float &invfy1 = mpCurrentKeyFrame->invfy;

    // 用于后面的点深度的验证;这里的1.5是经验值
    // mfScaleFactor = 1.2
    const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;

    // 记录三角化成功的地图点数目
    int nnew=0;

    // Search matches with epipolar restriction and triangulate
    // Step 2：遍历相邻关键帧，搜索匹配并用极线约束剔除误匹配，最终三角化
    for(size_t i=0; i<vpNeighKFs.size(); i++)
    {
        // ! 疑似bug，正确应该是 if(i>0 && !CheckNewKeyFrames())
        if(i>0 && CheckNewKeyFrames())
            return;

        KeyFrame* pKF2 = vpNeighKFs[i];

        // Check first that baseline is not too short
        // 相邻的关键帧光心在世界坐标系中的坐标
        cv::Mat Ow2 = pKF2->GetCameraCenter();
        // 基线向量，两个关键帧间的相机位移
        cv::Mat vBaseline = Ow2-Ow1;
        // 基线长度
        const float baseline = cv::norm(vBaseline);

        // Step 3：判断相机运动的基线是不是足够长
        if(!mbMonocular)
        {
            // 如果是双目相机，关键帧间距小于本身的基线时不生成3D点
            // 因为太短的基线下能够恢复的地图点不稳定
            if(baseline<pKF2->mb)
            continue;
        }
        else    
        {
            // 单目相机情况
            // 相邻关键帧的场景深度中值
            const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
            // 基线与景深的比例
            const float ratioBaselineDepth = baseline/medianDepthKF2;
            // 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点
            if(ratioBaselineDepth<0.01)
                continue;
        }

        // Compute Fundamental Matrix
        // Step 4：根据两个关键帧的位姿计算它们之间的基础矩阵
        cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

        // Search matches that fullfil epipolar constraint
        // Step 5：通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对
        vector<pair<size_t,size_t> > vMatchedIndices;
        matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

        cv::Mat Rcw2 = pKF2->GetRotation();
        cv::Mat Rwc2 = Rcw2.t();
        cv::Mat tcw2 = pKF2->GetTranslation();
        cv::Mat Tcw2(3,4,CV_32F);
        Rcw2.copyTo(Tcw2.colRange(0,3));
        tcw2.copyTo(Tcw2.col(3));

        const float &fx2 = pKF2->fx;
        const float &fy2 = pKF2->fy;
        const float &cx2 = pKF2->cx;
        const float &cy2 = pKF2->cy;
        const float &invfx2 = pKF2->invfx;
        const float &invfy2 = pKF2->invfy;

        // Triangulate each match
        // Step 6：对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
        const int nmatches = vMatchedIndices.size();
        for(int ikp=0; ikp<nmatches; ikp++)
        {
            // Step 6.1：取出匹配特征点

            // 当前匹配对在当前关键帧中的索引
            const int &idx1 = vMatchedIndices[ikp].first;
            
            // 当前匹配对在邻接关键帧中的索引
            const int &idx2 = vMatchedIndices[ikp].second;

            // 当前匹配在当前关键帧中的特征点
            const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
            // mvuRight中存放着双目的深度值，如果不是双目，其值将为-1
            const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
            bool bStereo1 = kp1_ur>=0;

            // 当前匹配在邻接关键帧中的特征点
            const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
            // mvuRight中存放着双目的深度值，如果不是双目，其值将为-1
            const float kp2_ur = pKF2->mvuRight[idx2];
            bool bStereo2 = kp2_ur>=0;

            // Check parallax between rays
            // Step 6.2：利用匹配点反投影得到视差角
            // 特征点反投影,其实得到的是在各自相机坐标系下的一个归一化的方向向量,和这个点的反投影射线重合
            cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
            cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);

            // 由相机坐标系转到世界坐标系(得到的是那条反投影射线的一个同向向量在世界坐标系下的表示,还是只能够表示方向)，得到视差角余弦值
            cv::Mat ray1 = Rwc1*xn1;
            cv::Mat ray2 = Rwc2*xn2;
            // 匹配点射线夹角余弦值
            const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));

            // 加1是为了让cosParallaxStereo随便初始化为一个很大的值
            float cosParallaxStereo = cosParallaxRays+1;  
            // cosParallaxStereo1、cosParallaxStereo2在后面可能不存在，需要初始化为较大的值
            float cosParallaxStereo1 = cosParallaxStereo;
            float cosParallaxStereo2 = cosParallaxStereo;

            // Step 6.3：对于双目，利用双目得到视差角；单目相机没有特殊操作
            if(bStereo1)
                // 传感器是双目相机,并且当前的关键帧的这个点有对应的深度
                // 假设是平行的双目相机，计算出双目相机观察这个点的时候的视差角余弦
                cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
            else if(bStereo2)
                // 传感器是双目相机,并且邻接的关键帧的这个点有对应的深度，和上面一样操作
                cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
            
            // 得到双目观测的视差角中最小的那个
            cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);

            // Step 6.4：三角化恢复3D点
            cv::Mat x3D;
            // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1°
            // cosParallaxRays < cosParallaxStereo 表明匹配点对夹角大于双目本身观察三维点夹角
            // 匹配点对夹角大，用三角法恢复3D点
            // 参考：https://github.com/raulmur/ORB_SLAM2/issues/345
            if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
            {
                // Linear Triangulation Method
                // 见Initializer.cc的 Triangulate 函数,实现是一样的,顶多就是把投影矩阵换成了变换矩阵
                cv::Mat A(4,4,CV_32F);
                A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
                A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
                A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
                A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);

                cv::Mat w,u,vt;
                cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);

                x3D = vt.row(3).t();
                // 归一化之前的检查
                if(x3D.at<float>(3)==0)
                    continue;
                // 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
                x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
            }
            // 匹配点对夹角小，用双目恢复3D点
            else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)  
            {
                // 如果是双目，用视差角更大的那个双目信息来恢复，直接用已知3D点反投影了
                x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);                
            }
            else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)  
            {
                x3D = pKF2->UnprojectStereo(idx2);
            }
            else
                continue; //No stereo and very low parallax, 放弃

            // 为方便后续计算，转换成为了行向量
            cv::Mat x3Dt = x3D.t();

            //Check triangulation in front of cameras
            // Step 6.5：检测生成的3D点是否在相机前方,不在的话就放弃这个点
            float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
            if(z1<=0)
                continue;

            float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
            if(z2<=0)
                continue;

            //Check reprojection error in first keyframe
            // Step 6.6：计算3D点在当前关键帧下的重投影误差
            const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
            const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
            const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
            const float invz1 = 1.0/z1;

            if(!bStereo1)
            {
                // 单目情况下
                float u1 = fx1*x1*invz1+cx1;
                float v1 = fy1*y1*invz1+cy1;
                float errX1 = u1 - kp1.pt.x;
                float errY1 = v1 - kp1.pt.y;
                // 假设测量有一个像素的偏差，2自由度卡方检验阈值是5.991
                if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
                    continue;
            }
            else
            {
                // 双目情况
                float u1 = fx1*x1*invz1+cx1;
                // 根据视差公式计算假想的右目坐标
                float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;     
                float v1 = fy1*y1*invz1+cy1;
                float errX1 = u1 - kp1.pt.x;
                float errY1 = v1 - kp1.pt.y;
                float errX1_r = u1_r - kp1_ur;
                // 自由度为3，卡方检验阈值是7.8
                if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
                    continue;
            }

            //Check reprojection error in second keyframe
            // 计算3D点在另一个关键帧下的重投影误差，操作同上
            const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
            const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
            const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
            const float invz2 = 1.0/z2;
            if(!bStereo2)
            {
                float u2 = fx2*x2*invz2+cx2;
                float v2 = fy2*y2*invz2+cy2;
                float errX2 = u2 - kp2.pt.x;
                float errY2 = v2 - kp2.pt.y;
                if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
                    continue;
            }
            else
            {
                float u2 = fx2*x2*invz2+cx2;
                float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;
                float v2 = fy2*y2*invz2+cy2;
                float errX2 = u2 - kp2.pt.x;
                float errY2 = v2 - kp2.pt.y;
                float errX2_r = u2_r - kp2_ur;
                if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
                    continue;
            }

            //Check scale consistency
            // Step 6.7：检查尺度连续性

            // 世界坐标系下，3D点与相机间的向量，方向由相机指向3D点
            cv::Mat normal1 = x3D-Ow1;
            float dist1 = cv::norm(normal1);

            cv::Mat normal2 = x3D-Ow2;
            float dist2 = cv::norm(normal2);

            if(dist1==0 || dist2==0)
                continue;

            // ratioDist是不考虑金字塔尺度下的距离比例
            const float ratioDist = dist2/dist1;
            // 金字塔尺度因子的比例
            const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];

            /*if(fabs(ratioDist-ratioOctave)>ratioFactor)
                continue;*/

            // 距离的比例和图像金字塔的比例不应该差太多，否则就跳过
            if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
                continue;

            // Triangulation is succesfull
            // Step 6.8：三角化生成3D点成功，构造成MapPoint
            MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);

            // Step 6.9：为该MapPoint添加属性：
            // a.观测到该MapPoint的关键帧
            pMP->AddObservation(mpCurrentKeyFrame,idx1);            
            pMP->AddObservation(pKF2,idx2);

            mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
            pKF2->AddMapPoint(pMP,idx2);

            // b.该MapPoint的描述子
            pMP->ComputeDistinctiveDescriptors();

            // c.该MapPoint的平均观测方向和深度范围
            pMP->UpdateNormalAndDepth();

            mpMap->AddMapPoint(pMP);

            // Step 6.10：将新产生的点放入检测队列
            // 这些MapPoints都会经过MapPointCulling函数的检验
            mlpRecentAddedMapPoints.push_back(pMP);

            nnew++;
        }
    }
}

五、结语

通过该篇博客，对 LocalMapping::Run() 函数中调用的函数 CreateNewMapPoints() 进行了讲解。那么接下来就是对 SearchInNeighbors() 进行讲解了，其主要功能是检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点。

江南才尽，年少无知！

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
打赏
8
评论
(01)ORB-SLAM2源码无死角解析-(49) 局部建图线程→MapPointCulling,CreateNewMapPoints

通过上一篇博客，对局部建图线程 LocalMapping::Run() 做了整体的介绍，然后分析了 ProcessNewKeyFrame() 函数，该函数主要功能为计算关键帧的BoW、更新观测、描述子、共视图，插入到地图等。下面主要是对 LocalMapping::Run() 中调用的 MapPointCulling() 函数进行讲解，其位于 src/LocalMapping.cc 文件中。...
复制链接

扫一扫